From 4e878a20a1bcc1d41a9ff82b8ffbb732e3effab4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 16:49:25 +0100 Subject: [PATCH 01/46] Add IsStoppedBTNode Signed-off-by: Tony Najjar --- nav2_behavior_tree/CMakeLists.txt | 3 + .../condition/is_stopped_condition.hpp | 95 ++++++++++++++++ .../condition/is_stopped_condition.cpp | 102 ++++++++++++++++++ 3 files changed, 200 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp create mode 100644 nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 30955681deb..3aaec265445 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -95,6 +95,9 @@ list(APPEND plugin_libs nav2_clear_costmap_service_bt_node) add_library(nav2_is_stuck_condition_bt_node SHARED plugins/condition/is_stuck_condition.cpp) list(APPEND plugin_libs nav2_is_stuck_condition_bt_node) +add_library(nav2_is_stopped_condition_bt_node SHARED plugins/condition/is_stopped_condition.cpp) +list(APPEND plugin_libs nav2_is_stopped_condition_bt_node) + add_library(nav2_transform_available_condition_bt_node SHARED plugins/condition/transform_available_condition.cpp) list(APPEND plugin_libs nav2_transform_available_condition_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp new file mode 100644 index 00000000000..43a03ee34aa --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -0,0 +1,95 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp/condition_node.h" +#include "nav_msgs/msg/odometry.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief A BT::ConditionNode that tracks robot odometry and returns SUCCESS + * if robot is stuck somewhere and FAILURE otherwise + */ +class IsStoppedCondition : public BT::ConditionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::IsStoppedCondition + * @param condition_name Name for the XML tag for this node + * @param conf BT node configuration + */ + IsStoppedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + IsStoppedCondition() = delete; + + /** + * @brief A destructor for nav2_behavior_tree::IsStoppedCondition + */ + ~IsStoppedCondition() override; + + /** + * @brief Callback function for odom topic + * @param msg Shared pointer to nav_msgs::msg::Odometry::SharedPtr message + */ + void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg); + + /** + * @brief The main override required by a BT action + * @return BT::NodeStatus Status of tick execution + */ + BT::NodeStatus tick() override; + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort("velocity_threshold", 0.1, "Velocity threshold below which robot is considered stopped"), + BT::InputPort("time_stopped_threshold", 1000, "Time threshold for which the velocity needs to be below the threshold to consider the robot stopped") + }; + } + +private: + // The node that will be used for any ROS operations + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + std::thread callback_group_executor_thread; + + std::atomic is_stopped_; + double velocity_threshold_; + std::chrono::milliseconds time_stopped_threshold_; + rclcpp::Time stopped_stamp_; + std::mutex mutex_; + + // Listen to odometry + rclcpp::Subscription::SharedPtr odom_sub_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_ diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp new file mode 100644 index 00000000000..c0dc266a3c5 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -0,0 +1,102 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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_behavior_tree/plugins/condition/is_stopped_condition.hpp" + +using namespace std::chrono_literals; // NOLINT + +namespace nav2_behavior_tree +{ + +IsStoppedCondition::IsStoppedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + is_stopped_(false), + velocity_threshold_(0.1), + time_stopped_threshold_(1000), + stopped_stamp_(rclcpp::Time(0)) +{ + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + callback_group_executor_thread = std::thread([this]() {callback_group_executor_.spin();}); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + odom_sub_ = node_->create_subscription( + "odom", + rclcpp::SystemDefaultsQoS(), + std::bind(&IsStoppedCondition::onOdomReceived, this, std::placeholders::_1), + sub_option); +} + +IsStoppedCondition::~IsStoppedCondition() +{ + RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStoppedCondition BT node"); + callback_group_executor_.cancel(); + callback_group_executor_thread.join(); +} + +void IsStoppedCondition::onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg) +{ + getInput("velocity_threshold", velocity_threshold_); + getInput("time_stopped_threshold", time_stopped_threshold_); + + // Check if the robot is stopped for a certain amount of time + if (msg->twist.twist.linear.x < velocity_threshold_ && + msg->twist.twist.linear.y < velocity_threshold_ && + msg->twist.twist.angular.z < velocity_threshold_) + { + if (stopped_stamp_ == rclcpp::Time(0)) { + stopped_stamp_ = rclcpp::Time(msg->header.stamp); + } else if (rclcpp::Time(msg->header.stamp) - stopped_stamp_ > rclcpp::Duration(time_stopped_threshold_)) { + is_stopped_ = true; + } + } else { + stopped_stamp_ = rclcpp::Time(0); + + std::lock_guard lock(mutex_); + is_stopped_ = false; + } +} + +BT::NodeStatus IsStoppedCondition::tick() +{ + std::lock_guard lock(mutex_); + if (is_stopped_) { + return BT::NodeStatus::SUCCESS; + } + else if (stopped_stamp_ != rclcpp::Time(0)) { + // Robot was stopped but not for long enough + return BT::NodeStatus::RUNNING; + } + else { + return BT::NodeStatus::FAILURE; + } +} + + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("IsStopped"); +} From 9139f9f81d0e0fabbae7f1ed63e307a2f5713427 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 16:52:58 +0100 Subject: [PATCH 02/46] add topic name + reformat Signed-off-by: Tony Najjar --- .../plugins/condition/is_stopped_condition.hpp | 11 +++++++++-- .../plugins/condition/is_stopped_condition.cpp | 14 ++++++++------ 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index 43a03ee34aa..ac34fdcbbce 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -68,8 +68,14 @@ class IsStoppedCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("velocity_threshold", 0.1, "Velocity threshold below which robot is considered stopped"), - BT::InputPort("time_stopped_threshold", 1000, "Time threshold for which the velocity needs to be below the threshold to consider the robot stopped") + BT::InputPort("velocity_threshold", 0.1, + "Velocity threshold below which robot is considered stopped"), + BT::InputPort("time_stopped_threshold", 1000, + "Time threshold for which the velocity needs to be below the threshold to consider the robot stopped"), + BT::InputPort( + "topic_name", + "odom", + "the odometry topic name"), }; } @@ -80,6 +86,7 @@ class IsStoppedCondition : public BT::ConditionNode rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::thread callback_group_executor_thread; + std::string topic_name_; std::atomic is_stopped_; double velocity_threshold_; std::chrono::milliseconds time_stopped_threshold_; diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index c0dc266a3c5..6080d975e6c 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -32,6 +32,8 @@ IsStoppedCondition::IsStoppedCondition( stopped_stamp_(rclcpp::Time(0)) { node_ = config().blackboard->get("node"); + getInput("topic_name", topic_name_); + callback_group_ = node_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -41,7 +43,7 @@ IsStoppedCondition::IsStoppedCondition( rclcpp::SubscriptionOptions sub_option; sub_option.callback_group = callback_group_; odom_sub_ = node_->create_subscription( - "odom", + topic_name_, rclcpp::SystemDefaultsQoS(), std::bind(&IsStoppedCondition::onOdomReceived, this, std::placeholders::_1), sub_option); @@ -66,7 +68,9 @@ void IsStoppedCondition::onOdomReceived(const typename nav_msgs::msg::Odometry:: { if (stopped_stamp_ == rclcpp::Time(0)) { stopped_stamp_ = rclcpp::Time(msg->header.stamp); - } else if (rclcpp::Time(msg->header.stamp) - stopped_stamp_ > rclcpp::Duration(time_stopped_threshold_)) { + } else if (rclcpp::Time(msg->header.stamp) - stopped_stamp_ > + rclcpp::Duration(time_stopped_threshold_)) + { is_stopped_ = true; } } else { @@ -82,12 +86,10 @@ BT::NodeStatus IsStoppedCondition::tick() std::lock_guard lock(mutex_); if (is_stopped_) { return BT::NodeStatus::SUCCESS; - } - else if (stopped_stamp_ != rclcpp::Time(0)) { + } else if (stopped_stamp_ != rclcpp::Time(0)) { // Robot was stopped but not for long enough return BT::NodeStatus::RUNNING; - } - else { + } else { return BT::NodeStatus::FAILURE; } } From c7ec42efddc9b43a235d617622841487ed37ca9a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 16:55:55 +0100 Subject: [PATCH 03/46] fix comment Signed-off-by: Tony Najjar --- .../plugins/condition/is_stopped_condition.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index ac34fdcbbce..efe37182095 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -28,7 +28,7 @@ namespace nav2_behavior_tree /** * @brief A BT::ConditionNode that tracks robot odometry and returns SUCCESS - * if robot is stuck somewhere and FAILURE otherwise + * if robot is considered stopped for long enough, RUNNING if stopped but not for long enough and FAILURE otherwise */ class IsStoppedCondition : public BT::ConditionNode { @@ -99,4 +99,4 @@ class IsStoppedCondition : public BT::ConditionNode } // namespace nav2_behavior_tree -#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_ +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STOPPED_CONDITION_HPP_ From 5a8ea9209eac33c3258d1e019cc7c8839a45b43a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 18:20:09 +0100 Subject: [PATCH 04/46] fix abs Signed-off-by: Tony Najjar --- .../plugins/condition/is_stopped_condition.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 6080d975e6c..5a9e34d094c 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -29,7 +29,7 @@ IsStoppedCondition::IsStoppedCondition( is_stopped_(false), velocity_threshold_(0.1), time_stopped_threshold_(1000), - stopped_stamp_(rclcpp::Time(0)) + stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { node_ = config().blackboard->get("node"); getInput("topic_name", topic_name_); @@ -60,13 +60,14 @@ void IsStoppedCondition::onOdomReceived(const typename nav_msgs::msg::Odometry:: { getInput("velocity_threshold", velocity_threshold_); getInput("time_stopped_threshold", time_stopped_threshold_); + std::lock_guard lock(mutex_); // Check if the robot is stopped for a certain amount of time - if (msg->twist.twist.linear.x < velocity_threshold_ && - msg->twist.twist.linear.y < velocity_threshold_ && - msg->twist.twist.angular.z < velocity_threshold_) + if (abs(msg->twist.twist.linear.x) < velocity_threshold_ && + abs(msg->twist.twist.linear.y) < velocity_threshold_ && + abs(msg->twist.twist.angular.z) < velocity_threshold_) { - if (stopped_stamp_ == rclcpp::Time(0)) { + if (stopped_stamp_ == rclcpp::Time(0, 0, RCL_ROS_TIME)) { stopped_stamp_ = rclcpp::Time(msg->header.stamp); } else if (rclcpp::Time(msg->header.stamp) - stopped_stamp_ > rclcpp::Duration(time_stopped_threshold_)) @@ -74,9 +75,7 @@ void IsStoppedCondition::onOdomReceived(const typename nav_msgs::msg::Odometry:: is_stopped_ = true; } } else { - stopped_stamp_ = rclcpp::Time(0); - - std::lock_guard lock(mutex_); + stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); is_stopped_ = false; } } @@ -86,8 +85,9 @@ BT::NodeStatus IsStoppedCondition::tick() std::lock_guard lock(mutex_); if (is_stopped_) { return BT::NodeStatus::SUCCESS; - } else if (stopped_stamp_ != rclcpp::Time(0)) { + } else if (stopped_stamp_ != rclcpp::Time(0, 0, RCL_ROS_TIME)) { // Robot was stopped but not for long enough + RCLCPP_INFO(node_->get_logger(), "Robot is not stopped, waiting"); return BT::NodeStatus::RUNNING; } else { return BT::NodeStatus::FAILURE; From 9b5a93ef0b88da7022fd74849c63f632192c4045 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 25 Nov 2024 18:23:56 +0100 Subject: [PATCH 05/46] remove log Signed-off-by: Tony Najjar --- nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 5a9e34d094c..a6775b656f9 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -87,7 +87,6 @@ BT::NodeStatus IsStoppedCondition::tick() return BT::NodeStatus::SUCCESS; } else if (stopped_stamp_ != rclcpp::Time(0, 0, RCL_ROS_TIME)) { // Robot was stopped but not for long enough - RCLCPP_INFO(node_->get_logger(), "Robot is not stopped, waiting"); return BT::NodeStatus::RUNNING; } else { return BT::NodeStatus::FAILURE; From 89c8d341f7bb22b7f00be2528d5215e46e2abd2e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 26 Nov 2024 11:28:16 +0100 Subject: [PATCH 06/46] add getter functions for raw twist Signed-off-by: Tony Najjar --- .../include/nav2_util/odometry_utils.hpp | 35 +++++++++++++++++-- 1 file changed, 33 insertions(+), 2 deletions(-) diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp index 185a86d1471..276f1a0414e 100644 --- a/nav2_util/include/nav2_util/odometry_utils.hpp +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -68,13 +68,44 @@ class OdomSmoother * @brief Get twist msg from smoother * @return twist Twist msg */ - inline geometry_msgs::msg::Twist getTwist() {return vel_smooth_.twist;} + inline geometry_msgs::msg::Twist getTwist() + { + std::lock_guard lock(odom_mutex_); + return vel_smooth_.twist; + } /** * @brief Get twist stamped msg from smoother * @return twist TwistStamped msg */ - inline geometry_msgs::msg::TwistStamped getTwistStamped() {return vel_smooth_;} + inline geometry_msgs::msg::TwistStamped getTwistStamped() + { + std::lock_guard lock(odom_mutex_); + return vel_smooth_; + } + + /** + * @brief Get raw twist msg from smoother (without smoothing) + * @return twist Twist msg + */ + inline geometry_msgs::msg::Twist getRawTwist() + { + std::lock_guard lock(odom_mutex_); + return odom_history_.back().twist.twist; + } + + /** + * @brief Get raw twist stamped msg from smoother (without smoothing) + * @return twist TwistStamped msg + */ + inline geometry_msgs::msg::TwistStamped getRawTwistStamped() + { + std::lock_guard lock(odom_mutex_); + geometry_msgs::msg::TwistStamped twist_stamped; + twist_stamped.header = odom_history_.back().header; + twist_stamped.twist = odom_history_.back().twist.twist; + return twist_stamped; + } protected: /** From c0098c4bcfb599b017a88c9f5d586c30abac856f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 26 Nov 2024 11:28:27 +0100 Subject: [PATCH 07/46] remove unused code Signed-off-by: Tony Najjar --- nav2_behavior_tree/plugins/decorator/speed_controller.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index 87c9eb5bf16..f5b4eb3bd86 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -50,8 +50,6 @@ SpeedController::SpeedController( d_rate_ = max_rate_ - min_rate_; d_speed_ = max_speed_ - min_speed_; - std::string odom_topic; - node_->get_parameter_or("odom_topic", odom_topic, std::string("odom")); odom_smoother_ = config().blackboard->get>( "odom_smoother"); } From 18b8b761d1ac383e60cfa12fd953b7dcf0094c36 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 26 Nov 2024 11:28:45 +0100 Subject: [PATCH 08/46] use odomsmoother Signed-off-by: Tony Najjar --- .../condition/is_stopped_condition.hpp | 21 +----- .../condition/is_stopped_condition.cpp | 65 ++++++------------- 2 files changed, 22 insertions(+), 64 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index efe37182095..51b249afb50 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -22,6 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" #include "nav_msgs/msg/odometry.hpp" +#include "nav2_util/odometry_utils.hpp" namespace nav2_behavior_tree { @@ -49,12 +50,6 @@ class IsStoppedCondition : public BT::ConditionNode */ ~IsStoppedCondition() override; - /** - * @brief Callback function for odom topic - * @param msg Shared pointer to nav_msgs::msg::Odometry::SharedPtr message - */ - void onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg); - /** * @brief The main override required by a BT action * @return BT::NodeStatus Status of tick execution @@ -72,29 +67,17 @@ class IsStoppedCondition : public BT::ConditionNode "Velocity threshold below which robot is considered stopped"), BT::InputPort("time_stopped_threshold", 1000, "Time threshold for which the velocity needs to be below the threshold to consider the robot stopped"), - BT::InputPort( - "topic_name", - "odom", - "the odometry topic name"), }; } private: - // The node that will be used for any ROS operations rclcpp::Node::SharedPtr node_; - rclcpp::CallbackGroup::SharedPtr callback_group_; - rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - std::thread callback_group_executor_thread; - std::string topic_name_; - std::atomic is_stopped_; double velocity_threshold_; std::chrono::milliseconds time_stopped_threshold_; rclcpp::Time stopped_stamp_; - std::mutex mutex_; - // Listen to odometry - rclcpp::Subscription::SharedPtr odom_sub_; + std::shared_ptr odom_smoother_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index a6775b656f9..14c21b43a0c 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -26,74 +26,49 @@ IsStoppedCondition::IsStoppedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - is_stopped_(false), velocity_threshold_(0.1), time_stopped_threshold_(1000), stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { node_ = config().blackboard->get("node"); - getInput("topic_name", topic_name_); - - callback_group_ = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, - false); - callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - callback_group_executor_thread = std::thread([this]() {callback_group_executor_.spin();}); - - rclcpp::SubscriptionOptions sub_option; - sub_option.callback_group = callback_group_; - odom_sub_ = node_->create_subscription( - topic_name_, - rclcpp::SystemDefaultsQoS(), - std::bind(&IsStoppedCondition::onOdomReceived, this, std::placeholders::_1), - sub_option); + odom_smoother_ = config().blackboard->get>( + "odom_smoother"); } IsStoppedCondition::~IsStoppedCondition() { RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsStoppedCondition BT node"); - callback_group_executor_.cancel(); - callback_group_executor_thread.join(); } -void IsStoppedCondition::onOdomReceived(const typename nav_msgs::msg::Odometry::SharedPtr msg) +BT::NodeStatus IsStoppedCondition::tick() { - getInput("velocity_threshold", velocity_threshold_); - getInput("time_stopped_threshold", time_stopped_threshold_); - std::lock_guard lock(mutex_); + auto twist = odom_smoother_->getRawTwistStamped(); + + // if there is no timestamp, set it to now + if (twist.header.stamp.sec == 0 && twist.header.stamp.nanosec == 0) { + twist.header.stamp = node_->get_clock()->now(); + } - // Check if the robot is stopped for a certain amount of time - if (abs(msg->twist.twist.linear.x) < velocity_threshold_ && - abs(msg->twist.twist.linear.y) < velocity_threshold_ && - abs(msg->twist.twist.angular.z) < velocity_threshold_) + if (abs(twist.twist.linear.x) < velocity_threshold_ && + abs(twist.twist.linear.y) < velocity_threshold_ && + abs(twist.twist.angular.z) < velocity_threshold_) { if (stopped_stamp_ == rclcpp::Time(0, 0, RCL_ROS_TIME)) { - stopped_stamp_ = rclcpp::Time(msg->header.stamp); - } else if (rclcpp::Time(msg->header.stamp) - stopped_stamp_ > - rclcpp::Duration(time_stopped_threshold_)) - { - is_stopped_ = true; + stopped_stamp_ = rclcpp::Time(twist.header.stamp); + } + + if (node_->get_clock()->now() - stopped_stamp_ > rclcpp::Duration(time_stopped_threshold_)) { + return BT::NodeStatus::SUCCESS; + } else { + return BT::NodeStatus::RUNNING; } - } else { - stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - is_stopped_ = false; - } -} -BT::NodeStatus IsStoppedCondition::tick() -{ - std::lock_guard lock(mutex_); - if (is_stopped_) { - return BT::NodeStatus::SUCCESS; - } else if (stopped_stamp_ != rclcpp::Time(0, 0, RCL_ROS_TIME)) { - // Robot was stopped but not for long enough - return BT::NodeStatus::RUNNING; } else { + stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); return BT::NodeStatus::FAILURE; } } - } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" From 5c0560aa89853070c6388ccd91c4b503cf25bd86 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 26 Nov 2024 11:40:22 +0100 Subject: [PATCH 09/46] fix formatting Signed-off-by: Tony Najjar --- .../plugins/condition/is_stopped_condition.hpp | 7 ++++--- .../plugins/condition/is_stopped_condition.cpp | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index 51b249afb50..388f471d317 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" @@ -65,8 +66,8 @@ class IsStoppedCondition : public BT::ConditionNode return { BT::InputPort("velocity_threshold", 0.1, "Velocity threshold below which robot is considered stopped"), - BT::InputPort("time_stopped_threshold", 1000, - "Time threshold for which the velocity needs to be below the threshold to consider the robot stopped"), + BT::InputPort("duration_stopped", 1000, + "Duration (ms) the velocity must remain below the threshold."), }; } @@ -74,7 +75,7 @@ class IsStoppedCondition : public BT::ConditionNode rclcpp::Node::SharedPtr node_; double velocity_threshold_; - std::chrono::milliseconds time_stopped_threshold_; + std::chrono::milliseconds duration_stopped_; rclcpp::Time stopped_stamp_; std::shared_ptr odom_smoother_; diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 14c21b43a0c..7e38e8659d4 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -27,7 +27,7 @@ IsStoppedCondition::IsStoppedCondition( const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), velocity_threshold_(0.1), - time_stopped_threshold_(1000), + duration_stopped_(1000), stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { node_ = config().blackboard->get("node"); @@ -57,7 +57,7 @@ BT::NodeStatus IsStoppedCondition::tick() stopped_stamp_ = rclcpp::Time(twist.header.stamp); } - if (node_->get_clock()->now() - stopped_stamp_ > rclcpp::Duration(time_stopped_threshold_)) { + if (node_->get_clock()->now() - stopped_stamp_ > rclcpp::Duration(duration_stopped_)) { return BT::NodeStatus::SUCCESS; } else { return BT::NodeStatus::RUNNING; From 66ed384b5bec683d68370e02c7bc6171eaa45a5b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 26 Nov 2024 11:45:00 +0100 Subject: [PATCH 10/46] update groot Signed-off-by: Tony Najjar --- .../plugins/condition/is_stopped_condition.hpp | 2 +- nav2_behavior_tree/nav2_tree_nodes.xml | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index 388f471d317..2456c2315da 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -67,7 +67,7 @@ class IsStoppedCondition : public BT::ConditionNode BT::InputPort("velocity_threshold", 0.1, "Velocity threshold below which robot is considered stopped"), BT::InputPort("duration_stopped", 1000, - "Duration (ms) the velocity must remain below the threshold."), + "Duration (ms) the velocity must remain below the threshold"), }; } diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 71c20ee4ecc..67059def0eb 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -227,6 +227,11 @@ + + Velocity threshold below which robot is considered stopped + Duration (ms) the velocity must remain below the threshold + + Child frame for transform Parent frame for transform From 91230b2b619a6adf4ffacf6bc2da8cf016297baa Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 27 Nov 2024 10:26:00 +0100 Subject: [PATCH 11/46] Add test Signed-off-by: Tony Najjar --- .../condition/is_stopped_condition.hpp | 2 +- .../test/plugins/condition/CMakeLists.txt | 2 + .../plugins/condition/test_is_stopped.cpp | 107 ++++++++++++++++++ .../include/nav2_util/odometry_utils.hpp | 24 ++++ nav2_util/src/odometry_utils.cpp | 3 +- 5 files changed, 136 insertions(+), 2 deletions(-) create mode 100644 nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index 2456c2315da..cafe2b666f8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -64,7 +64,7 @@ class IsStoppedCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort("velocity_threshold", 0.1, + BT::InputPort("velocity_threshold", 0.01, "Velocity threshold below which robot is considered stopped"), BT::InputPort("duration_stopped", 1000, "Duration (ms) the velocity must remain below the threshold"), diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt index de380c769af..106c4a96f27 100644 --- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt @@ -16,6 +16,8 @@ plugin_add_test(test_condition_transform_available test_transform_available.cpp plugin_add_test(test_condition_is_stuck test_is_stuck.cpp nav2_is_stuck_condition_bt_node) +plugin_add_test(test_condition_is_stopped test_is_stopped.cpp nav2_is_stopped_condition_bt_node) + plugin_add_test(test_condition_is_battery_charging test_is_battery_charging.cpp nav2_is_battery_charging_condition_bt_node) plugin_add_test(test_condition_is_battery_low test_is_battery_low.cpp nav2_is_battery_low_condition_bt_node) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp new file mode 100644 index 00000000000..aaf1afe6cab --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp @@ -0,0 +1,107 @@ +// Copyright (c) 2024 Angsa Robotics +// +// 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/odometry_utils.hpp" + +#include "utils/test_behavior_tree_fixture.hpp" +#include "nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class IsStoppedTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture +{ +public: + void SetUp() + { + odom_smoother_ = std::make_shared(node_); + config_->blackboard->set( + "odom_smoother", odom_smoother_); // NOLINT + bt_node_ = std::make_shared("is_stopped", *config_); + } + + void TearDown() + { + bt_node_.reset(); + odom_smoother_.reset(); + } + +protected: + static std::shared_ptr bt_node_; + static std::shared_ptr odom_smoother_; +}; + +std::shared_ptr +IsStoppedTestFixture::bt_node_ = nullptr; +std::shared_ptr +IsStoppedTestFixture::odom_smoother_ = nullptr; + + +TEST_F(IsStoppedTestFixture, test_behavior) +{ + auto odom_pub = node_->create_publisher("odom", + rclcpp::SystemDefaultsQoS()); + nav_msgs::msg::Odometry odom_msg; + + // Test FAILURE when robot is moving + auto time = node_->now(); + odom_msg.header.stamp = time; + odom_msg.twist.twist.linear.x = 1.0; + odom_pub->publish(odom_msg); + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(500ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + // Test RUNNING when robot is stopped but not long enough + odom_msg.header.stamp = node_->now(); + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(500ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); + + // Test SUCCESS when robot is stopped for long enough + odom_msg.header.stamp = node_->now(); + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(1100ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + + // Test FAILURE immediately after robot starts moving + odom_msg.header.stamp = node_->now(); + odom_msg.twist.twist.angular.z = 0.1; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_util/include/nav2_util/odometry_utils.hpp b/nav2_util/include/nav2_util/odometry_utils.hpp index 276f1a0414e..ab9132b5ffe 100644 --- a/nav2_util/include/nav2_util/odometry_utils.hpp +++ b/nav2_util/include/nav2_util/odometry_utils.hpp @@ -71,6 +71,12 @@ class OdomSmoother inline geometry_msgs::msg::Twist getTwist() { std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::Twist twist; + return twist; + } return vel_smooth_.twist; } @@ -81,6 +87,12 @@ class OdomSmoother inline geometry_msgs::msg::TwistStamped getTwistStamped() { std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::TwistStamped twist_stamped; + return twist_stamped; + } return vel_smooth_; } @@ -91,6 +103,12 @@ class OdomSmoother inline geometry_msgs::msg::Twist getRawTwist() { std::lock_guard lock(odom_mutex_); + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + geometry_msgs::msg::Twist twist; + return twist; + } return odom_history_.back().twist.twist; } @@ -102,6 +120,11 @@ class OdomSmoother { std::lock_guard lock(odom_mutex_); geometry_msgs::msg::TwistStamped twist_stamped; + if (!received_odom_) { + RCLCPP_ERROR(rclcpp::get_logger("OdomSmoother"), + "OdomSmoother has not received any data yet, returning empty Twist"); + return twist_stamped; + } twist_stamped.header = odom_history_.back().header; twist_stamped.twist = odom_history_.back().twist.twist; return twist_stamped; @@ -119,6 +142,7 @@ class OdomSmoother */ void updateState(); + bool received_odom_; rclcpp::Subscription::SharedPtr odom_sub_; nav_msgs::msg::Odometry odom_cumulate_; geometry_msgs::msg::TwistStamped vel_smooth_; diff --git a/nav2_util/src/odometry_utils.cpp b/nav2_util/src/odometry_utils.cpp index 9bf585bdfe2..6b50c332bcc 100644 --- a/nav2_util/src/odometry_utils.cpp +++ b/nav2_util/src/odometry_utils.cpp @@ -27,7 +27,7 @@ OdomSmoother::OdomSmoother( const rclcpp::Node::WeakPtr & parent, double filter_duration, const std::string & odom_topic) -: odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) +: received_odom_(false), odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) { auto node = parent.lock(); odom_sub_ = node->create_subscription( @@ -66,6 +66,7 @@ OdomSmoother::OdomSmoother( void OdomSmoother::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { std::lock_guard lock(odom_mutex_); + received_odom_ = true; // update cumulated odom only if history is not empty if (!odom_history_.empty()) { From d2c2db323e4573042a67c562d939c63d5f7a70ec Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 27 Nov 2024 11:00:07 +0100 Subject: [PATCH 12/46] reset at success Signed-off-by: Tony Najjar --- nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 7e38e8659d4..4ac5352f461 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -58,6 +58,7 @@ BT::NodeStatus IsStoppedCondition::tick() } if (node_->get_clock()->now() - stopped_stamp_ > rclcpp::Duration(duration_stopped_)) { + stopped_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); return BT::NodeStatus::SUCCESS; } else { return BT::NodeStatus::RUNNING; From 37a56365d3eadeac2850c6d506e68508c0fbef93 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Nov 2024 12:56:38 +0100 Subject: [PATCH 13/46] FIX velocity_threshold_ Signed-off-by: Tony Najjar --- nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 4ac5352f461..9dc629b18f9 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -26,7 +26,7 @@ IsStoppedCondition::IsStoppedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), - velocity_threshold_(0.1), + velocity_threshold_(0.01), duration_stopped_(1000), stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { From d237785666574f809398f6e0f5104bc9a25246d4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Nov 2024 13:28:58 +0100 Subject: [PATCH 14/46] Fix stopped Node Signed-off-by: Tony Najjar --- .../condition/is_stopped_condition.hpp | 6 +++- .../condition/is_stopped_condition.cpp | 7 +++-- .../plugins/condition/test_is_stopped.cpp | 30 ++++++++++++++----- 3 files changed, 31 insertions(+), 12 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp index cafe2b666f8..40197939d1f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp @@ -24,6 +24,10 @@ #include "behaviortree_cpp/condition_node.h" #include "nav_msgs/msg/odometry.hpp" #include "nav2_util/odometry_utils.hpp" +#include "nav2_behavior_tree/bt_utils.hpp" + + +using namespace std::chrono_literals; // NOLINT namespace nav2_behavior_tree { @@ -66,7 +70,7 @@ class IsStoppedCondition : public BT::ConditionNode return { BT::InputPort("velocity_threshold", 0.01, "Velocity threshold below which robot is considered stopped"), - BT::InputPort("duration_stopped", 1000, + BT::InputPort("duration_stopped", 1000ms, "Duration (ms) the velocity must remain below the threshold"), }; } diff --git a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp index 9dc629b18f9..f0b32d4c1c0 100644 --- a/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stopped_condition.cpp @@ -17,8 +17,6 @@ #include "nav2_behavior_tree/plugins/condition/is_stopped_condition.hpp" -using namespace std::chrono_literals; // NOLINT - namespace nav2_behavior_tree { @@ -27,7 +25,7 @@ IsStoppedCondition::IsStoppedCondition( const BT::NodeConfiguration & conf) : BT::ConditionNode(condition_name, conf), velocity_threshold_(0.01), - duration_stopped_(1000), + duration_stopped_(1000ms), stopped_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { node_ = config().blackboard->get("node"); @@ -42,6 +40,9 @@ IsStoppedCondition::~IsStoppedCondition() BT::NodeStatus IsStoppedCondition::tick() { + getInput("velocity_threshold", velocity_threshold_); + getInput("duration_stopped", duration_stopped_); + auto twist = odom_smoother_->getRawTwistStamped(); // if there is no timestamp, set it to now diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp index aaf1afe6cab..a94e4285fd9 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp @@ -34,6 +34,8 @@ class IsStoppedTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture odom_smoother_ = std::make_shared(node_); config_->blackboard->set( "odom_smoother", odom_smoother_); // NOLINT + std::chrono::milliseconds duration = 100ms; + config_->input_ports["duration_stopped"] = std::to_string(duration.count()) + "ms"; // to make the test faster bt_node_ = std::make_shared("is_stopped", *config_); } @@ -65,22 +67,17 @@ TEST_F(IsStoppedTestFixture, test_behavior) odom_msg.header.stamp = time; odom_msg.twist.twist.linear.x = 1.0; odom_pub->publish(odom_msg); - odom_pub->publish(odom_msg); - std::this_thread::sleep_for(500ms); + std::this_thread::sleep_for(10ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); // Test RUNNING when robot is stopped but not long enough odom_msg.header.stamp = node_->now(); odom_msg.twist.twist.linear.x = 0.001; odom_pub->publish(odom_msg); - std::this_thread::sleep_for(500ms); + std::this_thread::sleep_for(90ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); - // Test SUCCESS when robot is stopped for long enough - odom_msg.header.stamp = node_->now(); - odom_msg.twist.twist.linear.x = 0.001; - odom_pub->publish(odom_msg); - std::this_thread::sleep_for(1100ms); + std::this_thread::sleep_for(20ms); // 20ms + 90ms = 110ms EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); // Test FAILURE immediately after robot starts moving @@ -89,6 +86,23 @@ TEST_F(IsStoppedTestFixture, test_behavior) odom_pub->publish(odom_msg); std::this_thread::sleep_for(10ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + + // Test SUCCESS when robot is stopped for long with a back-dated timestamp + odom_msg.header.stamp = node_->now() - rclcpp::Duration(2, 0); + odom_msg.twist.twist.angular.z = 0; + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); // wait just enough for the message to be received + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + + // Test SUCCESS when robot is stopped for long enough without a stamp for odometry + odom_msg.header.stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); + odom_msg.twist.twist.linear.x = 0.001; + odom_pub->publish(odom_msg); + std::this_thread::sleep_for(10ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); // In the first tick, the timestamp is set + std::this_thread::sleep_for(110ms); + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); } int main(int argc, char ** argv) From 5a4204f34e455da8d63115755bdf43855bd46ae6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Nov 2024 13:52:07 +0100 Subject: [PATCH 15/46] Add tests to odometry_utils Signed-off-by: Tony Najjar --- nav2_util/test/test_odometry_utils.cpp | 45 ++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/nav2_util/test/test_odometry_utils.cpp b/nav2_util/test/test_odometry_utils.cpp index 7d5b8bf9fe7..6871949303e 100644 --- a/nav2_util/test/test_odometry_utils.cpp +++ b/nav2_util/test/test_odometry_utils.cpp @@ -32,6 +32,34 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; +TEST(OdometryUtils, test_uninitialized) +{ + auto node = std::make_shared("test_node"); + nav2_util::OdomSmoother odom_smoother(node, 0.3, "odom"); + geometry_msgs::msg::Twist twist_msg; + geometry_msgs::msg::TwistStamped twist_stamped_msg; + + twist_msg = odom_smoother.getTwist(); + EXPECT_EQ(twist_msg.linear.x, 0.0); + EXPECT_EQ(twist_msg.linear.y, 0.0); + EXPECT_EQ(twist_msg.angular.z, 0.0); + + twist_msg = odom_smoother.getRawTwist(); + EXPECT_EQ(twist_msg.linear.x, 0.0); + EXPECT_EQ(twist_msg.linear.y, 0.0); + EXPECT_EQ(twist_msg.angular.z, 0.0); + + twist_stamped_msg = odom_smoother.getTwistStamped(); + EXPECT_EQ(twist_stamped_msg.twist.linear.x, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.linear.y, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.angular.z, 0.0); + + twist_stamped_msg = odom_smoother.getRawTwistStamped(); + EXPECT_EQ(twist_stamped_msg.twist.linear.x, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.linear.y, 0.0); + EXPECT_EQ(twist_stamped_msg.twist.angular.z, 0.0); +} + TEST(OdometryUtils, test_smoothed_velocity) { auto node = std::make_shared("test_node"); @@ -41,6 +69,7 @@ TEST(OdometryUtils, test_smoothed_velocity) nav_msgs::msg::Odometry odom_msg; geometry_msgs::msg::Twist twist_msg; + geometry_msgs::msg::Twist twist_raw_msg; auto time = node->now(); @@ -67,9 +96,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 1.5); EXPECT_EQ(twist_msg.linear.y, 1.5); EXPECT_EQ(twist_msg.angular.z, 1.5); + EXPECT_EQ(twist_raw_msg.linear.x, 2.0); + EXPECT_EQ(twist_raw_msg.linear.y, 2.0); + EXPECT_EQ(twist_raw_msg.angular.z, 2.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.2); odom_msg.twist.twist.linear.x = 3.0; @@ -81,9 +114,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 2.0); EXPECT_EQ(twist_msg.linear.y, 2.0); EXPECT_EQ(twist_msg.angular.z, 2.0); + EXPECT_EQ(twist_raw_msg.linear.x, 3.0); + EXPECT_EQ(twist_raw_msg.linear.y, 3.0); + EXPECT_EQ(twist_raw_msg.angular.z, 3.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(0.45); odom_msg.twist.twist.linear.x = 4.0; @@ -95,9 +132,13 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 3.5); EXPECT_EQ(twist_msg.linear.y, 3.5); EXPECT_EQ(twist_msg.angular.z, 3.5); + EXPECT_EQ(twist_raw_msg.linear.x, 4.0); + EXPECT_EQ(twist_raw_msg.linear.y, 4.0); + EXPECT_EQ(twist_raw_msg.angular.z, 4.0); odom_msg.header.stamp = time + rclcpp::Duration::from_seconds(1.0); odom_msg.twist.twist.linear.x = 5.0; @@ -109,7 +150,11 @@ TEST(OdometryUtils, test_smoothed_velocity) rclcpp::spin_some(node); twist_msg = odom_smoother.getTwist(); + twist_raw_msg = odom_smoother.getRawTwist(); EXPECT_EQ(twist_msg.linear.x, 5.0); EXPECT_EQ(twist_msg.linear.y, 5.0); EXPECT_EQ(twist_msg.angular.z, 5.0); + EXPECT_EQ(twist_raw_msg.linear.x, 5.0); + EXPECT_EQ(twist_raw_msg.linear.y, 5.0); + EXPECT_EQ(twist_raw_msg.angular.z, 5.0); } From 0295ef95b13b9f90edb5f249997996527ff59b7c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Nov 2024 13:55:28 +0100 Subject: [PATCH 16/46] fix linting Signed-off-by: Tony Najjar --- .../test/plugins/condition/test_is_stopped.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp index a94e4285fd9..26ae239a180 100644 --- a/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp +++ b/nav2_behavior_tree/test/plugins/condition/test_is_stopped.cpp @@ -34,8 +34,9 @@ class IsStoppedTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture odom_smoother_ = std::make_shared(node_); config_->blackboard->set( "odom_smoother", odom_smoother_); // NOLINT + // shorten duration_stopped from default to make the test faster std::chrono::milliseconds duration = 100ms; - config_->input_ports["duration_stopped"] = std::to_string(duration.count()) + "ms"; // to make the test faster + config_->input_ports["duration_stopped"] = std::to_string(duration.count()) + "ms"; bt_node_ = std::make_shared("is_stopped", *config_); } @@ -77,7 +78,7 @@ TEST_F(IsStoppedTestFixture, test_behavior) std::this_thread::sleep_for(90ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); // Test SUCCESS when robot is stopped for long enough - std::this_thread::sleep_for(20ms); // 20ms + 90ms = 110ms + std::this_thread::sleep_for(20ms); // 20ms + 90ms = 110ms EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); // Test FAILURE immediately after robot starts moving @@ -92,7 +93,7 @@ TEST_F(IsStoppedTestFixture, test_behavior) odom_msg.twist.twist.angular.z = 0; odom_msg.twist.twist.linear.x = 0.001; odom_pub->publish(odom_msg); - std::this_thread::sleep_for(10ms); // wait just enough for the message to be received + std::this_thread::sleep_for(10ms); // wait just enough for the message to be received EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); // Test SUCCESS when robot is stopped for long enough without a stamp for odometry @@ -100,7 +101,8 @@ TEST_F(IsStoppedTestFixture, test_behavior) odom_msg.twist.twist.linear.x = 0.001; odom_pub->publish(odom_msg); std::this_thread::sleep_for(10ms); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); // In the first tick, the timestamp is set + // In the first tick, the timestamp is set + EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); std::this_thread::sleep_for(110ms); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); } From fec198fe24c7b6b2c02f04e2fc21b14e6455cd76 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Nov 2024 16:44:47 +0100 Subject: [PATCH 17/46] Adapt goalUpdater to modify goals as well Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.hpp | 22 ++++++++++++++---- .../plugins/decorator/goal_updater_node.cpp | 23 +++++++++++++++++++ nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/msg/PosesStamped.msg | 1 + 4 files changed, 43 insertions(+), 4 deletions(-) create mode 100644 nav2_msgs/msg/PosesStamped.msg diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index ddce12cf02e..c92460826e2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -1,5 +1,6 @@ // Copyright (c) 2018 Intel Corporation // Copyright (c) 2020 Francisco Martin Rico +// Copyright (c) 2024 Angsa Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,8 +19,10 @@ #include #include +#include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/poses_stamped.hpp" #include "behaviortree_cpp/decorator_node.h" @@ -52,9 +55,9 @@ class GoalUpdater : public BT::DecoratorNode { return { BT::InputPort("input_goal", "Original Goal"), - BT::OutputPort( - "output_goal", - "Received Goal by subscription"), + BT::InputPort("input_goals", "Original Goals"), + BT::OutputPort("output_goal", "Received Goal by subscription"), + BT::OutputPort("output_goals", "Received Goals by subscription") }; } @@ -71,15 +74,26 @@ class GoalUpdater : public BT::DecoratorNode */ void callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + /** + * @brief Callback function for goals update topic + * @param msg Shared pointer to vector of geometry_msgs::msg::PoseStamped message + */ + void callback_updated_goals(const nav2_msgs::msg::PosesStamped::SharedPtr msg); + rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp::Subscription::SharedPtr goals_sub_; geometry_msgs::msg::PoseStamped last_goal_received_; + nav2_msgs::msg::PosesStamped last_goals_received_; rclcpp::Node::SharedPtr node_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + // mutex + std::mutex mutex_; }; } // namespace nav2_behavior_tree -#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_ +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_ \ No newline at end of file diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 4127bc68439..6b4b5c39ab5 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2018 Intel Corporation // Copyright (c) 2020 Francisco Martin Rico +// Copyright (c) 2024 Angsa Robotics // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -40,7 +41,9 @@ GoalUpdater::GoalUpdater( callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); std::string goal_updater_topic; + std::string goals_updater_topic; node_->get_parameter_or("goal_updater_topic", goal_updater_topic, "goal_update"); + node_->get_parameter_or("goals_updater_topic", goals_updater_topic, "goals_update"); rclcpp::SubscriptionOptions sub_option; sub_option.callback_group = callback_group_; @@ -49,13 +52,20 @@ GoalUpdater::GoalUpdater( rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goal, this, _1), sub_option); + goals_sub_ = node_->create_subscription( + goals_updater_topic, + rclcpp::SystemDefaultsQoS(), + std::bind(&GoalUpdater::callback_updated_goals, this, _1), + sub_option); } inline BT::NodeStatus GoalUpdater::tick() { geometry_msgs::msg::PoseStamped goal; + nav2_msgs::msg::PosesStamped goals; getInput("input_goal", goal); + getInput("input_goals", goals); // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin callback_group_executor_.spin_all(std::chrono::milliseconds(1)); @@ -74,16 +84,29 @@ inline BT::NodeStatus GoalUpdater::tick() } } + if (!last_goals_received_.poses.empty()) { + goals = last_goals_received_; + } + setOutput("output_goal", goal); + setOutput("output_goals", goals); return child_node_->executeTick(); } void GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + std::lock_guard lock(mutex_); last_goal_received_ = *msg; } +void +GoalUpdater::callback_updated_goals(const nav2_msgs::msg::PosesStamped::SharedPtr msg) +{ + std::lock_guard lock(mutex_); + last_goals_received_ = *msg; +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp/bt_factory.h" diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index a9f93b5ecbc..ae2ceec8812 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -29,6 +29,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" + "msg/PosesStamped.msg" "msg/MissedWaypoint.msg" "srv/GetCosts.srv" "srv/GetCostmap.srv" diff --git a/nav2_msgs/msg/PosesStamped.msg b/nav2_msgs/msg/PosesStamped.msg new file mode 100644 index 00000000000..dafb5fff0ee --- /dev/null +++ b/nav2_msgs/msg/PosesStamped.msg @@ -0,0 +1 @@ +geometry_msgs/PoseStamped[] poses \ No newline at end of file From 5a4f61f7bbc1dd0f148c9932f9ffa78b46dd3337 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Nov 2024 16:50:29 +0100 Subject: [PATCH 18/46] fix formatting Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.hpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index c92460826e2..a94f6cd357e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -56,7 +56,8 @@ class GoalUpdater : public BT::DecoratorNode return { BT::InputPort("input_goal", "Original Goal"), BT::InputPort("input_goals", "Original Goals"), - BT::OutputPort("output_goal", "Received Goal by subscription"), + BT::OutputPort("output_goal", + "Received Goal by subscription"), BT::OutputPort("output_goals", "Received Goals by subscription") }; } @@ -89,11 +90,9 @@ class GoalUpdater : public BT::DecoratorNode rclcpp::Node::SharedPtr node_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - - // mutex std::mutex mutex_; }; } // namespace nav2_behavior_tree -#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_ \ No newline at end of file +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_ From 13b47aa7f580adc2cea7fe875de74d9fe09f1126 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 28 Nov 2024 18:36:34 +0100 Subject: [PATCH 19/46] fixes Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.hpp | 7 ++++--- nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp | 6 +++--- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index a94f6cd357e..1c16b4aebc5 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -38,6 +38,7 @@ namespace nav2_behavior_tree class GoalUpdater : public BT::DecoratorNode { public: + typedef std::vector Goals; /** * @brief A constructor for nav2_behavior_tree::GoalUpdater * @param xml_tag_name Name for the XML tag for this node @@ -55,10 +56,10 @@ class GoalUpdater : public BT::DecoratorNode { return { BT::InputPort("input_goal", "Original Goal"), - BT::InputPort("input_goals", "Original Goals"), + BT::InputPort("input_goals", "Original Goals"), BT::OutputPort("output_goal", "Received Goal by subscription"), - BT::OutputPort("output_goals", "Received Goals by subscription") + BT::OutputPort("output_goals", "Received Goals by subscription") }; } @@ -85,7 +86,7 @@ class GoalUpdater : public BT::DecoratorNode rclcpp::Subscription::SharedPtr goals_sub_; geometry_msgs::msg::PoseStamped last_goal_received_; - nav2_msgs::msg::PosesStamped last_goals_received_; + Goals last_goals_received_; rclcpp::Node::SharedPtr node_; rclcpp::CallbackGroup::SharedPtr callback_group_; diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 6b4b5c39ab5..33ee87b3441 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -62,7 +62,7 @@ GoalUpdater::GoalUpdater( inline BT::NodeStatus GoalUpdater::tick() { geometry_msgs::msg::PoseStamped goal; - nav2_msgs::msg::PosesStamped goals; + Goals goals; getInput("input_goal", goal); getInput("input_goals", goals); @@ -84,7 +84,7 @@ inline BT::NodeStatus GoalUpdater::tick() } } - if (!last_goals_received_.poses.empty()) { + if (!last_goals_received_.empty()) { goals = last_goals_received_; } @@ -104,7 +104,7 @@ void GoalUpdater::callback_updated_goals(const nav2_msgs::msg::PosesStamped::SharedPtr msg) { std::lock_guard lock(mutex_); - last_goals_received_ = *msg; + last_goals_received_ = msg->poses; } } // namespace nav2_behavior_tree From 0e9ebc47c9517c2e1118150ebb19fcc4b90825b2 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 3 Dec 2024 14:02:51 +0100 Subject: [PATCH 20/46] change name of msg Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.hpp | 6 +++--- nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp | 4 ++-- nav2_msgs/CMakeLists.txt | 2 +- nav2_msgs/msg/PoseStampedArray.msg | 4 ++++ nav2_msgs/msg/PosesStamped.msg | 1 - 5 files changed, 10 insertions(+), 7 deletions(-) create mode 100644 nav2_msgs/msg/PoseStampedArray.msg delete mode 100644 nav2_msgs/msg/PosesStamped.msg diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 1c16b4aebc5..b40b8f825b8 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -22,7 +22,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_msgs/msg/poses_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "behaviortree_cpp/decorator_node.h" @@ -80,10 +80,10 @@ class GoalUpdater : public BT::DecoratorNode * @brief Callback function for goals update topic * @param msg Shared pointer to vector of geometry_msgs::msg::PoseStamped message */ - void callback_updated_goals(const nav2_msgs::msg::PosesStamped::SharedPtr msg); + void callback_updated_goals(const nav2_msgs::msg::PoseStampedArray::SharedPtr msg); rclcpp::Subscription::SharedPtr goal_sub_; - rclcpp::Subscription::SharedPtr goals_sub_; + rclcpp::Subscription::SharedPtr goals_sub_; geometry_msgs::msg::PoseStamped last_goal_received_; Goals last_goals_received_; diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 33ee87b3441..fd32e4c8c4d 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -52,7 +52,7 @@ GoalUpdater::GoalUpdater( rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goal, this, _1), sub_option); - goals_sub_ = node_->create_subscription( + goals_sub_ = node_->create_subscription( goals_updater_topic, rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goals, this, _1), @@ -101,7 +101,7 @@ GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::Shared } void -GoalUpdater::callback_updated_goals(const nav2_msgs::msg::PosesStamped::SharedPtr msg) +GoalUpdater::callback_updated_goals(const nav2_msgs::msg::PoseStampedArray::SharedPtr msg) { std::lock_guard lock(mutex_); last_goals_received_ = msg->poses; diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index ae2ceec8812..e7bfb22cda7 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -29,7 +29,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" - "msg/PosesStamped.msg" + "msg/PoseStampedArray.msg" "msg/MissedWaypoint.msg" "srv/GetCosts.srv" "srv/GetCostmap.srv" diff --git a/nav2_msgs/msg/PoseStampedArray.msg b/nav2_msgs/msg/PoseStampedArray.msg new file mode 100644 index 00000000000..1a4e04dc0d1 --- /dev/null +++ b/nav2_msgs/msg/PoseStampedArray.msg @@ -0,0 +1,4 @@ +# An array of stamped poses with a header for global reference. + +std_msgs/Header header +geometry_msgs/PoseStamped[] poses \ No newline at end of file diff --git a/nav2_msgs/msg/PosesStamped.msg b/nav2_msgs/msg/PosesStamped.msg deleted file mode 100644 index dafb5fff0ee..00000000000 --- a/nav2_msgs/msg/PosesStamped.msg +++ /dev/null @@ -1 +0,0 @@ -geometry_msgs/PoseStamped[] poses \ No newline at end of file From db1b59d05aa753d6f874b7988002f944a716e04e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 5 Dec 2024 11:22:19 +0100 Subject: [PATCH 21/46] Make input goals be Goals again for compatibility Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.hpp | 3 +- .../plugins/decorator/goal_updater_node.cpp | 24 ++++++++--- .../decorator/test_goal_updater_node.cpp | 42 ++++++++++++++++--- 3 files changed, 56 insertions(+), 13 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index b40b8f825b8..0552bc9203d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -86,12 +86,11 @@ class GoalUpdater : public BT::DecoratorNode rclcpp::Subscription::SharedPtr goals_sub_; geometry_msgs::msg::PoseStamped last_goal_received_; - Goals last_goals_received_; + nav2_msgs::msg::PoseStampedArray last_goals_received_; rclcpp::Node::SharedPtr node_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; - std::mutex mutex_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index fd32e4c8c4d..2df8fb4c4da 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -83,12 +83,26 @@ inline BT::NodeStatus GoalUpdater::tick() last_goal_received_time.seconds(), goal_time.seconds()); } } + setOutput("output_goal", goal); - if (!last_goals_received_.empty()) { - goals = last_goals_received_; + if (last_goals_received_.header.stamp != rclcpp::Time(0) && !last_goals_received_.poses.empty()) { + auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp); + rclcpp::Time most_recent_goal_time = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type()); + for (const auto & g : goals) { + if (rclcpp::Time(g.header.stamp) > most_recent_goal_time) { + most_recent_goal_time = rclcpp::Time(g.header.stamp); + } + } + if (last_goals_received_time > most_recent_goal_time) { + goals = last_goals_received_.poses; + } else { + RCLCPP_WARN( + node_->get_logger(), "None of the received goals (most recent: %f) are more recent than the " + "current goals (oldest: %f). Ignoring the received goals.", + last_goals_received_time.seconds(), most_recent_goal_time.seconds()); + } } - setOutput("output_goal", goal); setOutput("output_goals", goals); return child_node_->executeTick(); } @@ -96,15 +110,13 @@ inline BT::NodeStatus GoalUpdater::tick() void GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { - std::lock_guard lock(mutex_); last_goal_received_ = *msg; } void GoalUpdater::callback_updated_goals(const nav2_msgs::msg::PoseStampedArray::SharedPtr msg) { - std::lock_guard lock(mutex_); - last_goals_received_ = msg->poses; + last_goals_received_ = *msg; } } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 821b2a64492..15296f7b4e9 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -26,6 +26,7 @@ #include "nav2_behavior_tree/utils/test_action_server.hpp" #include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp" +typedef std::vector Goals; class GoalUpdaterTestFixture : public ::testing::Test { @@ -88,26 +89,29 @@ TEST_F(GoalUpdaterTestFixture, test_tick) R"( - + )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - auto goal_updater_pub = - node_->create_publisher("goal_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; + Goals goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; + goals.push_back(goal); config_->blackboard->set("goal", goal); + config_->blackboard->set("goals", goals); // tick tree without publishing updated goal and get updated_goal tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; + Goals updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); + EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); } TEST_F(GoalUpdaterTestFixture, test_older_goal_update) @@ -117,7 +121,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) R"( - + @@ -126,26 +130,38 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); auto goal_updater_pub = node_->create_publisher("goal_update", 10); + auto goals_updater_pub = + node_->create_publisher("goals_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; + Goals goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; + goals.push_back(goal); config_->blackboard->set("goal", goal); + config_->blackboard->set("goals", goals); // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update; + nav2_msgs::msg::PoseStampedArray goals_to_update; goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0); goal_to_update.pose.position.x = 2.0; + goals_to_update.header.stamp = goal_to_update.header.stamp; + goals_to_update.poses.push_back(goal_to_update); goal_updater_pub->publish(goal_to_update); + goals_updater_pub->publish(goals_to_update); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; + Goals updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); + EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); // expect to succeed and not update goal EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(updated_goal, goal); + EXPECT_EQ(updated_goals, goals); } TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) @@ -155,7 +171,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) R"( - + @@ -164,32 +180,48 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); auto goal_updater_pub = node_->create_publisher("goal_update", 10); + auto goals_updater_pub = + node_->create_publisher("goals_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; + Goals goals; goal.header.stamp = node_->now(); goal.pose.position.x = 1.0; + goals.push_back(goal); config_->blackboard->set("goal", goal); + config_->blackboard->set("goals", goals); // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update_1; + nav2_msgs::msg::PoseStampedArray goals_to_update_1; goal_to_update_1.header.stamp = node_->now(); goal_to_update_1.pose.position.x = 2.0; + goals_to_update_1.header.stamp = goal_to_update_1.header.stamp; + goals_to_update_1.poses.push_back(goal_to_update_1); geometry_msgs::msg::PoseStamped goal_to_update_2; + nav2_msgs::msg::PoseStampedArray goals_to_update_2; goal_to_update_2.header.stamp = node_->now(); goal_to_update_2.pose.position.x = 3.0; + goals_to_update_2.header.stamp = goal_to_update_2.header.stamp; + goals_to_update_2.poses.push_back(goal_to_update_2); goal_updater_pub->publish(goal_to_update_1); + goals_updater_pub->publish(goals_to_update_1); goal_updater_pub->publish(goal_to_update_2); + goals_updater_pub->publish(goals_to_update_2); tree_->rootNode()->executeTick(); geometry_msgs::msg::PoseStamped updated_goal; + Goals updated_goals; EXPECT_TRUE(config_->blackboard->get("updated_goal", updated_goal)); + EXPECT_TRUE(config_->blackboard->get("updated_goals", updated_goals)); // expect to succeed EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); // expect to update goal with latest goal update EXPECT_EQ(updated_goal, goal_to_update_2); + EXPECT_EQ(updated_goals, goals_to_update_2.poses); } int main(int argc, char ** argv) From 4cd7ebf5f5d282c6bd07bf1aaba4f62f89b791d5 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 5 Dec 2024 14:13:14 +0100 Subject: [PATCH 22/46] fix Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.cpp | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 2df8fb4c4da..a2f0c300ad4 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -71,21 +71,19 @@ inline BT::NodeStatus GoalUpdater::tick() callback_group_executor_.spin_all(std::chrono::milliseconds(1)); callback_group_executor_.spin_all(std::chrono::milliseconds(49)); - if (last_goal_received_.header.stamp != rclcpp::Time(0)) { - auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); - auto goal_time = rclcpp::Time(goal.header.stamp); - if (last_goal_received_time > goal_time) { - goal = last_goal_received_; - } else { - RCLCPP_WARN( - node_->get_logger(), "The timestamp of the received goal (%f) is older than the " - "current goal (%f). Ignoring the received goal.", - last_goal_received_time.seconds(), goal_time.seconds()); - } + auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); + auto goal_time = rclcpp::Time(goal.header.stamp); + if (last_goal_received_time > goal_time) { + goal = last_goal_received_; + } else { + RCLCPP_WARN( + node_->get_logger(), "The timestamp of the received goal (%f) is older than the " + "current goal (%f). Ignoring the received goal.", + last_goal_received_time.seconds(), goal_time.seconds()); } setOutput("output_goal", goal); - if (last_goals_received_.header.stamp != rclcpp::Time(0) && !last_goals_received_.poses.empty()) { + if (!last_goals_received_.poses.empty()) { auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp); rclcpp::Time most_recent_goal_time = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type()); for (const auto & g : goals) { @@ -98,12 +96,12 @@ inline BT::NodeStatus GoalUpdater::tick() } else { RCLCPP_WARN( node_->get_logger(), "None of the received goals (most recent: %f) are more recent than the " - "current goals (oldest: %f). Ignoring the received goals.", + "current goals (oldest current goal: %f). Ignoring the received goals.", last_goals_received_time.seconds(), most_recent_goal_time.seconds()); } + setOutput("output_goals", goals); } - setOutput("output_goals", goals); return child_node_->executeTick(); } From 012a503b489cf33fd561a5b0d8b54249b14cc0f7 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 5 Dec 2024 14:16:01 +0100 Subject: [PATCH 23/46] Revert "fix" This reverts commit 8303cdc141f3d24887f36a36dce073c0ee2f75f6. Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.cpp | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index a2f0c300ad4..2df8fb4c4da 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -71,19 +71,21 @@ inline BT::NodeStatus GoalUpdater::tick() callback_group_executor_.spin_all(std::chrono::milliseconds(1)); callback_group_executor_.spin_all(std::chrono::milliseconds(49)); - auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); - auto goal_time = rclcpp::Time(goal.header.stamp); - if (last_goal_received_time > goal_time) { - goal = last_goal_received_; - } else { - RCLCPP_WARN( - node_->get_logger(), "The timestamp of the received goal (%f) is older than the " - "current goal (%f). Ignoring the received goal.", - last_goal_received_time.seconds(), goal_time.seconds()); + if (last_goal_received_.header.stamp != rclcpp::Time(0)) { + auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); + auto goal_time = rclcpp::Time(goal.header.stamp); + if (last_goal_received_time > goal_time) { + goal = last_goal_received_; + } else { + RCLCPP_WARN( + node_->get_logger(), "The timestamp of the received goal (%f) is older than the " + "current goal (%f). Ignoring the received goal.", + last_goal_received_time.seconds(), goal_time.seconds()); + } } setOutput("output_goal", goal); - if (!last_goals_received_.poses.empty()) { + if (last_goals_received_.header.stamp != rclcpp::Time(0) && !last_goals_received_.poses.empty()) { auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp); rclcpp::Time most_recent_goal_time = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type()); for (const auto & g : goals) { @@ -96,12 +98,12 @@ inline BT::NodeStatus GoalUpdater::tick() } else { RCLCPP_WARN( node_->get_logger(), "None of the received goals (most recent: %f) are more recent than the " - "current goals (oldest current goal: %f). Ignoring the received goals.", + "current goals (oldest: %f). Ignoring the received goals.", last_goals_received_time.seconds(), most_recent_goal_time.seconds()); } - setOutput("output_goals", goals); } + setOutput("output_goals", goals); return child_node_->executeTick(); } From 81fc608a409d8b5024398c55f56c1b7e4c39e655 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 5 Dec 2024 14:57:10 +0100 Subject: [PATCH 24/46] refactoring Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index 2df8fb4c4da..da15d3aa653 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -71,21 +71,31 @@ inline BT::NodeStatus GoalUpdater::tick() callback_group_executor_.spin_all(std::chrono::milliseconds(1)); callback_group_executor_.spin_all(std::chrono::milliseconds(49)); - if (last_goal_received_.header.stamp != rclcpp::Time(0)) { + if (last_goal_received_.header.stamp == rclcpp::Time(0)) { + // if the goal doesn't have a timestamp, we don't do any timestamp-checking and accept it + setOutput("output_goal", last_goal_received_); + } + else { auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); auto goal_time = rclcpp::Time(goal.header.stamp); if (last_goal_received_time > goal_time) { - goal = last_goal_received_; + setOutput("output_goal", last_goal_received_); } else { RCLCPP_WARN( node_->get_logger(), "The timestamp of the received goal (%f) is older than the " "current goal (%f). Ignoring the received goal.", last_goal_received_time.seconds(), goal_time.seconds()); + setOutput("output_goal", goal); } } - setOutput("output_goal", goal); - if (last_goals_received_.header.stamp != rclcpp::Time(0) && !last_goals_received_.poses.empty()) { + if (last_goals_received_.poses.empty()) { + setOutput("output_goals", goals); + } + else if (last_goals_received_.header.stamp == rclcpp::Time(0)) { + setOutput("output_goals", last_goals_received_.poses); + } + else { auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp); rclcpp::Time most_recent_goal_time = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type()); for (const auto & g : goals) { @@ -94,16 +104,16 @@ inline BT::NodeStatus GoalUpdater::tick() } } if (last_goals_received_time > most_recent_goal_time) { - goals = last_goals_received_.poses; + setOutput("output_goals", last_goals_received_.poses); } else { RCLCPP_WARN( node_->get_logger(), "None of the received goals (most recent: %f) are more recent than the " "current goals (oldest: %f). Ignoring the received goals.", last_goals_received_time.seconds(), most_recent_goal_time.seconds()); + setOutput("output_goals", goals); } } - setOutput("output_goals", goals); return child_node_->executeTick(); } From 799045d03a04d7dacb75ab70517e8d474d885112 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 5 Dec 2024 14:58:35 +0100 Subject: [PATCH 25/46] ament Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index da15d3aa653..dc41131be4a 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -74,8 +74,7 @@ inline BT::NodeStatus GoalUpdater::tick() if (last_goal_received_.header.stamp == rclcpp::Time(0)) { // if the goal doesn't have a timestamp, we don't do any timestamp-checking and accept it setOutput("output_goal", last_goal_received_); - } - else { + } else { auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); auto goal_time = rclcpp::Time(goal.header.stamp); if (last_goal_received_time > goal_time) { @@ -91,13 +90,11 @@ inline BT::NodeStatus GoalUpdater::tick() if (last_goals_received_.poses.empty()) { setOutput("output_goals", goals); - } - else if (last_goals_received_.header.stamp == rclcpp::Time(0)) { + } else if (last_goals_received_.header.stamp == rclcpp::Time(0)) { setOutput("output_goals", last_goals_received_.poses); - } - else { + } else { auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp); - rclcpp::Time most_recent_goal_time = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type()); + rclcpp::Time most_recent_goal_time = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type()); for (const auto & g : goals) { if (rclcpp::Time(g.header.stamp) > most_recent_goal_time) { most_recent_goal_time = rclcpp::Time(g.header.stamp); @@ -107,7 +104,8 @@ inline BT::NodeStatus GoalUpdater::tick() setOutput("output_goals", last_goals_received_.poses); } else { RCLCPP_WARN( - node_->get_logger(), "None of the received goals (most recent: %f) are more recent than the " + node_->get_logger(), + "None of the received goals (most recent: %f) are more recent than the " "current goals (oldest: %f). Ignoring the received goals.", last_goals_received_time.seconds(), most_recent_goal_time.seconds()); setOutput("output_goals", goals); From c657a9f0f140175c58daec38316d3c9067592eab Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 9 Dec 2024 09:43:36 +0100 Subject: [PATCH 26/46] ignore if no timestamps Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index dc41131be4a..fcd63eb5dad 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -72,8 +72,10 @@ inline BT::NodeStatus GoalUpdater::tick() callback_group_executor_.spin_all(std::chrono::milliseconds(49)); if (last_goal_received_.header.stamp == rclcpp::Time(0)) { - // if the goal doesn't have a timestamp, we don't do any timestamp-checking and accept it - setOutput("output_goal", last_goal_received_); + // if the goal doesn't have a timestamp, we reject it + RCLCPP_WARN( + node_->get_logger(), "The received goal has no timestamp. Ignoring."); + setOutput("output_goal", goal); } else { auto last_goal_received_time = rclcpp::Time(last_goal_received_.header.stamp); auto goal_time = rclcpp::Time(goal.header.stamp); @@ -101,7 +103,9 @@ inline BT::NodeStatus GoalUpdater::tick() } } if (last_goals_received_time > most_recent_goal_time) { - setOutput("output_goals", last_goals_received_.poses); + RCLCPP_WARN( + node_->get_logger(), "The received goals array has no timestamp. Ignoring."); + setOutput("output_goals", goals); } else { RCLCPP_WARN( node_->get_logger(), From f5bc15a377ef0ec5f0aba234866562e45c5fecbb Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 9 Dec 2024 10:26:40 +0100 Subject: [PATCH 27/46] facepalm Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index fcd63eb5dad..cd5a7a03049 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -93,7 +93,9 @@ inline BT::NodeStatus GoalUpdater::tick() if (last_goals_received_.poses.empty()) { setOutput("output_goals", goals); } else if (last_goals_received_.header.stamp == rclcpp::Time(0)) { - setOutput("output_goals", last_goals_received_.poses); + RCLCPP_WARN( + node_->get_logger(), "The received goals array has no timestamp. Ignoring."); + setOutput("output_goals", goals); } else { auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp); rclcpp::Time most_recent_goal_time = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type()); @@ -103,9 +105,7 @@ inline BT::NodeStatus GoalUpdater::tick() } } if (last_goals_received_time > most_recent_goal_time) { - RCLCPP_WARN( - node_->get_logger(), "The received goals array has no timestamp. Ignoring."); - setOutput("output_goals", goals); + setOutput("output_goals", last_goals_received_.poses); } else { RCLCPP_WARN( node_->get_logger(), From 9d2f2b13a114be8742d2fa592efccef27bd3209d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 11 Dec 2024 10:20:31 +0100 Subject: [PATCH 28/46] update groot nodes Signed-off-by: Tony Najjar --- nav2_behavior_tree/nav2_tree_nodes.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 71c20ee4ecc..b04b35a3f41 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -320,7 +320,9 @@ Original goal in + Original goals in Output goal set by subscription + Output goals set by subscription From bdef481c1cabefafcbc60e6d33484f4ae2cc6ca5 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 11 Dec 2024 11:41:51 +0100 Subject: [PATCH 29/46] Use PoseStampedArray Signed-off-by: Tony Najjar --- .../include/nav2_behavior_tree/bt_utils.hpp | 16 +++--- .../compute_path_through_poses_action.hpp | 2 +- .../action/navigate_through_poses_action.hpp | 5 +- .../remove_in_collision_goals_action.hpp | 12 ++-- .../action/remove_passed_goals_action.hpp | 10 ++-- .../globally_updated_goal_condition.hpp | 6 +- .../condition/goal_updated_condition.hpp | 6 +- .../decorator/goal_updated_controller.hpp | 4 +- .../plugins/decorator/speed_controller.hpp | 4 +- .../remove_in_collision_goals_action.cpp | 12 ++-- .../action/remove_passed_goals_action.cpp | 12 ++-- .../globally_updated_goal_condition.cpp | 2 +- .../condition/goal_updated_condition.cpp | 2 +- .../decorator/goal_updated_controller.cpp | 2 +- .../plugins/decorator/speed_controller.cpp | 2 +- ...test_compute_path_through_poses_action.cpp | 26 ++++----- .../action/test_get_pose_from_path_action.cpp | 12 ++-- .../test_navigate_through_poses_action.cpp | 10 ++-- .../test_remove_in_collision_goals_action.cpp | 30 +++++----- .../test_remove_passed_goals_action.cpp | 28 +++++----- .../test_goal_updated_controller.cpp | 8 +-- .../decorator/test_speed_controller.cpp | 2 +- nav2_behavior_tree/test/test_bt_utils.cpp | 56 +++++++++---------- .../navigators/navigate_through_poses.hpp | 3 +- .../src/navigators/navigate_through_poses.cpp | 18 +++--- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- .../test/unit/costmap_cost_service_test.cpp | 4 +- nav2_msgs/CMakeLists.txt | 1 + .../action/ComputePathThroughPoses.action | 2 +- nav2_msgs/action/FollowWaypoints.action | 2 +- nav2_msgs/action/NavigateThroughPoses.action | 2 +- nav2_msgs/msg/PoseStampedArray.msg | 4 ++ nav2_msgs/srv/GetCosts.srv | 2 +- 33 files changed, 157 insertions(+), 152 deletions(-) create mode 100644 nav2_msgs/msg/PoseStampedArray.msg diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index 91e3e241b91..f505733588b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -24,7 +24,7 @@ #include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav_msgs/msg/path.hpp" namespace BT @@ -105,19 +105,19 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) } /** - * @brief Parse XML string to std::vector + * @brief Parse XML string to nav2_msgs::msg::PoseStampedArray * @param key XML string - * @return std::vector + * @return nav2_msgs::msg::PoseStampedArray */ template<> -inline std::vector convertFromString(const StringView key) +inline nav2_msgs::msg::PoseStampedArray convertFromString(const StringView key) { // 9 real numbers separated by semicolons auto parts = BT::splitString(key, ';'); if (parts.size() % 9 != 0) { - throw std::runtime_error("invalid number of fields for std::vector attribute)"); + throw std::runtime_error("invalid number of fields for PoseStampedArray attribute)"); } else { - std::vector poses; + nav2_msgs::msg::PoseStampedArray pose_stamped_array; for (size_t i = 0; i < parts.size(); i += 9) { geometry_msgs::msg::PoseStamped pose_stamped; pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString(parts[i])); @@ -129,9 +129,9 @@ inline std::vector convertFromString(const Stri pose_stamped.pose.orientation.y = BT::convertFromString(parts[i + 6]); pose_stamped.pose.orientation.z = BT::convertFromString(parts[i + 7]); pose_stamped.pose.orientation.w = BT::convertFromString(parts[i + 8]); - poses.push_back(pose_stamped); + pose_stamped_array.poses.push_back(pose_stamped); } - return poses; + return pose_stamped_array; } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index fe5d2e9dd25..7781de4d3b4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -75,7 +75,7 @@ class ComputePathThroughPosesAction { return providedBasicPorts( { - BT::InputPort>( + BT::InputPort( "goals", "Destinations to plan through"), BT::InputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index 44ad055ea24..2b2676c9392 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -18,8 +18,7 @@ #include #include -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/quaternion.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -74,7 +73,7 @@ class NavigateThroughPosesAction : public BtActionNode>( + BT::InputPort( "goals", "Destinations to plan through"), BT::InputPort("behavior_tree", "Behavior tree to run"), BT::OutputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp index 8992b4d516a..9e3e634498a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_service_node.hpp" #include "nav2_msgs/srv/get_costs.hpp" @@ -30,8 +30,6 @@ namespace nav2_behavior_tree class RemoveInCollisionGoals : public BtServiceNode { public: - typedef std::vector Goals; - /** * @brief A constructor for nav2_behavior_tree::RemoveInCollisionGoals * @param service_node_name Service name this node creates a client for @@ -54,7 +52,8 @@ class RemoveInCollisionGoals : public BtServiceNode { return providedBasicPorts( { - BT::InputPort("input_goals", "Original goals to remove from"), + BT::InputPort("input_goals", + "Original goals to remove from"), BT::InputPort( "cost_threshold", 254.0, "Cost threshold for considering a goal in collision"), @@ -62,7 +61,8 @@ class RemoveInCollisionGoals : public BtServiceNode BT::InputPort( "consider_unknown_as_obstacle", false, "Whether to consider unknown cost as obstacle"), - BT::OutputPort("output_goals", "Goals with in-collision goals removed"), + BT::OutputPort("output_goals", + "Goals with in-collision goals removed"), }); } @@ -70,7 +70,7 @@ class RemoveInCollisionGoals : public BtServiceNode bool use_footprint_; bool consider_unknown_as_obstacle_; double cost_threshold_; - Goals input_goals_; + nav2_msgs::msg::PoseStampedArray input_goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index fcfade6d17b..ab97450de91 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -19,7 +19,7 @@ #include #include -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "behaviortree_cpp/action_node.h" @@ -31,8 +31,6 @@ namespace nav2_behavior_tree class RemovePassedGoals : public BT::ActionNodeBase { public: - typedef std::vector Goals; - RemovePassedGoals( const std::string & xml_tag_name, const BT::NodeConfiguration & conf); @@ -45,8 +43,10 @@ class RemovePassedGoals : public BT::ActionNodeBase static BT::PortsList providedPorts() { return { - BT::InputPort("input_goals", "Original goals to remove viapoints from"), - BT::OutputPort("output_goals", "Goals with passed viapoints removed"), + BT::InputPort("input_goals", + "Original goals to remove viapoints from"), + BT::OutputPort("output_goals", + "Goals with passed viapoints removed"), BT::InputPort("radius", 0.5, "radius to goal for it to be considered for removal"), BT::InputPort("global_frame", "Global frame"), BT::InputPort("robot_base_frame", "Robot base frame"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index 12344a5d3f7..a884f84c805 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -59,7 +59,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort>( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -70,7 +70,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode bool first_time; rclcpp::Node::SharedPtr node_; geometry_msgs::msg::PoseStamped goal_; - std::vector goals_; + nav2_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index 22893e33ee3..b609abb5b30 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -19,7 +19,7 @@ #include #include "behaviortree_cpp/condition_node.h" -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree @@ -56,7 +56,7 @@ class GoalUpdatedCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort>( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -65,7 +65,7 @@ class GoalUpdatedCondition : public BT::ConditionNode private: geometry_msgs::msg::PoseStamped goal_; - std::vector goals_; + nav2_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp index 63a7f606558..6c469b4760b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -50,7 +50,7 @@ class GoalUpdatedController : public BT::DecoratorNode static BT::PortsList providedPorts() { return { - BT::InputPort>( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -66,7 +66,7 @@ class GoalUpdatedController : public BT::DecoratorNode bool goal_was_updated_; geometry_msgs::msg::PoseStamped goal_; - std::vector goals_; + nav2_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 59a9fc55210..0664280f7b2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -58,7 +58,7 @@ class SpeedController : public BT::DecoratorNode BT::InputPort("max_rate", 1.0, "Maximum rate"), BT::InputPort("min_speed", 0.0, "Minimum speed"), BT::InputPort("max_speed", 0.5, "Maximum speed"), - BT::InputPort>( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -120,7 +120,7 @@ class SpeedController : public BT::DecoratorNode // current goal geometry_msgs::msg::PoseStamped goal_; - std::vector goals_; + nav2_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index d6f9ee662d0..074eed93309 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -39,7 +39,7 @@ void RemoveInCollisionGoals::on_tick() getInput("input_goals", input_goals_); getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_); - if (input_goals_.empty()) { + if (input_goals_.poses.empty()) { setOutput("output_goals", input_goals_); should_send_request_ = false; return; @@ -47,24 +47,24 @@ void RemoveInCollisionGoals::on_tick() request_ = std::make_shared(); request_->use_footprint = use_footprint_; - for (const auto & goal : input_goals_) { - request_->poses.push_back(goal); + for (const auto & goal : input_goals_.poses) { + request_->poses.poses.push_back(goal); } } BT::NodeStatus RemoveInCollisionGoals::on_completion( std::shared_ptr response) { - Goals valid_goal_poses; + nav2_msgs::msg::PoseStampedArray valid_goal_poses; for (size_t i = 0; i < response->costs.size(); ++i) { if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) || response->costs[i] < cost_threshold_) { - valid_goal_poses.push_back(input_goals_[i]); + valid_goal_poses.poses.push_back(input_goals_.poses[i]); } } // Inform if all goals have been removed - if (valid_goal_poses.empty()) { + if (valid_goal_poses.poses.empty()) { RCLCPP_INFO( node_->get_logger(), "All goals are in collision and have been removed from the list"); diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index 723e704e437..d027533df07 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -49,10 +49,10 @@ inline BT::NodeStatus RemovePassedGoals::tick() initialize(); } - Goals goal_poses; + nav2_msgs::msg::PoseStampedArray goal_poses; getInput("input_goals", goal_poses); - if (goal_poses.empty()) { + if (goal_poses.poses.empty()) { setOutput("output_goals", goal_poses); return BT::NodeStatus::SUCCESS; } @@ -61,21 +61,21 @@ inline BT::NodeStatus RemovePassedGoals::tick() geometry_msgs::msg::PoseStamped current_pose; if (!nav2_util::getCurrentPose( - current_pose, *tf_, goal_poses[0].header.frame_id, robot_base_frame_, + current_pose, *tf_, goal_poses.poses[0].header.frame_id, robot_base_frame_, transform_tolerance_)) { return BT::NodeStatus::FAILURE; } double dist_to_goal; - while (goal_poses.size() > 1) { - dist_to_goal = euclidean_distance(goal_poses[0].pose, current_pose.pose); + while (goal_poses.poses.size() > 1) { + dist_to_goal = euclidean_distance(goal_poses.poses[0].pose, current_pose.pose); if (dist_to_goal > viapoint_achieved_radius_) { break; } - goal_poses.erase(goal_poses.begin()); + goal_poses.poses.erase(goal_poses.poses.begin()); } setOutput("output_goals", goal_poses); diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp index 1e1e557e5f3..cc71e8007e2 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -38,7 +38,7 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() return BT::NodeStatus::SUCCESS; } - std::vector current_goals; + nav2_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index 962eef70dd2..2e312912eaa 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -33,7 +33,7 @@ BT::NodeStatus GoalUpdatedCondition::tick() return BT::NodeStatus::FAILURE; } - std::vector current_goals; + nav2_msgs::msg::PoseStampedArray current_goals; geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goals", current_goals); BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp index b2f235dbd1f..5dbd5f6cb86 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -44,7 +44,7 @@ BT::NodeStatus GoalUpdatedController::tick() setStatus(BT::NodeStatus::RUNNING); - std::vector current_goals; + nav2_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index f5b4eb3bd86..61898dd2227 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -66,7 +66,7 @@ inline BT::NodeStatus SpeedController::tick() first_tick_ = true; } - std::vector current_goals; + nav2_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 02e7c865d8d..2c4a208ead2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -44,7 +44,7 @@ class ComputePathThroughPosesActionServer const auto goal = goal_handle->get_goal(); auto result = std::make_shared(); result->path.poses.resize(2); - result->path.poses[1].pose.position.x = goal->goals[0].pose.position.x; + result->path.poses[1].pose.position.x = goal->goals.poses[0].pose.position.x; if (goal->use_start) { result->path.poses[0].pose.position.x = goal->start.pose.position.x; } else { @@ -137,9 +137,9 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - std::vector goals; - goals.resize(1); - goals[0].pose.position.x = 1.0; + nav2_msgs::msg::PoseStampedArray goals; + goals.poses.resize(1); + goals.poses[0].pose.position.x = 1.0; config_->blackboard->set("goals", goals); // tick until node succeeds @@ -150,7 +150,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); - EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, 1.0); EXPECT_FALSE(action_server_->getCurrentGoal()->use_start); EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); @@ -166,7 +166,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal - goals[0].pose.position.x = -2.5; + goals.poses[0].pose.position.x = -2.5; config_->blackboard->set("goals", goals); while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { @@ -174,7 +174,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) } EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, -2.5); EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); @@ -202,9 +202,9 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) config_->blackboard->set("start", start); // create new goal and set it on blackboard - std::vector goals; - goals.resize(1); - goals[0].pose.position.x = 1.0; + nav2_msgs::msg::PoseStampedArray goals; + goals.poses.resize(1); + goals.poses[0].pose.position.x = 1.0; config_->blackboard->set("goals", goals); // tick until node succeeds @@ -215,7 +215,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); - EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, 1.0); EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0); EXPECT_TRUE(action_server_->getCurrentGoal()->use_start); EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); @@ -232,7 +232,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); // set new goal and new start - goals[0].pose.position.x = -2.5; + goals.poses[0].pose.position.x = -2.5; start.pose.position.x = -1.5; config_->blackboard->set("goals", goals); config_->blackboard->set("start", start); @@ -242,7 +242,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) } EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(action_server_->getCurrentGoal()->goals[0].pose.position.x, -2.5); + EXPECT_EQ(action_server_->getCurrentGoal()->goals.poses[0].pose.position.x, -2.5); EXPECT_TRUE(config_->blackboard->get("path", path)); EXPECT_EQ(path.poses.size(), 2u); diff --git a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp index b296d706dc1..a2311eaf471 100644 --- a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp @@ -21,7 +21,7 @@ #include #include "nav_msgs/msg/path.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "behaviortree_cpp/bt_factory.h" @@ -97,11 +97,11 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) // create new path and set it on blackboard nav_msgs::msg::Path path; - std::vector goals; - goals.resize(2); - goals[0].pose.position.x = 1.0; - goals[1].pose.position.x = 2.0; - path.poses = goals; + nav2_msgs::msg::PoseStampedArray goals; + goals.poses.resize(2); + goals.poses[0].pose.position.x = 1.0; + goals.poses[1].pose.position.x = 2.0; + path.poses = goals.poses; path.header.frame_id = "test_frame_1"; config_->blackboard->set("path", path); diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index 9ffd637d66c..bfa1ffac707 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -74,7 +74,7 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test "wait_for_service_timeout", std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); - std::vector poses; + nav2_msgs::msg::PoseStampedArray poses; config_->blackboard->set( "goals", poses); @@ -132,10 +132,10 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - std::vector poses; - poses.resize(1); - poses[0].pose.position.x = -2.5; - poses[0].pose.orientation.x = 1.0; + nav2_msgs::msg::PoseStampedArray poses; + poses.poses.resize(1); + poses.poses[0].pose.position.x = -2.5; + poses.poses[0].pose.orientation.x = 1.0; config_->blackboard->set("goals", poses); // tick until node succeeds diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp index dbb4889298d..b91300f7985 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp @@ -125,19 +125,19 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - std::vector poses; - poses.resize(4); - poses[0].pose.position.x = 0.0; - poses[0].pose.position.y = 0.0; + nav2_msgs::msg::PoseStampedArray poses; + poses.poses.resize(4); + poses.poses[0].pose.position.x = 0.0; + poses.poses[0].pose.position.y = 0.0; - poses[1].pose.position.x = 0.5; - poses[1].pose.position.y = 0.0; + poses.poses[1].pose.position.x = 0.5; + poses.poses[1].pose.position.y = 0.0; - poses[2].pose.position.x = 1.0; - poses[2].pose.position.y = 0.0; + poses.poses[2].pose.position.x = 1.0; + poses.poses[2].pose.position.y = 0.0; - poses[3].pose.position.x = 2.0; - poses[3].pose.position.y = 0.0; + poses.poses[3].pose.position.x = 2.0; + poses.poses[3].pose.position.y = 0.0; config_->blackboard->set("goals", poses); @@ -147,13 +147,13 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) } // check that it removed the point in range - std::vector output_poses; + nav2_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.size(), 3u); - EXPECT_EQ(output_poses[0], poses[0]); - EXPECT_EQ(output_poses[1], poses[1]); - EXPECT_EQ(output_poses[2], poses[2]); + EXPECT_EQ(output_poses.poses.size(), 3u); + EXPECT_EQ(output_poses.poses[0], poses.poses[0]); + EXPECT_EQ(output_poses.poses[1], poses.poses[1]); + EXPECT_EQ(output_poses.poses[2], poses.poses[2]); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index 1ddc5c45f3c..5a07ac1659d 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -114,19 +114,19 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - std::vector poses; - poses.resize(4); - poses[0].pose.position.x = 0.0; - poses[0].pose.position.y = 0.0; + nav2_msgs::msg::PoseStampedArray poses; + poses.poses.resize(4); + poses.poses[0].pose.position.x = 0.0; + poses.poses[0].pose.position.y = 0.0; - poses[1].pose.position.x = 0.5; - poses[1].pose.position.y = 0.0; + poses.poses[1].pose.position.x = 0.5; + poses.poses[1].pose.position.y = 0.0; - poses[2].pose.position.x = 1.0; - poses[2].pose.position.y = 0.0; + poses.poses[2].pose.position.x = 1.0; + poses.poses[2].pose.position.y = 0.0; - poses[3].pose.position.x = 2.0; - poses[3].pose.position.y = 0.0; + poses.poses[3].pose.position.x = 2.0; + poses.poses[3].pose.position.y = 0.0; config_->blackboard->set("goals", poses); @@ -136,12 +136,12 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) } // check that it removed the point in range - std::vector output_poses; + nav2_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.size(), 2u); - EXPECT_EQ(output_poses[0], poses[2]); - EXPECT_EQ(output_poses[1], poses[3]); + EXPECT_EQ(output_poses.poses.size(), 2u); + EXPECT_EQ(output_poses.poses[0], poses.poses[2]); + EXPECT_EQ(output_poses.poses[1], poses.poses[3]); } int main(int argc, char ** argv) diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp index 4259e87a53e..7aa211f32a1 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -32,8 +32,8 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree // Setting fake goals on blackboard geometry_msgs::msg::PoseStamped goal1; goal1.header.stamp = node_->now(); - std::vector poses1; - poses1.push_back(goal1); + nav2_msgs::msg::PoseStampedArray poses1; + poses1.poses.push_back(goal1); config_->blackboard->set("goal", goal1); config_->blackboard->set("goals", poses1); bt_node_ = std::make_shared( @@ -63,8 +63,8 @@ TEST_F(GoalUpdatedControllerTestFixture, test_behavior) // Creating updated fake-goals geometry_msgs::msg::PoseStamped goal2; goal2.header.stamp = node_->now(); - std::vector poses2; - poses2.push_back(goal2); + nav2_msgs::msg::PoseStampedArray poses2; + poses2.poses.push_back(goal2); // starting in idle EXPECT_EQ(bt_node_->status(), BT::NodeStatus::IDLE); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index 903b6385ed3..76fab537b06 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp @@ -42,7 +42,7 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi goal.header.stamp = node_->now(); config_->blackboard->set("goal", goal); - std::vector fake_poses; + nav2_msgs::msg::PoseStampedArray fake_poses; config_->blackboard->set("goals", fake_poses); // NOLINT config_->input_ports["min_rate"] = 0.1; diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 008e0a36b77..4a0a37d97e1 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -194,68 +194,68 @@ TEST(PoseStampedPortTest, test_correct_syntax) EXPECT_EQ(value.pose.orientation.w, 7.0); } -TEST(PoseStampedVectorPortTest, test_wrong_syntax) +TEST(PoseStampedArrayPortTest, test_wrong_syntax) { std::string xml_txt = R"( - + )"; BT::BehaviorTreeFactory factory; - factory.registerNodeType>>( - "PoseStampedVectorPortTest"); + factory.registerNodeType>( + "PoseStampedArrayPortTest"); EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); xml_txt = R"( - + )"; EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); } -TEST(PoseStampedVectorPortTest, test_correct_syntax) +TEST(PoseStampedArrayPortTest, test_correct_syntax) { std::string xml_txt = R"( - + )"; BT::BehaviorTreeFactory factory; - factory.registerNodeType>>( - "PoseStampedVectorPortTest"); + factory.registerNodeType>( + "PoseStampedArrayPortTest"); auto tree = factory.createTreeFromText(xml_txt); tree = factory.createTreeFromText(xml_txt); - std::vector values; + nav2_msgs::msg::PoseStampedArray values; tree.rootNode()->getInput("test", values); - EXPECT_EQ(rclcpp::Time(values[0].header.stamp).nanoseconds(), 0); - EXPECT_EQ(values[0].header.frame_id, "map"); - EXPECT_EQ(values[0].pose.position.x, 1.0); - EXPECT_EQ(values[0].pose.position.y, 2.0); - EXPECT_EQ(values[0].pose.position.z, 3.0); - EXPECT_EQ(values[0].pose.orientation.x, 4.0); - EXPECT_EQ(values[0].pose.orientation.y, 5.0); - EXPECT_EQ(values[0].pose.orientation.z, 6.0); - EXPECT_EQ(values[0].pose.orientation.w, 7.0); - EXPECT_EQ(rclcpp::Time(values[1].header.stamp).nanoseconds(), 0); - EXPECT_EQ(values[1].header.frame_id, "odom"); - EXPECT_EQ(values[1].pose.position.x, 8.0); - EXPECT_EQ(values[1].pose.position.y, 9.0); - EXPECT_EQ(values[1].pose.position.z, 10.0); - EXPECT_EQ(values[1].pose.orientation.x, 11.0); - EXPECT_EQ(values[1].pose.orientation.y, 12.0); - EXPECT_EQ(values[1].pose.orientation.z, 13.0); - EXPECT_EQ(values[1].pose.orientation.w, 14.0); + EXPECT_EQ(rclcpp::Time(values.poses[0].header.stamp).nanoseconds(), 0); + EXPECT_EQ(values.poses[0].header.frame_id, "map"); + EXPECT_EQ(values.poses[0].pose.position.x, 1.0); + EXPECT_EQ(values.poses[0].pose.position.y, 2.0); + EXPECT_EQ(values.poses[0].pose.position.z, 3.0); + EXPECT_EQ(values.poses[0].pose.orientation.x, 4.0); + EXPECT_EQ(values.poses[0].pose.orientation.y, 5.0); + EXPECT_EQ(values.poses[0].pose.orientation.z, 6.0); + EXPECT_EQ(values.poses[0].pose.orientation.w, 7.0); + EXPECT_EQ(rclcpp::Time(values.poses[1].header.stamp).nanoseconds(), 0); + EXPECT_EQ(values.poses[1].header.frame_id, "odom"); + EXPECT_EQ(values.poses[1].pose.position.x, 8.0); + EXPECT_EQ(values.poses[1].pose.position.y, 9.0); + EXPECT_EQ(values.poses[1].pose.position.z, 10.0); + EXPECT_EQ(values.poses[1].pose.orientation.x, 11.0); + EXPECT_EQ(values.poses[1].pose.orientation.y, 12.0); + EXPECT_EQ(values.poses[1].pose.orientation.z, 13.0); + EXPECT_EQ(values.poses[1].pose.orientation.w, 14.0); } TEST(PathPortTest, test_wrong_syntax) diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index aee07de14c9..c13a5137d09 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_msgs/msg/pose_stamped_array.hpp" #include "nav2_core/behavior_tree_navigator.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav_msgs/msg/path.hpp" @@ -40,8 +41,6 @@ class NavigateThroughPosesNavigator { public: using ActionT = nav2_msgs::action::NavigateThroughPoses; - typedef std::vector Goals; - /** * @brief A constructor for NavigateThroughPosesNavigator */ diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 98d86499800..d2da0a3615b 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -102,10 +102,10 @@ NavigateThroughPosesNavigator::onLoop() auto blackboard = bt_action_server_->getBlackboard(); - Goals goal_poses; + nav2_msgs::msg::PoseStampedArray goal_poses; [[maybe_unused]] auto res = blackboard->get(goals_blackboard_id_, goal_poses); - if (goal_poses.size() == 0) { + if (goal_poses.poses.size() == 0) { bt_action_server_->publishFeedback(feedback_msg); return; } @@ -173,7 +173,7 @@ NavigateThroughPosesNavigator::onLoop() feedback_msg->number_of_recoveries = recovery_count; feedback_msg->current_pose = current_pose; feedback_msg->navigation_time = clock_->now() - start_time_; - feedback_msg->number_of_poses_remaining = goal_poses.size(); + feedback_msg->number_of_poses_remaining = goal_poses.poses.size(); bt_action_server_->publishFeedback(feedback_msg); } @@ -212,8 +212,8 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) bool NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal) { - Goals goal_poses = goal->poses; - for (auto & goal_pose : goal_poses) { + nav2_msgs::msg::PoseStampedArray pose_stamped_array = goal->poses; + for (auto & goal_pose : pose_stamped_array) { if (!nav2_util::transformPoseInTargetFrame( goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, feedback_utils_.transform_tolerance)) @@ -226,10 +226,11 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr } } - if (goal_poses.size() > 0) { + if (pose_stamped_array.poses.size() > 0) { RCLCPP_INFO( logger_, "Begin navigating from current location through %zu poses to (%.2f, %.2f)", - goal_poses.size(), goal_poses.back().pose.position.x, goal_poses.back().pose.position.y); + pose_stamped_array.poses.size(), pose_stamped_array.poses.back().pose.position.x, + pose_stamped_array.poses.back().pose.position.y); } // Reset state for new action feedback @@ -238,7 +239,8 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard->set(goals_blackboard_id_, std::move(goal_poses)); + blackboard->set(goals_blackboard_id_, + std::move(pose_stamped_array)); return true; } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 5413fd3d2c1..a9f27a00bf0 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -826,7 +826,7 @@ void Costmap2DROS::getCostsCallback( Costmap2D * costmap = layered_costmap_->getCostmap(); - for (const auto & pose : request->poses) { + for (const auto & pose : request->poses.poses) { geometry_msgs::msg::PoseStamped pose_transformed; transformPoseToGlobalFrame(pose, pose_transformed); bool in_bounds = costmap->worldToMap( diff --git a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp index 15780196b17..118479343a7 100644 --- a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp @@ -53,7 +53,7 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint) geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = 0.5; pose.pose.position.y = 1.0; - request->poses.push_back(pose); + request->poses.poses.push_back(pose); request->use_footprint = false; auto result_future = client_->async_send_request(request); @@ -78,7 +78,7 @@ TEST_F(GetCostServiceTest, TestWithFootprint) tf2::Quaternion q; q.setRPY(0, 0, 0.5); pose.pose.orientation = tf2::toMsg(q); - request->poses.push_back(pose); + request->poses.poses.push_back(pose); request->use_footprint = true; auto result_future = client_->async_send_request(request); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index a9f93b5ecbc..fbb6997083d 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,6 +30,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/MissedWaypoint.msg" + "msg/PoseStampedArray.msg" "srv/GetCosts.srv" "srv/GetCostmap.srv" "srv/IsPathValid.srv" diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index bedd9396797..962c0c2c9b8 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,5 +1,5 @@ #goal definition -geometry_msgs/PoseStamped[] goals +nav2_msgs/PoseStampedArray goals geometry_msgs/PoseStamped start string planner_id bool use_start # If false, use current robot pose as path start, if true, use start above instead diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index 9b5d8af05f4..33185936532 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -1,7 +1,7 @@ #goal definition uint32 number_of_loops uint32 goal_index 0 -geometry_msgs/PoseStamped[] poses +nav2_msgs/PoseStampedArray poses --- #result definition diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index 898e4540ef7..a7f145b746e 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -1,6 +1,6 @@ #goal definition -geometry_msgs/PoseStamped[] poses +nav2_msgs/PoseStampedArray poses string behavior_tree --- #result definition diff --git a/nav2_msgs/msg/PoseStampedArray.msg b/nav2_msgs/msg/PoseStampedArray.msg new file mode 100644 index 00000000000..1a4e04dc0d1 --- /dev/null +++ b/nav2_msgs/msg/PoseStampedArray.msg @@ -0,0 +1,4 @@ +# An array of stamped poses with a header for global reference. + +std_msgs/Header header +geometry_msgs/PoseStamped[] poses \ No newline at end of file diff --git a/nav2_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv index 55ebad4f0a2..43009661906 100644 --- a/nav2_msgs/srv/GetCosts.srv +++ b/nav2_msgs/srv/GetCosts.srv @@ -1,6 +1,6 @@ # Get costmap costs at given poses bool use_footprint -geometry_msgs/PoseStamped[] poses +nav2_msgs/PoseStampedArray poses --- float32[] costs \ No newline at end of file From 038f0513eca09165b57eb9277fb9b182d5bd6060 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 11 Dec 2024 11:51:35 +0100 Subject: [PATCH 30/46] fix Signed-off-by: Tony Najjar --- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 03435a913e4..333f040c947 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -102,7 +102,7 @@ void CostmapCostTool::callCostService(float x, float y) pose.header.stamp = node->now(); pose.pose.position.x = x; pose.pose.position.y = y; - request->poses.push_back(pose); + request->poses.poses.push_back(pose); request->use_footprint = false; // Call local costmap service From 622e6e106e61b38e6db171d53ddbda567e263e6b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 11 Dec 2024 12:58:19 +0100 Subject: [PATCH 31/46] fixes Signed-off-by: Tony Najjar --- .../test_remove_in_collision_goals_action.cpp | 32 ++++---- .../include/nav2_rviz_plugins/nav2_panel.hpp | 6 +- nav2_rviz_plugins/src/nav2_panel.cpp | 75 ++++++++++--------- 3 files changed, 57 insertions(+), 56 deletions(-) diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp index a1038ead686..2db2a7250b8 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp @@ -194,19 +194,19 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fa tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - std::vector poses; - poses.resize(4); - poses[0].pose.position.x = 0.0; - poses[0].pose.position.y = 0.0; + nav2_msgs::msg::PoseStampedArray poses; + poses.poses.resize(4); + poses.poses[0].pose.position.x = 0.0; + poses.poses[0].pose.position.y = 0.0; - poses[1].pose.position.x = 0.5; - poses[1].pose.position.y = 0.0; + poses.poses[1].pose.position.x = 0.5; + poses.poses[1].pose.position.y = 0.0; - poses[2].pose.position.x = 1.0; - poses[2].pose.position.y = 0.0; + poses.poses[2].pose.position.x = 1.0; + poses.poses[2].pose.position.y = 0.0; - poses[3].pose.position.x = 2.0; - poses[3].pose.position.y = 0.0; + poses.poses[3].pose.position.x = 2.0; + poses.poses[3].pose.position.y = 0.0; config_->blackboard->set("goals", poses); @@ -218,14 +218,14 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fa // check that it failed and returned the original goals EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); - std::vector output_poses; + nav2_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); - EXPECT_EQ(output_poses.size(), 4u); - EXPECT_EQ(output_poses[0], poses[0]); - EXPECT_EQ(output_poses[1], poses[1]); - EXPECT_EQ(output_poses[2], poses[2]); - EXPECT_EQ(output_poses[3], poses[3]); + EXPECT_EQ(output_poses.poses.size(), 4u); + EXPECT_EQ(output_poses.poses[0], poses.poses[0]); + EXPECT_EQ(output_poses.poses[1], poses.poses[1]); + EXPECT_EQ(output_poses.poses[2], poses.poses[2]); + EXPECT_EQ(output_poses.poses[3], poses.poses[3]); } int main(int argc, char ** argv) diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index 471dce5ce5a..c7aac8ba645 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -102,7 +102,7 @@ private Q_SLOTS: std::vector orientation); void startWaypointFollowing(std::vector poses); void startNavigation(geometry_msgs::msg::PoseStamped); - void startNavThroughPoses(std::vector poses); + void startNavThroughPoses(nav2_msgs::msg::PoseStampedArray poses); using NavigationGoalHandle = rclcpp_action::ClientGoalHandle; using WaypointFollowerGoalHandle = @@ -196,8 +196,8 @@ private Q_SLOTS: QState * accumulated_wp_{nullptr}; QState * accumulated_nav_through_poses_{nullptr}; - std::vector acummulated_poses_; - std::vector store_poses_; + nav2_msgs::msg::PoseStampedArray acummulated_poses_; + nav2_msgs::msg::PoseStampedArray store_poses_; // Publish the visual markers with the waypoints void updateWpNavigationMarkers(); diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index e10f1eccdb4..2a0018a8851 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -675,7 +675,7 @@ void Nav2Panel::loophandler() void Nav2Panel::handleGoalLoader() { - acummulated_poses_.clear(); + acummulated_poses_ = nav2_msgs::msg::PoseStampedArray(); std::cout << "Loading Waypoints!" << std::endl; @@ -699,7 +699,7 @@ void Nav2Panel::handleGoalLoader() auto waypoint = waypoint_iter[it->first.as()]; auto pose = waypoint["pose"].as>(); auto orientation = waypoint["orientation"].as>(); - acummulated_poses_.push_back(convert_to_msg(pose, orientation)); + acummulated_poses_.poses.push_back(convert_to_msg(pose, orientation)); } // Publishing Waypoint Navigation marker after loading wp's @@ -731,7 +731,7 @@ void Nav2Panel::handleGoalSaver() { // Check if the waypoints are accumulated - if (acummulated_poses_.empty()) { + if (acummulated_poses_.poses.empty()) { std::cout << "No accumulated Points to Save!" << std::endl; return; } else { @@ -744,18 +744,19 @@ void Nav2Panel::handleGoalSaver() out << YAML::BeginMap; // Save WPs to data structure - for (unsigned int i = 0; i < acummulated_poses_.size(); ++i) { + for (unsigned int i = 0; i < acummulated_poses_.poses.size(); ++i) { out << YAML::Key << "waypoint" + std::to_string(i); out << YAML::BeginMap; out << YAML::Key << "pose"; std::vector pose = - {acummulated_poses_[i].pose.position.x, acummulated_poses_[i].pose.position.y, - acummulated_poses_[i].pose.position.z}; + {acummulated_poses_.poses[i].pose.position.x, acummulated_poses_.poses[i].pose.position.y, + acummulated_poses_.poses[i].pose.position.z}; out << YAML::Value << pose; out << YAML::Key << "orientation"; std::vector orientation = - {acummulated_poses_[i].pose.orientation.w, acummulated_poses_[i].pose.orientation.x, - acummulated_poses_[i].pose.orientation.y, acummulated_poses_[i].pose.orientation.z}; + {acummulated_poses_.poses[i].pose.orientation.w, acummulated_poses_.poses[i].pose.orientation.x, + acummulated_poses_.poses[i].pose.orientation.y, + acummulated_poses_.poses[i].pose.orientation.z}; out << YAML::Value << orientation; out << YAML::EndMap; } @@ -836,10 +837,10 @@ Nav2Panel::onInitialize() // Clearing all the stored values once reaching the final goal if ( loop_count_ == stoi(nr_of_loops_->displayText().toStdString()) && - goal_index_ == static_cast(store_poses_.size()) - 1 && + goal_index_ == static_cast(store_poses_.poses.size()) - 1 && msg->status_list.back().status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) { - store_poses_.clear(); + store_poses_ = nav2_msgs::msg::PoseStampedArray(); waypoint_status_indicator_->clear(); loop_no_ = "0"; loop_count_ = 0; @@ -935,8 +936,8 @@ Nav2Panel::onCancel() &Nav2Panel::onCancelButtonPressed, this)); waypoint_status_indicator_->clear(); - store_poses_.clear(); - acummulated_poses_.clear(); + store_poses_ = nav2_msgs::msg::PoseStampedArray(); + acummulated_poses_ = nav2_msgs::msg::PoseStampedArray(); } void Nav2Panel::onResumedWp() @@ -966,12 +967,12 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame) pose.pose.position.z = 0.0; pose.pose.orientation = orientationAroundZAxis(theta); - if (store_poses_.empty()) { + if (store_poses_.poses.empty()) { if (state_machine_.configuration().contains(accumulating_)) { waypoint_status_indicator_->clear(); - acummulated_poses_.push_back(pose); + acummulated_poses_.poses.push_back(pose); } else { - acummulated_poses_.clear(); + acummulated_poses_ = nav2_msgs::msg::PoseStampedArray(); updateWpNavigationMarkers(); std::cout << "Start navigation" << std::endl; startNavigation(pose); @@ -1030,7 +1031,7 @@ Nav2Panel::onCancelButtonPressed() void Nav2Panel::onAccumulatedWp() { - if (acummulated_poses_.empty()) { + if (acummulated_poses_.poses.empty()) { state_machine_.postEvent(new ROSActionQEvent(QActionState::INACTIVE)); waypoint_status_indicator_->setText( " Note: Uh oh! Someone forgot to select the waypoints"); @@ -1052,7 +1053,7 @@ Nav2Panel::onAccumulatedWp() /** Making sure that the pose array does not get updated * between the process**/ - if (store_poses_.empty()) { + if (store_poses_.poses.empty()) { std::cout << "Start waypoint" << std::endl; // Setting the final loop value on the text box for sanity @@ -1065,12 +1066,12 @@ Nav2Panel::onAccumulatedWp() if (store_initial_pose_) { try { init_transform = tf2_buffer_->lookupTransform( - acummulated_poses_[0].header.frame_id, base_frame_, + acummulated_poses_.poses[0].header.frame_id, base_frame_, tf2::TimePointZero); } catch (const tf2::TransformException & ex) { RCLCPP_INFO( client_node_->get_logger(), "Could not transform %s to %s: %s", - acummulated_poses_[0].header.frame_id.c_str(), base_frame_.c_str(), ex.what()); + acummulated_poses_.poses[0].header.frame_id.c_str(), base_frame_.c_str(), ex.what()); return; } @@ -1086,24 +1087,24 @@ Nav2Panel::onAccumulatedWp() initial_pose.pose.orientation.w = init_transform.transform.rotation.w; // inserting the acummulated pose - acummulated_poses_.insert(acummulated_poses_.begin(), initial_pose); + acummulated_poses_.poses.insert(acummulated_poses_.poses.begin(), initial_pose); updateWpNavigationMarkers(); initial_pose_stored_ = true; if (loop_count_ == 0) { goal_index_ = 1; } } else if (!store_initial_pose_ && initial_pose_stored_) { - acummulated_poses_.erase( - acummulated_poses_.begin(), - acummulated_poses_.begin()); + acummulated_poses_.poses.erase( + acummulated_poses_.poses.begin(), + acummulated_poses_.poses.begin()); } } else { std::cout << "Resuming waypoint" << std::endl; } - startWaypointFollowing(acummulated_poses_); + startWaypointFollowing(acummulated_poses_.poses); store_poses_ = acummulated_poses_; - acummulated_poses_.clear(); + acummulated_poses_ = nav2_msgs::msg::PoseStampedArray(); } void @@ -1116,8 +1117,8 @@ Nav2Panel::onAccumulatedNTP() void Nav2Panel::onAccumulating() { - acummulated_poses_.clear(); - store_poses_.clear(); + acummulated_poses_ = nav2_msgs::msg::PoseStampedArray(); + store_poses_ = nav2_msgs::msg::PoseStampedArray(); loop_count_ = 0; loop_no_ = "0"; initial_pose_stored_ = false; @@ -1254,7 +1255,7 @@ Nav2Panel::startWaypointFollowing(std::vector p } void -Nav2Panel::startNavThroughPoses(std::vector poses) +Nav2Panel::startNavThroughPoses(nav2_msgs::msg::PoseStampedArray poses) { auto is_action_server_ready = nav_through_poses_action_client_->wait_for_action_server(std::chrono::seconds(5)); @@ -1272,8 +1273,8 @@ Nav2Panel::startNavThroughPoses(std::vector pos RCLCPP_DEBUG( client_node_->get_logger(), "Sending a path of %zu waypoints:", - nav_through_poses_goal_.poses.size()); - for (auto waypoint : nav_through_poses_goal_.poses) { + nav_through_poses_goal_.poses.poses.size()); + for (auto waypoint : nav_through_poses_goal_.poses.poses) { RCLCPP_DEBUG( client_node_->get_logger(), "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); @@ -1384,14 +1385,14 @@ Nav2Panel::updateWpNavigationMarkers() auto marker_array = std::make_unique(); - for (size_t i = 0; i < acummulated_poses_.size(); i++) { + for (size_t i = 0; i < acummulated_poses_.poses.size(); i++) { // Draw a green arrow at the waypoint pose visualization_msgs::msg::Marker arrow_marker; - arrow_marker.header = acummulated_poses_[i].header; + arrow_marker.header = acummulated_poses_.poses[i].header; arrow_marker.id = getUniqueId(); arrow_marker.type = visualization_msgs::msg::Marker::ARROW; arrow_marker.action = visualization_msgs::msg::Marker::ADD; - arrow_marker.pose = acummulated_poses_[i].pose; + arrow_marker.pose = acummulated_poses_.poses[i].pose; arrow_marker.scale.x = 0.3; arrow_marker.scale.y = 0.05; arrow_marker.scale.z = 0.02; @@ -1405,11 +1406,11 @@ Nav2Panel::updateWpNavigationMarkers() // Draw a red circle at the waypoint pose visualization_msgs::msg::Marker circle_marker; - circle_marker.header = acummulated_poses_[i].header; + circle_marker.header = acummulated_poses_.poses[i].header; circle_marker.id = getUniqueId(); circle_marker.type = visualization_msgs::msg::Marker::SPHERE; circle_marker.action = visualization_msgs::msg::Marker::ADD; - circle_marker.pose = acummulated_poses_[i].pose; + circle_marker.pose = acummulated_poses_.poses[i].pose; circle_marker.scale.x = 0.05; circle_marker.scale.y = 0.05; circle_marker.scale.z = 0.05; @@ -1423,11 +1424,11 @@ Nav2Panel::updateWpNavigationMarkers() // Draw the waypoint number visualization_msgs::msg::Marker marker_text; - marker_text.header = acummulated_poses_[i].header; + marker_text.header = acummulated_poses_.poses[i].header; marker_text.id = getUniqueId(); marker_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker_text.action = visualization_msgs::msg::Marker::ADD; - marker_text.pose = acummulated_poses_[i].pose; + marker_text.pose = acummulated_poses_.poses[i].pose; marker_text.pose.position.z += 0.2; // draw it on top of the waypoint marker_text.scale.x = 0.07; marker_text.scale.y = 0.07; From 27d0965efe91e05528d95149dce2ab264052f52e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 11 Dec 2024 13:03:38 +0100 Subject: [PATCH 32/46] fix Signed-off-by: Tony Najjar --- nav2_msgs/action/FollowWaypoints.action | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index 33185936532..9b5d8af05f4 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -1,7 +1,7 @@ #goal definition uint32 number_of_loops uint32 goal_index 0 -nav2_msgs/PoseStampedArray poses +geometry_msgs/PoseStamped[] poses --- #result definition From 15c1396737a7daa5c4c5f82a63b0f076c183fd4a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 11 Dec 2024 13:19:17 +0100 Subject: [PATCH 33/46] fix Signed-off-by: Tony Najjar --- nav2_bt_navigator/src/navigators/navigate_through_poses.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index d2da0a3615b..8b8ee0962eb 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -213,7 +213,7 @@ bool NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal) { nav2_msgs::msg::PoseStampedArray pose_stamped_array = goal->poses; - for (auto & goal_pose : pose_stamped_array) { + for (auto & goal_pose : pose_stamped_array.poses) { if (!nav2_util::transformPoseInTargetFrame( goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, feedback_utils_.transform_tolerance)) From 0a5cc27acab7dbcee4b8ee2bf7e8d912c94a0870 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 11 Dec 2024 13:35:08 +0100 Subject: [PATCH 34/46] fix Signed-off-by: Tony Najjar --- nav2_planner/src/planner_server.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ccce9813544..03d92d49ed0 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -394,7 +394,7 @@ void PlannerServer::computePlanThroughPoses() getPreemptedGoalIfRequested(action_server_poses_, goal); - if (goal->goals.empty()) { + if (goal->goals.poses.empty()) { throw nav2_core::NoViapointsGiven("No viapoints given"); } @@ -409,7 +409,7 @@ void PlannerServer::computePlanThroughPoses() }; // Get consecutive paths through these points - for (unsigned int i = 0; i != goal->goals.size(); i++) { + for (unsigned int i = 0; i != goal->goals.poses.size(); i++) { // Get starting point if (i == 0) { curr_start = start; @@ -419,7 +419,7 @@ void PlannerServer::computePlanThroughPoses() curr_start = concat_path.poses.back(); curr_start.header = concat_path.header; } - curr_goal = goal->goals[i]; + curr_goal = goal->goals.poses[i]; // Transform them into the global frame if (!transformPosesToGlobalFrame(curr_start, curr_goal)) { From cc1648010348bed18a9740059b2a288caf4122ce Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 09:33:02 +0100 Subject: [PATCH 35/46] use geometry_msgs Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.hpp | 6 +++--- .../plugins/decorator/goal_updater_node.cpp | 4 ++-- .../test/plugins/decorator/test_goal_updater_node.cpp | 10 +++++----- nav2_msgs/CMakeLists.txt | 1 - nav2_msgs/msg/PoseStampedArray.msg | 4 ---- 5 files changed, 10 insertions(+), 15 deletions(-) delete mode 100644 nav2_msgs/msg/PoseStampedArray.msg diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 0552bc9203d..45fb893279a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -80,13 +80,13 @@ class GoalUpdater : public BT::DecoratorNode * @brief Callback function for goals update topic * @param msg Shared pointer to vector of geometry_msgs::msg::PoseStamped message */ - void callback_updated_goals(const nav2_msgs::msg::PoseStampedArray::SharedPtr msg); + void callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg); rclcpp::Subscription::SharedPtr goal_sub_; - rclcpp::Subscription::SharedPtr goals_sub_; + rclcpp::Subscription::SharedPtr goals_sub_; geometry_msgs::msg::PoseStamped last_goal_received_; - nav2_msgs::msg::PoseStampedArray last_goals_received_; + geometry_msgs::msg::PoseStampedArray last_goals_received_; rclcpp::Node::SharedPtr node_; rclcpp::CallbackGroup::SharedPtr callback_group_; diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index cd5a7a03049..a4fe9c74b68 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -52,7 +52,7 @@ GoalUpdater::GoalUpdater( rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goal, this, _1), sub_option); - goals_sub_ = node_->create_subscription( + goals_sub_ = node_->create_subscription( goals_updater_topic, rclcpp::SystemDefaultsQoS(), std::bind(&GoalUpdater::callback_updated_goals, this, _1), @@ -126,7 +126,7 @@ GoalUpdater::callback_updated_goal(const geometry_msgs::msg::PoseStamped::Shared } void -GoalUpdater::callback_updated_goals(const nav2_msgs::msg::PoseStampedArray::SharedPtr msg) +GoalUpdater::callback_updated_goals(const geometry_msgs::msg::PoseStampedArray::SharedPtr msg) { last_goals_received_ = *msg; } diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp index 15296f7b4e9..4c5536c9e1f 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updater_node.cpp @@ -131,7 +131,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) auto goal_updater_pub = node_->create_publisher("goal_update", 10); auto goals_updater_pub = - node_->create_publisher("goals_update", 10); + node_->create_publisher("goals_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; @@ -144,7 +144,7 @@ TEST_F(GoalUpdaterTestFixture, test_older_goal_update) // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update; - nav2_msgs::msg::PoseStampedArray goals_to_update; + geometry_msgs::msg::PoseStampedArray goals_to_update; goal_to_update.header.stamp = rclcpp::Time(goal.header.stamp) - rclcpp::Duration(1, 0); goal_to_update.pose.position.x = 2.0; goals_to_update.header.stamp = goal_to_update.header.stamp; @@ -181,7 +181,7 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) auto goal_updater_pub = node_->create_publisher("goal_update", 10); auto goals_updater_pub = - node_->create_publisher("goals_update", 10); + node_->create_publisher("goals_update", 10); // create new goal and set it on blackboard geometry_msgs::msg::PoseStamped goal; @@ -194,14 +194,14 @@ TEST_F(GoalUpdaterTestFixture, test_get_latest_goal_update) // publish updated_goal older than goal geometry_msgs::msg::PoseStamped goal_to_update_1; - nav2_msgs::msg::PoseStampedArray goals_to_update_1; + geometry_msgs::msg::PoseStampedArray goals_to_update_1; goal_to_update_1.header.stamp = node_->now(); goal_to_update_1.pose.position.x = 2.0; goals_to_update_1.header.stamp = goal_to_update_1.header.stamp; goals_to_update_1.poses.push_back(goal_to_update_1); geometry_msgs::msg::PoseStamped goal_to_update_2; - nav2_msgs::msg::PoseStampedArray goals_to_update_2; + geometry_msgs::msg::PoseStampedArray goals_to_update_2; goal_to_update_2.header.stamp = node_->now(); goal_to_update_2.pose.position.x = 3.0; goals_to_update_2.header.stamp = goal_to_update_2.header.stamp; diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index e7bfb22cda7..a9f93b5ecbc 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -29,7 +29,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" - "msg/PoseStampedArray.msg" "msg/MissedWaypoint.msg" "srv/GetCosts.srv" "srv/GetCostmap.srv" diff --git a/nav2_msgs/msg/PoseStampedArray.msg b/nav2_msgs/msg/PoseStampedArray.msg deleted file mode 100644 index 1a4e04dc0d1..00000000000 --- a/nav2_msgs/msg/PoseStampedArray.msg +++ /dev/null @@ -1,4 +0,0 @@ -# An array of stamped poses with a header for global reference. - -std_msgs/Header header -geometry_msgs/PoseStamped[] poses \ No newline at end of file From fe0da93edcfd16ae6e0fcd8dc29b176e681ab560 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 09:34:22 +0100 Subject: [PATCH 36/46] fix import Signed-off-by: Tony Najjar --- .../nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 45fb893279a..6c99d668023 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -22,7 +22,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_msgs/msg/pose_stamped_array.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "behaviortree_cpp/decorator_node.h" From 1305c8e2cac2f57c8e09871662b56f2990358b8c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 09:37:45 +0100 Subject: [PATCH 37/46] use geometry_msgs Signed-off-by: Tony Najjar --- .../include/nav2_behavior_tree/bt_utils.hpp | 10 +++++----- .../compute_path_through_poses_action.hpp | 2 +- .../action/navigate_through_poses_action.hpp | 4 ++-- .../remove_in_collision_goals_action.hpp | 8 ++++---- .../action/remove_passed_goals_action.hpp | 6 +++--- .../globally_updated_goal_condition.hpp | 6 +++--- .../condition/goal_updated_condition.hpp | 6 +++--- .../decorator/goal_updated_controller.hpp | 4 ++-- .../plugins/decorator/speed_controller.hpp | 4 ++-- .../remove_in_collision_goals_action.cpp | 2 +- .../action/remove_passed_goals_action.cpp | 2 +- .../globally_updated_goal_condition.cpp | 2 +- .../condition/goal_updated_condition.cpp | 2 +- .../decorator/goal_updated_controller.cpp | 2 +- .../plugins/decorator/speed_controller.cpp | 2 +- .../test_compute_path_through_poses_action.cpp | 4 ++-- .../action/test_get_pose_from_path_action.cpp | 4 ++-- .../test_navigate_through_poses_action.cpp | 4 ++-- .../test_remove_in_collision_goals_action.cpp | 8 ++++---- .../action/test_remove_passed_goals_action.cpp | 4 ++-- .../decorator/test_goal_updated_controller.cpp | 4 ++-- .../decorator/test_speed_controller.cpp | 2 +- nav2_behavior_tree/test/test_bt_utils.cpp | 6 +++--- .../navigators/navigate_through_poses.hpp | 2 +- .../src/navigators/navigate_through_poses.cpp | 6 +++--- .../include/nav2_rviz_plugins/nav2_panel.hpp | 6 +++--- nav2_rviz_plugins/src/nav2_panel.cpp | 18 +++++++++--------- 27 files changed, 65 insertions(+), 65 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp index f505733588b..173cc1e7b5b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp @@ -24,7 +24,7 @@ #include "behaviortree_cpp/behavior_tree.h" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "nav2_msgs/msg/pose_stamped_array.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav_msgs/msg/path.hpp" namespace BT @@ -105,19 +105,19 @@ inline geometry_msgs::msg::PoseStamped convertFromString(const StringView key) } /** - * @brief Parse XML string to nav2_msgs::msg::PoseStampedArray + * @brief Parse XML string to geometry_msgs::msg::PoseStampedArray * @param key XML string - * @return nav2_msgs::msg::PoseStampedArray + * @return geometry_msgs::msg::PoseStampedArray */ template<> -inline nav2_msgs::msg::PoseStampedArray convertFromString(const StringView key) +inline geometry_msgs::msg::PoseStampedArray convertFromString(const StringView key) { // 9 real numbers separated by semicolons auto parts = BT::splitString(key, ';'); if (parts.size() % 9 != 0) { throw std::runtime_error("invalid number of fields for PoseStampedArray attribute)"); } else { - nav2_msgs::msg::PoseStampedArray pose_stamped_array; + geometry_msgs::msg::PoseStampedArray pose_stamped_array; for (size_t i = 0; i < parts.size(); i += 9) { geometry_msgs::msg::PoseStamped pose_stamped; pose_stamped.header.stamp = rclcpp::Time(BT::convertFromString(parts[i])); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index 7781de4d3b4..6a74de1bced 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -75,7 +75,7 @@ class ComputePathThroughPosesAction { return providedBasicPorts( { - BT::InputPort( + BT::InputPort( "goals", "Destinations to plan through"), BT::InputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index 2b2676c9392..ea24baadae1 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -18,7 +18,7 @@ #include #include -#include "nav2_msgs/msg/pose_stamped_array.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" @@ -73,7 +73,7 @@ class NavigateThroughPosesAction : public BtActionNode( + BT::InputPort( "goals", "Destinations to plan through"), BT::InputPort("behavior_tree", "Behavior tree to run"), BT::OutputPort( diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp index 9e3e634498a..5ed0e213ea7 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp @@ -20,7 +20,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "nav2_msgs/msg/pose_stamped_array.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_service_node.hpp" #include "nav2_msgs/srv/get_costs.hpp" @@ -52,7 +52,7 @@ class RemoveInCollisionGoals : public BtServiceNode { return providedBasicPorts( { - BT::InputPort("input_goals", + BT::InputPort("input_goals", "Original goals to remove from"), BT::InputPort( "cost_threshold", 254.0, @@ -61,7 +61,7 @@ class RemoveInCollisionGoals : public BtServiceNode BT::InputPort( "consider_unknown_as_obstacle", false, "Whether to consider unknown cost as obstacle"), - BT::OutputPort("output_goals", + BT::OutputPort("output_goals", "Goals with in-collision goals removed"), }); } @@ -70,7 +70,7 @@ class RemoveInCollisionGoals : public BtServiceNode bool use_footprint_; bool consider_unknown_as_obstacle_; double cost_threshold_; - nav2_msgs::msg::PoseStampedArray input_goals_; + geometry_msgs::msg::PoseStampedArray input_goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp index ab97450de91..77c2fc367e9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp @@ -19,7 +19,7 @@ #include #include -#include "nav2_msgs/msg/pose_stamped_array.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_util/geometry_utils.hpp" #include "nav2_util/robot_utils.hpp" #include "behaviortree_cpp/action_node.h" @@ -43,9 +43,9 @@ class RemovePassedGoals : public BT::ActionNodeBase static BT::PortsList providedPorts() { return { - BT::InputPort("input_goals", + BT::InputPort("input_goals", "Original goals to remove viapoints from"), - BT::OutputPort("output_goals", + BT::OutputPort("output_goals", "Goals with passed viapoints removed"), BT::InputPort("radius", 0.5, "radius to goal for it to be considered for removal"), BT::InputPort("global_frame", "Global frame"), diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp index a884f84c805..5e730017d92 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "behaviortree_cpp/condition_node.h" -#include "nav2_msgs/msg/pose_stamped_array.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_utils.hpp" @@ -59,7 +59,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -70,7 +70,7 @@ class GloballyUpdatedGoalCondition : public BT::ConditionNode bool first_time; rclcpp::Node::SharedPtr node_; geometry_msgs::msg::PoseStamped goal_; - nav2_msgs::msg::PoseStampedArray goals_; + geometry_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp index b609abb5b30..4ea81d896b2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp @@ -19,7 +19,7 @@ #include #include "behaviortree_cpp/condition_node.h" -#include "nav2_msgs/msg/pose_stamped_array.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_behavior_tree/bt_utils.hpp" namespace nav2_behavior_tree @@ -56,7 +56,7 @@ class GoalUpdatedCondition : public BT::ConditionNode static BT::PortsList providedPorts() { return { - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -65,7 +65,7 @@ class GoalUpdatedCondition : public BT::ConditionNode private: geometry_msgs::msg::PoseStamped goal_; - nav2_msgs::msg::PoseStampedArray goals_; + geometry_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp index 6c469b4760b..49215ae9a14 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp @@ -50,7 +50,7 @@ class GoalUpdatedController : public BT::DecoratorNode static BT::PortsList providedPorts() { return { - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -66,7 +66,7 @@ class GoalUpdatedController : public BT::DecoratorNode bool goal_was_updated_; geometry_msgs::msg::PoseStamped goal_; - nav2_msgs::msg::PoseStampedArray goals_; + geometry_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp index 0664280f7b2..3d1dcff2962 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/speed_controller.hpp @@ -58,7 +58,7 @@ class SpeedController : public BT::DecoratorNode BT::InputPort("max_rate", 1.0, "Maximum rate"), BT::InputPort("min_speed", 0.0, "Minimum speed"), BT::InputPort("max_speed", 0.5, "Maximum speed"), - BT::InputPort( + BT::InputPort( "goals", "Vector of navigation goals"), BT::InputPort( "goal", "Navigation goal"), @@ -120,7 +120,7 @@ class SpeedController : public BT::DecoratorNode // current goal geometry_msgs::msg::PoseStamped goal_; - nav2_msgs::msg::PoseStampedArray goals_; + geometry_msgs::msg::PoseStampedArray goals_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index f18ab1b54b8..8a5d7228212 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -63,7 +63,7 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion( return BT::NodeStatus::FAILURE; } - nav2_msgs::msg::PoseStampedArray valid_goal_poses; + geometry_msgs::msg::PoseStampedArray valid_goal_poses; for (size_t i = 0; i < response->costs.size(); ++i) { if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) || response->costs[i] < cost_threshold_) diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp index d027533df07..9b44a0635ef 100644 --- a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp @@ -49,7 +49,7 @@ inline BT::NodeStatus RemovePassedGoals::tick() initialize(); } - nav2_msgs::msg::PoseStampedArray goal_poses; + geometry_msgs::msg::PoseStampedArray goal_poses; getInput("input_goals", goal_poses); if (goal_poses.poses.empty()) { diff --git a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp index cc71e8007e2..fcd3a665134 100644 --- a/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.cpp @@ -38,7 +38,7 @@ BT::NodeStatus GloballyUpdatedGoalCondition::tick() return BT::NodeStatus::SUCCESS; } - nav2_msgs::msg::PoseStampedArray current_goals; + geometry_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp index 2e312912eaa..06ddf76ae95 100644 --- a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -33,7 +33,7 @@ BT::NodeStatus GoalUpdatedCondition::tick() return BT::NodeStatus::FAILURE; } - nav2_msgs::msg::PoseStampedArray current_goals; + geometry_msgs::msg::PoseStampedArray current_goals; geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goals", current_goals); BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp index 5dbd5f6cb86..d78c5d1e1fd 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp @@ -44,7 +44,7 @@ BT::NodeStatus GoalUpdatedController::tick() setStatus(BT::NodeStatus::RUNNING); - nav2_msgs::msg::PoseStampedArray current_goals; + geometry_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index 61898dd2227..347f1ccd25c 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -66,7 +66,7 @@ inline BT::NodeStatus SpeedController::tick() first_tick_ = true; } - nav2_msgs::msg::PoseStampedArray current_goals; + geometry_msgs::msg::PoseStampedArray current_goals; BT::getInputOrBlackboard("goals", current_goals); geometry_msgs::msg::PoseStamped current_goal; BT::getInputOrBlackboard("goal", current_goal); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 2c4a208ead2..bb813205456 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -137,7 +137,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav2_msgs::msg::PoseStampedArray goals; + geometry_msgs::msg::PoseStampedArray goals; goals.poses.resize(1); goals.poses[0].pose.position.x = 1.0; config_->blackboard->set("goals", goals); @@ -202,7 +202,7 @@ TEST_F(ComputePathThroughPosesActionTestFixture, test_tick_use_start) config_->blackboard->set("start", start); // create new goal and set it on blackboard - nav2_msgs::msg::PoseStampedArray goals; + geometry_msgs::msg::PoseStampedArray goals; goals.poses.resize(1); goals.poses[0].pose.position.x = 1.0; config_->blackboard->set("goals", goals); diff --git a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp index a2311eaf471..3596af739c3 100644 --- a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp @@ -21,7 +21,7 @@ #include #include "nav_msgs/msg/path.hpp" -#include "nav2_msgs/msg/pose_stamped_array.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "behaviortree_cpp/bt_factory.h" @@ -97,7 +97,7 @@ TEST_F(GetPoseFromPathTestFixture, test_tick) // create new path and set it on blackboard nav_msgs::msg::Path path; - nav2_msgs::msg::PoseStampedArray goals; + geometry_msgs::msg::PoseStampedArray goals; goals.poses.resize(2); goals.poses[0].pose.position.x = 1.0; goals.poses[1].pose.position.x = 2.0; diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index bfa1ffac707..cf0c1079068 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -74,7 +74,7 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test "wait_for_service_timeout", std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); - nav2_msgs::msg::PoseStampedArray poses; + geometry_msgs::msg::PoseStampedArray poses; config_->blackboard->set( "goals", poses); @@ -132,7 +132,7 @@ TEST_F(NavigateThroughPosesActionTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - nav2_msgs::msg::PoseStampedArray poses; + geometry_msgs::msg::PoseStampedArray poses; poses.poses.resize(1); poses.poses[0].pose.position.x = -2.5; poses.poses[0].pose.orientation.x = 1.0; diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp index 2db2a7250b8..6a0c2823f2c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp @@ -147,7 +147,7 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_su tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav2_msgs::msg::PoseStampedArray poses; + geometry_msgs::msg::PoseStampedArray poses; poses.poses.resize(4); poses.poses[0].pose.position.x = 0.0; poses.poses[0].pose.position.y = 0.0; @@ -171,7 +171,7 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_su EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); // check that it removed the point in range - nav2_msgs::msg::PoseStampedArray output_poses; + geometry_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); EXPECT_EQ(output_poses.poses.size(), 3u); @@ -194,7 +194,7 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fa tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav2_msgs::msg::PoseStampedArray poses; + geometry_msgs::msg::PoseStampedArray poses; poses.poses.resize(4); poses.poses[0].pose.position.x = 0.0; poses.poses[0].pose.position.y = 0.0; @@ -218,7 +218,7 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fa // check that it failed and returned the original goals EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); - nav2_msgs::msg::PoseStampedArray output_poses; + geometry_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); EXPECT_EQ(output_poses.poses.size(), 4u); diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp index 5a07ac1659d..9ed40d8bad7 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_passed_goals_action.cpp @@ -114,7 +114,7 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); // create new goal and set it on blackboard - nav2_msgs::msg::PoseStampedArray poses; + geometry_msgs::msg::PoseStampedArray poses; poses.poses.resize(4); poses.poses[0].pose.position.x = 0.0; poses.poses[0].pose.position.y = 0.0; @@ -136,7 +136,7 @@ TEST_F(RemovePassedGoalsTestFixture, test_tick) } // check that it removed the point in range - nav2_msgs::msg::PoseStampedArray output_poses; + geometry_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); EXPECT_EQ(output_poses.poses.size(), 2u); diff --git a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp index 7aa211f32a1..d1c3c5e4207 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp @@ -32,7 +32,7 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree // Setting fake goals on blackboard geometry_msgs::msg::PoseStamped goal1; goal1.header.stamp = node_->now(); - nav2_msgs::msg::PoseStampedArray poses1; + geometry_msgs::msg::PoseStampedArray poses1; poses1.poses.push_back(goal1); config_->blackboard->set("goal", goal1); config_->blackboard->set("goals", poses1); @@ -63,7 +63,7 @@ TEST_F(GoalUpdatedControllerTestFixture, test_behavior) // Creating updated fake-goals geometry_msgs::msg::PoseStamped goal2; goal2.header.stamp = node_->now(); - nav2_msgs::msg::PoseStampedArray poses2; + geometry_msgs::msg::PoseStampedArray poses2; poses2.poses.push_back(goal2); // starting in idle diff --git a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp index 76fab537b06..ae07ee3c21b 100644 --- a/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp +++ b/nav2_behavior_tree/test/plugins/decorator/test_speed_controller.cpp @@ -42,7 +42,7 @@ class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFi goal.header.stamp = node_->now(); config_->blackboard->set("goal", goal); - nav2_msgs::msg::PoseStampedArray fake_poses; + geometry_msgs::msg::PoseStampedArray fake_poses; config_->blackboard->set("goals", fake_poses); // NOLINT config_->input_ports["min_rate"] = 0.1; diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp index 4a0a37d97e1..518a1f070e2 100644 --- a/nav2_behavior_tree/test/test_bt_utils.cpp +++ b/nav2_behavior_tree/test/test_bt_utils.cpp @@ -205,7 +205,7 @@ TEST(PoseStampedArrayPortTest, test_wrong_syntax) )"; BT::BehaviorTreeFactory factory; - factory.registerNodeType>( + factory.registerNodeType>( "PoseStampedArrayPortTest"); EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception); @@ -231,12 +231,12 @@ TEST(PoseStampedArrayPortTest, test_correct_syntax) )"; BT::BehaviorTreeFactory factory; - factory.registerNodeType>( + factory.registerNodeType>( "PoseStampedArrayPortTest"); auto tree = factory.createTreeFromText(xml_txt); tree = factory.createTreeFromText(xml_txt); - nav2_msgs::msg::PoseStampedArray values; + geometry_msgs::msg::PoseStampedArray values; tree.rootNode()->getInput("test", values); EXPECT_EQ(rclcpp::Time(values.poses[0].header.stamp).nanoseconds(), 0); EXPECT_EQ(values.poses[0].header.frame_id, "map"); diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index c13a5137d09..38f7228c7b5 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav2_msgs/msg/pose_stamped_array.hpp" +#include "geometry_msgs/msg/pose_stamped_array.hpp" #include "nav2_core/behavior_tree_navigator.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" #include "nav_msgs/msg/path.hpp" diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 8b8ee0962eb..58f66b1fba8 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -102,7 +102,7 @@ NavigateThroughPosesNavigator::onLoop() auto blackboard = bt_action_server_->getBlackboard(); - nav2_msgs::msg::PoseStampedArray goal_poses; + geometry_msgs::msg::PoseStampedArray goal_poses; [[maybe_unused]] auto res = blackboard->get(goals_blackboard_id_, goal_poses); if (goal_poses.poses.size() == 0) { @@ -212,7 +212,7 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) bool NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal) { - nav2_msgs::msg::PoseStampedArray pose_stamped_array = goal->poses; + geometry_msgs::msg::PoseStampedArray pose_stamped_array = goal->poses; for (auto & goal_pose : pose_stamped_array.poses) { if (!nav2_util::transformPoseInTargetFrame( goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame, @@ -239,7 +239,7 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr blackboard->set("number_recoveries", 0); // NOLINT // Update the goal pose on the blackboard - blackboard->set(goals_blackboard_id_, + blackboard->set(goals_blackboard_id_, std::move(pose_stamped_array)); return true; diff --git a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp index c7aac8ba645..3e07ba8c2bc 100644 --- a/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp +++ b/nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp @@ -102,7 +102,7 @@ private Q_SLOTS: std::vector orientation); void startWaypointFollowing(std::vector poses); void startNavigation(geometry_msgs::msg::PoseStamped); - void startNavThroughPoses(nav2_msgs::msg::PoseStampedArray poses); + void startNavThroughPoses(geometry_msgs::msg::PoseStampedArray poses); using NavigationGoalHandle = rclcpp_action::ClientGoalHandle; using WaypointFollowerGoalHandle = @@ -196,8 +196,8 @@ private Q_SLOTS: QState * accumulated_wp_{nullptr}; QState * accumulated_nav_through_poses_{nullptr}; - nav2_msgs::msg::PoseStampedArray acummulated_poses_; - nav2_msgs::msg::PoseStampedArray store_poses_; + geometry_msgs::msg::PoseStampedArray acummulated_poses_; + geometry_msgs::msg::PoseStampedArray store_poses_; // Publish the visual markers with the waypoints void updateWpNavigationMarkers(); diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 2a0018a8851..36b74a3dfd8 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -675,7 +675,7 @@ void Nav2Panel::loophandler() void Nav2Panel::handleGoalLoader() { - acummulated_poses_ = nav2_msgs::msg::PoseStampedArray(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); std::cout << "Loading Waypoints!" << std::endl; @@ -840,7 +840,7 @@ Nav2Panel::onInitialize() goal_index_ == static_cast(store_poses_.poses.size()) - 1 && msg->status_list.back().status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED) { - store_poses_ = nav2_msgs::msg::PoseStampedArray(); + store_poses_ = geometry_msgs::msg::PoseStampedArray(); waypoint_status_indicator_->clear(); loop_no_ = "0"; loop_count_ = 0; @@ -936,8 +936,8 @@ Nav2Panel::onCancel() &Nav2Panel::onCancelButtonPressed, this)); waypoint_status_indicator_->clear(); - store_poses_ = nav2_msgs::msg::PoseStampedArray(); - acummulated_poses_ = nav2_msgs::msg::PoseStampedArray(); + store_poses_ = geometry_msgs::msg::PoseStampedArray(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); } void Nav2Panel::onResumedWp() @@ -972,7 +972,7 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame) waypoint_status_indicator_->clear(); acummulated_poses_.poses.push_back(pose); } else { - acummulated_poses_ = nav2_msgs::msg::PoseStampedArray(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); updateWpNavigationMarkers(); std::cout << "Start navigation" << std::endl; startNavigation(pose); @@ -1104,7 +1104,7 @@ Nav2Panel::onAccumulatedWp() startWaypointFollowing(acummulated_poses_.poses); store_poses_ = acummulated_poses_; - acummulated_poses_ = nav2_msgs::msg::PoseStampedArray(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); } void @@ -1117,8 +1117,8 @@ Nav2Panel::onAccumulatedNTP() void Nav2Panel::onAccumulating() { - acummulated_poses_ = nav2_msgs::msg::PoseStampedArray(); - store_poses_ = nav2_msgs::msg::PoseStampedArray(); + acummulated_poses_ = geometry_msgs::msg::PoseStampedArray(); + store_poses_ = geometry_msgs::msg::PoseStampedArray(); loop_count_ = 0; loop_no_ = "0"; initial_pose_stored_ = false; @@ -1255,7 +1255,7 @@ Nav2Panel::startWaypointFollowing(std::vector p } void -Nav2Panel::startNavThroughPoses(nav2_msgs::msg::PoseStampedArray poses) +Nav2Panel::startNavThroughPoses(geometry_msgs::msg::PoseStampedArray poses) { auto is_action_server_ready = nav_through_poses_action_client_->wait_for_action_server(std::chrono::seconds(5)); From d96cd2d325bd035be2656cff2f52abd7508e8424 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 09:41:09 +0100 Subject: [PATCH 38/46] more fixes Signed-off-by: Tony Najjar --- nav2_msgs/action/ComputePathThroughPoses.action | 2 +- nav2_msgs/action/NavigateThroughPoses.action | 2 +- nav2_msgs/msg/PoseStampedArray.msg | 4 ---- nav2_msgs/srv/GetCosts.srv | 2 +- 4 files changed, 3 insertions(+), 7 deletions(-) delete mode 100644 nav2_msgs/msg/PoseStampedArray.msg diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index 962c0c2c9b8..0e07373c338 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,5 +1,5 @@ #goal definition -nav2_msgs/PoseStampedArray goals +geometry_msgs/PoseStampedArray goals geometry_msgs/PoseStamped start string planner_id bool use_start # If false, use current robot pose as path start, if true, use start above instead diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index a7f145b746e..9b2896398f6 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -1,6 +1,6 @@ #goal definition -nav2_msgs/PoseStampedArray poses +geometry_msgs/PoseStampedArray poses string behavior_tree --- #result definition diff --git a/nav2_msgs/msg/PoseStampedArray.msg b/nav2_msgs/msg/PoseStampedArray.msg deleted file mode 100644 index 1a4e04dc0d1..00000000000 --- a/nav2_msgs/msg/PoseStampedArray.msg +++ /dev/null @@ -1,4 +0,0 @@ -# An array of stamped poses with a header for global reference. - -std_msgs/Header header -geometry_msgs/PoseStamped[] poses \ No newline at end of file diff --git a/nav2_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv index c782b01b987..2afcc556c4e 100644 --- a/nav2_msgs/srv/GetCosts.srv +++ b/nav2_msgs/srv/GetCosts.srv @@ -1,7 +1,7 @@ # Get costmap costs at given poses bool use_footprint -nav2_msgs/PoseStampedArray poses +geometry_msgs/PoseStampedArray poses --- float32[] costs bool success \ No newline at end of file From 5d0b1e52ead400e239cfdedaec3b7129a5a77d85 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 09:41:16 +0100 Subject: [PATCH 39/46] . Signed-off-by: Tony Najjar --- nav2_msgs/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index fbb6997083d..a9f93b5ecbc 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -30,7 +30,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Particle.msg" "msg/ParticleCloud.msg" "msg/MissedWaypoint.msg" - "msg/PoseStampedArray.msg" "srv/GetCosts.srv" "srv/GetCostmap.srv" "srv/IsPathValid.srv" From 7a261b7ecf8312674cb23b59d98d133761525ccc Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 09:43:24 +0100 Subject: [PATCH 40/46] revert Signed-off-by: Tony Najjar --- nav2_msgs/srv/GetCosts.srv | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv index 2afcc556c4e..c5ca48a3099 100644 --- a/nav2_msgs/srv/GetCosts.srv +++ b/nav2_msgs/srv/GetCosts.srv @@ -1,7 +1,7 @@ # Get costmap costs at given poses bool use_footprint -geometry_msgs/PoseStampedArray poses +geometry_msgs/PoseStamped[] poses --- float32[] costs bool success \ No newline at end of file From 36c21deeacb175b57625dde9ac0a552c59f23503 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 09:49:23 +0100 Subject: [PATCH 41/46] . Signed-off-by: Tony Najjar --- tools/underlay.repos | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/tools/underlay.repos b/tools/underlay.repos index 005f11ec3f0..e443ce12489 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -35,3 +35,8 @@ repositories: type: git url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git version: 091b5ff12436890a54de6325df3573ae110e912b + ros/common_interfaces: + type: git + url: https://github.com/ros2/common_interfaces.git + version: rolling + From 30aed4b1d09a6a4f883dd88b55b573444ea204ca Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 10:32:44 +0100 Subject: [PATCH 42/46] fix Signed-off-by: Tony Najjar --- .../plugins/action/remove_in_collision_goals_action.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp | 4 ++-- nav2_rviz_plugins/src/costmap_cost_tool.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index 48468a7a6c9..76b50ac3f51 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -48,7 +48,7 @@ void RemoveInCollisionGoals::on_tick() request_->use_footprint = use_footprint_; for (const auto & goal : input_goals_.poses) { - request_->poses.poses.push_back(goal); + request_->poses.push_back(goal); } } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 345709733a7..98f5cbbef0c 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -831,7 +831,7 @@ void Costmap2DROS::getCostsCallback( Costmap2D * costmap = layered_costmap_->getCostmap(); response->success = true; - for (const auto & pose : request->poses.poses) { + for (const auto & pose : request->poses) { geometry_msgs::msg::PoseStamped pose_transformed; if (!transformPoseToGlobalFrame(pose, pose_transformed)) { RCLCPP_ERROR( diff --git a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp index 118479343a7..15780196b17 100644 --- a/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp +++ b/nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp @@ -53,7 +53,7 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint) geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = 0.5; pose.pose.position.y = 1.0; - request->poses.poses.push_back(pose); + request->poses.push_back(pose); request->use_footprint = false; auto result_future = client_->async_send_request(request); @@ -78,7 +78,7 @@ TEST_F(GetCostServiceTest, TestWithFootprint) tf2::Quaternion q; q.setRPY(0, 0, 0.5); pose.pose.orientation = tf2::toMsg(q); - request->poses.poses.push_back(pose); + request->poses.push_back(pose); request->use_footprint = true; auto result_future = client_->async_send_request(request); diff --git a/nav2_rviz_plugins/src/costmap_cost_tool.cpp b/nav2_rviz_plugins/src/costmap_cost_tool.cpp index 333f040c947..03435a913e4 100644 --- a/nav2_rviz_plugins/src/costmap_cost_tool.cpp +++ b/nav2_rviz_plugins/src/costmap_cost_tool.cpp @@ -102,7 +102,7 @@ void CostmapCostTool::callCostService(float x, float y) pose.header.stamp = node->now(); pose.pose.position.x = x; pose.pose.position.y = y; - request->poses.poses.push_back(pose); + request->poses.push_back(pose); request->use_footprint = false; // Call local costmap service From 9f4f804f7edc2e5b0b99b2ef5e5171b9f466a44d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 14:17:02 +0100 Subject: [PATCH 43/46] add common_interfaces Signed-off-by: Tony Najjar --- tools/underlay.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/tools/underlay.repos b/tools/underlay.repos index 005f11ec3f0..94a84457a09 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -35,3 +35,7 @@ repositories: type: git url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git version: 091b5ff12436890a54de6325df3573ae110e912b + ros/common_interfaces: + type: git + url: https://github.com/ros2/common_interfaces.git + version: rolling From 4b891c10abb569b00935ecd3c7ee4b355af2c1e6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 14:21:38 +0100 Subject: [PATCH 44/46] fix Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp index a4fe9c74b68..be6a03c8387 100644 --- a/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp +++ b/nav2_behavior_tree/plugins/decorator/goal_updater_node.cpp @@ -98,20 +98,15 @@ inline BT::NodeStatus GoalUpdater::tick() setOutput("output_goals", goals); } else { auto last_goals_received_time = rclcpp::Time(last_goals_received_.header.stamp); - rclcpp::Time most_recent_goal_time = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type()); - for (const auto & g : goals) { - if (rclcpp::Time(g.header.stamp) > most_recent_goal_time) { - most_recent_goal_time = rclcpp::Time(g.header.stamp); - } - } - if (last_goals_received_time > most_recent_goal_time) { + auto goals_time = rclcpp::Time(goals.header.stamp); + if (last_goals_received_time > goals_time) { setOutput("output_goals", last_goals_received_.poses); } else { RCLCPP_WARN( node_->get_logger(), "None of the received goals (most recent: %f) are more recent than the " "current goals (oldest: %f). Ignoring the received goals.", - last_goals_received_time.seconds(), most_recent_goal_time.seconds()); + last_goals_received_time.seconds(), goals_time.seconds()); setOutput("output_goals", goals); } } From bc1d94c42dafcf0246b93c4b5f20ff27e121a2e6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 14:23:32 +0100 Subject: [PATCH 45/46] use PoseStampedArray Signed-off-by: Tony Najjar --- .../plugins/decorator/goal_updater_node.hpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 6c99d668023..6951cf62375 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -38,7 +38,6 @@ namespace nav2_behavior_tree class GoalUpdater : public BT::DecoratorNode { public: - typedef std::vector Goals; /** * @brief A constructor for nav2_behavior_tree::GoalUpdater * @param xml_tag_name Name for the XML tag for this node @@ -56,10 +55,10 @@ class GoalUpdater : public BT::DecoratorNode { return { BT::InputPort("input_goal", "Original Goal"), - BT::InputPort("input_goals", "Original Goals"), + BT::InputPort("input_goals", "Original Goals"), BT::OutputPort("output_goal", "Received Goal by subscription"), - BT::OutputPort("output_goals", "Received Goals by subscription") + BT::OutputPort("output_goals", "Received Goals by subscription") }; } From d5521f6618c07dad1517d194ac012f340c928a85 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 7 Jan 2025 14:32:27 +0100 Subject: [PATCH 46/46] reformat Signed-off-by: Tony Najjar --- .../nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp index 6951cf62375..d5c16ee61c0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp @@ -58,7 +58,8 @@ class GoalUpdater : public BT::DecoratorNode BT::InputPort("input_goals", "Original Goals"), BT::OutputPort("output_goal", "Received Goal by subscription"), - BT::OutputPort("output_goals", "Received Goals by subscription") + BT::OutputPort("output_goals", + "Received Goals by subscription") }; }