navigation: updates global interface skeleton

Auterion · Nov 20, 2023 · 33332b9 · 33332b9
1 parent 8dbd48e
commit 33332b9
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 14 deletions.
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
@@ -6,10 +6,11 @@
 #pragma once
 
 #include <optional>
-#include <px4_msgs/msg/aux_global_position.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <Eigen/Eigen>
 
+#include <px4_msgs/msg/aux_global_position.hpp>
+#include <px4_ros2/navigation/experimental/navigation_interface_codes.hpp>
 
 using namespace Eigen;
 using namespace px4_msgs::msg;
@@ -23,17 +24,21 @@ struct GlobalPositionEstimate
 
 };
 
-class GlobalNavigationInterface : public rclcpp::Node
+class GlobalNavigationInterface
 {
 public:
-  explicit GlobalNavigationInterface(uint8_t altitude_frame);
-  ~GlobalNavigationInterface();
+  explicit GlobalNavigationInterface(rclcpp::Node & node, uint8_t altitude_frame);
+  ~GlobalNavigationInterface() = default;
 
-  void update(GlobalPositionEstimate & global_position_estimate);
+  /**
+   * @brief Publish global position estimate to FMU.
+   */
+  int update(const GlobalPositionEstimate & global_position_estimate) const;
 
-private:
   const std::string AUX_GLOBAL_POSITION_TOPIC = "/fmu/in/aux_global_position";
 
+private:
+  rclcpp::Node & _node;
   rclcpp::Publisher<AuxGlobalPosition>::SharedPtr _aux_global_position_pub;
 
   uint8_t _altitude_frame;

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
@@ -8,10 +8,8 @@
 #include <optional>
 #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;

diff --git a/px4_ros2_cpp/src/navigation/experimental/global_navigation_interface.cpp b/px4_ros2_cpp/src/navigation/experimental/global_navigation_interface.cpp
@@ -9,20 +9,25 @@
 namespace px4_ros2
 {
 
-GlobalNavigationInterface::GlobalNavigationInterface(uint8_t altitude_frame)
-: Node("global_navigation_interface_node"),
-  _aux_global_position_pub(create_publisher<AuxGlobalPosition>(AUX_GLOBAL_POSITION_TOPIC, 10)),
-  _altitude_frame(altitude_frame) {}
+GlobalNavigationInterface::GlobalNavigationInterface(rclcpp::Node & node, uint8_t altitude_frame)
+: _node(node),
+  _altitude_frame(altitude_frame)
+{
+  _aux_global_position_pub =
+    node.create_publisher<AuxGlobalPosition>(AUX_GLOBAL_POSITION_TOPIC, 10);
+}
 
-void GlobalNavigationInterface::update(GlobalPositionEstimate & global_position_estimate)
+int GlobalNavigationInterface::update(const GlobalPositionEstimate & global_position_estimate) const
 {
   AuxGlobalPosition aux_global_position;
 
   aux_global_position.timestamp_sample = global_position_estimate.timestamp_sample;
 
   // Publish
-  aux_global_position.timestamp = this->now().nanoseconds() * 1e-3;
+  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);
 }
 
 } // namespace px4_ros2