From c619545ac5e7502b16b40abaa58aae3f937ce5a9 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 6 Mar 2020 16:41:58 -0800 Subject: [PATCH 1/4] adding navigate to pose feedback and remove random crawl from master --- nav2_behavior_tree/CMakeLists.txt | 3 -- nav2_behavior_tree/README.md | 1 - .../nav2_behavior_tree/bt_service_node.hpp | 3 +- .../plugins/action/back_up_action.cpp | 8 +++ .../plugins/action/clear_costmap_service.cpp | 8 +++ .../plugins/action/follow_path_action.cpp | 2 +- .../plugins/action/random_crawl_action.cpp | 54 ------------------- .../plugins/action/spin_action.cpp | 8 +++ .../plugins/action/wait_action.cpp | 8 +++ .../params/nav2_multirobot_params_1.yaml | 2 + .../params/nav2_multirobot_params_2.yaml | 2 + nav2_bringup/bringup/params/nav2_params.yaml | 2 + .../nav2_bt_navigator/bt_navigator.hpp | 11 +++- nav2_bt_navigator/src/bt_navigator.cpp | 35 +++++++++++- nav2_msgs/CMakeLists.txt | 1 - nav2_msgs/action/NavigateToPose.action | 5 +- nav2_msgs/action/RandomCrawl.action | 7 --- 17 files changed, 88 insertions(+), 72 deletions(-) delete mode 100644 nav2_behavior_tree/plugins/action/random_crawl_action.cpp delete mode 100644 nav2_msgs/action/RandomCrawl.action diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 117c2629d5e..4902131be3d 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -92,9 +92,6 @@ list(APPEND plugin_libs nav2_recovery_node_bt_node) add_library(nav2_navigate_to_pose_action_bt_node SHARED plugins/action/navigate_to_pose_action.cpp) list(APPEND plugin_libs nav2_navigate_to_pose_action_bt_node) -add_library(nav2_random_crawl_action_bt_node SHARED plugins/action/random_crawl_action.cpp) -list(APPEND plugin_libs nav2_random_crawl_action_bt_node) - add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp) list(APPEND plugin_libs nav2_pipeline_sequence_bt_node) diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 9fa155c74bf..c2ec71b0017 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -73,7 +73,6 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a | TransformAvailable | Condition | Checks if a TF transform is available. Returns failure if it cannot be found. Once found, it will always return success. Useful for initial condition checks. | | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. | | RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. | -| RandomCrawl | Action | This BT action invokes the RandomCrawl ROS2 action server, which is implemented by the nav2_experimental/nav2_rl/nav2_turtlebot3_rl (in the nav2_experimental branch) experimental module. The RandomCrawl action server will direct the robot to randomly navigate its environment without hitting any obstacles. | | RecoveryNode | Control | The RecoveryNode is a control flow node with two children. It returns SUCCESS if and only if the first child returns SUCCESS. The second child will be executed only if the first child returns FAILURE. If the second child SUCCEEDS, then the first child will be executed again. The user can specify how many times the recovery actions should be taken before returning FAILURE. In nav2, the RecoveryNode is included in Behavior Trees to implement recovery actions upon failures. | Spin | Action | Invokes the Spin ROS2 action server, which is implemented by the nav2_recoveries module. This action is using in nav2 Behavior Trees as a recovery behavior. | | PipelineSequence | Control | Ticks the first child till it succeeds, then ticks the first and second children till the second one succeeds. It then ticks the first, second, and third children until the third succeeds, and so on, and so on. If at any time a child returns RUNNING, that doesn't change the behavior. If at any time a child returns FAILURE, that stops all children and returns FAILURE overall.| diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 93c40ff47ef..7ca2a088a8b 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -36,6 +36,8 @@ class BtServiceNode : public BT::CoroActionNode { node_ = config().blackboard->get("node"); + request_ = std::make_shared(); + // Get the required items from the blackboard server_timeout_ = config().blackboard->get("server_timeout"); @@ -104,7 +106,6 @@ class BtServiceNode : public BT::CoroActionNode // Fill in service request with information if necessary virtual void on_tick() { - request_ = std::make_shared(); } // An opportunity to do something after diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 21326342a33..cc1b37dcbc5 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -51,6 +51,14 @@ class BackUpAction : public BtActionNode goal_.speed = speed; } + void on_tick() override + { + int recovery_count = 0; + config().blackboard->get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->set("number_recoveries", recovery_count); // NOLINT + } + static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index 144244788fe..95f44258996 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -34,6 +34,14 @@ class ClearEntireCostmapService : public BtServiceNode(service_node_name, conf) { } + + void on_tick() override + { + int recovery_count = 0; + config().blackboard->get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->set("number_recoveries", recovery_count); // NOLINT + } }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index 6f056b921fa..dfd5e92c978 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -52,7 +52,7 @@ class FollowPathAction : public BtActionNode // Check if the goal has been updated if (config().blackboard->get("path_updated")) { // Reset the flag in the blackboard - config().blackboard->set("path_updated", false); + config().blackboard->set("path_updated", false); // NOLINT // Grab the new goal and set the flag so that we send the new goal to // the action server on the next loop iteration diff --git a/nav2_behavior_tree/plugins/action/random_crawl_action.cpp b/nav2_behavior_tree/plugins/action/random_crawl_action.cpp deleted file mode 100644 index 0cff27603e0..00000000000 --- a/nav2_behavior_tree/plugins/action/random_crawl_action.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NAV2_BEHAVIOR_TREE__RANDOM_CRAWL_ACTION_HPP_ -#define NAV2_BEHAVIOR_TREE__RANDOM_CRAWL_ACTION_HPP_ - -#include -#include - -#include "nav2_behavior_tree/bt_action_node.hpp" -#include "nav2_msgs/action/random_crawl.hpp" - -namespace nav2_behavior_tree -{ - -class RandomCrawlAction : public BtActionNode -{ -public: - explicit RandomCrawlAction( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf) - : BtActionNode(xml_tag_name, action_name, conf) - { - } -}; - -} // namespace nav2_behavior_tree - -#include "behaviortree_cpp_v3/bt_factory.h" -BT_REGISTER_NODES(factory) -{ - BT::NodeBuilder builder = - [](const std::string & name, const BT::NodeConfiguration & config) - { - return std::make_unique( - name, "random_crawl", config); - }; - factory.registerBuilder( - "RandomCrawl", builder); -} - -#endif // NAV2_BEHAVIOR_TREE__RANDOM_CRAWL_ACTION_HPP_ diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index afa5af2bcd1..e00b8f7cc60 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -42,6 +42,14 @@ class SpinAction : public BtActionNode goal_.target_yaw = dist; } + void on_tick() override + { + int recovery_count = 0; + config().blackboard->get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->set("number_recoveries", recovery_count); // NOLINT + } + static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index 85d0af0cd72..69f2ea7318d 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -46,6 +46,14 @@ class WaitAction : public BtActionNode goal_.time.sec = duration; } + void on_tick() override + { + int recovery_count = 0; + config().blackboard->get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->set("number_recoveries", recovery_count); // NOLINT + } + // Any BT node that accepts parameters must provide a requiredNodeParameters method static BT::PortsList providedPorts() { diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index e37d52ed1cb..ae5f4fbd72b 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -49,6 +49,8 @@ amcl_rclcpp_node: bt_navigator: ros__parameters: use_sim_time: True + global_frame: map + robot_base_frame: base_link bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 44276b9e30c..4d24bcdfa9e 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -49,6 +49,8 @@ amcl_rclcpp_node: bt_navigator: ros__parameters: use_sim_time: True + global_frame: map + robot_base_frame: base_link bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 4e6af4da433..74ec75110fe 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -49,6 +49,8 @@ amcl_rclcpp_node: bt_navigator: ros__parameters: use_sim_time: True + global_frame: map + robot_base_frame: base_link bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 0ac93a922b5..4c82b3cd49a 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -88,7 +88,9 @@ class BtNavigator : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; - using ActionServer = nav2_util::SimpleActionServer; + using Action = nav2_msgs::action::NavigateToPose; + + using ActionServer = nav2_util::SimpleActionServer; // Our action server implements the NavigateToPose action std::unique_ptr action_server_; @@ -123,7 +125,7 @@ class BtNavigator : public nav2_util::LifecycleNode std::vector plugin_lib_names_; // A client that we'll use to send a command message to our own task server - rclcpp_action::Client::SharedPtr self_client_; + rclcpp_action::Client::SharedPtr self_client_; // A regular, non-spinning ROS node that we can use for calls to the action client rclcpp::Node::SharedPtr client_node_; @@ -131,6 +133,11 @@ class BtNavigator : public nav2_util::LifecycleNode // Spinning transform that can be used by the BT nodes std::shared_ptr tf_; std::shared_ptr tf_listener_; + + // Metrics for feedback + rclcpp::Time start_time_; + std::string robot_frame_; + std::string global_frame_; }; } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index dc478cb61a1..d453af51ef0 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -28,7 +28,8 @@ namespace nav2_bt_navigator { BtNavigator::BtNavigator() -: nav2_util::LifecycleNode("bt_navigator", "", true) +: nav2_util::LifecycleNode("bt_navigator", "", true), + start_time_(0) { RCLCPP_INFO(get_logger(), "Creating"); @@ -53,6 +54,8 @@ BtNavigator::BtNavigator() // Declare this node's parameters declare_parameter("bt_xml_filename"); declare_parameter("plugin_lib_names", plugin_libs); + declare_parameter("global_frame", std::string("map")); + declare_parameter("robot_base_frame", std::string("base_link")); } BtNavigator::~BtNavigator() @@ -96,6 +99,8 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) // Get the libraries to pull plugins from plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array(); + global_frame_ = get_parameter("global_frame").as_string(); + robot_frame_ = get_parameter("robot_base_frame").as_string(); // Create the class that registers our custom nodes and executes the BT bt_ = std::make_unique(plugin_lib_names_); @@ -109,6 +114,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT blackboard_->set("path_updated", false); // NOLINT blackboard_->set("initial_pose_received", false); // NOLINT + blackboard_->set("number_recoveries", 0); // NOLINT // Get the BT filename to use from the node parameter std::string bt_xml_filename; @@ -216,6 +222,7 @@ BtNavigator::navigateToPose() BT::Tree tree = bt_->buildTreeFromText(xml_string_, blackboard_); RosTopicLogger topic_logger(client_node_, tree); + std::shared_ptr feedback_msg = std::make_shared(); auto on_loop = [&]() { if (action_server_->is_preempt_requested()) { @@ -224,6 +231,30 @@ BtNavigator::navigateToPose() initializeGoalPose(); } topic_logger.flush(); + + // action server feedback (pose, duration of task, + // number of recoveries, and distance remaining to goal) + geometry_msgs::msg::TransformStamped transform; + geometry_msgs::msg::PoseStamped current_pose; + transform = tf_->lookupTransform(robot_frame_, global_frame_, tf2::TimePointZero); + current_pose.header = transform.header; + current_pose.pose.position.x = transform.transform.translation.x; + current_pose.pose.position.y = transform.transform.translation.y; + current_pose.pose.position.z = transform.transform.translation.z; + current_pose.pose.orientation = transform.transform.rotation; + feedback_msg->current_pose = current_pose; + + geometry_msgs::msg::Point & c_pose = current_pose.pose.position; + geometry_msgs::msg::PoseStamped goal_pose; + blackboard_->get("goal", goal_pose); + feedback_msg->l2_distance_remaining = sqrt( + (c_pose.x - goal_pose.pose.position.x) * (c_pose.x - goal_pose.pose.position.x) + + (c_pose.y - goal_pose.pose.position.y) * (c_pose.y - goal_pose.pose.position.y)); + int recovery_count = 0; + blackboard_->get("number_recoveries", recovery_count); + feedback_msg->number_of_recoveries = recovery_count; + feedback_msg->navigation_time = now() - start_time_; + action_server_->publish_feedback(feedback_msg); }; // Execute the BT that was previously created in the configure step @@ -254,6 +285,8 @@ void BtNavigator::initializeGoalPose() { auto goal = action_server_->get_current_goal(); + start_time_ = now(); + blackboard_->set("number_recoveries", 0); // NOLINT RCLCPP_INFO( get_logger(), "Begin navigating from current location to (%.2f, %.2f)", diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 97a0c68e43b..9a289f206aa 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -31,7 +31,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/Wait.action" "action/Spin.action" "action/DummyRecovery.action" - "action/RandomCrawl.action" "action/FollowWaypoints.action" DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs ) diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 10c7d5de2cc..43d09e6f920 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -4,4 +4,7 @@ geometry_msgs/PoseStamped pose #result definition std_msgs/Empty result --- -#feedback +geometry_msgs/PoseStamped current_pose +builtin_interfaces/Duration navigation_time +int16 number_of_recoveries +float32 l2_distance_remaining diff --git a/nav2_msgs/action/RandomCrawl.action b/nav2_msgs/action/RandomCrawl.action deleted file mode 100644 index 8077c1acaac..00000000000 --- a/nav2_msgs/action/RandomCrawl.action +++ /dev/null @@ -1,7 +0,0 @@ -#goal definition -std_msgs/Empty target ---- -#result definition -std_msgs/Empty result ---- -#feedback From d3eea8e8d1a064c094bd108c4e995b57644dea6b Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 6 Mar 2020 17:52:47 -0800 Subject: [PATCH 2/4] adding controller feedback --- .../include/nav2_controller/nav2_controller.hpp | 4 +++- nav2_controller/src/nav2_controller.cpp | 8 ++++++++ nav2_msgs/action/FollowPath.action | 2 ++ 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index 3187655d56a..d5e1b0366da 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -107,7 +107,8 @@ class ControllerServer : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; - using ActionServer = nav2_util::SimpleActionServer; + using Action = nav2_msgs::action::FollowPath; + using ActionServer = nav2_util::SimpleActionServer; // Our action server implements the FollowPath action std::unique_ptr action_server_; @@ -215,6 +216,7 @@ class ControllerServer : public nav2_util::LifecycleNode // Whether we've published the single controller warning yet bool single_controller_warning_given_{false}; + geometry_msgs::msg::Pose end_pose_; }; } // namespace nav2_controller diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index bf9e5c9a24f..909bf1d15b1 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -302,6 +302,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) RCLCPP_DEBUG( get_logger(), "Path end point is (%.2f, %.2f)", end_pose.pose.position.x, end_pose.pose.position.y); + end_pose_ = end_pose.pose; } void ControllerServer::computeAndPublishVelocity() @@ -321,6 +322,13 @@ void ControllerServer::computeAndPublishVelocity() pose, nav_2d_utils::twist2Dto3D(twist)); + std::shared_ptr feedback = std::make_shared(); + feedback->speed = cmd_vel_2d.twist.linear.x; + feedback->distance_to_goal = sqrt( + (end_pose_.position.x - pose.pose.position.x) * (end_pose_.position.x - pose.pose.position.x) + + (end_pose_.position.y - pose.pose.position.y) * (end_pose_.position.y - pose.pose.position.y)); + action_server_->publish_feedback(feedback); + RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds()); publishVelocity(cmd_vel_2d); } diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index e1241fe36c5..8b9384492cd 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -6,3 +6,5 @@ string controller_id std_msgs/Empty result --- #feedback +float32 distance_to_goal +float32 speed From 2f55fa97346077aeb478828637eb72748cf8aead Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Fri, 6 Mar 2020 18:00:45 -0800 Subject: [PATCH 3/4] recovery feedback actions --- nav2_msgs/action/BackUp.action | 2 +- nav2_msgs/action/Spin.action | 2 +- nav2_recoveries/plugins/back_up.cpp | 5 +++++ nav2_recoveries/plugins/spin.cpp | 4 ++++ 4 files changed, 11 insertions(+), 2 deletions(-) diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action index e8be2e59835..906bdd9fffd 100644 --- a/nav2_msgs/action/BackUp.action +++ b/nav2_msgs/action/BackUp.action @@ -5,4 +5,4 @@ float32 speed #result definition std_msgs/Empty result --- -#feedback +float32 distance_traveled diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action index 021df0d6d7c..d6c926695e3 100644 --- a/nav2_msgs/action/Spin.action +++ b/nav2_msgs/action/Spin.action @@ -4,4 +4,4 @@ float32 target_yaw #result definition std_msgs/Empty result --- -#feedback +float32 angular_distance_traveled diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index fc785d6964d..f9fd733d8e2 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -76,6 +76,11 @@ Status BackUp::onCycleUpdate() stopRobot(); return Status::SUCCEEDED; } + + auto feedback = std::make_shared(); + feedback->distance_traveled = distance; + action_server_->publish_feedback(feedback); + // TODO(mhpanah): cmd_vel value should be passed as a parameter geometry_msgs::msg::Twist cmd_vel; cmd_vel.linear.y = 0.0; diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 547d56c1699..23f36eee4ba 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -103,6 +103,10 @@ Status Spin::onCycleUpdate() return Status::SUCCEEDED; } + auto feedback = std::make_shared(); + feedback->angular_distance_traveled = relative_yaw; + action_server_->publish_feedback(feedback); + double vel = sqrt(2 * rotational_acc_lim_ * relative_yaw); vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); From cda8ddb2d51f608fd9ac0273a9443074d35a20a7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 27 Mar 2020 14:54:30 -0700 Subject: [PATCH 4/4] Update nav2_controller/src/nav2_controller.cpp Co-Authored-By: Carl Delsey <1828778+cdelsey@users.noreply.github.com> --- nav2_controller/src/nav2_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 909bf1d15b1..0dfe708d70b 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -323,7 +323,7 @@ void ControllerServer::computeAndPublishVelocity() nav_2d_utils::twist2Dto3D(twist)); std::shared_ptr feedback = std::make_shared(); - feedback->speed = cmd_vel_2d.twist.linear.x; + feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y); feedback->distance_to_goal = sqrt( (end_pose_.position.x - pose.pose.position.x) * (end_pose_.position.x - pose.pose.position.x) + (end_pose_.position.y - pose.pose.position.y) * (end_pose_.position.y - pose.pose.position.y));