Skip to content

Commit f0b0be6

Browse files
ottojoottojo
committed
TwistSubscriber: add constructor using NodeInterfaces to allow use with non-lifecycle nodes
Signed-off-by: Jonas Otto <[email protected]>
1 parent 3ee33f7 commit f0b0be6

File tree

9 files changed

+134
-47
lines changed

9 files changed

+134
-47
lines changed

‎nav2_behaviors/plugins/assisted_teleop.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,7 @@ void AssistedTeleop::onConfigure()
5151
node->get_parameter("cmd_vel_teleop", cmd_vel_teleop);
5252

5353
vel_sub_ = std::make_unique<nav2_util::TwistSubscriber>(
54-
node,
54+
*node,
5555
cmd_vel_teleop, rclcpp::SystemDefaultsQoS(),
5656
[&](geometry_msgs::msg::Twist::SharedPtr msg) {
5757
teleop_twist_.twist = *msg;

‎nav2_collision_monitor/src/collision_monitor_node.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & state)
6565
}
6666

6767
cmd_vel_in_sub_ = std::make_unique<nav2_util::TwistSubscriber>(
68-
shared_from_this(),
68+
*this,
6969
cmd_vel_in_topic,
7070
1,
7171
std::bind(&CollisionMonitor::cmdVelInCallbackUnstamped, this, std::placeholders::_1),

‎nav2_costmap_2d/include/nav2_costmap_2d/plugin_container_layer.hpp‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ class PluginContainerLayer : public CostmapLayer
4646
/**
4747
* @brief Initialization process of layer on startup
4848
*/
49-
virtual void onInitialize();
49+
virtual void onInitialize() override;
5050
/**
5151
* @brief Update the bounds of the master costmap by this layer's update
5252
*dimensions
@@ -65,7 +65,7 @@ class PluginContainerLayer : public CostmapLayer
6565
double * min_x,
6666
double * min_y,
6767
double * max_x,
68-
double * max_y);
68+
double * max_y) override;
6969
/**
7070
* @brief Update the costs in the master costmap in the window
7171
* @param master_grid The master costmap grid to update
@@ -79,26 +79,26 @@ class PluginContainerLayer : public CostmapLayer
7979
int min_i,
8080
int min_j,
8181
int max_i,
82-
int max_j);
83-
virtual void onFootprintChanged();
82+
int max_j) override;
83+
virtual void onFootprintChanged() override;
8484
/** @brief Update the footprint to match size of the parent costmap. */
85-
virtual void matchSize();
85+
virtual void matchSize() override;
8686
/**
8787
* @brief Deactivate the layer
8888
*/
89-
virtual void deactivate();
89+
virtual void deactivate() override;
9090
/**
9191
* @brief Activate the layer
9292
*/
93-
virtual void activate();
93+
virtual void activate() override;
9494
/**
9595
* @brief Reset this costmap
9696
*/
97-
virtual void reset();
97+
virtual void reset() override;
9898
/**
9999
* @brief If clearing operations should be processed on this layer or not
100100
*/
101-
virtual bool isClearable();
101+
virtual bool isClearable() override;
102102
/**
103103
* @brief Clear an area in the constituent costmaps with the given dimension
104104
* if invert, then clear everything except these dimensions

‎nav2_util/include/nav2_util/node_utils.hpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,8 @@
1616
#ifndef NAV2_UTIL__NODE_UTILS_HPP_
1717
#define NAV2_UTIL__NODE_UTILS_HPP_
1818

19+
#include <rclcpp/node_interfaces/node_interfaces.hpp>
20+
#include <rclcpp/node_interfaces/node_parameters_interface.hpp>
1921
#include <vector>
2022
#include <string>
2123
#include "rclcpp/rclcpp.hpp"
@@ -103,6 +105,21 @@ void declare_parameter_if_not_declared(
103105
}
104106
}
105107

108+
/// Declares static ROS2 parameter and sets it to a given value if it was not already declared
109+
/* Declares static ROS2 parameter and sets it to a given value
110+
* if it was not already declared.
111+
*
112+
* \param[in] node A node in which given parameter to be declared
113+
* \param[in] parameter_name The name of parameter
114+
* \param[in] default_value Parameter value to initialize with
115+
* \param[in] parameter_descriptor Parameter descriptor (optional)
116+
*/
117+
void declare_parameter_if_not_declared(
118+
rclcpp::node_interfaces::NodeInterfaces<rclcpp::node_interfaces::NodeParametersInterface> node,
119+
const std::string & parameter_name,
120+
const rclcpp::ParameterValue & default_value,
121+
const ParameterDescriptor & parameter_descriptor = ParameterDescriptor());
122+
106123
/// Declares static ROS2 parameter with given type if it was not already declared
107124
/* Declares static ROS2 parameter with given type if it was not already declared.
108125
*

‎nav2_util/include/nav2_util/twist_subscriber.hpp‎

Lines changed: 89 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,10 @@
2323

2424
#include "geometry_msgs/msg/twist.hpp"
2525
#include "geometry_msgs/msg/twist_stamped.hpp"
26+
#include "rclcpp/create_subscription.hpp"
27+
#include "rclcpp/node_interfaces/node_interfaces.hpp"
28+
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
29+
#include "rclcpp/node_interfaces/node_topics_interface.hpp"
2630
#include "rclcpp/parameter_service.hpp"
2731
#include "rclcpp/rclcpp.hpp"
2832
#include "rclcpp/qos.hpp"
@@ -79,31 +83,56 @@ class TwistSubscriber
7983
* @param TwistCallback The subscriber callback for Twist messages
8084
* @param TwistStampedCallback The subscriber callback for TwistStamped messages
8185
*/
82-
template<typename TwistCallbackT,
83-
typename TwistStampedCallbackT
84-
>
86+
// template<typename TwistCallbackT,
87+
// typename TwistStampedCallbackT
88+
// >
89+
// explicit TwistSubscriber(
90+
// nav2_util::LifecycleNode::SharedPtr node,
91+
// const std::string & topic,
92+
// const rclcpp::QoS & qos,
93+
// TwistCallbackT && TwistCallback,
94+
// TwistStampedCallbackT && TwistStampedCallback
95+
// )
96+
// : TwistSubscriber(
97+
// rclcpp::node_interfaces::NodeInterfaces<
98+
// rclcpp::node_interfaces::NodeParametersInterface>(*node),
99+
// topic, qos, std::forward<TwistCallbackT>(TwistCallback),
100+
// std::forward<TwistStampedCallbackT>(TwistStampedCallback)) {}
101+
102+
/**
103+
* @brief A constructor that supports either Twist and TwistStamped
104+
* @param node The node to add the Twist subscriber to
105+
* @param topic The subscriber topic name
106+
* @param qos The subscriber quality of service
107+
* @param TwistCallback The subscriber callback for Twist messages
108+
* @param TwistStampedCallback The subscriber callback for TwistStamped
109+
* messages
110+
*/
111+
template<typename TwistCallbackT, typename TwistStampedCallbackT>
85112
explicit TwistSubscriber(
86-
nav2_util::LifecycleNode::SharedPtr node,
87-
const std::string & topic,
88-
const rclcpp::QoS & qos,
113+
rclcpp::node_interfaces::NodeInterfaces<
114+
rclcpp::node_interfaces::NodeParametersInterface,
115+
rclcpp::node_interfaces::NodeTopicsInterface>
116+
node,
117+
const std::string & topic, const rclcpp::QoS & qos,
89118
TwistCallbackT && TwistCallback,
90-
TwistStampedCallbackT && TwistStampedCallback
91-
)
119+
TwistStampedCallbackT && TwistStampedCallback)
92120
{
93121
nav2_util::declare_parameter_if_not_declared(
94-
node, "enable_stamped_cmd_vel",
95-
rclcpp::ParameterValue(false));
96-
node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
122+
rclcpp::node_interfaces::NodeInterfaces<
123+
rclcpp::node_interfaces::NodeParametersInterface>(node),
124+
"enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
125+
is_stamped_ = node.get_node_parameters_interface()
126+
->get_parameter("enable_stamped_cmd_vel")
127+
.as_bool();
97128
if (is_stamped_) {
98-
twist_stamped_sub_ = node->create_subscription<geometry_msgs::msg::TwistStamped>(
99-
topic,
100-
qos,
101-
std::forward<TwistStampedCallbackT>(TwistStampedCallback));
129+
twist_stamped_sub_ =
130+
rclcpp::create_subscription<geometry_msgs::msg::TwistStamped>(
131+
node, topic, qos,
132+
std::forward<TwistStampedCallbackT>(TwistStampedCallback));
102133
} else {
103-
twist_sub_ = node->create_subscription<geometry_msgs::msg::Twist>(
104-
topic,
105-
qos,
106-
std::forward<TwistCallbackT>(TwistCallback));
134+
twist_sub_ = rclcpp::create_subscription<geometry_msgs::msg::Twist>(
135+
node, topic, qos, std::forward<TwistCallbackT>(TwistCallback));
107136
}
108137
}
109138

