navigation: adds local interface checks, updates example node

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

@@ -13,68 +13,54 @@
 
 using namespace std::chrono_literals; // NOLINT
 
-static const std::string kName = "local_navigation_test";
-static const std::string kNodeName = "example_local_navigation_node";
-
-class LocalNavigationTest : public px4_ros2::Context
+class ExampleLocalNavigationNode : public rclcpp::Node
 {
 public:
-  explicit LocalNavigationTest(rclcpp::Node & node)
-  : Context(node), _node(node)
+  ExampleLocalNavigationNode()
+  : Node("example_local_navigation_node")
   {
-    const uint8_t pose_frame = 0;
-    const uint8_t velocity_frame = 0;
+    // Enable debug output
+    auto ret =
+      rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
+
+    if (ret != RCUTILS_RET_OK) {
+      RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
+      rcutils_reset_error();
+    }
 
+    // Instantiate local navigation interface
+    const uint8_t pose_frame = AuxLocalPosition::POSE_FRAME_NED;
+    const uint8_t velocity_frame = AuxLocalPosition::VELOCITY_FRAME_NED;
     _local_navigation_interface = std::make_shared<px4_ros2::LocalNavigationInterface>(
       *this,
       pose_frame,
       velocity_frame);
 
+    _timer =
+      create_wall_timer(1s, std::bind(&ExampleLocalNavigationNode::updateAuxLocalPosition, this));
+
+    RCLCPP_INFO(get_logger(), "example_local_navigation_node running!");
   }
 
   void updateAuxLocalPosition()
   {
     px4_ros2::LocalPositionEstimate local_position_estimate {};
 
-    local_position_estimate.timestamp_sample = _node.get_clock()->now().nanoseconds() * 1e-3;
-    local_position_estimate.velocity_xy = Eigen::Vector2f{1.f, 2.f};
-    local_position_estimate.velocity_xy_variance = Eigen::Vector2f{0.3f, 0.4f};
-
-    _local_navigation_interface->update(local_position_estimate);
-  }
-
-private:
-  rclcpp::Node & _node;
-  std::shared_ptr<px4_ros2::LocalNavigationInterface> _local_navigation_interface;
-};
-
-class TestNode : public rclcpp::Node
-{
-public:
-  TestNode()
-  : Node(kNodeName)
-  {
-    // Enable debug output
-    auto ret =
-      rcutils_logging_set_logger_level(get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
-
-    if (ret != RCUTILS_RET_OK) {
-      RCLCPP_ERROR(get_logger(), "Error setting severity: %s", rcutils_get_error_string().str);
-      rcutils_reset_error();
-    }
+    local_position_estimate.timestamp_sample = this->now().nanoseconds() * 1e-3;
 
-    _local_navigation_test = std::make_unique<LocalNavigationTest>(*this);
+    local_position_estimate.velocity_xy = Eigen::Vector2f {1.f, 2.f};
+    local_position_estimate.velocity_xy_variance = Eigen::Vector2f {0.3f, 0.4f};
 
-    RCLCPP_INFO(get_logger(), "TestNode running!");
+    local_position_estimate.position_z = 12.3f;
+    local_position_estimate.position_z_variance = 0.33f;
 
-    int counter = 10;
-    while (counter-- > 0) {
-      _local_navigation_test->updateAuxLocalPosition();
-      sleep(1);
-    }
+    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);
   }
 
 private:
-  std::unique_ptr<LocalNavigationTest> _local_navigation_test;
+  std::shared_ptr<px4_ros2::LocalNavigationInterface> _local_navigation_interface;
+  rclcpp::TimerBase::SharedPtr _timer;
 };

examples/cpp/navigation/local_navigation/src/main.cpp

@@ -11,7 +11,7 @@
 int main(int argc, char * argv[])
 {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<TestNode>());
+  rclcpp::spin(std::make_shared<ExampleLocalNavigationNode>());
   rclcpp::shutdown();
   return 0;
 }

px4_ros2_cpp/CMakeLists.txt

