navigation: applies PR review suggestions

.gitignore

@@ -385,6 +385,3 @@ pyrightconfig.json
 build/
 install/
 log/
-
-# Vscode
-.vscode

examples/cpp/navigation/global_navigation/include/global_navigation.hpp

@@ -46,7 +46,32 @@ class ExampleGlobalNavigationNode : public rclcpp::Node
     global_position_estimate.altitude_agl = 12.4f;
     global_position_estimate.positional_uncertainty = 0.4f;
 
-    _global_navigation_interface->update(global_position_estimate);
+    NavigationInterfaceReturnCode retcode;
+    retcode = _global_navigation_interface->update(global_position_estimate);
+
+    switch (retcode) {
+      case NavigationInterfaceReturnCode::SUCCESS:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: Success.");
+        break;
+      case NavigationInterfaceReturnCode::ESTIMATE_EMPTY:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: estimate empty.");
+        break;
+      case NavigationInterfaceReturnCode::ESTIMATE_VARIANCE_INVALID:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: variance invalid.");
+        break;
+      case NavigationInterfaceReturnCode::ESTIMATE_FRAME_UNKNOWN:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: estimate has unknown frame.");
+        break;
+      case NavigationInterfaceReturnCode::ESTIMATE_VALUE_NAN:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: estimate contains NAN.");
+        break;
+      case NavigationInterfaceReturnCode::ESTIMATE_MISSING_TIMESTAMP:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: estimate missing timestamp.");
+        break;
+      default:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with unknown return code: %i", (int)retcode);
+        break;
+    }
   }
 
 private:

examples/cpp/navigation/local_navigation/include/local_navigation.hpp

@@ -56,7 +56,33 @@ class ExampleLocalNavigationNode : public rclcpp::Node
     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};
 
-    _local_navigation_interface->update(local_position_estimate);
+    NavigationInterfaceReturnCode retcode;
+    retcode = _local_navigation_interface->update(local_position_estimate);
+
+    switch (retcode) {
+      case NavigationInterfaceReturnCode::SUCCESS:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: Success.");
+        break;
+      case NavigationInterfaceReturnCode::ESTIMATE_EMPTY:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: estimate empty.");
+        break;
+      case NavigationInterfaceReturnCode::ESTIMATE_VARIANCE_INVALID:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: variance invalid.");
+        break;
+      case NavigationInterfaceReturnCode::ESTIMATE_FRAME_UNKNOWN:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: estimate has unknown frame.");
+        break;
+      case NavigationInterfaceReturnCode::ESTIMATE_VALUE_NAN:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: estimate contains NAN.");
+        break;
+      case NavigationInterfaceReturnCode::ESTIMATE_MISSING_TIMESTAMP:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with: estimate missing timestamp.");
+        break;
+      default:
+        RCLCPP_DEBUG(get_logger(), "Interface returned with unknown return code: %i", (int)retcode);
+        break;
+    }
+
   }
 
 private:

px4_ros2_cpp/CMakeLists.txt

