From f0b0be6e2df7600116183d193f50e6a8be3e6de9 Mon Sep 17 00:00:00 2001 From: Jonas Otto Date: Mon, 14 Jul 2025 18:13:36 +0200 Subject: [PATCH] TwistSubscriber: add constructor using NodeInterfaces to allow use with non-lifecycle nodes Signed-off-by: Jonas Otto --- nav2_behaviors/plugins/assisted_teleop.cpp | 2 +- .../src/collision_monitor_node.cpp | 2 +- .../plugin_container_layer.hpp | 18 +-- nav2_util/include/nav2_util/node_utils.hpp | 17 +++ .../include/nav2_util/twist_subscriber.hpp | 120 +++++++++++++----- nav2_util/src/node_utils.cpp | 12 ++ nav2_util/test/test_twist_subscriber.cpp | 4 +- .../src/velocity_smoother.cpp | 2 +- .../test/test_velocity_smoother.cpp | 4 +- 9 files changed, 134 insertions(+), 47 deletions(-) diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index 9e85229a3c1..a8ccdbc215f 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -51,7 +51,7 @@ void AssistedTeleop::onConfigure() node->get_parameter("cmd_vel_teleop", cmd_vel_teleop); vel_sub_ = std::make_unique( - node, + *node, cmd_vel_teleop, rclcpp::SystemDefaultsQoS(), [&](geometry_msgs::msg::Twist::SharedPtr msg) { teleop_twist_.twist = *msg; diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 2b0991c9941..7c7765516f9 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -65,7 +65,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state) } cmd_vel_in_sub_ = std::make_unique( - shared_from_this(), + *this, cmd_vel_in_topic, 1, std::bind(&CollisionMonitor::cmdVelInCallbackUnstamped, this, std::placeholders::_1), diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp index e98c7b8136f..fb1d23e2ece 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp @@ -46,7 +46,7 @@ class PluginContainerLayer : public CostmapLayer /** * @brief Initialization process of layer on startup */ - virtual void onInitialize(); + virtual void onInitialize() override; /** * @brief Update the bounds of the master costmap by this layer's update *dimensions @@ -65,7 +65,7 @@ class PluginContainerLayer : public CostmapLayer double * min_x, double * min_y, double * max_x, - double * max_y); + double * max_y) override; /** * @brief Update the costs in the master costmap in the window * @param master_grid The master costmap grid to update @@ -79,26 +79,26 @@ class PluginContainerLayer : public CostmapLayer int min_i, int min_j, int max_i, - int max_j); - virtual void onFootprintChanged(); + int max_j) override; + virtual void onFootprintChanged() override; /** @brief Update the footprint to match size of the parent costmap. */ - virtual void matchSize(); + virtual void matchSize() override; /** * @brief Deactivate the layer */ - virtual void deactivate(); + virtual void deactivate() override; /** * @brief Activate the layer */ - virtual void activate(); + virtual void activate() override; /** * @brief Reset this costmap */ - virtual void reset(); + virtual void reset() override; /** * @brief If clearing operations should be processed on this layer or not */ - virtual bool isClearable(); + virtual bool isClearable() override; /** * @brief Clear an area in the constituent costmaps with the given dimension * if invert, then clear everything except these dimensions diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index b69cd1796dc..8d7c019b77a 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -16,6 +16,8 @@ #ifndef NAV2_UTIL__NODE_UTILS_HPP_ #define NAV2_UTIL__NODE_UTILS_HPP_ +#include +#include #include #include #include "rclcpp/rclcpp.hpp" @@ -103,6 +105,21 @@ void declare_parameter_if_not_declared( } } +/// Declares static ROS2 parameter and sets it to a given value if it was not already declared +/* Declares static ROS2 parameter and sets it to a given value + * if it was not already declared. + * + * \param[in] node A node in which given parameter to be declared + * \param[in] parameter_name The name of parameter + * \param[in] default_value Parameter value to initialize with + * \param[in] parameter_descriptor Parameter descriptor (optional) + */ +void declare_parameter_if_not_declared( + rclcpp::node_interfaces::NodeInterfaces node, + const std::string & parameter_name, + const rclcpp::ParameterValue & default_value, + const ParameterDescriptor & parameter_descriptor = ParameterDescriptor()); + /// Declares static ROS2 parameter with given type if it was not already declared /* Declares static ROS2 parameter with given type if it was not already declared. * diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp index 01f9ca25fed..c476bf69415 100644 --- a/nav2_util/include/nav2_util/twist_subscriber.hpp +++ b/nav2_util/include/nav2_util/twist_subscriber.hpp @@ -23,6 +23,10 @@ #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" +#include "rclcpp/create_subscription.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/parameter_service.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp/qos.hpp" @@ -79,31 +83,56 @@ class TwistSubscriber * @param TwistCallback The subscriber callback for Twist messages * @param TwistStampedCallback The subscriber callback for TwistStamped messages */ - template +// template +// explicit TwistSubscriber( +// nav2_util::LifecycleNode::SharedPtr node, +// const std::string & topic, +// const rclcpp::QoS & qos, +// TwistCallbackT && TwistCallback, +// TwistStampedCallbackT && TwistStampedCallback +// ) +// : TwistSubscriber( +// rclcpp::node_interfaces::NodeInterfaces< +// rclcpp::node_interfaces::NodeParametersInterface>(*node), +// topic, qos, std::forward(TwistCallback), +// std::forward(TwistStampedCallback)) {} + + /** + * @brief A constructor that supports either Twist and TwistStamped + * @param node The node to add the Twist subscriber to + * @param topic The subscriber topic name + * @param qos The subscriber quality of service + * @param TwistCallback The subscriber callback for Twist messages + * @param TwistStampedCallback The subscriber callback for TwistStamped + * messages + */ + template explicit TwistSubscriber( - nav2_util::LifecycleNode::SharedPtr node, - const std::string & topic, - const rclcpp::QoS & qos, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface> + node, + const std::string & topic, const rclcpp::QoS & qos, TwistCallbackT && TwistCallback, - TwistStampedCallbackT && TwistStampedCallback - ) + TwistStampedCallbackT && TwistStampedCallback) { nav2_util::declare_parameter_if_not_declared( - node, "enable_stamped_cmd_vel", - rclcpp::ParameterValue(false)); - node->get_parameter("enable_stamped_cmd_vel", is_stamped_); + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeParametersInterface>(node), + "enable_stamped_cmd_vel", rclcpp::ParameterValue(false)); + is_stamped_ = node.get_node_parameters_interface() + ->get_parameter("enable_stamped_cmd_vel") + .as_bool(); if (is_stamped_) { - twist_stamped_sub_ = node->create_subscription( - topic, - qos, - std::forward(TwistStampedCallback)); + twist_stamped_sub_ = + rclcpp::create_subscription( + node, topic, qos, + std::forward(TwistStampedCallback)); } else { - twist_sub_ = node->create_subscription( - topic, - qos, - std::forward(TwistCallback)); + twist_sub_ = rclcpp::create_subscription( + node, topic, qos, std::forward(TwistCallback)); } } @@ -115,23 +144,52 @@ class TwistSubscriber * @param TwistStampedCallback The subscriber callback for TwistStamped messages * @throw std::invalid_argument When configured with an invalid ROS parameter */ +// template +// explicit TwistSubscriber( +// nav2_util::LifecycleNode::SharedPtr node, +// const std::string & topic, +// const rclcpp::QoS & qos, +// TwistStampedCallbackT && TwistStampedCallback +// ) +// : TwistSubscriber( +// rclcpp::node_interfaces::NodeInterfaces< +// rclcpp::node_interfaces::NodeParametersInterface>(*node), +// topic, qos, +// std::forward(TwistStampedCallback)) {} + + /** + * @brief A constructor that only supports TwistStamped + * @param node The node to add the TwistStamped subscriber to + * @param topic The subscriber topic name + * @param qos The subscriber quality of service + * @param TwistStampedCallback The subscriber callback for TwistStamped + * messages + * @throw std::invalid_argument When configured with an invalid ROS parameter + */ template explicit TwistSubscriber( - nav2_util::LifecycleNode::SharedPtr node, - const std::string & topic, - const rclcpp::QoS & qos, - TwistStampedCallbackT && TwistStampedCallback - ) + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface> + node, + const std::string & topic, const rclcpp::QoS & qos, + TwistStampedCallbackT && TwistStampedCallback) { nav2_util::declare_parameter_if_not_declared( - node, "enable_stamped_cmd_vel", - rclcpp::ParameterValue(false)); - node->get_parameter("enable_stamped_cmd_vel", is_stamped_); + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeParametersInterface>(node), + "enable_stamped_cmd_vel", rclcpp::ParameterValue(false)); + is_stamped_ = node.get_node_parameters_interface() + ->get_parameter("enable_stamped_cmd_vel") + .as_bool(); if (is_stamped_) { - twist_stamped_sub_ = node->create_subscription( - topic, - qos, - std::forward(TwistStampedCallback)); + twist_stamped_sub_ = + node.get_node_topics_interface()->create_subscription( + topic, + rclcpp::create_subscription_factory< + geometry_msgs::msg::TwistStamped>( + std::forward(TwistStampedCallback)), + qos); } else { throw std::invalid_argument( "enable_stamped_cmd_vel must be true when using this constructor!"); diff --git a/nav2_util/src/node_utils.cpp b/nav2_util/src/node_utils.cpp index 993eaf53b6e..03e00033a5e 100644 --- a/nav2_util/src/node_utils.cpp +++ b/nav2_util/src/node_utils.cpp @@ -75,6 +75,18 @@ std::string time_to_string(size_t len) return output; } +void declare_parameter_if_not_declared( + rclcpp::node_interfaces::NodeInterfaces node, + const std::string & parameter_name, + const rclcpp::ParameterValue & default_value, + const ParameterDescriptor & parameter_descriptor) +{ + if (!node.get_node_parameters_interface()->has_parameter(parameter_name)) { + node.get_node_parameters_interface()->declare_parameter(parameter_name, default_value, + parameter_descriptor); + } +} + std::string generate_internal_node_name(const std::string & prefix) { return sanitize_node_name(prefix) + "_" + time_to_string(8); diff --git a/nav2_util/test/test_twist_subscriber.cpp b/nav2_util/test/test_twist_subscriber.cpp index 56728873682..c3932742b15 100644 --- a/nav2_util/test/test_twist_subscriber.cpp +++ b/nav2_util/test/test_twist_subscriber.cpp @@ -32,7 +32,7 @@ TEST(TwistSubscriber, Unstamped) geometry_msgs::msg::TwistStamped sub_msg {}; auto vel_subscriber = std::make_unique( - sub_node, "cmd_vel", 1, + *sub_node, "cmd_vel", 1, [&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;}, [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;} ); @@ -69,7 +69,7 @@ TEST(TwistSubscriber, Stamped) geometry_msgs::msg::TwistStamped sub_msg {}; auto vel_subscriber = std::make_unique( - sub_node, "cmd_vel", 1, + *sub_node, "cmd_vel", 1, [&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;}, [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;} ); diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 3e2fb782c81..031c8aaba4c 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -148,7 +148,7 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) // Setup inputs / outputs smoothed_cmd_pub_ = std::make_unique(node, "cmd_vel_smoothed", 1); cmd_sub_ = std::make_unique( - node, + *node, "cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1), std::bind(&VelocitySmoother::inputCommandStampedCallback, this, std::placeholders::_1) diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index 76c5e0498b6..9119c0e8caf 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -71,7 +71,7 @@ TEST(VelocitySmootherTest, openLoopTestTimer) std::vector linear_vels; auto subscription = nav2_util::TwistSubscriber( - smoother, + *smoother, "cmd_vel_smoothed", 1, [&](geometry_msgs::msg::Twist::SharedPtr msg) { @@ -125,7 +125,7 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer) std::vector linear_vels; auto subscription = nav2_util::TwistSubscriber( - smoother, + *smoother, "cmd_vel_smoothed", 1, [&](geometry_msgs::msg::Twist::SharedPtr msg) {