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 1d11dba..56baa89 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 @@ -68,7 +68,6 @@ class GlobalNavigationInterface : public NavigationInterfaceBase::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 index c5d0318..a0b0e73 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 @@ -77,7 +77,6 @@ class LocalNavigationInterface : public NavigationInterfaceBase::SharedPtr _aux_local_position_pub; uint8_t _pose_frame; 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 index 0adfdfa..02cef26 100644 --- 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 @@ -15,7 +15,8 @@ template class NavigationInterfaceBase { public: - explicit NavigationInterfaceBase() = default; + explicit NavigationInterfaceBase(Context & context) + : _node(context.node()) {} virtual ~NavigationInterfaceBase() = default; /** @@ -29,6 +30,8 @@ class NavigationInterfaceBase virtual bool _isVarianceValid(const EstimateType & estimate) const = 0; virtual bool _isFrameValid(const EstimateType & estimate) const = 0; virtual bool _isValueNotNAN(const EstimateType & estimate) const = 0; + + rclcpp::Node & _node; }; } // namespace px4_ros2 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 14f2b51..91fac66 100644 --- a/px4_ros2_cpp/src/navigation/experimental/global_navigation_interface.cpp +++ b/px4_ros2_cpp/src/navigation/experimental/global_navigation_interface.cpp @@ -10,7 +10,7 @@ namespace px4_ros2 { GlobalNavigationInterface::GlobalNavigationInterface(Context & context) -: _node(context.node()) +: NavigationInterfaceBase(context) { _aux_global_position_pub = context.node().create_publisher( 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 dcfec71..afffd34 100644 --- a/px4_ros2_cpp/src/navigation/experimental/local_navigation_interface.cpp +++ b/px4_ros2_cpp/src/navigation/experimental/local_navigation_interface.cpp @@ -12,7 +12,7 @@ namespace px4_ros2 LocalNavigationInterface::LocalNavigationInterface( Context & context, const uint8_t pose_frame, const uint8_t velocity_frame) -: _node(context.node()) +: NavigationInterfaceBase(context) { // Check that selected pose and velocity reference frames are valid auto it_pose_frame = std::find(