From 7998d53d8e533a8ee1d2f202822a831f724c7608 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 26 Aug 2023 23:18:29 -0600 Subject: [PATCH 01/38] Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman --- .../nav2_controller/controller_server.hpp | 3 +- nav2_controller/src/controller_server.cpp | 5 +- .../include/nav2_util/twist_publisher.hpp | 128 ++++++++++++++++++ 3 files changed, 132 insertions(+), 4 deletions(-) create mode 100644 nav2_util/include/nav2_util/twist_publisher.hpp diff --git a/nav2_controller/include/nav2_controller/controller_server.hpp b/nav2_controller/include/nav2_controller/controller_server.hpp index 992c8fab041..0f250dee148 100644 --- a/nav2_controller/include/nav2_controller/controller_server.hpp +++ b/nav2_controller/include/nav2_controller/controller_server.hpp @@ -33,6 +33,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/simple_action_server.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/twist_publisher.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" @@ -229,7 +230,7 @@ class ControllerServer : public nav2_util::LifecycleNode // Publishers and subscribers std::unique_ptr odom_sub_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_publisher_; + std::unique_ptr vel_publisher_; rclcpp::Subscription::SharedPtr speed_limit_sub_; // Progress Checker Plugin diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 76754c58151..26134a4eb29 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -215,7 +215,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/) "Controller Server has %s controllers available.", controller_ids_concat_.c_str()); odom_sub_ = std::make_unique(node); - vel_publisher_ = create_publisher("cmd_vel", 1); + vel_publisher_ = std::make_unique(node, "cmd_vel", 1); double action_server_result_timeout; get_parameter("action_server_result_timeout", action_server_result_timeout); @@ -693,9 +693,8 @@ void ControllerServer::updateGlobalPath() void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity) { - auto cmd_vel = std::make_unique(velocity.twist); if (vel_publisher_->is_activated() && vel_publisher_->get_subscription_count() > 0) { - vel_publisher_->publish(std::move(cmd_vel)); + vel_publisher_->publish(velocity); } } diff --git a/nav2_util/include/nav2_util/twist_publisher.hpp b/nav2_util/include/nav2_util/twist_publisher.hpp new file mode 100644 index 00000000000..e9118737991 --- /dev/null +++ b/nav2_util/include/nav2_util/twist_publisher.hpp @@ -0,0 +1,128 @@ +// Copyright (C) 2023 Ryan Friedman +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef NAV2_UTIL__TWIST_PUBLISHER_HPP_ +#define NAV2_UTIL__TWIST_PUBLISHER_HPP_ + + +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp/qos.hpp" + +namespace nav2_util +{ + +/** + * @class nav2_util::TwistPublisher + * @brief A simple wrapper on a Twist publisher that provides either Twist or TwistStamped + * + * The default is to publish Twist to preserve backwards compatibility, but it can be overridden + * using the "enable_stamped_cmd_vel" parameter to publish TwistStamped. + * + */ + +class TwistPublisher +{ +public: + /** + * @brief A constructor + * @param service_name name of the service to call + * @param provided_node Node to create the service client off of + */ + explicit TwistPublisher( + nav2_util::LifecycleNode::SharedPtr nh, + const std::string & topic, + const rclcpp::QoS & qos + ) + : topic_(topic), node_(nh) + { + assert(nh != nullptr); + node_->declare_parameter("enable_stamped_cmd_vel", false); + node_->get_parameter("enable_stamped_cmd_vel", is_stamped_); + if (is_stamped_) { + twist_stamped_pub_ = node_->create_publisher( + topic_, + qos); + } else { + twist_pub_ = node_->create_publisher( + topic_, + qos); + } + } + + + void on_activate() + { + if (is_stamped_) { + twist_stamped_pub_->on_activate(); + } else { + twist_pub_->on_activate(); + } + } + + void on_deactivate() + { + if (is_stamped_) { + twist_stamped_pub_->on_deactivate(); + } else { + twist_pub_->on_deactivate(); + } + } + + [[nodiscard]] bool is_activated() const + { + if (is_stamped_) { + return twist_stamped_pub_->is_activated(); + } else { + return twist_pub_->is_activated(); + } + } + + void publish(const geometry_msgs::msg::TwistStamped & velocity) + { + if (is_stamped_) { + twist_stamped_pub_->publish(velocity); + } else { + [[maybe_unused]] const auto cmd_vel = std::make_unique( + velocity.twist); + twist_pub_->publish(velocity.twist); + } + } + + [[nodiscard]] size_t get_subscription_count() const + { + if (is_stamped_) { + return twist_stamped_pub_->get_subscription_count(); + } else { + return twist_pub_->get_subscription_count(); + } + } + +protected: + std::string topic_; + nav2_util::LifecycleNode::SharedPtr node_; + bool is_stamped_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr twist_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + twist_stamped_pub_; +}; + +} // namespace nav2_util + +#endif // NAV2_UTIL__TWIST_PUBLISHER_HPP_ From 529d77c024dd8a9747add902b142a0ffc211058a Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sun, 27 Aug 2023 16:33:14 -0600 Subject: [PATCH 02/38] Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman --- nav2_util/include/nav2_util/twist_publisher.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/nav2_util/include/nav2_util/twist_publisher.hpp b/nav2_util/include/nav2_util/twist_publisher.hpp index e9118737991..c8a57ca8b38 100644 --- a/nav2_util/include/nav2_util/twist_publisher.hpp +++ b/nav2_util/include/nav2_util/twist_publisher.hpp @@ -42,8 +42,9 @@ class TwistPublisher public: /** * @brief A constructor - * @param service_name name of the service to call - * @param provided_node Node to create the service client off of + * @param nh The node + * @param topic publisher topic name + * @param qos publisher quality of service */ explicit TwistPublisher( nav2_util::LifecycleNode::SharedPtr nh, @@ -99,8 +100,6 @@ class TwistPublisher if (is_stamped_) { twist_stamped_pub_->publish(velocity); } else { - [[maybe_unused]] const auto cmd_vel = std::make_unique( - velocity.twist); twist_pub_->publish(velocity.twist); } } From 1f15c5e58f6d66d8c151af734399197d1c261e35 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Tue, 29 Aug 2023 08:50:33 -0600 Subject: [PATCH 03/38] Remove stored node and assert Signed-off-by: Ryan Friedman --- nav2_util/include/nav2_util/twist_publisher.hpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/nav2_util/include/nav2_util/twist_publisher.hpp b/nav2_util/include/nav2_util/twist_publisher.hpp index c8a57ca8b38..a38f7c8f76a 100644 --- a/nav2_util/include/nav2_util/twist_publisher.hpp +++ b/nav2_util/include/nav2_util/twist_publisher.hpp @@ -47,21 +47,20 @@ class TwistPublisher * @param qos publisher quality of service */ explicit TwistPublisher( - nav2_util::LifecycleNode::SharedPtr nh, + nav2_util::LifecycleNode::SharedPtr node, const std::string & topic, const rclcpp::QoS & qos ) - : topic_(topic), node_(nh) + : topic_(topic) { - assert(nh != nullptr); - node_->declare_parameter("enable_stamped_cmd_vel", false); - node_->get_parameter("enable_stamped_cmd_vel", is_stamped_); + node->declare_parameter("enable_stamped_cmd_vel", false); + node->get_parameter("enable_stamped_cmd_vel", is_stamped_); if (is_stamped_) { - twist_stamped_pub_ = node_->create_publisher( + twist_stamped_pub_ = node->create_publisher( topic_, qos); } else { - twist_pub_ = node_->create_publisher( + twist_pub_ = node->create_publisher( topic_, qos); } @@ -115,7 +114,6 @@ class TwistPublisher protected: std::string topic_; - nav2_util::LifecycleNode::SharedPtr node_; bool is_stamped_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr twist_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr From 0e38e99ccf41c78bc8c165e86de9604c07344159 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 1 Sep 2023 12:21:44 -0600 Subject: [PATCH 04/38] Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman --- .../include/nav2_util/twist_publisher.hpp | 13 +++-- nav2_util/test/CMakeLists.txt | 4 ++ nav2_util/test/test_twist_publisher.cpp | 52 +++++++++++++++++++ 3 files changed, 66 insertions(+), 3 deletions(-) create mode 100644 nav2_util/test/test_twist_publisher.cpp diff --git a/nav2_util/include/nav2_util/twist_publisher.hpp b/nav2_util/include/nav2_util/twist_publisher.hpp index a38f7c8f76a..eaca6d7edcc 100644 --- a/nav2_util/include/nav2_util/twist_publisher.hpp +++ b/nav2_util/include/nav2_util/twist_publisher.hpp @@ -21,9 +21,14 @@ #include #include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "rclcpp/parameter_service.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_publisher.hpp" #include "rclcpp/qos.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" + +#include "lifecycle_node.hpp" +#include "node_utils.hpp" namespace nav2_util { @@ -53,7 +58,10 @@ class TwistPublisher ) : topic_(topic) { - node->declare_parameter("enable_stamped_cmd_vel", false); + using nav2_util::declare_parameter_if_not_declared; + declare_parameter_if_not_declared( + node, "enable_stamped_cmd_vel", + rclcpp::ParameterValue{false}); node->get_parameter("enable_stamped_cmd_vel", is_stamped_); if (is_stamped_) { twist_stamped_pub_ = node->create_publisher( @@ -66,7 +74,6 @@ class TwistPublisher } } - void on_activate() { if (is_stamped_) { diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index cfd747b9f1b..b4147107c07 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -44,3 +44,7 @@ target_link_libraries(test_robot_utils ${library_name}) ament_add_gtest(test_array_parser test_array_parser.cpp) target_link_libraries(test_array_parser ${library_name}) + +ament_add_gtest(test_twist_publisher test_twist_publisher.cpp) +ament_target_dependencies(test_twist_publisher rclcpp_lifecycle) +target_link_libraries(test_twist_publisher ${library_name}) diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp new file mode 100644 index 00000000000..baaf0bf6a51 --- /dev/null +++ b/nav2_util/test/test_twist_publisher.cpp @@ -0,0 +1,52 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_util/twist_publisher.hpp" +#include "nav2_util/lifecycle_utils.hpp" +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" + +using nav2_util::startup_lifecycle_nodes; +using nav2_util::reset_lifecycle_nodes; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(TwistPublisher, Unstamped) +{ + auto node = std::make_shared("test_node", ""); + node->declare_parameter("enable_stamped_cmd_vel", false); + + node->configure(); + node->activate(); + + auto vel_publisher = std::make_unique(node, "cmd_vel", 1); + ASSERT_EQ(vel_publisher->get_subscription_count(), 0); + + vel_publisher->on_activate(); + + const geometry_msgs::msg::TwistStamped twist_stamped {}; + vel_publisher->publish(twist_stamped); + node->deactivate(); + SUCCEED(); +} From 14710b63267ae9eeb080c0f9c194d3971946de95 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 1 Sep 2023 12:39:35 -0600 Subject: [PATCH 05/38] Add missing spin call to solve timeout Signed-off-by: Ryan Friedman --- nav2_util/test/test_twist_publisher.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp index baaf0bf6a51..be6d76388cd 100644 --- a/nav2_util/test/test_twist_publisher.cpp +++ b/nav2_util/test/test_twist_publisher.cpp @@ -47,6 +47,7 @@ TEST(TwistPublisher, Unstamped) const geometry_msgs::msg::TwistStamped twist_stamped {}; vel_publisher->publish(twist_stamped); + rclcpp::spin_some(node->get_node_base_interface()); node->deactivate(); SUCCEED(); } From a9c707838473cca89dd28ea3efefa842d7716202 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 1 Sep 2023 13:18:14 -0600 Subject: [PATCH 06/38] Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman --- nav2_util/test/test_twist_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp index be6d76388cd..a176d41db9d 100644 --- a/nav2_util/test/test_twist_publisher.cpp +++ b/nav2_util/test/test_twist_publisher.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2019 Intel Corporation +// Copyright (C) 2023 Ryan Friedman // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 0b7cbbb29d94862a2aae0d39338a0daeba91abf3 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 2 Sep 2023 11:13:04 -0600 Subject: [PATCH 07/38] Add full test coverage with subscriber Signed-off-by: Ryan Friedman --- nav2_util/test/test_twist_publisher.cpp | 80 +++++++++++++++++++++---- 1 file changed, 67 insertions(+), 13 deletions(-) diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp index a176d41db9d..743bd0083c6 100644 --- a/nav2_util/test/test_twist_publisher.cpp +++ b/nav2_util/test/test_twist_publisher.cpp @@ -27,27 +27,81 @@ using nav2_util::reset_lifecycle_nodes; class RclCppFixture { public: - RclCppFixture() {rclcpp::init(0, nullptr);} - ~RclCppFixture() {rclcpp::shutdown();} + RclCppFixture() {} + ~RclCppFixture() {} }; RclCppFixture g_rclcppfixture; TEST(TwistPublisher, Unstamped) { - auto node = std::make_shared("test_node", ""); - node->declare_parameter("enable_stamped_cmd_vel", false); + rclcpp::init(0, nullptr); + auto pub_node = std::make_shared("pub_node", ""); + pub_node->configure(); + auto vel_publisher = std::make_unique(pub_node, "cmd_vel", 1); + ASSERT_EQ(vel_publisher->get_subscription_count(), 0); + EXPECT_FALSE(vel_publisher->is_activated()); + pub_node->activate(); + EXPECT_TRUE(vel_publisher->is_activated()); + vel_publisher->on_activate(); + auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node->get_node_base_interface());}); + + auto sub_node = std::make_shared("sub_node", ""); + sub_node->configure(); + sub_node->activate(); - node->configure(); - node->activate(); + geometry_msgs::msg::TwistStamped pub_msg {}; + pub_msg.twist.linear.x = 42.0; - auto vel_publisher = std::make_unique(node, "cmd_vel", 1); + geometry_msgs::msg::Twist sub_msg {}; + auto my_sub = sub_node->create_subscription( + "cmd_vel", 10, + [&](const geometry_msgs::msg::Twist msg) {sub_msg = msg;}); + + vel_publisher->publish(pub_msg); + rclcpp::spin_some(sub_node->get_node_base_interface()); + + EXPECT_EQ(pub_msg.twist.linear.x, sub_msg.linear.x); + ASSERT_EQ(vel_publisher->get_subscription_count(), 1); + pub_node->deactivate(); + sub_node->deactivate(); + rclcpp::shutdown(); + // Have to join thread after rclcpp is shut down otherwise test hangs. + pub_thread.join(); +} + +TEST(TwistPublisher, Stamped) +{ + rclcpp::init(0, nullptr); + auto pub_node = std::make_shared("pub_node", ""); + pub_node->declare_parameter("enable_stamped_cmd_vel", true); + pub_node->configure(); + auto vel_publisher = std::make_unique(pub_node, "cmd_vel", 1); ASSERT_EQ(vel_publisher->get_subscription_count(), 0); + EXPECT_FALSE(vel_publisher->is_activated()); + pub_node->activate(); + EXPECT_TRUE(vel_publisher->is_activated()); + auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node->get_node_base_interface());}); - vel_publisher->on_activate(); + auto sub_node = std::make_shared("sub_node", ""); + sub_node->configure(); + sub_node->activate(); + + geometry_msgs::msg::TwistStamped pub_msg {}; + pub_msg.twist.linear.x = 42.0; + pub_msg.header.frame_id = "foo"; + + geometry_msgs::msg::TwistStamped sub_msg {}; + auto my_sub = sub_node->create_subscription( + "cmd_vel", 10, + [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;}); - const geometry_msgs::msg::TwistStamped twist_stamped {}; - vel_publisher->publish(twist_stamped); - rclcpp::spin_some(node->get_node_base_interface()); - node->deactivate(); - SUCCEED(); + vel_publisher->publish(pub_msg); + rclcpp::spin_some(sub_node->get_node_base_interface()); + ASSERT_EQ(vel_publisher->get_subscription_count(), 1); + EXPECT_EQ(pub_msg, sub_msg); + pub_node->deactivate(); + sub_node->deactivate(); + rclcpp::shutdown(); + // Have to join thread after rclcpp is shut down otherwise test hangs. + pub_thread.join(); } From 4d222fb73fa55d0c0b6b1f202fc7e7148d718ab9 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 2 Sep 2023 11:16:54 -0600 Subject: [PATCH 08/38] Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman --- nav2_util/test/test_twist_publisher.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp index 743bd0083c6..08984081db4 100644 --- a/nav2_util/test/test_twist_publisher.cpp +++ b/nav2_util/test/test_twist_publisher.cpp @@ -24,14 +24,6 @@ using nav2_util::startup_lifecycle_nodes; using nav2_util::reset_lifecycle_nodes; -class RclCppFixture -{ -public: - RclCppFixture() {} - ~RclCppFixture() {} -}; -RclCppFixture g_rclcppfixture; - TEST(TwistPublisher, Unstamped) { rclcpp::init(0, nullptr); From 9e9560d658d19924b6bdfaaab5d85461e98dd48f Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Tue, 5 Sep 2023 19:19:50 -0600 Subject: [PATCH 09/38] Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman --- .../plugins/assisted_teleop.hpp | 2 +- .../plugins/drive_on_heading.hpp | 17 ++++++++------- .../include/nav2_behaviors/plugins/spin.hpp | 3 ++- .../include/nav2_behaviors/timed_behavior.hpp | 21 +++++++++++-------- nav2_behaviors/plugins/assisted_teleop.cpp | 20 +++++++++--------- nav2_behaviors/plugins/spin.cpp | 12 +++++------ 6 files changed, 40 insertions(+), 35 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp index 33f9ef7762d..5e91f91d838 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp @@ -100,7 +100,7 @@ class AssistedTeleop : public TimedBehavior double projection_time_; double simulation_time_step_; - geometry_msgs::msg::Twist teleop_twist_; + geometry_msgs::msg::TwistStamped teleop_twist_; bool preempt_teleop_{false}; // subscribers diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index cd0a1674285..5620391f782 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -24,6 +24,7 @@ #include "nav2_msgs/action/drive_on_heading.hpp" #include "nav2_msgs/action/back_up.hpp" #include "nav2_util/node_utils.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" namespace nav2_behaviors { @@ -125,23 +126,23 @@ class DriveOnHeading : public TimedBehavior return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE}; } - auto cmd_vel = std::make_unique(); - cmd_vel->linear.y = 0.0; - cmd_vel->angular.z = 0.0; - cmd_vel->linear.x = command_speed_; + auto cmd_vel = geometry_msgs::msg::TwistStamped(); + cmd_vel.twist.linear.y = 0.0; + cmd_vel.twist.angular.z = 0.0; + cmd_vel.twist.linear.x = command_speed_; geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) { + if (!isCollisionFree(distance, cmd_vel.twist, pose2d)) { this->stopRobot(); RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading"); return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD}; } - this->vel_pub_->publish(std::move(cmd_vel)); + this->vel_pub_->publish(cmd_vel); return ResultStatus{Status::RUNNING, ActionT::Result::NONE}; } @@ -162,7 +163,7 @@ class DriveOnHeading : public TimedBehavior */ bool isCollisionFree( const double & distance, - geometry_msgs::msg::Twist * cmd_vel, + const geometry_msgs::msg::Twist & cmd_vel, geometry_msgs::msg::Pose2D & pose2d) { // Simulate ahead by simulate_ahead_time_ in this->cycle_frequency_ increments @@ -174,7 +175,7 @@ class DriveOnHeading : public TimedBehavior bool fetch_data = true; while (cycle_count < max_cycle_count) { - sim_position_change = cmd_vel->linear.x * (cycle_count / this->cycle_frequency_); + sim_position_change = cmd_vel.linear.x * (cycle_count / this->cycle_frequency_); pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta); pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta); cycle_count++; diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp index 01358ddbd33..eeb84462220 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp @@ -22,6 +22,7 @@ #include "nav2_behaviors/timed_behavior.hpp" #include "nav2_msgs/action/spin.hpp" #include "geometry_msgs/msg/quaternion.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" namespace nav2_behaviors { @@ -79,7 +80,7 @@ class Spin : public TimedBehavior */ bool isCollisionFree( const double & distance, - geometry_msgs::msg::Twist * cmd_vel, + const geometry_msgs::msg::Twist & cmd_vel, geometry_msgs::msg::Pose2D & pose2d); SpinAction::Feedback::SharedPtr feedback_; diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index d01e14179ff..1cc602aff5f 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -29,8 +29,9 @@ #include "tf2_ros/transform_listener.h" #include "tf2_ros/create_timer_ros.h" #include "geometry_msgs/msg/twist.hpp" -#include "nav2_util/simple_action_server.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/twist_publisher.hpp" +#include "nav2_util/simple_action_server.hpp" #include "nav2_core/behavior.hpp" #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" @@ -150,7 +151,7 @@ class TimedBehavior : public nav2_core::Behavior local_collision_checker_ = local_collision_checker; global_collision_checker_ = global_collision_checker; - vel_pub_ = node->template create_publisher("cmd_vel", 1); + vel_pub_ = std::make_unique(node, "cmd_vel", 1); onConfigure(); } @@ -185,7 +186,7 @@ class TimedBehavior : public nav2_core::Behavior rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::string behavior_name_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_pub_; + std::unique_ptr vel_pub_; std::shared_ptr action_server_; std::shared_ptr local_collision_checker_; std::shared_ptr global_collision_checker_; @@ -289,12 +290,14 @@ class TimedBehavior : public nav2_core::Behavior // Stop the robot with a commanded velocity void stopRobot() { - auto cmd_vel = std::make_unique(); - cmd_vel->linear.x = 0.0; - cmd_vel->linear.y = 0.0; - cmd_vel->angular.z = 0.0; - - vel_pub_->publish(std::move(cmd_vel)); + auto cmd_vel = std::make_unique(); + cmd_vel->header.frame_id = "base_link"; + cmd_vel->header.stamp = rclcpp::Time(); + cmd_vel->twist.linear.x = 0.0; + cmd_vel->twist.linear.y = 0.0; + cmd_vel->twist.angular.z = 0.0; + + // vel_pub_->publish(std::move(cmd_vel)); } }; diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index a0fc9729991..312d2c04f09 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -73,7 +73,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptr/*result*/) { - teleop_twist_ = geometry_msgs::msg::Twist(); + teleop_twist_ = geometry_msgs::msg::TwistStamped(); preempt_teleop_ = false; } @@ -115,11 +115,11 @@ ResultStatus AssistedTeleop::onCycleUpdate() projected_pose.y = current_pose.pose.position.y; projected_pose.theta = tf2::getYaw(current_pose.pose.orientation); - geometry_msgs::msg::Twist scaled_twist = teleop_twist_; + geometry_msgs::msg::TwistStamped scaled_twist = teleop_twist_; for (double time = simulation_time_step_; time < projection_time_; time += simulation_time_step_) { - projected_pose = projectPose(projected_pose, teleop_twist_, simulation_time_step_); + projected_pose = projectPose(projected_pose, teleop_twist_.twist, simulation_time_step_); if (!local_collision_checker_->isCollisionFree(projected_pose)) { if (time == simulation_time_step_) { @@ -128,9 +128,9 @@ ResultStatus AssistedTeleop::onCycleUpdate() *clock_, 1000, behavior_name_.c_str() << " collided on first time step, setting velocity to zero"); - scaled_twist.linear.x = 0.0f; - scaled_twist.linear.y = 0.0f; - scaled_twist.angular.z = 0.0f; + scaled_twist.twist.linear.x = 0.0f; + scaled_twist.twist.linear.y = 0.0f; + scaled_twist.twist.angular.z = 0.0f; break; } else { RCLCPP_DEBUG_STREAM_THROTTLE( @@ -139,9 +139,9 @@ ResultStatus AssistedTeleop::onCycleUpdate() 1000, behavior_name_.c_str() << " collision approaching in " << time << " seconds"); double scale_factor = time / projection_time_; - scaled_twist.linear.x *= scale_factor; - scaled_twist.linear.y *= scale_factor; - scaled_twist.angular.z *= scale_factor; + scaled_twist.twist.linear.x *= scale_factor; + scaled_twist.twist.linear.y *= scale_factor; + scaled_twist.twist.angular.z *= scale_factor; break; } } @@ -173,7 +173,7 @@ geometry_msgs::msg::Pose2D AssistedTeleop::projectPose( void AssistedTeleop::teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { - teleop_twist_ = *msg; + teleop_twist_.twist = *msg; } void AssistedTeleop::preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr) diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index 55a7872247a..2f0b3bc7dba 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -138,28 +138,28 @@ ResultStatus Spin::onCycleUpdate() double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw); vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); - auto cmd_vel = std::make_unique(); - cmd_vel->angular.z = copysign(vel, cmd_yaw_); + auto cmd_vel = geometry_msgs::msg::TwistStamped(); + cmd_vel.twist.angular.z = copysign(vel, cmd_yaw_); geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) { + if (!isCollisionFree(relative_yaw_, cmd_vel.twist, pose2d)) { stopRobot(); RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin"); return ResultStatus{Status::FAILED, SpinActionResult::COLLISION_AHEAD}; } - vel_pub_->publish(std::move(cmd_vel)); + vel_pub_->publish(cmd_vel); return ResultStatus{Status::RUNNING, SpinActionResult::NONE}; } bool Spin::isCollisionFree( const double & relative_yaw, - geometry_msgs::msg::Twist * cmd_vel, + const geometry_msgs::msg::Twist & cmd_vel, geometry_msgs::msg::Pose2D & pose2d) { // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments @@ -170,7 +170,7 @@ bool Spin::isCollisionFree( bool fetch_data = true; while (cycle_count < max_cycle_count) { - sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_); + sim_position_change = cmd_vel.angular.z * (cycle_count / cycle_frequency_); pose2d.theta = init_pose.theta + sim_position_change; cycle_count++; From 437ac3389f09bf0c3889c712f520f8f0c13e1f80 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Tue, 5 Sep 2023 21:46:29 -0600 Subject: [PATCH 10/38] Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman --- .../nav2_collision_monitor/collision_monitor_node.hpp | 2 ++ nav2_collision_monitor/src/collision_monitor_node.cpp | 5 +++++ nav2_util/include/nav2_util/robot_utils.hpp | 4 +++- nav2_util/src/robot_utils.cpp | 5 +++++ 4 files changed, 15 insertions(+), 1 deletion(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 09a13658092..f61e8a9b331 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -22,6 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" #include "visualization_msgs/msg/marker_array.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -96,6 +97,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @param msg Input cmd_vel message */ void cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg); + void cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); /** * @brief Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds, * quit to publish 0-velocity. diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 29e3f3031de..884a08f509a 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -191,6 +191,11 @@ void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPt process({msg->linear.x, msg->linear.y, msg->angular.z}); } +void CollisionMonitor::cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + cmdVelInCallback(std::make_shared(msg->twist)); +} + void CollisionMonitor::publishVelocity(const Action & robot_action) { if (robot_action.req_vel.isZero()) { diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index bcd2d2ae835..3cc8f87267b 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -22,6 +22,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -103,7 +104,8 @@ bool getTransform( * @param msg Twist message to validate * @return True if valid, false if contains unactionable values */ -bool validateTwist(const geometry_msgs::msg::Twist & msg); +[[nodiscard]] bool validateTwist(const geometry_msgs::msg::Twist & msg); +[[nodiscard]] bool validateTwist(const geometry_msgs::msg::TwistStamped & msg); } // end namespace nav2_util diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index e6a6e10cf41..6933b716945 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -172,4 +172,9 @@ bool validateTwist(const geometry_msgs::msg::Twist & msg) return true; } +bool validateTwist(const geometry_msgs::msg::TwistStamped & msg) +{ + return validateTwist(msg.twist); +} + } // end namespace nav2_util From fc862f1204038fe18fdfa5e12566130101eadc4f Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Tue, 5 Sep 2023 21:57:40 -0600 Subject: [PATCH 11/38] Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman --- nav2_velocity_smoother/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md index 6ffbe5d372e..3270db3fdea 100644 --- a/nav2_velocity_smoother/README.md +++ b/nav2_velocity_smoother/README.md @@ -65,8 +65,8 @@ velocity_smoother: | Topic | Type | Use | |------------------|-------------------------|-------------------------------| -| smoothed_cmd_vel | geometry_msgs/Twist | Publish smoothed velocities | -| cmd_vel | geometry_msgs/Twist | Subscribe to input velocities | +| smoothed_cmd_vel | geometry_msgs/Twist or geometry_msgs/TwistStamped | Publish smoothed velocities | +| cmd_vel | geometry_msgs/Twist or geometry_msgs/TwistStamped | Subscribe to input velocities | ## Install From 561831857a233d969af293bc91c15864b5d2888d Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Tue, 5 Sep 2023 21:57:40 -0600 Subject: [PATCH 12/38] Add TwistSubscriber implementation Signed-off-by: Ryan Friedman --- .../include/nav2_util/twist_subscriber.hpp | 136 ++++++++++++++++++ nav2_util/test/CMakeLists.txt | 4 + nav2_util/test/test_twist_subscriber.cpp | 70 +++++++++ 3 files changed, 210 insertions(+) create mode 100644 nav2_util/include/nav2_util/twist_subscriber.hpp create mode 100644 nav2_util/test/test_twist_subscriber.cpp diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp new file mode 100644 index 00000000000..2a8be88188d --- /dev/null +++ b/nav2_util/include/nav2_util/twist_subscriber.hpp @@ -0,0 +1,136 @@ +// Copyright (C) 2023 Ryan Friedman +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef NAV2_UTIL__TWIST_SUBSCRIBER_HPP_ +#define NAV2_UTIL__TWIST_SUBSCRIBER_HPP_ + + +#include +#include + +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "rclcpp/parameter_service.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/qos.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" + +#include "lifecycle_node.hpp" +#include "node_utils.hpp" + +namespace nav2_util +{ + +/** + * @class nav2_util::TwistSubscriber + * @brief A simple wrapper on a Twist subscriber that receives either Twist or TwistStamped + * + * The default is to subscribe to Twist to preserve backwards compatibility, but it can be overridden + * using the "enable_stamped_cmd_vel" parameter to subscribe to TwistStamped. + * + */ + +class TwistSubscriber +{ +public: + /** + * @brief A constructor + * @param nh The node + * @param topic publisher topic name + * @param qos publisher quality of service + */ + template + explicit TwistSubscriber( + nav2_util::LifecycleNode::SharedPtr node, + const std::string & topic, + const rclcpp::QoS & qos, + TwistCallbackT && TwistCallback, + TwistStampedCallbackT && TwistStampedCallback + ) + : { + using nav2_util::declare_parameter_if_not_declared; + declare_parameter_if_not_declared( + node, "enable_stamped_cmd_vel", + rclcpp::ParameterValue{false}); + node->get_parameter("enable_stamped_cmd_vel", is_stamped_); + if (is_stamped_) { + twist_stamped_sub_ = node->create_subscription( + topic, + qos, + std::forward(TwistStampedCallback)); + } else { + twist_sub_ = node->create_subscription( + topic, + qos, + std::forward(TwistCallback)); + } + } + + // void on_activate() + // { + // if (is_stamped_) { + // twist_stamped_pub_->on_activate(); + // } else { + // twist_pub_->on_activate(); + // } + // } + + // void on_deactivate() + // { + // if (is_stamped_) { + // twist_stamped_pub_->on_deactivate(); + // } else { + // twist_pub_->on_deactivate(); + // } + // } + + // [[nodiscard]] bool is_activated() const + // { + // if (is_stamped_) { + // return twist_stamped_pub_->is_activated(); + // } else { + // return twist_pub_->is_activated(); + // } + // } + + // void publish(const geometry_msgs::msg::TwistStamped & velocity) + // { + // if (is_stamped_) { + // twist_stamped_pub_->publish(velocity); + // } else { + // twist_pub_->publish(velocity.twist); + // } + // } + + // [[nodiscard]] size_t get_subscription_count() const + // { + // if (is_stamped_) { + // return twist_stamped_pub_->get_subscription_count(); + // } else { + // return twist_pub_->get_subscription_count(); + // } + // } + +protected: + bool is_stamped_; + rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::Subscription::SharedPtr twist_stamped_sub_; +}; + +} // namespace nav2_util + +#endif // NAV2_UTIL__TWIST_SUBSCRIBER_HPP_ diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index b4147107c07..fb4b6c107f2 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -48,3 +48,7 @@ target_link_libraries(test_array_parser ${library_name}) ament_add_gtest(test_twist_publisher test_twist_publisher.cpp) ament_target_dependencies(test_twist_publisher rclcpp_lifecycle) target_link_libraries(test_twist_publisher ${library_name}) + +ament_add_gtest(test_twist_subscriber test_twist_subscriber.cpp) +ament_target_dependencies(test_twist_subscriber rclcpp_lifecycle) +target_link_libraries(test_twist_subscriber ${library_name}) diff --git a/nav2_util/test/test_twist_subscriber.cpp b/nav2_util/test/test_twist_subscriber.cpp new file mode 100644 index 00000000000..447733dea6b --- /dev/null +++ b/nav2_util/test/test_twist_subscriber.cpp @@ -0,0 +1,70 @@ +// Copyright (C) 2023 Ryan Friedman +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "nav2_util/twist_subscriber.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +// #include "nav2_util/lifecycle_utils.hpp" +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +// #include "rclcpp_lifecycle/lifecycle_publisher.hpp" + +// using nav2_util::startup_lifecycle_nodes; +// using nav2_util::reset_lifecycle_nodes; + +TEST(TwistPublisher, Unstamped) +{ + rclcpp::init(0, nullptr); + auto sub_node = std::make_shared("pub_node", ""); + sub_node->configure(); + sub_node->activate(); + + geometry_msgs::msg::TwistStamped sub_msg {}; + auto vel_subscriber = std::make_unique( + sub_node, "cmd_vel", 1, + [&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;}, + [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;} + ); +// ASSERT_EQ(vel_publisher->get_subscription_count(), 0); +// EXPECT_FALSE(vel_publisher->is_activated()); +// pub_node->activate(); +// EXPECT_TRUE(vel_publisher->is_activated()); +// vel_publisher->on_activate(); +// auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node->get_node_base_interface());}); + +// auto sub_node = std::make_shared("sub_node", ""); +// pub_node->configure(); + + +// geometry_msgs::msg::TwistStamped pub_msg {}; +// pub_msg.twist.linear.x = 42.0; + +// geometry_msgs::msg::Twist sub_msg {}; +// auto my_sub = sub_node->create_subscription( +// "cmd_vel", 10, +// [&](const geometry_msgs::msg::Twist msg) {sub_msg = msg;}); + +// vel_publisher->publish(pub_msg); +// rclcpp::spin_some(sub_node->get_node_base_interface()); + +// EXPECT_EQ(pub_msg.twist.linear.x, sub_msg.linear.x); +// ASSERT_EQ(vel_publisher->get_subscription_count(), 1); +// pub_node->deactivate(); + sub_node->deactivate(); + rclcpp::shutdown(); + // Have to join thread after rclcpp is shut down otherwise test hangs. +// pub_thread.join(); +} From cf409dd35da641271b64d602b2e0a658000a4793 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Wed, 25 Oct 2023 12:41:56 -0600 Subject: [PATCH 13/38] Fix syntax errors Signed-off-by: Ryan Friedman --- nav2_util/include/nav2_util/twist_subscriber.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp index 2a8be88188d..da9692d9cf4 100644 --- a/nav2_util/include/nav2_util/twist_subscriber.hpp +++ b/nav2_util/include/nav2_util/twist_subscriber.hpp @@ -60,8 +60,7 @@ class TwistSubscriber const rclcpp::QoS & qos, TwistCallbackT && TwistCallback, TwistStampedCallbackT && TwistStampedCallback - ) - : { + ) { using nav2_util::declare_parameter_if_not_declared; declare_parameter_if_not_declared( node, "enable_stamped_cmd_vel", From 9383fcbc3f08a854ca14c4415e5b1927cea91858 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Thu, 26 Oct 2023 09:44:38 -0600 Subject: [PATCH 14/38] Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman --- .../test/test_velocity_smoother.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index c58e20b22c4..86505069199 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -23,6 +23,7 @@ #include "nav2_velocity_smoother/velocity_smoother.hpp" #include "nav_msgs/msg/odometry.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "nav2_util/twist_subscriber.hpp" using namespace std::chrono_literals; @@ -66,11 +67,14 @@ TEST(VelocitySmootherTest, openLoopTestTimer) smoother->activate(state); std::vector linear_vels; - auto subscription = smoother->create_subscription( + auto subscription = nav2_util::TwistSubscriber( + smoother, "cmd_vel_smoothed", 1, [&](geometry_msgs::msg::Twist::SharedPtr msg) { linear_vels.push_back(msg->linear.x); + }, [&](geometry_msgs::msg::TwistStamped::SharedPtr msg) { + linear_vels.push_back(msg->twist.linear.x); }); // Send a velocity command @@ -117,11 +121,14 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer) smoother->activate(state); std::vector linear_vels; - auto subscription = smoother->create_subscription( + auto subscription = nav2_util::TwistSubscriber( + smoother, "cmd_vel_smoothed", 1, [&](geometry_msgs::msg::Twist::SharedPtr msg) { linear_vels.push_back(msg->linear.x); + }, [&](geometry_msgs::msg::TwistStamped::SharedPtr msg) { + linear_vels.push_back(msg->twist.linear.x); }); auto odom_pub = smoother->create_publisher("odom", 1); From eed43566d932247007c8867bf4c399ad5a3900e8 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Thu, 26 Oct 2023 10:01:53 -0600 Subject: [PATCH 15/38] Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman --- .../nav2_behaviors/plugins/assisted_teleop.hpp | 9 ++------- nav2_behaviors/plugins/assisted_teleop.cpp | 17 ++++++++--------- 2 files changed, 10 insertions(+), 16 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp index 5e91f91d838..f245191ad21 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp @@ -23,6 +23,7 @@ #include "std_msgs/msg/empty.hpp" #include "nav2_behaviors/timed_behavior.hpp" #include "nav2_msgs/action/assisted_teleop.hpp" +#include "nav2_util/twist_subscriber.hpp" namespace nav2_behaviors { @@ -82,12 +83,6 @@ class AssistedTeleop : public TimedBehavior const geometry_msgs::msg::Twist & twist, double projection_time); - /** - * @brief Callback function for velocity subscriber - * @param msg received Twist message - */ - void teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg); - /** * @brief Callback function to preempt assisted teleop * @param msg empty message @@ -104,7 +99,7 @@ class AssistedTeleop : public TimedBehavior bool preempt_teleop_{false}; // subscribers - rclcpp::Subscription::SharedPtr vel_sub_; + std::shared_ptr vel_sub_; rclcpp::Subscription::SharedPtr preempt_teleop_sub_; rclcpp::Duration command_time_allowance_{0, 0}; diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index 312d2c04f09..31194cd7544 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -50,11 +50,15 @@ void AssistedTeleop::onConfigure() std::string cmd_vel_teleop; node->get_parameter("cmd_vel_teleop", cmd_vel_teleop); - vel_sub_ = node->create_subscription( + + vel_sub_ = std::make_shared( + node, cmd_vel_teleop, rclcpp::SystemDefaultsQoS(), - std::bind( - &AssistedTeleop::teleopVelocityCallback, - this, std::placeholders::_1)); + [&](geometry_msgs::msg::Twist::SharedPtr msg) { + teleop_twist_.twist = *msg; + }, [&](geometry_msgs::msg::TwistStamped::SharedPtr msg) { + teleop_twist_ = *msg; + }); preempt_teleop_sub_ = node->create_subscription( "preempt_teleop", rclcpp::SystemDefaultsQoS(), @@ -171,11 +175,6 @@ geometry_msgs::msg::Pose2D AssistedTeleop::projectPose( return projected_pose; } -void AssistedTeleop::teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg) -{ - teleop_twist_.twist = *msg; -} - void AssistedTeleop::preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr) { preempt_teleop_ = true; From 1b5873569cd3e702b29dbd824f216a0133607d27 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Mon, 30 Oct 2023 20:53:56 -0600 Subject: [PATCH 16/38] Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman --- .../nav2_collision_monitor/collision_monitor_node.hpp | 5 +++-- nav2_collision_monitor/src/collision_monitor_node.cpp | 10 +++++++--- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index f61e8a9b331..db3171b3cb9 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -29,6 +29,7 @@ #include "tf2_ros/transform_listener.h" #include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/twist_subscriber.hpp" #include "nav2_msgs/msg/collision_monitor_state.hpp" #include "nav2_collision_monitor/types.hpp" @@ -205,8 +206,8 @@ class CollisionMonitor : public nav2_util::LifecycleNode std::vector> sources_; // Input/output speed controls - /// @beirf Input cmd_vel subscriber - rclcpp::Subscription::SharedPtr cmd_vel_in_sub_; + /// @brief Input cmd_vel subscriber + std::shared_ptr cmd_vel_in_sub_; /// @brief Output cmd_vel publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr cmd_vel_out_pub_; diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 884a08f509a..2aaf3d66a1a 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -63,9 +63,13 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::FAILURE; } - cmd_vel_in_sub_ = this->create_subscription( - cmd_vel_in_topic, 1, - std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1)); + cmd_vel_in_sub_ = std::make_shared( + shared_from_this(), + cmd_vel_in_topic, + 1, + std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1), + std::bind(&CollisionMonitor::cmdVelInCallbackStamped, this, std::placeholders::_1)); + cmd_vel_out_pub_ = this->create_publisher( cmd_vel_out_topic, 1); From dc4afe402653746ca095c8921c597f0ee927d6ad Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Mon, 30 Oct 2023 21:03:56 -0600 Subject: [PATCH 17/38] Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman --- .../nav2_velocity_smoother/velocity_smoother.hpp | 6 +++++- nav2_velocity_smoother/src/velocity_smoother.cpp | 13 +++++++++++-- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp index a059e74076c..2744a8652fc 100644 --- a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp +++ b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp @@ -26,6 +26,9 @@ #include "nav2_util/node_utils.hpp" #include "nav2_util/odometry_utils.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/twist_subscriber.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" namespace nav2_velocity_smoother { @@ -114,6 +117,7 @@ class VelocitySmoother : public nav2_util::LifecycleNode * @param msg Twist message */ void inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + void inputCommandStampedCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); /** * @brief Main worker timer function @@ -131,7 +135,7 @@ class VelocitySmoother : public nav2_util::LifecycleNode std::unique_ptr odom_smoother_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr smoothed_cmd_pub_; - rclcpp::Subscription::SharedPtr cmd_sub_; + std::shared_ptr cmd_sub_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Clock::SharedPtr clock_; diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index d2dad659130..a00edf6245a 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -129,9 +129,12 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) // Setup inputs / outputs smoothed_cmd_pub_ = create_publisher("cmd_vel_smoothed", 1); - cmd_sub_ = create_subscription( + cmd_sub_ = std::make_shared( + shared_from_this(), "cmd_vel", rclcpp::QoS(1), - std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1)); + std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1), + std::bind(&VelocitySmoother::inputCommandStampedCallback, this, std::placeholders::_1) + ); declare_parameter_if_not_declared(node, "use_realtime_priority", rclcpp::ParameterValue(false)); bool use_realtime_priority = false; @@ -211,6 +214,12 @@ void VelocitySmoother::inputCommandCallback(const geometry_msgs::msg::Twist::Sha last_command_time_ = now(); } +void VelocitySmoother::inputCommandStampedCallback( + geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + inputCommandCallback(std::make_shared(msg->twist)); +} + double VelocitySmoother::findEtaConstraint( const double v_curr, const double v_cmd, const double accel, const double decel) { From 14f898c38076999cf74b80a98b49c7a270239e6b Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Mon, 30 Oct 2023 21:05:41 -0600 Subject: [PATCH 18/38] Remove unused code Signed-off-by: Ryan Friedman --- .../include/nav2_util/twist_subscriber.hpp | 48 +------------------ 1 file changed, 2 insertions(+), 46 deletions(-) diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp index da9692d9cf4..4f542723cfc 100644 --- a/nav2_util/include/nav2_util/twist_subscriber.hpp +++ b/nav2_util/include/nav2_util/twist_subscriber.hpp @@ -60,7 +60,8 @@ class TwistSubscriber const rclcpp::QoS & qos, TwistCallbackT && TwistCallback, TwistStampedCallbackT && TwistStampedCallback - ) { + ) + { using nav2_util::declare_parameter_if_not_declared; declare_parameter_if_not_declared( node, "enable_stamped_cmd_vel", @@ -79,51 +80,6 @@ class TwistSubscriber } } - // void on_activate() - // { - // if (is_stamped_) { - // twist_stamped_pub_->on_activate(); - // } else { - // twist_pub_->on_activate(); - // } - // } - - // void on_deactivate() - // { - // if (is_stamped_) { - // twist_stamped_pub_->on_deactivate(); - // } else { - // twist_pub_->on_deactivate(); - // } - // } - - // [[nodiscard]] bool is_activated() const - // { - // if (is_stamped_) { - // return twist_stamped_pub_->is_activated(); - // } else { - // return twist_pub_->is_activated(); - // } - // } - - // void publish(const geometry_msgs::msg::TwistStamped & velocity) - // { - // if (is_stamped_) { - // twist_stamped_pub_->publish(velocity); - // } else { - // twist_pub_->publish(velocity.twist); - // } - // } - - // [[nodiscard]] size_t get_subscription_count() const - // { - // if (is_stamped_) { - // return twist_stamped_pub_->get_subscription_count(); - // } else { - // return twist_pub_->get_subscription_count(); - // } - // } - protected: bool is_stamped_; rclcpp::Subscription::SharedPtr twist_sub_; From 90c0fb5eb7180946daea35a6f4274393e94bc139 Mon Sep 17 00:00:00 2001 From: pedro-fuoco Date: Fri, 8 Sep 2023 23:50:45 -0300 Subject: [PATCH 19/38] add timestamp and frame_id to TwistStamped message --- nav2_controller/src/controller_server.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 26134a4eb29..31dcb960ad4 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -595,6 +595,8 @@ void ControllerServer::computeAndPublishVelocity() nav_2d_utils::twist2Dto3D(twist), goal_checkers_[current_goal_checker_].get()); last_valid_cmd_time_ = now(); + cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID(); + cmd_vel_2d.header.stamp = last_valid_cmd_time_; // Only no valid control exception types are valid to attempt to have control patience, as // other types will not be resolved with more attempts } catch (nav2_core::NoValidControl & e) { From e38d4c742c65daa4ae56ae07e031936a8234a748 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Mon, 30 Oct 2023 22:55:53 -0600 Subject: [PATCH 20/38] Add missing utility include Signed-off-by: Ryan Friedman --- nav2_util/include/nav2_util/twist_subscriber.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp index 4f542723cfc..c64fb7c248f 100644 --- a/nav2_util/include/nav2_util/twist_subscriber.hpp +++ b/nav2_util/include/nav2_util/twist_subscriber.hpp @@ -19,6 +19,7 @@ #include #include +#include #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" From 811d74c8a3e6e144a7a9038af695545602d7cdff Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Thu, 7 Dec 2023 20:02:49 -0700 Subject: [PATCH 21/38] Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman --- nav2_behaviors/README.md | 3 +++ nav2_collision_monitor/README.md | 2 ++ nav2_controller/README.md | 2 ++ nav2_util/README.md | 32 +++++++++++++++++++++++++++++++- nav2_velocity_smoother/README.md | 3 +++ 5 files changed, 41 insertions(+), 1 deletion(-) diff --git a/nav2_behaviors/README.md b/nav2_behaviors/README.md index 5ecd14af5c5..fdc24a26b31 100644 --- a/nav2_behaviors/README.md +++ b/nav2_behaviors/README.md @@ -13,3 +13,6 @@ The value of the centralized behavior server is to **share resources** amongst s See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_behavior_plugin.html). See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins. + +The `TimedBehavior` template makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). +The `AssistedTeleop` behavior makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). \ No newline at end of file diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 1c5b32dc7d1..4af1e890da4 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -88,3 +88,5 @@ The zones around the robot and the data sources are the same as for the Collisio ### Configuration Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html). + +The `CollisionMonitor` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). diff --git a/nav2_controller/README.md b/nav2_controller/README.md index 3a77144f160..98b76c33555 100644 --- a/nav2_controller/README.md +++ b/nav2_controller/README.md @@ -7,3 +7,5 @@ An execution module implementing the `nav2_msgs::action::FollowPath` action serv See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available controller plugins. See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-controller-server.html) for additional parameter descriptions and a [tutorial about writing controller plugins](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html). + +The `ControllerServer` makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). diff --git a/nav2_util/README.md b/nav2_util/README.md index 844370983c8..18f5dd51512 100644 --- a/nav2_util/README.md +++ b/nav2_util/README.md @@ -7,6 +7,36 @@ The `nav2_util` package contains utilities abstracted from individual packages w - Simplified service clients - Simplified action servers - Transformation and robot pose helpers +- Twist Subscriber and Twist Publisher The long-term aim is for these utilities to find more permanent homes in other packages (within and outside of Nav2) or migrate to the raw tools made available in ROS 2. - \ No newline at end of file + +## Twist Publisher and Twist Subscriber for commanded velocities + +### Background + +The Twist Publisher and Twist Subscriber are utility classes to assist NAV2 transition from +[Twist](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/Twist.msg) to [TwistStamped](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/TwistStamped.msg). + +This utility is in support of the following [GSoC Project](https://navigation.ros.org/2021summerOfCode/projects/twist_n_config.html#convert-twist-to-twiststamped-in-ecosystem-and-run-time-configuration). +Details on the migration are found in [#1594](https://github.com/ros-planning/navigation2/issues/1594). + +Certain applications of NAV2, such as in ROS Aerial mandate the usage of `TwistStamped`, while many other applications still use `Twist`. + +The utility has the following effect: +* Allows use of either `Twist` or `TwistStamped`, controlled by ROS parameter `enable_stamped_cmd_vel` +* Preserves existing topic names without duplication of data + +Every node in `nav2` that subscribes or publishes velocity commands with `Twist` now supports this optional behavior. +The behavior up through ROS 2 Iron is preserved - using `Twist`. In a future ROS 2 version, when enough of the +ROS ecosystem has moved to `TwistStamped`, the default may change. + +### Usage + +See [test/test_twist_publisher.cpp](test/test_twist_publisher.cpp) or [test/test_twist_subscriber.cpp](test/test_twist_subscriber.cpp) for examples. + +To use `Twist`, you can rely on the default value of `enable_stamped_cmd_vel`, or set it to `False` in your parameter file. +To use `TwistStamped`, set `enable_stamped_cmd_vel` to `True`. +Each node in `nav2` that uses a `TwistPublisher` or `TwistSubscriber` has it documented in the README. + +The parameter is read-only, therefore it must be set at launch time. Do not try changing the value at runtime. diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md index 3270db3fdea..d0a7288e8ad 100644 --- a/nav2_velocity_smoother/README.md +++ b/nav2_velocity_smoother/README.md @@ -59,6 +59,7 @@ velocity_smoother: max_decel: [-2.5, 0.0, -3.2] # Maximum deceleration, ordered [Ax, Ay, Aw] odom_topic: "odom" # Topic of odometry to use for estimating current velocities odom_duration: 0.1 # Period of time (s) to sample odometry information in for velocity estimation + enable_stamped_cmd_vel: false ``` ## Topics @@ -85,3 +86,5 @@ When in doubt, open-loop is a reasonable choice for most users. The minimum and maximum velocities for rotation (e.g. ``Vw``) represent left and right turns. While we make it possible to specify these separately, most users would be wise to set these values the same (but signed) for rotation. Additionally, the parameters are signed, so it is important to specify maximum deceleration with negative signs to represent deceleration. Minimum velocities with negatives when moving backward, so backward movement can be restricted by setting this to ``0``. Deadband velocities are minimum thresholds, below which we set its value to `0`. This can be useful when your robot's breaking torque from stand still is non-trivial so sending very small values will pull high amounts of current. + +The `VelocitySmoother` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities). \ No newline at end of file From 64f78ee586739dc64ff234ca7c5e2e4b14bf71db Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Thu, 7 Dec 2023 20:03:12 -0700 Subject: [PATCH 22/38] Use pass-by-reference * Instead of std::move(std::unique_ptr) Signed-off-by: Ryan Friedman --- .../include/nav2_behaviors/timed_behavior.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 1cc602aff5f..478aeda41eb 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -290,14 +290,14 @@ class TimedBehavior : public nav2_core::Behavior // Stop the robot with a commanded velocity void stopRobot() { - auto cmd_vel = std::make_unique(); - cmd_vel->header.frame_id = "base_link"; - cmd_vel->header.stamp = rclcpp::Time(); - cmd_vel->twist.linear.x = 0.0; - cmd_vel->twist.linear.y = 0.0; - cmd_vel->twist.angular.z = 0.0; - - // vel_pub_->publish(std::move(cmd_vel)); + auto cmd_vel = geometry_msgs::msg::TwistStamped(); + cmd_vel.header.frame_id = "base_link"; + cmd_vel.header.stamp = rclcpp::Time(); + cmd_vel.twist.linear.x = 0.0; + cmd_vel.twist.linear.y = 0.0; + cmd_vel.twist.angular.z = 0.0; + + vel_pub_->publish(cmd_vel); } }; From b7b2426ce7d5a31b64d69e223af447df89ef36a8 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Thu, 7 Dec 2023 21:07:02 -0700 Subject: [PATCH 23/38] Finish twist subscriber tests Signed-off-by: Ryan Friedman --- nav2_util/test/test_twist_subscriber.cpp | 81 ++++++++++++++++-------- 1 file changed, 54 insertions(+), 27 deletions(-) diff --git a/nav2_util/test/test_twist_subscriber.cpp b/nav2_util/test/test_twist_subscriber.cpp index 447733dea6b..56728873682 100644 --- a/nav2_util/test/test_twist_subscriber.cpp +++ b/nav2_util/test/test_twist_subscriber.cpp @@ -16,19 +16,54 @@ #include #include "nav2_util/twist_subscriber.hpp" +#include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -// #include "nav2_util/lifecycle_utils.hpp" + #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -// #include "rclcpp_lifecycle/lifecycle_publisher.hpp" -// using nav2_util::startup_lifecycle_nodes; -// using nav2_util::reset_lifecycle_nodes; -TEST(TwistPublisher, Unstamped) +TEST(TwistSubscriber, Unstamped) +{ + rclcpp::init(0, nullptr); + auto sub_node = std::make_shared("sub_node", ""); + sub_node->configure(); + sub_node->activate(); + + geometry_msgs::msg::TwistStamped sub_msg {}; + auto vel_subscriber = std::make_unique( + sub_node, "cmd_vel", 1, + [&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;}, + [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;} + ); + + auto pub_node = std::make_shared("pub_node", ""); + pub_node->configure(); + + geometry_msgs::msg::TwistStamped pub_msg {}; + pub_msg.twist.linear.x = 42.0; + + auto vel_pub = + pub_node->create_publisher("cmd_vel", 1); + + pub_node->activate(); + vel_pub->on_activate(); + + vel_pub->publish(pub_msg.twist); + rclcpp::spin_some(sub_node->get_node_base_interface()); + ASSERT_EQ(vel_pub->get_subscription_count(), 1); + EXPECT_EQ(pub_msg, sub_msg); + + pub_node->deactivate(); + sub_node->deactivate(); + rclcpp::shutdown(); +} + +TEST(TwistSubscriber, Stamped) { rclcpp::init(0, nullptr); - auto sub_node = std::make_shared("pub_node", ""); + auto sub_node = std::make_shared("sub_node", ""); + sub_node->declare_parameter("enable_stamped_cmd_vel", true); sub_node->configure(); sub_node->activate(); @@ -38,33 +73,25 @@ TEST(TwistPublisher, Unstamped) [&](const geometry_msgs::msg::Twist msg) {sub_msg.twist = msg;}, [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;} ); -// ASSERT_EQ(vel_publisher->get_subscription_count(), 0); -// EXPECT_FALSE(vel_publisher->is_activated()); -// pub_node->activate(); -// EXPECT_TRUE(vel_publisher->is_activated()); -// vel_publisher->on_activate(); -// auto pub_thread = std::thread([&]() {rclcpp::spin(pub_node->get_node_base_interface());}); -// auto sub_node = std::make_shared("sub_node", ""); -// pub_node->configure(); + auto pub_node = std::make_shared("pub_node", ""); + pub_node->configure(); + geometry_msgs::msg::TwistStamped pub_msg {}; + pub_msg.twist.linear.x = 42.0; -// geometry_msgs::msg::TwistStamped pub_msg {}; -// pub_msg.twist.linear.x = 42.0; + auto vel_pub = + pub_node->create_publisher("cmd_vel", 1); -// geometry_msgs::msg::Twist sub_msg {}; -// auto my_sub = sub_node->create_subscription( -// "cmd_vel", 10, -// [&](const geometry_msgs::msg::Twist msg) {sub_msg = msg;}); + pub_node->activate(); + vel_pub->on_activate(); -// vel_publisher->publish(pub_msg); -// rclcpp::spin_some(sub_node->get_node_base_interface()); + vel_pub->publish(pub_msg); + rclcpp::spin_some(sub_node->get_node_base_interface()); + ASSERT_EQ(vel_pub->get_subscription_count(), 1); + EXPECT_EQ(pub_msg, sub_msg); -// EXPECT_EQ(pub_msg.twist.linear.x, sub_msg.linear.x); -// ASSERT_EQ(vel_publisher->get_subscription_count(), 1); -// pub_node->deactivate(); + pub_node->deactivate(); sub_node->deactivate(); rclcpp::shutdown(); - // Have to join thread after rclcpp is shut down otherwise test hangs. -// pub_thread.join(); } From 56b61ac855cbd301a21cd1361ac97bd7bab31508 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 8 Dec 2023 18:43:43 -0700 Subject: [PATCH 24/38] Add other constructor and docs Signed-off-by: Ryan Friedman --- .../include/nav2_util/twist_subscriber.hpp | 123 +++++++++++++++--- 1 file changed, 108 insertions(+), 15 deletions(-) diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp index c64fb7c248f..aa05f3a2442 100644 --- a/nav2_util/include/nav2_util/twist_subscriber.hpp +++ b/nav2_util/include/nav2_util/twist_subscriber.hpp @@ -36,10 +36,35 @@ namespace nav2_util /** * @class nav2_util::TwistSubscriber - * @brief A simple wrapper on a Twist subscriber that receives either Twist or TwistStamped + * @brief A simple wrapper on a Twist subscriber that receives either + * geometry_msgs::msg::TwistStamped or geometry_msgs::msg::Twist * - * The default is to subscribe to Twist to preserve backwards compatibility, but it can be overridden - * using the "enable_stamped_cmd_vel" parameter to subscribe to TwistStamped. + * @note Usage: + * The default behavior is to subscribe to Twist, which preserves backwards compatibility + * with ROS distributions up to Iron. + * The behavior can be overridden using the "enable_stamped_cmd_vel" parameter. + * By setting that to "True", the TwistSubscriber class would subscribe to TwistStamped. + * + * @note Why use Twist Stamped over Twist? + * Twist has been used widely in many ROS applications, typically for body-frame velocity control, + * and between ROS nodes on the same computer. Many ROS interfaces are moving to using TwistStamped + * because it is more robust for stale data protection. This protection is especially important + * when sending velocity control over lossy communication links. + * An example application where this matters is a drone with a Linux computer running a ROS + * controller that sends Twist commands to an embedded autopilot. If the autopilot failed to + * recognize a highly latent connection, it could result in instability or a crash because of the + * decreased phase margin for control. + * TwistStamped also has a frame ID, allowing explicit control for multiple frames, rather than + * relying on an assumption of body-frame control or having to create a different topic. + * Adding a header is low-cost for most ROS applications; the header can be set to an empty string + * if bandwidth is of concern. + * + * @note Implementation Design Notes: + * Compared to the naive approach of setting up one subscriber for each message type, + * only one subscriber is created at a time; the other is nullptr. + * This reduces RAM usage and ROS discovery traffic. + * This approach allows NAV2 libraries to be flexible in which Twist message they support, + * while maintaining a stable API in a ROS distribution. * */ @@ -47,10 +72,12 @@ class TwistSubscriber { public: /** - * @brief A constructor - * @param nh The node - * @param topic publisher topic name - * @param qos publisher quality of service + * @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 */ templateget_parameter("enable_stamped_cmd_vel", is_stamped_); + nav2_util::declare_parameter_if_not_declared( + node, stamped_param_name_, + rclcpp::ParameterValue{is_stamped_}); + node->get_parameter(stamped_param_name_, is_stamped_); if (is_stamped_) { twist_stamped_sub_ = node->create_subscription( topic, @@ -81,12 +107,79 @@ class TwistSubscriber } } + /** + * @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 + ) + { + nav2_util::declare_parameter_if_not_declared( + node, stamped_param_name_, + rclcpp::ParameterValue{is_stamped_}); + node->get_parameter(stamped_param_name_, is_stamped_); + if (is_stamped_) { + twist_stamped_sub_ = node->create_subscription( + topic, + qos, + std::forward(TwistStampedCallback)); + } else { + throw std::invalid_argument("The parameter '%s' must be true when using this constructor!", stamped_param_name_); + } + } + + // /** + // * @brief A constructor that only supports Twist + // * @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 + // * @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, + // TwistCallbackT && TwistCallback + // ) + // { + // nav2_util::declare_parameter_if_not_declared( + // node, stamped_param_name_, + // rclcpp::ParameterValue{is_stamped_}); + // node->get_parameter(stamped_param_name_, is_stamped_); + // if (is_stamped_) { + // throw std::invalid_argument("The parameter '%s' must be false when using this constructor!", stamped_param_name_); + // } else { + // twist_sub_ = node->create_subscription( + // topic, + // qos, + // std::forward(TwistCallback)); + // } + // } + protected: - bool is_stamped_; - rclcpp::Subscription::SharedPtr twist_sub_; - rclcpp::Subscription::SharedPtr twist_stamped_sub_; + //! @brief The user-configured value for ROS parameter enable_stamped_cmd_vel + bool is_stamped_{false}; + //! @brief The subscription when using Twist + rclcpp::Subscription::SharedPtr twist_sub_ {nullptr}; + //! @brief The subscription when using TwistStamped + rclcpp::Subscription::SharedPtr twist_stamped_sub_ {nullptr}; + //! @brief The name of the ROS parameter that controls whether twist data is stamped + const std::string stamped_param_name_ {"enable_stamped_cmd_vel"}; }; + + } // namespace nav2_util #endif // NAV2_UTIL__TWIST_SUBSCRIBER_HPP_ From 1e69dde4295110e99510259492e8f2323688c647 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 22 Dec 2023 19:03:08 -0700 Subject: [PATCH 25/38] Fix linter issues Signed-off-by: Ryan Friedman --- .../include/nav2_util/twist_subscriber.hpp | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp index aa05f3a2442..0fe18889310 100644 --- a/nav2_util/include/nav2_util/twist_subscriber.hpp +++ b/nav2_util/include/nav2_util/twist_subscriber.hpp @@ -36,31 +36,31 @@ namespace nav2_util /** * @class nav2_util::TwistSubscriber - * @brief A simple wrapper on a Twist subscriber that receives either + * @brief A simple wrapper on a Twist subscriber that receives either * geometry_msgs::msg::TwistStamped or geometry_msgs::msg::Twist * * @note Usage: * The default behavior is to subscribe to Twist, which preserves backwards compatibility - * with ROS distributions up to Iron. - * The behavior can be overridden using the "enable_stamped_cmd_vel" parameter. + * with ROS distributions up to Iron. + * The behavior can be overridden using the "enable_stamped_cmd_vel" parameter. * By setting that to "True", the TwistSubscriber class would subscribe to TwistStamped. - * + * * @note Why use Twist Stamped over Twist? - * Twist has been used widely in many ROS applications, typically for body-frame velocity control, - * and between ROS nodes on the same computer. Many ROS interfaces are moving to using TwistStamped - * because it is more robust for stale data protection. This protection is especially important - * when sending velocity control over lossy communication links. - * An example application where this matters is a drone with a Linux computer running a ROS - * controller that sends Twist commands to an embedded autopilot. If the autopilot failed to - * recognize a highly latent connection, it could result in instability or a crash because of the + * Twist has been used widely in many ROS applications, typically for body-frame velocity control, + * and between ROS nodes on the same computer. Many ROS interfaces are moving to using TwistStamped + * because it is more robust for stale data protection. This protection is especially important + * when sending velocity control over lossy communication links. + * An example application where this matters is a drone with a Linux computer running a ROS + * controller that sends Twist commands to an embedded autopilot. If the autopilot failed to + * recognize a highly latent connection, it could result in instability or a crash because of the * decreased phase margin for control. - * TwistStamped also has a frame ID, allowing explicit control for multiple frames, rather than + * TwistStamped also has a frame ID, allowing explicit control for multiple frames, rather than * relying on an assumption of body-frame control or having to create a different topic. - * Adding a header is low-cost for most ROS applications; the header can be set to an empty string + * Adding a header is low-cost for most ROS applications; the header can be set to an empty string * if bandwidth is of concern. - * + * * @note Implementation Design Notes: - * Compared to the naive approach of setting up one subscriber for each message type, + * Compared to the naive approach of setting up one subscriber for each message type, * only one subscriber is created at a time; the other is nullptr. * This reduces RAM usage and ROS discovery traffic. * This approach allows NAV2 libraries to be flexible in which Twist message they support, @@ -133,7 +133,9 @@ class TwistSubscriber qos, std::forward(TwistStampedCallback)); } else { - throw std::invalid_argument("The parameter '%s' must be true when using this constructor!", stamped_param_name_); + throw std::invalid_argument( + "The parameter '%s' must be true when using this constructor!", + stamped_param_name_); } } @@ -179,7 +181,6 @@ class TwistSubscriber }; - } // namespace nav2_util #endif // NAV2_UTIL__TWIST_SUBSCRIBER_HPP_ From cc78c018de046353d2b55d4bfc137f66c1023f25 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 22 Dec 2023 19:14:29 -0700 Subject: [PATCH 26/38] Manually fix paren alignment Signed-off-by: Ryan Friedman --- nav2_util/include/nav2_util/twist_publisher.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_util/include/nav2_util/twist_publisher.hpp b/nav2_util/include/nav2_util/twist_publisher.hpp index eaca6d7edcc..c8d77a85e21 100644 --- a/nav2_util/include/nav2_util/twist_publisher.hpp +++ b/nav2_util/include/nav2_util/twist_publisher.hpp @@ -54,8 +54,7 @@ class TwistPublisher explicit TwistPublisher( nav2_util::LifecycleNode::SharedPtr node, const std::string & topic, - const rclcpp::QoS & qos - ) + const rclcpp::QoS & qos) : topic_(topic) { using nav2_util::declare_parameter_if_not_declared; From 576c918351e58f4b5766d725a077d02f103d8f81 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 22 Dec 2023 19:19:38 -0700 Subject: [PATCH 27/38] Remove GSoC reference Signed-off-by: Ryan Friedman --- nav2_util/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_util/README.md b/nav2_util/README.md index 18f5dd51512..68637d1fe8b 100644 --- a/nav2_util/README.md +++ b/nav2_util/README.md @@ -18,7 +18,6 @@ The long-term aim is for these utilities to find more permanent homes in other p The Twist Publisher and Twist Subscriber are utility classes to assist NAV2 transition from [Twist](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/Twist.msg) to [TwistStamped](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/TwistStamped.msg). -This utility is in support of the following [GSoC Project](https://navigation.ros.org/2021summerOfCode/projects/twist_n_config.html#convert-twist-to-twiststamped-in-ecosystem-and-run-time-configuration). Details on the migration are found in [#1594](https://github.com/ros-planning/navigation2/issues/1594). Certain applications of NAV2, such as in ROS Aerial mandate the usage of `TwistStamped`, while many other applications still use `Twist`. From bd513a37d33dc0225fbee592fd4bd52a370f5e8f Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 22 Dec 2023 19:23:00 -0700 Subject: [PATCH 28/38] Document twist bool param in README Signed-off-by: Ryan Friedman --- nav2_velocity_smoother/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md index d0a7288e8ad..a9554997e28 100644 --- a/nav2_velocity_smoother/README.md +++ b/nav2_velocity_smoother/README.md @@ -59,7 +59,7 @@ velocity_smoother: max_decel: [-2.5, 0.0, -3.2] # Maximum deceleration, ordered [Ax, Ay, Aw] odom_topic: "odom" # Topic of odometry to use for estimating current velocities odom_duration: 0.1 # Period of time (s) to sample odometry information in for velocity estimation - enable_stamped_cmd_vel: false + enable_stamped_cmd_vel: false # Whether to stamp the velocity. True uses TwistStamped. False uses Twist ``` ## Topics From 69b02e828db809ce8bbdeaa955c9f63dc2590afb Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 22 Dec 2023 19:50:58 -0700 Subject: [PATCH 29/38] Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman --- .../collision_monitor_node.hpp | 5 ++-- .../src/collision_monitor_node.cpp | 30 ++++++++++--------- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index db3171b3cb9..757c6f1d777 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -29,6 +29,7 @@ #include "tf2_ros/transform_listener.h" #include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/twist_publisher.hpp" #include "nav2_util/twist_subscriber.hpp" #include "nav2_msgs/msg/collision_monitor_state.hpp" @@ -97,8 +98,8 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @brief Callback for input cmd_vel * @param msg Input cmd_vel message */ - void cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg); void cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); + void cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::ConstSharedPtr msg); /** * @brief Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds, * quit to publish 0-velocity. @@ -209,7 +210,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode /// @brief Input cmd_vel subscriber std::shared_ptr cmd_vel_in_sub_; /// @brief Output cmd_vel publisher - rclcpp_lifecycle::LifecyclePublisher::SharedPtr cmd_vel_out_pub_; + std::unique_ptr cmd_vel_out_pub_; /// @brief CollisionMonitor state publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 2aaf3d66a1a..cd108461192 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -67,11 +67,11 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) shared_from_this(), cmd_vel_in_topic, 1, - std::bind(&CollisionMonitor::cmdVelInCallback, this, std::placeholders::_1), + std::bind(&CollisionMonitor::cmdVelInCallbackUnstamped, this, std::placeholders::_1), std::bind(&CollisionMonitor::cmdVelInCallbackStamped, this, std::placeholders::_1)); - cmd_vel_out_pub_ = this->create_publisher( - cmd_vel_out_topic, 1); + auto node = shared_from_this(); + cmd_vel_out_pub_ = std::make_unique(node, cmd_vel_out_topic, 1); if (!state_topic.empty()) { state_pub_ = this->create_publisher( @@ -81,7 +81,6 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) collision_points_marker_pub_ = this->create_publisher( "~/collision_points_marker", 1); - auto node = shared_from_this(); nav2_util::declare_parameter_if_not_declared( node, "use_realtime_priority", rclcpp::ParameterValue(false)); bool use_realtime_priority = false; @@ -184,7 +183,7 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg) +void CollisionMonitor::cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { // If message contains NaN or Inf, ignore if (!nav2_util::validateTwist(*msg)) { @@ -192,12 +191,14 @@ void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPt return; } - process({msg->linear.x, msg->linear.y, msg->angular.z}); + process({msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z}); } -void CollisionMonitor::cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +void CollisionMonitor::cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::ConstSharedPtr msg) { - cmdVelInCallback(std::make_shared(msg->twist)); + geometry_msgs::msg::TwistStamped twist_stamped; + twist_stamped.twist = *msg; + cmdVelInCallbackStamped(std::make_shared(twist_stamped)); } void CollisionMonitor::publishVelocity(const Action & robot_action) @@ -213,14 +214,15 @@ void CollisionMonitor::publishVelocity(const Action & robot_action) } } - std::unique_ptr cmd_vel_out_msg = - std::make_unique(); - cmd_vel_out_msg->linear.x = robot_action.req_vel.x; - cmd_vel_out_msg->linear.y = robot_action.req_vel.y; - cmd_vel_out_msg->angular.z = robot_action.req_vel.tw; + auto cmd_vel_out_msg = geometry_msgs::msg::TwistStamped(); + cmd_vel_out_msg.header.stamp = this->now(); + + cmd_vel_out_msg.twist.linear.x = robot_action.req_vel.x; + cmd_vel_out_msg.twist.linear.y = robot_action.req_vel.y; + cmd_vel_out_msg.twist.angular.z = robot_action.req_vel.tw; // linear.z, angular.x and angular.y will remain 0.0 - cmd_vel_out_pub_->publish(std::move(cmd_vel_out_msg)); + cmd_vel_out_pub_->publish(cmd_vel_out_msg); } bool CollisionMonitor::getParameters( From a4c99cca975adaef91cfaf5952c8a2999c5fc140 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 22 Dec 2023 22:24:19 -0700 Subject: [PATCH 30/38] Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman --- .../velocity_smoother.hpp | 10 +-- .../src/velocity_smoother.cpp | 80 +++++++++++-------- .../test/test_velocity_smoother.cpp | 4 +- 3 files changed, 55 insertions(+), 39 deletions(-) diff --git a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp index 2744a8652fc..cf7e8a24c90 100644 --- a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp +++ b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp @@ -26,6 +26,7 @@ #include "nav2_util/node_utils.hpp" #include "nav2_util/odometry_utils.hpp" #include "nav2_util/robot_utils.hpp" +#include "nav2_util/twist_publisher.hpp" #include "nav2_util/twist_subscriber.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -116,7 +117,7 @@ class VelocitySmoother : public nav2_util::LifecycleNode * @brief Callback for incoming velocity commands * @param msg Twist message */ - void inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + void inputCommandCallback(const geometry_msgs::msg::Twist::ConstSharedPtr msg); void inputCommandStampedCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); /** @@ -133,14 +134,13 @@ class VelocitySmoother : public nav2_util::LifecycleNode // Network interfaces std::unique_ptr odom_smoother_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr - smoothed_cmd_pub_; + std::unique_ptr smoothed_cmd_pub_; std::shared_ptr cmd_sub_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Clock::SharedPtr clock_; - geometry_msgs::msg::Twist last_cmd_; - geometry_msgs::msg::Twist::SharedPtr command_; + geometry_msgs::msg::TwistStamped last_cmd_; + geometry_msgs::msg::TwistStamped::SharedPtr command_; // Parameters double smoothing_frequency_; diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index a00edf6245a..4e4c0c41af2 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -128,9 +128,9 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) } // Setup inputs / outputs - smoothed_cmd_pub_ = create_publisher("cmd_vel_smoothed", 1); + smoothed_cmd_pub_ = std::make_unique(node, "cmd_vel_smoothed", 1); cmd_sub_ = std::make_shared( - shared_from_this(), + node, "cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1), std::bind(&VelocitySmoother::inputCommandStampedCallback, this, std::placeholders::_1) @@ -202,22 +202,30 @@ VelocitySmoother::on_shutdown(const rclcpp_lifecycle::State &) return nav2_util::CallbackReturn::SUCCESS; } -void VelocitySmoother::inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg) +void VelocitySmoother::inputCommandStampedCallback( + const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { // If message contains NaN or Inf, ignore - if (!nav2_util::validateTwist(*msg)) { + if (!nav2_util::validateTwist(msg->twist)) { RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!"); return; } - command_ = msg; - last_command_time_ = now(); + command_ = std::make_shared(*msg); + if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0) { + last_command_time_ = now(); + } else { + last_command_time_ = msg->header.stamp; + } + } -void VelocitySmoother::inputCommandStampedCallback( - geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +void VelocitySmoother::inputCommandCallback( + geometry_msgs::msg::Twist::ConstSharedPtr msg) { - inputCommandCallback(std::make_shared(msg->twist)); + auto twist_stamped = geometry_msgs::msg::TwistStamped(); + twist_stamped.twist = *msg; + inputCommandStampedCallback(std::make_shared(twist_stamped)); } double VelocitySmoother::findEtaConstraint( @@ -281,31 +289,37 @@ void VelocitySmoother::smootherTimer() return; } - auto cmd_vel = std::make_unique(); + auto cmd_vel = std::make_unique(); // Check for velocity timeout. If nothing received, publish zeros to apply deceleration if (now() - last_command_time_ > velocity_timeout_) { - if (last_cmd_ == geometry_msgs::msg::Twist() || stopped_) { + if (last_cmd_ == geometry_msgs::msg::TwistStamped() || stopped_) { stopped_ = true; return; } - *command_ = geometry_msgs::msg::Twist(); + *command_ = geometry_msgs::msg::TwistStamped(); } stopped_ = false; // Get current velocity based on feedback type - geometry_msgs::msg::Twist current_; + geometry_msgs::msg::TwistStamped current_; if (open_loop_) { current_ = last_cmd_; } else { - current_ = odom_smoother_->getTwist(); + current_ = odom_smoother_->getTwistStamped(); } // Apply absolute velocity restrictions to the command - command_->linear.x = std::clamp(command_->linear.x, min_velocities_[0], max_velocities_[0]); - command_->linear.y = std::clamp(command_->linear.y, min_velocities_[1], max_velocities_[1]); - command_->angular.z = std::clamp(command_->angular.z, min_velocities_[2], max_velocities_[2]); + command_->twist.linear.x = std::clamp( + command_->twist.linear.x, min_velocities_[0], + max_velocities_[0]); + command_->twist.linear.y = std::clamp( + command_->twist.linear.y, min_velocities_[1], + max_velocities_[1]); + command_->twist.angular.z = std::clamp( + command_->twist.angular.z, min_velocities_[2], + max_velocities_[2]); // Find if any component is not within the acceleration constraints. If so, store the most // significant scale factor to apply to the vector , eta, to reduce all axes @@ -317,39 +331,41 @@ void VelocitySmoother::smootherTimer() double curr_eta = -1.0; curr_eta = findEtaConstraint( - current_.linear.x, command_->linear.x, max_accels_[0], max_decels_[0]); + current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0]); if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { eta = curr_eta; } curr_eta = findEtaConstraint( - current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1]); + current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1]); if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { eta = curr_eta; } curr_eta = findEtaConstraint( - current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2]); + current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2]); if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { eta = curr_eta; } } - cmd_vel->linear.x = applyConstraints( - current_.linear.x, command_->linear.x, max_accels_[0], max_decels_[0], eta); - cmd_vel->linear.y = applyConstraints( - current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1], eta); - cmd_vel->angular.z = applyConstraints( - current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2], eta); + cmd_vel->twist.linear.x = applyConstraints( + current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0], eta); + cmd_vel->twist.linear.y = applyConstraints( + current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1], eta); + cmd_vel->twist.angular.z = applyConstraints( + current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2], eta); last_cmd_ = *cmd_vel; // Apply deadband restrictions & publish - cmd_vel->linear.x = fabs(cmd_vel->linear.x) < deadband_velocities_[0] ? 0.0 : cmd_vel->linear.x; - cmd_vel->linear.y = fabs(cmd_vel->linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->linear.y; - cmd_vel->angular.z = fabs(cmd_vel->angular.z) < - deadband_velocities_[2] ? 0.0 : cmd_vel->angular.z; - - smoothed_cmd_pub_->publish(std::move(cmd_vel)); + cmd_vel->twist.linear.x = fabs(cmd_vel->twist.linear.x) < + deadband_velocities_[0] ? 0.0 : cmd_vel->twist.linear.x; + cmd_vel->twist.linear.y = fabs(cmd_vel->twist.linear.y) < + deadband_velocities_[1] ? 0.0 : cmd_vel->twist.linear.y; + cmd_vel->twist.angular.z = fabs(cmd_vel->twist.angular.z) < + deadband_velocities_[2] ? 0.0 : cmd_vel->twist.angular.z; + + smoothed_cmd_pub_->publish(*cmd_vel); } rcl_interfaces::msg::SetParametersResult diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index 86505069199..25754d0f355 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -48,7 +48,7 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother bool isOdomSmoother() {return odom_smoother_ ? true : false;} bool hasCommandMsg() {return last_command_time_.nanoseconds() != 0;} - geometry_msgs::msg::Twist::SharedPtr lastCommandMsg() {return command_;} + geometry_msgs::msg::TwistStamped::SharedPtr lastCommandMsg() {return command_;} void sendCommandMsg(geometry_msgs::msg::Twist::SharedPtr msg) {inputCommandCallback(msg);} }; @@ -573,7 +573,7 @@ TEST(VelocitySmootherTest, testCommandCallback) rclcpp::spin_some(smoother->get_node_base_interface()); EXPECT_TRUE(smoother->hasCommandMsg()); - EXPECT_EQ(smoother->lastCommandMsg()->linear.x, 100.0); + EXPECT_EQ(smoother->lastCommandMsg()->twist.linear.x, 100.0); } TEST(VelocitySmootherTest, testClosedLoopSub) From 60a3b6c9441693b87575dbb777ee4693e06dbcf1 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 22 Dec 2023 22:26:37 -0700 Subject: [PATCH 31/38] Remove nav2_util usage instructions Signed-off-by: Ryan Friedman --- nav2_util/README.md | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/nav2_util/README.md b/nav2_util/README.md index 68637d1fe8b..adf71e61c2e 100644 --- a/nav2_util/README.md +++ b/nav2_util/README.md @@ -29,13 +29,3 @@ The utility has the following effect: Every node in `nav2` that subscribes or publishes velocity commands with `Twist` now supports this optional behavior. The behavior up through ROS 2 Iron is preserved - using `Twist`. In a future ROS 2 version, when enough of the ROS ecosystem has moved to `TwistStamped`, the default may change. - -### Usage - -See [test/test_twist_publisher.cpp](test/test_twist_publisher.cpp) or [test/test_twist_subscriber.cpp](test/test_twist_subscriber.cpp) for examples. - -To use `Twist`, you can rely on the default value of `enable_stamped_cmd_vel`, or set it to `False` in your parameter file. -To use `TwistStamped`, set `enable_stamped_cmd_vel` to `True`. -Each node in `nav2` that uses a `TwistPublisher` or `TwistSubscriber` has it documented in the README. - -The parameter is read-only, therefore it must be set at launch time. Do not try changing the value at runtime. From 9cfb74fca9537c98275a90c86ead490b1888b8d4 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 22 Dec 2023 23:44:13 -0700 Subject: [PATCH 32/38] Remove unused Twist only subscriber Signed-off-by: Ryan Friedman --- .../include/nav2_util/twist_subscriber.hpp | 30 ------------------- 1 file changed, 30 deletions(-) diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp index 0fe18889310..9315c4f08f7 100644 --- a/nav2_util/include/nav2_util/twist_subscriber.hpp +++ b/nav2_util/include/nav2_util/twist_subscriber.hpp @@ -139,36 +139,6 @@ class TwistSubscriber } } - // /** - // * @brief A constructor that only supports Twist - // * @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 - // * @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, - // TwistCallbackT && TwistCallback - // ) - // { - // nav2_util::declare_parameter_if_not_declared( - // node, stamped_param_name_, - // rclcpp::ParameterValue{is_stamped_}); - // node->get_parameter(stamped_param_name_, is_stamped_); - // if (is_stamped_) { - // throw std::invalid_argument("The parameter '%s' must be false when using this constructor!", stamped_param_name_); - // } else { - // twist_sub_ = node->create_subscription( - // topic, - // qos, - // std::forward(TwistCallback)); - // } - // } - protected: //! @brief The user-configured value for ROS parameter enable_stamped_cmd_vel bool is_stamped_{false}; From 37dcfbee40d5fe8ec3abe41e5d9d4b886076643c Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Sat, 23 Dec 2023 11:00:12 -0700 Subject: [PATCH 33/38] More linter fixes Signed-off-by: Ryan Friedman --- nav2_velocity_smoother/src/velocity_smoother.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 4e4c0c41af2..abf95bfe539 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -217,7 +217,6 @@ void VelocitySmoother::inputCommandStampedCallback( } else { last_command_time_ = msg->header.stamp; } - } void VelocitySmoother::inputCommandCallback( From 127c17f713305641376cca3b929d46ee8db12420 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Tue, 26 Dec 2023 21:56:42 -0700 Subject: [PATCH 34/38] Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman --- .../nav2_behaviors/plugins/drive_on_heading.hpp | 12 ++++++------ .../include/nav2_behaviors/timed_behavior.hpp | 16 ++++++++-------- nav2_behaviors/plugins/spin.cpp | 8 ++++---- .../src/collision_monitor_node.cpp | 12 ++++++------ nav2_controller/src/controller_server.cpp | 5 +++-- 5 files changed, 27 insertions(+), 26 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 5620391f782..5f1fb973628 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -126,23 +126,23 @@ class DriveOnHeading : public TimedBehavior return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE}; } - auto cmd_vel = geometry_msgs::msg::TwistStamped(); - cmd_vel.twist.linear.y = 0.0; - cmd_vel.twist.angular.z = 0.0; - cmd_vel.twist.linear.x = command_speed_; + auto cmd_vel = std::make_unique(); + cmd_vel->twist.linear.y = 0.0; + cmd_vel->twist.angular.z = 0.0; + cmd_vel->twist.linear.x = command_speed_; geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(distance, cmd_vel.twist, pose2d)) { + if (!isCollisionFree(distance, cmd_vel->twist, pose2d)) { this->stopRobot(); RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading"); return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD}; } - this->vel_pub_->publish(cmd_vel); + this->vel_pub_->publish(*cmd_vel); return ResultStatus{Status::RUNNING, ActionT::Result::NONE}; } diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 478aeda41eb..ea516bd15a0 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -290,14 +290,14 @@ class TimedBehavior : public nav2_core::Behavior // Stop the robot with a commanded velocity void stopRobot() { - auto cmd_vel = geometry_msgs::msg::TwistStamped(); - cmd_vel.header.frame_id = "base_link"; - cmd_vel.header.stamp = rclcpp::Time(); - cmd_vel.twist.linear.x = 0.0; - cmd_vel.twist.linear.y = 0.0; - cmd_vel.twist.angular.z = 0.0; - - vel_pub_->publish(cmd_vel); + auto cmd_vel = std::make_unique(); + cmd_vel->header.frame_id = "base_link"; + cmd_vel->header.stamp = rclcpp::Time(); + cmd_vel->twist.linear.x = 0.0; + cmd_vel->twist.linear.y = 0.0; + cmd_vel->twist.angular.z = 0.0; + + vel_pub_->publish(*cmd_vel); } }; diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index 2f0b3bc7dba..410dd0ff03d 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -138,21 +138,21 @@ ResultStatus Spin::onCycleUpdate() double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw); vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); - auto cmd_vel = geometry_msgs::msg::TwistStamped(); - cmd_vel.twist.angular.z = copysign(vel, cmd_yaw_); + auto cmd_vel = std::make_unique(); + cmd_vel->twist.angular.z = copysign(vel, cmd_yaw_); geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(relative_yaw_, cmd_vel.twist, pose2d)) { + if (!isCollisionFree(relative_yaw_, cmd_vel->twist, pose2d)) { stopRobot(); RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin"); return ResultStatus{Status::FAILED, SpinActionResult::COLLISION_AHEAD}; } - vel_pub_->publish(cmd_vel); + vel_pub_->publish(*cmd_vel); return ResultStatus{Status::RUNNING, SpinActionResult::NONE}; } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index cd108461192..0ab209eabf8 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -214,15 +214,15 @@ void CollisionMonitor::publishVelocity(const Action & robot_action) } } - auto cmd_vel_out_msg = geometry_msgs::msg::TwistStamped(); - cmd_vel_out_msg.header.stamp = this->now(); + auto cmd_vel_out_msg = std::make_unique(); + cmd_vel_out_msg->header.stamp = this->now(); - cmd_vel_out_msg.twist.linear.x = robot_action.req_vel.x; - cmd_vel_out_msg.twist.linear.y = robot_action.req_vel.y; - cmd_vel_out_msg.twist.angular.z = robot_action.req_vel.tw; + cmd_vel_out_msg->twist.linear.x = robot_action.req_vel.x; + cmd_vel_out_msg->twist.linear.y = robot_action.req_vel.y; + cmd_vel_out_msg->twist.angular.z = robot_action.req_vel.tw; // linear.z, angular.x and angular.y will remain 0.0 - cmd_vel_out_pub_->publish(cmd_vel_out_msg); + cmd_vel_out_pub_->publish(*cmd_vel_out_msg); } bool CollisionMonitor::getParameters( diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 31dcb960ad4..c91481749cf 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -695,14 +695,15 @@ void ControllerServer::updateGlobalPath() void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity) { + auto cmd_vel = std::make_unique(velocity); if (vel_publisher_->is_activated() && vel_publisher_->get_subscription_count() > 0) { - vel_publisher_->publish(velocity); + vel_publisher_->publish(*cmd_vel); } } void ControllerServer::publishZeroVelocity() { - geometry_msgs::msg::TwistStamped velocity; + auto velocity = geometry_msgs::msg::TwistStamped(); velocity.twist.angular.x = 0; velocity.twist.angular.y = 0; velocity.twist.angular.z = 0; From 0e166ebebc3636104f1a17591d0be367aaf24b61 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 3 Jan 2024 16:56:55 -0800 Subject: [PATCH 35/38] Completing twist stamped migration --- .../plugins/drive_on_heading.hpp | 2 +- .../include/nav2_behaviors/timed_behavior.hpp | 2 +- nav2_behaviors/plugins/assisted_teleop.cpp | 14 ++++----- nav2_behaviors/plugins/spin.cpp | 2 +- .../collision_monitor_node.hpp | 6 ++-- .../src/collision_monitor_node.cpp | 15 +++++----- nav2_controller/src/controller_server.cpp | 4 +-- .../include/nav2_util/twist_publisher.hpp | 7 +++-- .../include/nav2_util/twist_subscriber.hpp | 17 +++++------ nav2_util/test/test_twist_publisher.cpp | 24 ++++++++------- .../velocity_smoother.hpp | 6 ++-- .../src/velocity_smoother.cpp | 30 +++++++++---------- 12 files changed, 64 insertions(+), 65 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 5f1fb973628..83aa3eede36 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -142,7 +142,7 @@ class DriveOnHeading : public TimedBehavior return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD}; } - this->vel_pub_->publish(*cmd_vel); + this->vel_pub_->publish(std::move(cmd_vel)); return ResultStatus{Status::RUNNING, ActionT::Result::NONE}; } diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index ea516bd15a0..acdab77d74d 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -297,7 +297,7 @@ class TimedBehavior : public nav2_core::Behavior cmd_vel->twist.linear.y = 0.0; cmd_vel->twist.angular.z = 0.0; - vel_pub_->publish(*cmd_vel); + vel_pub_->publish(std::move(cmd_vel)); } }; diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index 31194cd7544..82edd1b8223 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -119,7 +119,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() projected_pose.y = current_pose.pose.position.y; projected_pose.theta = tf2::getYaw(current_pose.pose.orientation); - geometry_msgs::msg::TwistStamped scaled_twist = teleop_twist_; + auto scaled_twist = std::make_unique(teleop_twist_); for (double time = simulation_time_step_; time < projection_time_; time += simulation_time_step_) { @@ -132,9 +132,9 @@ ResultStatus AssistedTeleop::onCycleUpdate() *clock_, 1000, behavior_name_.c_str() << " collided on first time step, setting velocity to zero"); - scaled_twist.twist.linear.x = 0.0f; - scaled_twist.twist.linear.y = 0.0f; - scaled_twist.twist.angular.z = 0.0f; + scaled_twist->twist.linear.x = 0.0f; + scaled_twist->twist.linear.y = 0.0f; + scaled_twist->twist.angular.z = 0.0f; break; } else { RCLCPP_DEBUG_STREAM_THROTTLE( @@ -143,9 +143,9 @@ ResultStatus AssistedTeleop::onCycleUpdate() 1000, behavior_name_.c_str() << " collision approaching in " << time << " seconds"); double scale_factor = time / projection_time_; - scaled_twist.twist.linear.x *= scale_factor; - scaled_twist.twist.linear.y *= scale_factor; - scaled_twist.twist.angular.z *= scale_factor; + scaled_twist->twist.linear.x *= scale_factor; + scaled_twist->twist.linear.y *= scale_factor; + scaled_twist->twist.angular.z *= scale_factor; break; } } diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index 410dd0ff03d..8c7bd413d18 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -152,7 +152,7 @@ ResultStatus Spin::onCycleUpdate() return ResultStatus{Status::FAILED, SpinActionResult::COLLISION_AHEAD}; } - vel_pub_->publish(*cmd_vel); + vel_pub_->publish(std::move(cmd_vel)); return ResultStatus{Status::RUNNING, SpinActionResult::NONE}; } diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 757c6f1d777..fdaac9093c1 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -98,8 +98,8 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @brief Callback for input cmd_vel * @param msg Input cmd_vel message */ - void cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); - void cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::ConstSharedPtr msg); + void cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::SharedPtr msg); + void cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::SharedPtr msg); /** * @brief Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds, * quit to publish 0-velocity. @@ -208,7 +208,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode // Input/output speed controls /// @brief Input cmd_vel subscriber - std::shared_ptr cmd_vel_in_sub_; + std::unique_ptr cmd_vel_in_sub_; /// @brief Output cmd_vel publisher std::unique_ptr cmd_vel_out_pub_; diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 0ab209eabf8..e6575068889 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -63,7 +63,7 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::FAILURE; } - cmd_vel_in_sub_ = std::make_shared( + cmd_vel_in_sub_ = std::make_unique( shared_from_this(), cmd_vel_in_topic, 1, @@ -183,7 +183,7 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -void CollisionMonitor::cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +void CollisionMonitor::cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::SharedPtr msg) { // If message contains NaN or Inf, ignore if (!nav2_util::validateTwist(*msg)) { @@ -194,11 +194,11 @@ void CollisionMonitor::cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped: process({msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z}); } -void CollisionMonitor::cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::ConstSharedPtr msg) +void CollisionMonitor::cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::SharedPtr msg) { - geometry_msgs::msg::TwistStamped twist_stamped; - twist_stamped.twist = *msg; - cmdVelInCallbackStamped(std::make_shared(twist_stamped)); + auto twist_stamped = std::make_shared(); + twist_stamped->twist = *msg; + cmdVelInCallbackStamped(twist_stamped); } void CollisionMonitor::publishVelocity(const Action & robot_action) @@ -216,13 +216,12 @@ void CollisionMonitor::publishVelocity(const Action & robot_action) auto cmd_vel_out_msg = std::make_unique(); cmd_vel_out_msg->header.stamp = this->now(); - cmd_vel_out_msg->twist.linear.x = robot_action.req_vel.x; cmd_vel_out_msg->twist.linear.y = robot_action.req_vel.y; cmd_vel_out_msg->twist.angular.z = robot_action.req_vel.tw; // linear.z, angular.x and angular.y will remain 0.0 - cmd_vel_out_pub_->publish(*cmd_vel_out_msg); + cmd_vel_out_pub_->publish(std::move(cmd_vel_out_msg)); } bool CollisionMonitor::getParameters( diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index c91481749cf..fa25e0b1379 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -697,13 +697,13 @@ void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & { auto cmd_vel = std::make_unique(velocity); if (vel_publisher_->is_activated() && vel_publisher_->get_subscription_count() > 0) { - vel_publisher_->publish(*cmd_vel); + vel_publisher_->publish(std::move(cmd_vel)); } } void ControllerServer::publishZeroVelocity() { - auto velocity = geometry_msgs::msg::TwistStamped(); + geometry_msgs::msg::TwistStamped velocity; velocity.twist.angular.x = 0; velocity.twist.angular.y = 0; velocity.twist.angular.z = 0; diff --git a/nav2_util/include/nav2_util/twist_publisher.hpp b/nav2_util/include/nav2_util/twist_publisher.hpp index c8d77a85e21..703dd567b2e 100644 --- a/nav2_util/include/nav2_util/twist_publisher.hpp +++ b/nav2_util/include/nav2_util/twist_publisher.hpp @@ -19,6 +19,7 @@ #include #include +#include #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -100,12 +101,12 @@ class TwistPublisher } } - void publish(const geometry_msgs::msg::TwistStamped & velocity) + void publish(std::unique_ptr velocity) { if (is_stamped_) { - twist_stamped_pub_->publish(velocity); + twist_stamped_pub_->publish(std::move(velocity)); } else { - twist_pub_->publish(velocity.twist); + twist_pub_->publish(std::move(std::make_unique(velocity->twist))); } } diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp index 9315c4f08f7..01f9ca25fed 100644 --- a/nav2_util/include/nav2_util/twist_subscriber.hpp +++ b/nav2_util/include/nav2_util/twist_subscriber.hpp @@ -91,9 +91,9 @@ class TwistSubscriber ) { nav2_util::declare_parameter_if_not_declared( - node, stamped_param_name_, - rclcpp::ParameterValue{is_stamped_}); - node->get_parameter(stamped_param_name_, is_stamped_); + node, "enable_stamped_cmd_vel", + rclcpp::ParameterValue(false)); + node->get_parameter("enable_stamped_cmd_vel", is_stamped_); if (is_stamped_) { twist_stamped_sub_ = node->create_subscription( topic, @@ -124,9 +124,9 @@ class TwistSubscriber ) { nav2_util::declare_parameter_if_not_declared( - node, stamped_param_name_, - rclcpp::ParameterValue{is_stamped_}); - node->get_parameter(stamped_param_name_, is_stamped_); + node, "enable_stamped_cmd_vel", + rclcpp::ParameterValue(false)); + node->get_parameter("enable_stamped_cmd_vel", is_stamped_); if (is_stamped_) { twist_stamped_sub_ = node->create_subscription( topic, @@ -134,8 +134,7 @@ class TwistSubscriber std::forward(TwistStampedCallback)); } else { throw std::invalid_argument( - "The parameter '%s' must be true when using this constructor!", - stamped_param_name_); + "enable_stamped_cmd_vel must be true when using this constructor!"); } } @@ -146,8 +145,6 @@ class TwistSubscriber rclcpp::Subscription::SharedPtr twist_sub_ {nullptr}; //! @brief The subscription when using TwistStamped rclcpp::Subscription::SharedPtr twist_stamped_sub_ {nullptr}; - //! @brief The name of the ROS parameter that controls whether twist data is stamped - const std::string stamped_param_name_ {"enable_stamped_cmd_vel"}; }; diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp index 08984081db4..94361e94aa8 100644 --- a/nav2_util/test/test_twist_publisher.cpp +++ b/nav2_util/test/test_twist_publisher.cpp @@ -41,23 +41,24 @@ TEST(TwistPublisher, Unstamped) sub_node->configure(); sub_node->activate(); - geometry_msgs::msg::TwistStamped pub_msg {}; - pub_msg.twist.linear.x = 42.0; + auto pub_msg = std::make_unique(); + pub_msg->twist.linear.x = 42.0; + auto pub_msg_copy = pub_msg->twist; geometry_msgs::msg::Twist sub_msg {}; auto my_sub = sub_node->create_subscription( "cmd_vel", 10, [&](const geometry_msgs::msg::Twist msg) {sub_msg = msg;}); - vel_publisher->publish(pub_msg); + vel_publisher->publish(std::move(pub_msg)); rclcpp::spin_some(sub_node->get_node_base_interface()); - EXPECT_EQ(pub_msg.twist.linear.x, sub_msg.linear.x); - ASSERT_EQ(vel_publisher->get_subscription_count(), 1); + EXPECT_EQ(pub_msg_copy.linear.x, sub_msg.linear.x); + EXPECT_EQ(vel_publisher->get_subscription_count(), 1); pub_node->deactivate(); sub_node->deactivate(); rclcpp::shutdown(); - // Have to join thread after rclcpp is shut down otherwise test hangs. + // // Have to join thread after rclcpp is shut down otherwise test hangs. pub_thread.join(); } @@ -78,19 +79,20 @@ TEST(TwistPublisher, Stamped) sub_node->configure(); sub_node->activate(); - geometry_msgs::msg::TwistStamped pub_msg {}; - pub_msg.twist.linear.x = 42.0; - pub_msg.header.frame_id = "foo"; + auto pub_msg = std::make_unique(); + pub_msg->twist.linear.x = 42.0; + pub_msg->header.frame_id = "foo"; + auto pub_msg_copy = *pub_msg; geometry_msgs::msg::TwistStamped sub_msg {}; auto my_sub = sub_node->create_subscription( "cmd_vel", 10, [&](const geometry_msgs::msg::TwistStamped msg) {sub_msg = msg;}); - vel_publisher->publish(pub_msg); + vel_publisher->publish(std::move(pub_msg)); rclcpp::spin_some(sub_node->get_node_base_interface()); ASSERT_EQ(vel_publisher->get_subscription_count(), 1); - EXPECT_EQ(pub_msg, sub_msg); + EXPECT_EQ(pub_msg_copy, sub_msg); pub_node->deactivate(); sub_node->deactivate(); rclcpp::shutdown(); diff --git a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp index cf7e8a24c90..f69a827a7db 100644 --- a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp +++ b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp @@ -117,8 +117,8 @@ class VelocitySmoother : public nav2_util::LifecycleNode * @brief Callback for incoming velocity commands * @param msg Twist message */ - void inputCommandCallback(const geometry_msgs::msg::Twist::ConstSharedPtr msg); - void inputCommandStampedCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); + void inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg); + void inputCommandStampedCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg); /** * @brief Main worker timer function @@ -135,7 +135,7 @@ class VelocitySmoother : public nav2_util::LifecycleNode // Network interfaces std::unique_ptr odom_smoother_; std::unique_ptr smoothed_cmd_pub_; - std::shared_ptr cmd_sub_; + std::unique_ptr cmd_sub_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Clock::SharedPtr clock_; diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index abf95bfe539..eabd99e2708 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -129,7 +129,7 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) // Setup inputs / outputs smoothed_cmd_pub_ = std::make_unique(node, "cmd_vel_smoothed", 1); - cmd_sub_ = std::make_shared( + cmd_sub_ = std::make_unique( node, "cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1), @@ -203,7 +203,7 @@ VelocitySmoother::on_shutdown(const rclcpp_lifecycle::State &) } void VelocitySmoother::inputCommandStampedCallback( - const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) + const geometry_msgs::msg::TwistStamped::SharedPtr msg) { // If message contains NaN or Inf, ignore if (!nav2_util::validateTwist(msg->twist)) { @@ -211,7 +211,7 @@ void VelocitySmoother::inputCommandStampedCallback( return; } - command_ = std::make_shared(*msg); + command_ = msg; if (msg->header.stamp.sec == 0 && msg->header.stamp.nanosec == 0) { last_command_time_ = now(); } else { @@ -220,11 +220,11 @@ void VelocitySmoother::inputCommandStampedCallback( } void VelocitySmoother::inputCommandCallback( - geometry_msgs::msg::Twist::ConstSharedPtr msg) + geometry_msgs::msg::Twist::SharedPtr msg) { - auto twist_stamped = geometry_msgs::msg::TwistStamped(); - twist_stamped.twist = *msg; - inputCommandStampedCallback(std::make_shared(twist_stamped)); + auto twist_stamped = std::make_shared(); + twist_stamped->twist = *msg; + inputCommandStampedCallback(twist_stamped); } double VelocitySmoother::findEtaConstraint( @@ -357,14 +357,14 @@ void VelocitySmoother::smootherTimer() last_cmd_ = *cmd_vel; // Apply deadband restrictions & publish - cmd_vel->twist.linear.x = fabs(cmd_vel->twist.linear.x) < - deadband_velocities_[0] ? 0.0 : cmd_vel->twist.linear.x; - cmd_vel->twist.linear.y = fabs(cmd_vel->twist.linear.y) < - deadband_velocities_[1] ? 0.0 : cmd_vel->twist.linear.y; - cmd_vel->twist.angular.z = fabs(cmd_vel->twist.angular.z) < - deadband_velocities_[2] ? 0.0 : cmd_vel->twist.angular.z; - - smoothed_cmd_pub_->publish(*cmd_vel); + cmd_vel->twist.linear.x = + fabs(cmd_vel->twist.linear.x) < deadband_velocities_[0] ? 0.0 : cmd_vel->twist.linear.x; + cmd_vel->twist.linear.y = + fabs(cmd_vel->twist.linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->twist.linear.y; + cmd_vel->twist.angular.z = + fabs(cmd_vel->twist.angular.z) < deadband_velocities_[2] ? 0.0 : cmd_vel->twist.angular.z; + + smoothed_cmd_pub_->publish(std::move(cmd_vel)); } rcl_interfaces::msg::SetParametersResult From 606d86f0da26079a12a29c3621269580eabd63a1 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 3 Jan 2024 17:00:39 -0800 Subject: [PATCH 36/38] shared to unique ptr Signed-off-by: Steve Macenski --- .../include/nav2_behaviors/plugins/assisted_teleop.hpp | 2 +- nav2_behaviors/plugins/assisted_teleop.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp index f245191ad21..772bb894a09 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp @@ -99,7 +99,7 @@ class AssistedTeleop : public TimedBehavior bool preempt_teleop_{false}; // subscribers - std::shared_ptr vel_sub_; + std::unique_ptr vel_sub_; rclcpp::Subscription::SharedPtr preempt_teleop_sub_; rclcpp::Duration command_time_allowance_{0, 0}; diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index 82edd1b8223..9e85229a3c1 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -50,8 +50,7 @@ void AssistedTeleop::onConfigure() std::string cmd_vel_teleop; node->get_parameter("cmd_vel_teleop", cmd_vel_teleop); - - vel_sub_ = std::make_shared( + vel_sub_ = std::make_unique( node, cmd_vel_teleop, rclcpp::SystemDefaultsQoS(), [&](geometry_msgs::msg::Twist::SharedPtr msg) { From 13f1a327dfb1f3d7637861f2e0aadcd7886fc75b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 4 Jan 2024 15:18:12 -0800 Subject: [PATCH 37/38] twist add stamps and properly propogated --- .../nav2_behaviors/plugins/drive_on_heading.hpp | 2 ++ .../include/nav2_behaviors/timed_behavior.hpp | 4 ++-- nav2_behaviors/plugins/spin.cpp | 2 ++ .../nav2_collision_monitor/collision_monitor_node.hpp | 6 ++++-- nav2_collision_monitor/src/collision_monitor_node.cpp | 11 ++++++----- nav2_velocity_smoother/src/velocity_smoother.cpp | 1 + 6 files changed, 17 insertions(+), 9 deletions(-) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 83aa3eede36..0b74a044629 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -127,6 +127,8 @@ class DriveOnHeading : public TimedBehavior } auto cmd_vel = std::make_unique(); + cmd_vel->header.stamp = this->clock_->now(); + cmd_vel->header.frame_id = this->robot_base_frame_; cmd_vel->twist.linear.y = 0.0; cmd_vel->twist.angular.z = 0.0; cmd_vel->twist.linear.x = command_speed_; diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index acdab77d74d..eab6330d507 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -291,8 +291,8 @@ class TimedBehavior : public nav2_core::Behavior void stopRobot() { auto cmd_vel = std::make_unique(); - cmd_vel->header.frame_id = "base_link"; - cmd_vel->header.stamp = rclcpp::Time(); + cmd_vel->header.frame_id = robot_base_frame_; + cmd_vel->header.stamp = clock_->now(); cmd_vel->twist.linear.x = 0.0; cmd_vel->twist.linear.y = 0.0; cmd_vel->twist.angular.z = 0.0; diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index 8c7bd413d18..5965675f643 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -139,6 +139,8 @@ ResultStatus Spin::onCycleUpdate() vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); auto cmd_vel = std::make_unique(); + cmd_vel->header.frame_id = robot_base_frame_; + cmd_vel->header.stamp = clock_->now(); cmd_vel->twist.angular.z = copysign(vel, cmd_yaw_); geometry_msgs::msg::Pose2D pose2d; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index fdaac9093c1..7362b4baebe 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -104,8 +104,9 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @brief Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds, * quit to publish 0-velocity. * @param robot_action Robot action to publish + * @param header TwistStamped header to use */ - void publishVelocity(const Action & robot_action); + void publishVelocity(const Action & robot_action, const std_msgs::msg::Header & header); /** * @brief Supporting routine obtaining all ROS-parameters @@ -149,8 +150,9 @@ class CollisionMonitor : public nav2_util::LifecycleNode /** * @brief Main processing routine * @param cmd_vel_in Input desired robot velocity + * @param header Twist header */ - void process(const Velocity & cmd_vel_in); + void process(const Velocity & cmd_vel_in, const std_msgs::msg::Header & header); /** * @brief Processes the polygon of STOP, SLOWDOWN and LIMIT action type diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index e6575068889..e3018e1020d 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -191,7 +191,7 @@ void CollisionMonitor::cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped: return; } - process({msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z}); + process({msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z}, msg->header); } void CollisionMonitor::cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::SharedPtr msg) @@ -201,7 +201,8 @@ void CollisionMonitor::cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::Shar cmdVelInCallbackStamped(twist_stamped); } -void CollisionMonitor::publishVelocity(const Action & robot_action) +void CollisionMonitor::publishVelocity( + const Action & robot_action, const std_msgs::msg::Header & header) { if (robot_action.req_vel.isZero()) { if (!robot_action_prev_.req_vel.isZero()) { @@ -215,7 +216,7 @@ void CollisionMonitor::publishVelocity(const Action & robot_action) } auto cmd_vel_out_msg = std::make_unique(); - cmd_vel_out_msg->header.stamp = this->now(); + cmd_vel_out_msg->header = header; cmd_vel_out_msg->twist.linear.x = robot_action.req_vel.x; cmd_vel_out_msg->twist.linear.y = robot_action.req_vel.y; cmd_vel_out_msg->twist.angular.z = robot_action.req_vel.tw; @@ -389,7 +390,7 @@ bool CollisionMonitor::configureSources( return true; } -void CollisionMonitor::process(const Velocity & cmd_vel_in) +void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg::Header & header) { // Current timestamp for all inner routines prolongation rclcpp::Time curr_time = this->now(); @@ -483,7 +484,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) } // Publish required robot velocity - publishVelocity(robot_action); + publishVelocity(robot_action, header); // Publish polygons for better visualization publishPolygons(); diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index eabd99e2708..02fb390cc0c 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -289,6 +289,7 @@ void VelocitySmoother::smootherTimer() } auto cmd_vel = std::make_unique(); + cmd_vel->header = command_->header; // Check for velocity timeout. If nothing received, publish zeros to apply deceleration if (now() - last_command_time_ > velocity_timeout_) { From 44f78452d322823347626a294ab0c271d2c8dbf8 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 15 Jan 2024 12:03:50 +0000 Subject: [PATCH 38/38] nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring --- nav2_util/include/nav2_util/twist_publisher.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_util/include/nav2_util/twist_publisher.hpp b/nav2_util/include/nav2_util/twist_publisher.hpp index 703dd567b2e..b86fdee5e78 100644 --- a/nav2_util/include/nav2_util/twist_publisher.hpp +++ b/nav2_util/include/nav2_util/twist_publisher.hpp @@ -106,7 +106,8 @@ class TwistPublisher if (is_stamped_) { twist_stamped_pub_->publish(std::move(velocity)); } else { - twist_pub_->publish(std::move(std::make_unique(velocity->twist))); + auto twist_msg = std::make_unique(velocity->twist); + twist_pub_->publish(std::move(twist_msg)); } }