@@ -115,23 +144,52 @@ class TwistSubscriber
115144
* @param TwistStampedCallback The subscriber callback for TwistStamped messages
116145
* @throw std::invalid_argument When configured with an invalid ROS parameter
117146
*/
147+
// template<typename TwistStampedCallbackT>
148+
// explicit TwistSubscriber(
149+
// nav2_util::LifecycleNode::SharedPtr node,
150+
// const std::string & topic,
151+
// const rclcpp::QoS & qos,
152+
// TwistStampedCallbackT && TwistStampedCallback
153+
// )
154+
// : TwistSubscriber(
155+
// rclcpp::node_interfaces::NodeInterfaces<
156+
// rclcpp::node_interfaces::NodeParametersInterface>(*node),
157+
// topic, qos,
158+
// std::forward<TwistStampedCallbackT>(TwistStampedCallback)) {}
159+
160+
/**
161+
* @brief A constructor that only supports TwistStamped
162+
* @param node The node to add the TwistStamped subscriber to
163+
* @param topic The subscriber topic name
164+
* @param qos The subscriber quality of service
165+
* @param TwistStampedCallback The subscriber callback for TwistStamped
166+
* messages
167+
* @throw std::invalid_argument When configured with an invalid ROS parameter
168+
*/
118169
template<typename TwistStampedCallbackT>
119170
explicit TwistSubscriber(
120-
nav2_util::LifecycleNode::SharedPtr node,
121-
const std::string & topic,
122-
const rclcpp::QoS & qos,
123-
TwistStampedCallbackT && TwistStampedCallback
124-
)
171+
rclcpp::node_interfaces::NodeInterfaces<
172+
rclcpp::node_interfaces::NodeParametersInterface,
173+
rclcpp::node_interfaces::NodeTopicsInterface>
174+
node,
175+
const std::string & topic, const rclcpp::QoS & qos,
176+
TwistStampedCallbackT && TwistStampedCallback)
125177
{
126178
nav2_util::declare_parameter_if_not_declared(
127-
node, "enable_stamped_cmd_vel",
128-
rclcpp::ParameterValue(false));
129-
node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
179+
rclcpp::node_interfaces::NodeInterfaces<
180+
rclcpp::node_interfaces::NodeParametersInterface>(node),
181+
"enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
182+
is_stamped_ = node.get_node_parameters_interface()
183+
->get_parameter("enable_stamped_cmd_vel")
184+
.as_bool();
130185
if (is_stamped_) {
131-
twist_stamped_sub_ = node->create_subscription<geometry_msgs::msg::TwistStamped>(
132-
topic,
133-
qos,
134-
std::forward<TwistStampedCallbackT>(TwistStampedCallback));
186+
twist_stamped_sub_ =
187+
node.get_node_topics_interface()->create_subscription(
188+
topic,
189+
rclcpp::create_subscription_factory<
190+
geometry_msgs::msg::TwistStamped>(
191+
std::forward<TwistStampedCallbackT>(TwistStampedCallback)),
192+
qos);
135193
} else {
136194
throw std::invalid_argument(
137195
"enable_stamped_cmd_vel must be true when using this constructor!");

‎nav2_util/src/node_utils.cpp‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,18 @@ std::string time_to_string(size_t len)
7575
return output;
7676
}
7777

78+
void declare_parameter_if_not_declared(
79+
rclcpp::node_interfaces::NodeInterfaces<rclcpp::node_interfaces::NodeParametersInterface> node,
80+
const std::string & parameter_name,
81+
const rclcpp::ParameterValue & default_value,
82+
const ParameterDescriptor & parameter_descriptor)
83+
{
84+
if (!node.get_node_parameters_interface()->has_parameter(parameter_name)) {
85+
node.get_node_parameters_interface()->declare_parameter(parameter_name, default_value,
86+
parameter_descriptor);
87+
}
88+
}
89+
7890
std::string generate_internal_node_name(const std::string & prefix)
7991
{
8092
return sanitize_node_name(prefix) + "_" + time_to_string(8);

‎nav2_util/test/test_twist_subscriber.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ TEST(TwistSubscriber, Unstamped)
3232

3333
geometry_msgs::msg::TwistStamped sub_msg {};
3434
auto vel_subscriber = std::make_unique<nav2_util::TwistSubscriber>(
35-
sub_node, "cmd_vel", 1,
35+
*sub_node, "cmd_vel", 1,
3636
[&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;},
3737
[&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;}
3838
);
@@ -69,7 +69,7 @@ TEST(TwistSubscriber, Stamped)
6969

7070
geometry_msgs::msg::TwistStamped sub_msg {};
7171
auto vel_subscriber = std::make_unique<nav2_util::TwistSubscriber>(
72-
sub_node, "cmd_vel", 1,
72+
*sub_node, "cmd_vel", 1,
7373
[&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;},
7474
[&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;}
7575
);

‎nav2_velocity_smoother/src/velocity_smoother.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -148,7 +148,7 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state)
148148
// Setup inputs / outputs
149149
smoothed_cmd_pub_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel_smoothed", 1);
150150
cmd_sub_ = std::make_unique<nav2_util::TwistSubscriber>(
151-
node,
151+
*node,
152152
"cmd_vel", rclcpp::QoS(1),
153153
std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1),
154154
std::bind(&VelocitySmoother::inputCommandStampedCallback, this, std::placeholders::_1)

‎nav2_velocity_smoother/test/test_velocity_smoother.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -71,7 +71,7 @@ TEST(VelocitySmootherTest, openLoopTestTimer)
7171

7272
std::vector<double> linear_vels;
7373
auto subscription = nav2_util::TwistSubscriber(
74-
smoother,
74+
*smoother,
7575
"cmd_vel_smoothed",
7676
1,
7777
[&](geometry_msgs::msg::Twist::SharedPtr msg) {
@@ -125,7 +125,7 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer)
125125

126126
std::vector<double> linear_vels;
127127
auto subscription = nav2_util::TwistSubscriber(
128-
smoother,
128+
*smoother,
129129
"cmd_vel_smoothed",
130130
1,
131131
[&](geometry_msgs::msg::Twist::SharedPtr msg) {

0 commit comments

Comments
 (0)