TwistSubscriber: add constructor using NodeInterfaces

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<nav2_util::TwistSubscriber>(
-    node,
+    *node,
     cmd_vel_teleop, rclcpp::SystemDefaultsQoS(),
     [&](geometry_msgs::msg::Twist::SharedPtr msg) {
       teleop_twist_.twist = *msg;

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<nav2_util::TwistSubscriber>(
-    shared_from_this(),
+    *this,
     cmd_vel_in_topic,
     1,
     std::bind(&CollisionMonitor::cmdVelInCallbackUnstamped, this, std::placeholders::_1),

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

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 <rclcpp/node_interfaces/node_interfaces.hpp>
+#include <rclcpp/node_interfaces/node_parameters_interface.hpp>
 #include <vector>
 #include <string>
 #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<rclcpp::node_interfaces::NodeParametersInterface> 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.
  *

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<typename TwistCallbackT,
-    typename TwistStampedCallbackT
-  >
+//   template<typename TwistCallbackT,
+//     typename TwistStampedCallbackT
+//   >
+//   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<TwistCallbackT>(TwistCallback),
+//       std::forward<TwistStampedCallbackT>(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<typename TwistCallbackT, typename TwistStampedCallbackT>
   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<geometry_msgs::msg::TwistStamped>(
-        topic,
-        qos,
-        std::forward<TwistStampedCallbackT>(TwistStampedCallback));
+      twist_stamped_sub_ =
+        rclcpp::create_subscription<geometry_msgs::msg::TwistStamped>(
+              node, topic, qos,
+              std::forward<TwistStampedCallbackT>(TwistStampedCallback));
     } else {
-      twist_sub_ = node->create_subscription<geometry_msgs::msg::Twist>(
-        topic,
-        qos,
-        std::forward<TwistCallbackT>(TwistCallback));
+      twist_sub_ = rclcpp::create_subscription<geometry_msgs::msg::Twist>(
+          node, topic, qos, std::forward<TwistCallbackT>(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<typename TwistStampedCallbackT>
+//   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<TwistStampedCallbackT>(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<typename TwistStampedCallbackT>
   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<geometry_msgs::msg::TwistStamped>(
-        topic,
-        qos,
-        std::forward<TwistStampedCallbackT>(TwistStampedCallback));
+      twist_stamped_sub_ =
+        node.get_node_topics_interface()->create_subscription(
+              topic,
+              rclcpp::create_subscription_factory<
+          geometry_msgs::msg::TwistStamped>(
+                  std::forward<TwistStampedCallbackT>(TwistStampedCallback)),
+              qos);
     } else {
       throw std::invalid_argument(
               "enable_stamped_cmd_vel must be true when using this constructor!");

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<rclcpp::node_interfaces::NodeParametersInterface> 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);

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<nav2_util::TwistSubscriber>(
-    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<nav2_util::TwistSubscriber>(
-    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;}
   );

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<nav2_util::TwistPublisher>(node, "cmd_vel_smoothed", 1);
   cmd_sub_ = std::make_unique<nav2_util::TwistSubscriber>(
-    node,
+    *node,
     "cmd_vel", rclcpp::QoS(1),
     std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1),
     std::bind(&VelocitySmoother::inputCommandStampedCallback, this, std::placeholders::_1)

nav2_velocity_smoother/test/test_velocity_smoother.cpp

@@ -71,7 +71,7 @@ TEST(VelocitySmootherTest, openLoopTestTimer)
 
   std::vector<double> 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<double> linear_vels;
   auto subscription = nav2_util::TwistSubscriber(
-    smoother,
+    *smoother,
     "cmd_vel_smoothed",
     1,
     [&](geometry_msgs::msg::Twist::SharedPtr msg) {