navigation: adds convertion function from return code to string

Auterion · Nov 23, 2023 · 6878d9e · 6878d9e
1 parent 7f40f10
commit 6878d9e
Show file tree

Hide file tree

Showing 7 changed files with 36 additions and 66 deletions.
diff --git a/examples/cpp/navigation/global_navigation/include/global_navigation.hpp b/examples/cpp/navigation/global_navigation/include/global_navigation.hpp
@@ -38,33 +38,10 @@ class GlobalNavigationTest : public px4_ros2::Context
     global_position_estimate.altitude_agl = 12.4f;
     global_position_estimate.positional_uncertainty = 0.4f;
 
-    NavigationInterfaceReturnCode retcode;
+    px4_ros2::NavigationInterfaceReturnCode retcode;
     retcode = _global_navigation_interface->update(global_position_estimate);
 
-    switch (retcode) {
-      case NavigationInterfaceReturnCode::SUCCESS:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: success.");
-        break;
-      case NavigationInterfaceReturnCode::ESTIMATE_EMPTY:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: estimate empty.");
-        break;
-      case NavigationInterfaceReturnCode::ESTIMATE_VARIANCE_INVALID:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: variance invalid.");
-        break;
-      case NavigationInterfaceReturnCode::ESTIMATE_FRAME_UNKNOWN:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: estimate has unknown frame.");
-        break;
-      case NavigationInterfaceReturnCode::ESTIMATE_VALUE_NAN:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: estimate contains NAN.");
-        break;
-      case NavigationInterfaceReturnCode::ESTIMATE_MISSING_TIMESTAMP:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: estimate missing timestamp.");
-        break;
-      default:
-        RCLCPP_DEBUG(
-          _node.get_logger(), "Interface returned with unknown return code: %i", (int)retcode);
-        break;
-    }
+    RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: %s.", resultToString(retcode));
   }
 
 private:

diff --git a/examples/cpp/navigation/local_navigation/include/local_navigation.hpp b/examples/cpp/navigation/local_navigation/include/local_navigation.hpp
@@ -47,33 +47,10 @@ class LocalNavigationTest : public px4_ros2::Context
     local_position_estimate.attitude_quaternion = Eigen::Quaternionf {0.1, -0.2, 0.3, 0.25};
     local_position_estimate.attitude_variance = Eigen::Vector3f {0.2, 0.1, 0.05};
 
-    NavigationInterfaceReturnCode retcode;
+    px4_ros2::NavigationInterfaceReturnCode retcode;
     retcode = _local_navigation_interface->update(local_position_estimate);
 
-    switch (retcode) {
-      case NavigationInterfaceReturnCode::SUCCESS:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: success.");
-        break;
-      case NavigationInterfaceReturnCode::ESTIMATE_EMPTY:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: estimate empty.");
-        break;
-      case NavigationInterfaceReturnCode::ESTIMATE_VARIANCE_INVALID:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: variance invalid.");
-        break;
-      case NavigationInterfaceReturnCode::ESTIMATE_FRAME_UNKNOWN:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: estimate has unknown frame.");
-        break;
-      case NavigationInterfaceReturnCode::ESTIMATE_VALUE_NAN:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: estimate contains NAN.");
-        break;
-      case NavigationInterfaceReturnCode::ESTIMATE_MISSING_TIMESTAMP:
-        RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: estimate missing timestamp.");
-        break;
-      default:
-        RCLCPP_DEBUG(
-          _node.get_logger(), "Interface returned with unknown return code: %i", (int)retcode);
-        break;
-    }
+    RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: %s.", resultToString(retcode));
   }
 
 private:

diff --git a/px4_ros2_cpp/CMakeLists.txt b/px4_ros2_cpp/CMakeLists.txt
@@ -38,7 +38,6 @@ set(HEADER_FILES
         include/px4_ros2/navigation/experimental/global_navigation_interface.hpp
         include/px4_ros2/navigation/experimental/local_navigation_interface.hpp
         include/px4_ros2/navigation/experimental/navigation_interface_base.hpp
-        include/px4_ros2/navigation/experimental/navigation_interface_common.hpp
         include/px4_ros2/odometry/global_position.hpp
         include/px4_ros2/odometry/local_position.hpp
 )

diff --git a/px4_ros2_cpp/include/px4_ros2/navigation/experimental/global_navigation_interface.hpp b/px4_ros2_cpp/include/px4_ros2/navigation/experimental/global_navigation_interface.hpp
@@ -11,7 +11,6 @@
 
 #include <px4_msgs/msg/aux_global_position.hpp>
 #include <px4_ros2/navigation/experimental/navigation_interface_base.hpp>
-#include <px4_ros2/navigation/experimental/navigation_interface_common.hpp>
 
 using namespace Eigen;
 using namespace px4_msgs::msg;

diff --git a/px4_ros2_cpp/include/px4_ros2/navigation/experimental/local_navigation_interface.hpp b/px4_ros2_cpp/include/px4_ros2/navigation/experimental/local_navigation_interface.hpp
@@ -11,7 +11,6 @@
 
 #include <px4_msgs/msg/vehicle_odometry.hpp>
 #include <px4_ros2/navigation/experimental/navigation_interface_base.hpp>
-#include <px4_ros2/navigation/experimental/navigation_interface_common.hpp>
 
 using namespace Eigen;
 using AuxLocalPosition = px4_msgs::msg::VehicleOdometry;

diff --git a/px4_ros2_cpp/include/px4_ros2/navigation/experimental/navigation_interface_base.hpp b/px4_ros2_cpp/include/px4_ros2/navigation/experimental/navigation_interface_base.hpp
@@ -6,11 +6,42 @@
 #pragma once
 
 #include <px4_ros2/common/context.hpp>
-#include <px4_ros2/navigation/experimental/navigation_interface_common.hpp>
 
 namespace px4_ros2
 {
 
+// Define navigation interface return codes
+enum class NavigationInterfaceReturnCode : int
+{
+  SUCCESS = 0,
+  ESTIMATE_EMPTY = 1,
+  ESTIMATE_VARIANCE_INVALID = 2,
+  ESTIMATE_FRAME_UNKNOWN = 3,
+  ESTIMATE_VALUE_NAN = 4,
+  ESTIMATE_MISSING_TIMESTAMP = 5
+};
+
+constexpr inline const char * resultToString(NavigationInterfaceReturnCode result) noexcept
+{
+  switch (result) {
+    case NavigationInterfaceReturnCode::SUCCESS: return "SUCCESS";
+
+    case NavigationInterfaceReturnCode::ESTIMATE_EMPTY: return "ESTIMATE_EMPTY";
+
+    case NavigationInterfaceReturnCode::ESTIMATE_VARIANCE_INVALID: return
+        "ESTIMATE_VARIANCE_INVALID";
+
+    case NavigationInterfaceReturnCode::ESTIMATE_FRAME_UNKNOWN: return "ESTIMATE_FRAME_UNKNOWN";
+
+    case NavigationInterfaceReturnCode::ESTIMATE_VALUE_NAN: return "ESTIMATE_VALUE_NAN";
+
+    case NavigationInterfaceReturnCode::ESTIMATE_MISSING_TIMESTAMP: return
+        "ESTIMATE_MISSING_TIMESTAMP ";
+  }
+
+  return "Unknown";
+}
+
 template<typename EstimateType>
 class NavigationInterfaceBase
 {

diff --git a/px4_ros2_cpp/include/px4_ros2/navigation/experimental/navigation_interface_common.hpp b/px4_ros2_cpp/include/px4_ros2/navigation/experimental/navigation_interface_common.hpp