@@ -35,11 +35,11 @@ set(HEADER_FILES
         include/px4_ros2/control/setpoint_types/experimental/attitude.hpp
         include/px4_ros2/control/setpoint_types/experimental/rates.hpp
         include/px4_ros2/control/setpoint_types/experimental/trajectory.hpp
+        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_common.hpp
         include/px4_ros2/odometry/global_position.hpp
         include/px4_ros2/odometry/local_position.hpp
-        include/px4_ros2/navigation/experimental/local_navigation_interface.hpp
-        include/px4_ros2/navigation/experimental/global_navigation_interface.hpp
-        include/px4_ros2/navigation/experimental/navigation_interface_codes.hpp
 )
 
 add_library(px4_ros2_cpp

px4_ros2_cpp/include/px4_ros2/navigation/experimental/global_navigation_interface.hpp

@@ -10,7 +10,7 @@
 #include <Eigen/Eigen>
 
 #include <px4_msgs/msg/aux_global_position.hpp>
-#include <px4_ros2/navigation/experimental/navigation_interface_codes.hpp>
+#include <px4_ros2/navigation/experimental/navigation_interface_common.hpp>
 
 using namespace Eigen;
 using namespace px4_msgs::msg;
@@ -41,7 +41,8 @@ class GlobalNavigationInterface
   /**
    * @brief Publish global position estimate to FMU.
    */
-  int update(const GlobalPositionEstimate & global_position_estimate) const;
+  NavigationInterfaceReturnCode update(const GlobalPositionEstimate & global_position_estimate)
+  const;
 
   const std::string AUX_GLOBAL_POSITION_TOPIC = "/fmu/in/aux_global_position";
 
@@ -61,6 +62,11 @@ class GlobalNavigationInterface
    */
   bool _checkFrameValid(const GlobalPositionEstimate & estimate) const;
 
+  /**
+   * @brief Check that if an estimate value is defined, none of its fields are NAN.
+   */
+  bool _checkValuesNotNAN(const GlobalPositionEstimate & estimate) const;
+
   rclcpp::Node & _node;
   rclcpp::Publisher<AuxGlobalPosition>::SharedPtr _aux_global_position_pub;
 

px4_ros2_cpp/include/px4_ros2/navigation/experimental/local_navigation_interface.hpp

@@ -10,7 +10,7 @@
 #include <Eigen/Eigen>
 
 #include <px4_msgs/msg/vehicle_odometry.hpp>
-#include <px4_ros2/navigation/experimental/navigation_interface_codes.hpp>
+#include <px4_ros2/navigation/experimental/navigation_interface_common.hpp>
 
 using namespace Eigen;
 using AuxLocalPosition = px4_msgs::msg::VehicleOdometry;
@@ -50,9 +50,9 @@ class LocalNavigationInterface
   /**
    * @brief Publish local position estimate to FMU.
    */
-  int update(const LocalPositionEstimate & local_position_estimate) const;
+  NavigationInterfaceReturnCode update(const LocalPositionEstimate & local_position_estimate) const;
 
-  const std::string AUX_LOCAL_POSITION_TOPIC = "/fmu/in/vehicle_odometry";
+  const std::string AUX_LOCAL_POSITION_TOPIC = "/fmu/in/vehicle_visual_odometry";
 
 private:
   /**
@@ -70,19 +70,24 @@ class LocalNavigationInterface
    */
   bool _checkFrameValid(const LocalPositionEstimate & estimate) const;
 
+  /**
+   * @brief Check that if an estimate value is defined, none of its fields are NAN.
+   */
+  bool _checkValuesNotNAN(const LocalPositionEstimate & estimate) const;
+
   rclcpp::Node & _node;
   rclcpp::Publisher<AuxLocalPosition>::SharedPtr _aux_local_position_pub;
 
   uint8_t _pose_frame;
   uint8_t _velocity_frame;
 
-  uint8_t _available_pose_frames[3] = {
+  static constexpr uint8_t _available_pose_frames[3] = {
     AuxLocalPosition::POSE_FRAME_UNKNOWN,
     AuxLocalPosition::POSE_FRAME_NED,
     AuxLocalPosition::POSE_FRAME_FRD
   };
 
-  uint8_t _available_velocity_frames[4] = {
+  static constexpr uint8_t _available_velocity_frames[4] = {
     AuxLocalPosition::VELOCITY_FRAME_UNKNOWN,
     AuxLocalPosition::VELOCITY_FRAME_NED,
     AuxLocalPosition::VELOCITY_FRAME_FRD,

px4_ros2_cpp/include/px4_ros2/navigation/experimental/navigation_interface_common.hpp

@@ -0,0 +1,12 @@
+#pragma once
+
+// 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
+};

px4_ros2_cpp/src/navigation/experimental/global_navigation_interface.cpp

@@ -16,16 +16,26 @@ GlobalNavigationInterface::GlobalNavigationInterface(rclcpp::Node & node)
     node.create_publisher<AuxGlobalPosition>(AUX_GLOBAL_POSITION_TOPIC, 10);
 }
 
-int GlobalNavigationInterface::update(const GlobalPositionEstimate & global_position_estimate) const
+NavigationInterfaceReturnCode GlobalNavigationInterface::update(
+  const GlobalPositionEstimate & global_position_estimate) const
 {
   // Run basic sanity checks on global position estimate
   if (_checkEstimateEmpty(global_position_estimate)) {
-    RCLCPP_WARN(_node.get_logger(), "Estimate values are all empty.");
-    return static_cast<int>(NavigationInterfaceCodes::ESTIMATE_EMPTY);
+    RCLCPP_DEBUG(_node.get_logger(), "Estimate values are all empty.");
+    return NavigationInterfaceReturnCode::ESTIMATE_EMPTY;
   }
 
   if (!_checkVarianceValid(global_position_estimate)) {
-    return static_cast<int>(NavigationInterfaceCodes::ESTIMATE_VARIANCE_INVALID);
+    return NavigationInterfaceReturnCode::ESTIMATE_VARIANCE_INVALID;
+  }
+
+  if (!_checkValuesNotNAN(global_position_estimate)) {
+    return NavigationInterfaceReturnCode::ESTIMATE_VALUE_NAN;
+  }
+
+  if (global_position_estimate.timestamp_sample == 0) {
+    RCLCPP_DEBUG(_node.get_logger(), "Estimate timestamp sample is empty.");
+    return NavigationInterfaceReturnCode::ESTIMATE_MISSING_TIMESTAMP;
   }
 
   // Populate aux global position
@@ -53,7 +63,7 @@ int GlobalNavigationInterface::update(const GlobalPositionEstimate & global_posi
   aux_global_position.timestamp = _node.get_clock()->now().nanoseconds() * 1e-3;
   _aux_global_position_pub->publish(aux_global_position);
 
-  return static_cast<int>(NavigationInterfaceCodes::SUCCESS);
+  return NavigationInterfaceReturnCode::SUCCESS;
 }
 
 bool GlobalNavigationInterface::_checkEstimateEmpty(const GlobalPositionEstimate & estimate) const
@@ -66,7 +76,7 @@ bool GlobalNavigationInterface::_checkVarianceValid(const GlobalPositionEstimate
   if (estimate.lat_lon.has_value() &&
     (!estimate.positional_uncertainty.has_value() || estimate.positional_uncertainty.value() <= 0))
   {
-    RCLCPP_WARN(
+    RCLCPP_DEBUG(
       _node.get_logger(),
       "Estimate value lat_lon has an invalid variance value.");
     return false;
@@ -80,4 +90,27 @@ bool GlobalNavigationInterface::_checkFrameValid(const GlobalPositionEstimate &
   return true;
 }
 
+bool GlobalNavigationInterface::_checkValuesNotNAN(const GlobalPositionEstimate & estimate) const
+{
+  if (estimate.lat_lon.has_value() && estimate.lat_lon.value().array().isNaN().any()) {
+    RCLCPP_DEBUG(
+      _node.get_logger(),
+      "Estimate value lat_lon is defined but contains a NAN.");
+    return false;
+  }
+  if (estimate.altitude_agl.has_value() && estimate.altitude_agl == NAN) {
+    RCLCPP_DEBUG(
+      _node.get_logger(),
+      "Estimate value altitude_agl is defined but contains a NAN.");
+    return false;
+  }
+  if (estimate.positional_uncertainty.has_value() && estimate.positional_uncertainty == NAN) {
+    RCLCPP_DEBUG(
+      _node.get_logger(),
+      "Estimate value positional_uncertainty is defined but contains a NAN.");
+    return false;
+  }
+  return true;
+}
+
 } // namespace px4_ros2