@@ -39,6 +39,7 @@ set(HEADER_FILES
         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/local_navigation_interface.hpp

@@ -6,11 +6,14 @@
 #pragma once
 
 #include <optional>
-#include <px4_msgs/msg/vehicle_odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <Eigen/Eigen>
 #include <px4_ros2/common/context.hpp>
 
+#include <px4_msgs/msg/vehicle_odometry.hpp>
+#include <px4_ros2/common/context.hpp>
+#include <px4_ros2/navigation/experimental/navigation_interface_codes.hpp>
+
 using namespace Eigen;
 using AuxLocalPosition = px4_msgs::msg::VehicleOdometry;
 
@@ -41,19 +44,53 @@ struct LocalPositionEstimate
 class LocalNavigationInterface
 {
 public:
-  explicit LocalNavigationInterface(Context & context, uint8_t pose_frame, uint8_t velocity_frame);
+  explicit LocalNavigationInterface(
+    rclcpp::Node & node, const uint8_t pose_frame,
+    const uint8_t velocity_frame);
   ~LocalNavigationInterface() = default;
 
-  void update(LocalPositionEstimate & local_position_estimate);
+  /**
+   * @brief Publish local position estimate to FMU.
+   */
+  int update(const LocalPositionEstimate & local_position_estimate) const;
 
-private:
   const std::string AUX_LOCAL_POSITION_TOPIC = "/fmu/in/vehicle_odometry";
 
+private:
+  /**
+   * @brief Check that at least one estimate value is defined.
+   */
+  bool _checkEstimateEmpty(const LocalPositionEstimate & estimate) const;
+
+  /**
+   * @brief Check that if an estimate value is defined, its variance is also defined and strictly greater than zero.
+   */
+  bool _checkVarianceValid(const LocalPositionEstimate & estimate) const;
+
+  /**
+   * @brief Check that if an estimate value is defined, its associated frame is not *FRAME_UNKNOWN.
+   */
+  bool _checkFrameValid(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] = {
+    AuxLocalPosition::POSE_FRAME_UNKNOWN,
+    AuxLocalPosition::POSE_FRAME_NED,
+    AuxLocalPosition::POSE_FRAME_FRD
+  };
+
+  uint8_t _available_velocity_frames[4] = {
+    AuxLocalPosition::VELOCITY_FRAME_UNKNOWN,
+    AuxLocalPosition::VELOCITY_FRAME_NED,
+    AuxLocalPosition::VELOCITY_FRAME_FRD,
+    AuxLocalPosition::VELOCITY_FRAME_BODY_FRD
+  };
+
 };
 
 } // namespace px4_ros2

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

@@ -0,0 +1,10 @@
+#pragma once
+
+// Define navigation interface return codes
+enum class NavigationInterfaceCodes : int
+{
+  SUCCESS = 0,
+  ESTIMATE_EMPTY = 1,
+  ESTIMATE_VARIANCE_INVALID = 2,
+  ESTIMATE_FRAME_UNKNOWN = 3
+};

px4_ros2_cpp/src/navigation/experimental/local_navigation_interface.cpp

@@ -10,20 +10,59 @@ namespace px4_ros2
 {
 
 LocalNavigationInterface::LocalNavigationInterface(
-  Context & context, uint8_t pose_frame,
-  uint8_t velocity_frame)
-: _node(context.node()),
+  rclcpp::Node & node, const uint8_t pose_frame,
+  const uint8_t velocity_frame)
+: _node(node),
   _pose_frame(pose_frame),
   _velocity_frame(velocity_frame)
 {
-  _aux_local_position_pub =
-    context.node().create_publisher<AuxLocalPosition>(
-    context.topicNamespacePrefix() + AUX_LOCAL_POSITION_TOPIC, 10);
+  // Check that pose and velocity reference frames are valid
+  auto it_pose_frame = std::find(
+    std::begin(_available_pose_frames), std::end(
+      _available_pose_frames), pose_frame);
+  if (it_pose_frame == std::end(_available_pose_frames)) {
+    RCLCPP_ERROR(
+      node.get_logger(), "Failed to initialize LocalNavigationInterface: invalid pose reference frame %i.",
+      pose_frame);
+    rclcpp::shutdown();
+  }
+
+  auto it_vel_frame =
+    std::find(
+    std::begin(_available_velocity_frames), std::end(
+      _available_velocity_frames), velocity_frame);
+  if (it_vel_frame == std::end(_available_velocity_frames)) {
+    RCLCPP_ERROR(
+      node.get_logger(), "Failed to initialize LocalNavigationInterface: invalid velocity reference frame %i.",
+      velocity_frame);
+    rclcpp::shutdown();
+  }
+
+  _aux_local_position_pub = node.create_publisher<AuxLocalPosition>(AUX_LOCAL_POSITION_TOPIC, 10);
 
 }
 
