diff --git a/examples/cpp/navigation/global_navigation/include/global_navigation.hpp b/examples/cpp/navigation/global_navigation/include/global_navigation.hpp index f2afc7d..ee92131 100644 --- a/examples/cpp/navigation/global_navigation/include/global_navigation.hpp +++ b/examples/cpp/navigation/global_navigation/include/global_navigation.hpp @@ -13,15 +13,12 @@ using namespace std::chrono_literals; // NOLINT -class GlobalNavigationTest : public px4_ros2::Context +class GlobalNavigationTest : public px4_ros2::GlobalNavigationInterface { public: explicit GlobalNavigationTest(rclcpp::Node & node) - : Context(node), _node(node) + : GlobalNavigationInterface(node) { - // Instantiate global navigation interface - _global_navigation_interface = std::make_shared(*this); - _timer = node.create_wall_timer(1s, std::bind(&GlobalNavigationTest::updateAuxGlobalPosition, this)); @@ -39,15 +36,13 @@ class GlobalNavigationTest : public px4_ros2::Context global_position_estimate.positional_uncertainty = 0.4f; px4_ros2::NavigationInterfaceReturnCode retcode; - retcode = _global_navigation_interface->update(global_position_estimate); + retcode = update(global_position_estimate); RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: %s.", resultToString(retcode)); } private: - std::shared_ptr _global_navigation_interface; rclcpp::TimerBase::SharedPtr _timer; - rclcpp::Node & _node; }; class ExampleGlobalNavigationNode : public rclcpp::Node diff --git a/examples/cpp/navigation/local_navigation/include/local_navigation.hpp b/examples/cpp/navigation/local_navigation/include/local_navigation.hpp index 84b0f62..e5086ea 100644 --- a/examples/cpp/navigation/local_navigation/include/local_navigation.hpp +++ b/examples/cpp/navigation/local_navigation/include/local_navigation.hpp @@ -12,20 +12,14 @@ using namespace std::chrono_literals; // NOLINT -class LocalNavigationTest : public px4_ros2::Context +class LocalNavigationTest : public px4_ros2::LocalNavigationInterface { public: - LocalNavigationTest(rclcpp::Node & node) - : Context(node), _node(node) + LocalNavigationTest( + rclcpp::Node & node, const uint8_t pose_frame, + const uint8_t velocity_frame) + : LocalNavigationInterface(node, pose_frame, velocity_frame) { - // 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( - *this, - pose_frame, - velocity_frame); - _timer = node.create_wall_timer(1s, std::bind(&LocalNavigationTest::updateAuxLocalPosition, this)); @@ -48,16 +42,13 @@ class LocalNavigationTest : public px4_ros2::Context local_position_estimate.attitude_variance = Eigen::Vector3f {0.2, 0.1, 0.05}; px4_ros2::NavigationInterfaceReturnCode retcode; - retcode = _local_navigation_interface->update(local_position_estimate); + retcode = update(local_position_estimate); RCLCPP_DEBUG(_node.get_logger(), "Interface returned with: %s.", resultToString(retcode)); } private: - std::shared_ptr _local_navigation_interface; rclcpp::TimerBase::SharedPtr _timer; - rclcpp::Node & _node; - }; class ExampleLocalNavigationNode : public rclcpp::Node @@ -75,7 +66,9 @@ class ExampleLocalNavigationNode : public rclcpp::Node rcutils_reset_error(); } - _interface = std::make_unique(*this); + const uint8_t pose_frame = AuxLocalPosition::POSE_FRAME_NED; + const uint8_t velocity_frame = AuxLocalPosition::VELOCITY_FRAME_NED; + _interface = std::make_unique(*this, pose_frame, velocity_frame); } 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 index acae78a..a9ee18e 100644 --- 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 @@ -35,7 +35,7 @@ struct GlobalPositionEstimate class GlobalNavigationInterface : public NavigationInterfaceBase { public: - explicit GlobalNavigationInterface(Context & context); + explicit GlobalNavigationInterface(rclcpp::Node & node); ~GlobalNavigationInterface() override = default; /** 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 index 848a7c9..7e07b04 100644 --- 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 @@ -43,7 +43,7 @@ class LocalNavigationInterface : public NavigationInterfaceBase -class NavigationInterfaceBase +class NavigationInterfaceBase : public Context { public: - explicit NavigationInterfaceBase(Context & context) - : _node(context.node()) {} + explicit NavigationInterfaceBase(rclcpp::Node & node) + : Context(node, ""), _node(node) {} virtual ~NavigationInterfaceBase() = default; /** diff --git a/px4_ros2_cpp/src/navigation/experimental/global_navigation_interface.cpp b/px4_ros2_cpp/src/navigation/experimental/global_navigation_interface.cpp index 91fac66..130339d 100644 --- a/px4_ros2_cpp/src/navigation/experimental/global_navigation_interface.cpp +++ b/px4_ros2_cpp/src/navigation/experimental/global_navigation_interface.cpp @@ -9,12 +9,12 @@ namespace px4_ros2 { -GlobalNavigationInterface::GlobalNavigationInterface(Context & context) -: NavigationInterfaceBase(context) +GlobalNavigationInterface::GlobalNavigationInterface(rclcpp::Node & node) +: NavigationInterfaceBase(node) { _aux_global_position_pub = - context.node().create_publisher( - context.topicNamespacePrefix() + AUX_GLOBAL_POSITION_TOPIC, 10); + node.create_publisher( + topicNamespacePrefix() + AUX_GLOBAL_POSITION_TOPIC, 10); } NavigationInterfaceReturnCode GlobalNavigationInterface::update( diff --git a/px4_ros2_cpp/src/navigation/experimental/local_navigation_interface.cpp b/px4_ros2_cpp/src/navigation/experimental/local_navigation_interface.cpp index afffd34..95da286 100644 --- a/px4_ros2_cpp/src/navigation/experimental/local_navigation_interface.cpp +++ b/px4_ros2_cpp/src/navigation/experimental/local_navigation_interface.cpp @@ -10,9 +10,9 @@ namespace px4_ros2 { LocalNavigationInterface::LocalNavigationInterface( - Context & context, const uint8_t pose_frame, + rclcpp::Node & node, const uint8_t pose_frame, const uint8_t velocity_frame) -: NavigationInterfaceBase(context) +: NavigationInterfaceBase(node) { // Check that selected pose and velocity reference frames are valid auto it_pose_frame = std::find( @@ -20,7 +20,7 @@ LocalNavigationInterface::LocalNavigationInterface( _available_pose_frames), pose_frame); if (it_pose_frame == std::end(_available_pose_frames)) { RCLCPP_WARN( - context.node().get_logger(), "Failed to initialize LocalNavigationInterface: invalid pose reference frame %i. Setting pose reference frame to POSE_FRAME_UNKNOWN.", + node.get_logger(), "Failed to initialize LocalNavigationInterface: invalid pose reference frame %i. Setting pose reference frame to POSE_FRAME_UNKNOWN.", pose_frame); _pose_frame = AuxLocalPosition::POSE_FRAME_UNKNOWN; } else { @@ -32,16 +32,15 @@ LocalNavigationInterface::LocalNavigationInterface( _available_velocity_frames), velocity_frame); if (it_vel_frame == std::end(_available_velocity_frames)) { RCLCPP_WARN( - context.node().get_logger(), "Failed to initialize LocalNavigationInterface: invalid velocity reference frame %i. Setting velocity reference frame to VELOCITY_FRAME_UNKNOWN.", + node.get_logger(), "Failed to initialize LocalNavigationInterface: invalid velocity reference frame %i. Setting velocity reference frame to VELOCITY_FRAME_UNKNOWN.", velocity_frame); _velocity_frame = AuxLocalPosition::VELOCITY_FRAME_UNKNOWN; } else { _velocity_frame = velocity_frame; } - _aux_local_position_pub = context.node().create_publisher( - AUX_LOCAL_POSITION_TOPIC, 10); - + _aux_local_position_pub = node.create_publisher( + topicNamespacePrefix() + AUX_LOCAL_POSITION_TOPIC, 10); } NavigationInterfaceReturnCode LocalNavigationInterface::update(