-void LocalNavigationInterface::update(LocalPositionEstimate & local_position_estimate)
+int LocalNavigationInterface::update(const LocalPositionEstimate & local_position_estimate) const
 {
+  // Run basic sanity checks on local position estimate
+  if (_checkEstimateEmpty(local_position_estimate)) {
+    RCLCPP_WARN(_node.get_logger(), "Estimate values are all empty.");
+    return static_cast<int>(NavigationInterfaceCodes::ESTIMATE_EMPTY);
+  }
+
+  if (!_checkVarianceValid(local_position_estimate)) {
+    return static_cast<int>(NavigationInterfaceCodes::ESTIMATE_VARIANCE_INVALID);
+  }
+
+  if (!_checkFrameValid(local_position_estimate)) {
+    return static_cast<int>(NavigationInterfaceCodes::ESTIMATE_FRAME_UNKNOWN);
+  }
+
+  if (local_position_estimate.timestamp_sample == 0) {
+    RCLCPP_WARN(_node.get_logger(), "Estimate sample timestamp is empty.");
+  }
+
+  // Populate aux local position message
   AuxLocalPosition aux_local_position;
 
   aux_local_position.timestamp_sample = local_position_estimate.timestamp_sample;
@@ -72,11 +111,91 @@ void LocalNavigationInterface::update(LocalPositionEstimate & local_position_est
   aux_local_position.velocity_variance[2] =
     local_position_estimate.velocity_z_variance.value_or(NAN);
 
+  // Angular velocity (unused at the moment)
+  aux_local_position.angular_velocity = {NAN, NAN, NAN};
+
   // Publish
   aux_local_position.timestamp = _node.get_clock()->now().nanoseconds() * 1e-3;
   _aux_local_position_pub->publish(aux_local_position);
 
-  std::cout << "Published aux local position!\n";
+  return static_cast<int>(NavigationInterfaceCodes::SUCCESS);
+
+}
+
+bool LocalNavigationInterface::_checkEstimateEmpty(const LocalPositionEstimate & estimate) const
+{
+  return !estimate.position_xy.has_value() &&
+         !estimate.position_z.has_value() &&
+         !estimate.velocity_xy.has_value() &&
+         !estimate.velocity_z.has_value() &&
+         !estimate.attitude_quaternion.has_value();
+}
+
+bool LocalNavigationInterface::_checkVarianceValid(const LocalPositionEstimate & estimate) const
+{
+  if (estimate.position_xy.has_value() &&
+    (!estimate.position_xy_variance.has_value() ||
+    (estimate.position_xy_variance.value().array() <= 0).any()))
+  {
+    RCLCPP_WARN(_node.get_logger(), "Estimate value position_xy has an invalid variance value.");
+    return false;
+  }
+
+  if (estimate.position_z.has_value() &&
+    (!estimate.position_z_variance.has_value() || estimate.position_z_variance <= 0))
+  {
+    RCLCPP_WARN(_node.get_logger(), "Estimate value position_z has an invalid variance value.");
+    return false;
+  }
+
+  if (estimate.velocity_xy.has_value() &&
+    (!estimate.velocity_xy_variance.has_value() ||
+    (estimate.velocity_xy_variance.value().array() <= 0).any()))
+  {
+    RCLCPP_WARN(_node.get_logger(), "Estimate value velocity_xy has an invalid variance value.");
+    return false;
+  }
+
+  if (estimate.velocity_z.has_value() &&
+    (!estimate.velocity_z_variance.has_value() || estimate.velocity_z_variance <= 0))
+  {
+    RCLCPP_WARN(_node.get_logger(), "Estimate value velocity_z has an invalid variance value.");
+    return false;
+  }
+
+  if (estimate.attitude_quaternion.has_value() &&
+    (!estimate.attitude_variance.has_value() ||
+    (estimate.attitude_variance.value().array() <= 0).any()))
+  {
+    RCLCPP_WARN(
+      _node.get_logger(), "Estimate value attitude_quaternion has an invalid variance value.");
+    return false;
+  }
+
+  return true;
+}
+
+bool LocalNavigationInterface::_checkFrameValid(const LocalPositionEstimate & estimate) const
+{
+  if ((estimate.position_xy.has_value() || estimate.position_z.has_value()) &&
+    _pose_frame == AuxLocalPosition::POSE_FRAME_UNKNOWN)
+  {
+    RCLCPP_WARN(
+      _node.get_logger(),
+      "Position estimate update requested but pose reference frame is set to POSE_FRAME_UNKNOWN.");
+    return false;
+  }
+
+  if ((estimate.velocity_xy.has_value() || estimate.velocity_z.has_value()) &&
+    _pose_frame == AuxLocalPosition::VELOCITY_FRAME_UNKNOWN)
+  {
+    RCLCPP_WARN(
+      _node.get_logger(),
+      "Velocity estimate update requested but velocity reference frame is set to VELOCITY_FRAME_UNKNOWN.");
+    return false;
+  }
+
+  return true;
 }
 
 } // namespace px4_ros2