From fc762f659d98ce4ca2ebc5d87e6fc556742ffc9a Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Sat, 16 May 2020 01:27:49 +0530 Subject: [PATCH] Add feedback in navigation2 actions (#1736) * adding navigate to pose feedback and remove random crawl from master * adding controller feedback * recovery feedback actions * Update nav2_controller/src/nav2_controller.cpp Co-Authored-By: Carl Delsey <1828778+cdelsey@users.noreply.github.com> * Add feedback in wait action and make general improvements Signed-off-by: Sarthak Mittal * Fix rebase errors * Make feedback reset across different goals Co-authored-by: stevemacenski Co-authored-by: Carl Delsey <1828778+cdelsey@users.noreply.github.com> --- nav2_behavior_tree/CMakeLists.txt | 3 -- nav2_behavior_tree/README.md | 1 - .../nav2_behavior_tree/bt_action_node.hpp | 19 ++++--- .../nav2_behavior_tree/bt_service_node.hpp | 8 +++ .../plugins/action/back_up_action.cpp | 5 ++ .../plugins/action/clear_costmap_service.cpp | 5 ++ .../plugins/action/random_crawl_action.cpp | 54 ------------------- .../plugins/action/spin_action.cpp | 5 ++ .../plugins/action/wait_action.cpp | 5 ++ .../condition/goal_reached_condition.cpp | 6 +-- .../plugins/decorator/distance_controller.cpp | 4 +- .../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 | 31 ++++++++++- .../nav2_controller/nav2_controller.hpp | 4 +- nav2_controller/src/nav2_controller.cpp | 15 ++++-- nav2_msgs/CMakeLists.txt | 1 - nav2_msgs/action/BackUp.action | 2 +- nav2_msgs/action/FollowPath.action | 2 + nav2_msgs/action/NavigateToPose.action | 5 +- nav2_msgs/action/RandomCrawl.action | 7 --- nav2_msgs/action/Spin.action | 2 +- nav2_msgs/action/Wait.action | 1 + nav2_recoveries/plugins/back_up.cpp | 7 ++- nav2_recoveries/plugins/back_up.hpp | 2 + nav2_recoveries/plugins/spin.cpp | 8 ++- nav2_recoveries/plugins/spin.hpp | 2 + nav2_recoveries/plugins/wait.cpp | 6 ++- nav2_recoveries/plugins/wait.hpp | 1 + .../include/nav2_util/geometry_utils.hpp | 24 ++++++--- nav2_util/test/CMakeLists.txt | 4 ++ nav2_util/test/test_geometry_utils.cpp | 51 ++++++++++++++++++ 34 files changed, 205 insertions(+), 102 deletions(-) delete mode 100644 nav2_behavior_tree/plugins/action/random_crawl_action.cpp delete mode 100644 nav2_msgs/action/RandomCrawl.action create mode 100644 nav2_util/test/test_geometry_utils.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 5b1a36bcac..81750481e4 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -98,9 +98,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 bd4c9c0d06..6872db8ac7 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -75,7 +75,6 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a | 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. | | DistanceController | Decorator | A node that controls the tick rate for its child based on the distance traveled. The distance to be traveled before replanning 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 `DistanceController` 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_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index c6e606bedb..b68401f5ee 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -179,7 +179,7 @@ class BtActionNode : public BT::ActionNodeBase if (should_cancel_goal()) { auto future_cancel = action_client_->async_cancel_goal(goal_handle_); if (rclcpp::spin_until_future_complete(node_, future_cancel) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( node_->get_logger(), @@ -202,13 +202,8 @@ class BtActionNode : public BT::ActionNodeBase auto status = goal_handle_->get_status(); // Check if the goal is still executing - if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || - status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) - { - return true; - } - - return false; + return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING; } @@ -238,6 +233,14 @@ class BtActionNode : public BT::ActionNodeBase } } + void increment_recovery_count() + { + int recovery_count = 0; + config().blackboard->get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->set("number_recoveries", recovery_count); // NOLINT + } + std::string action_name_; typename std::shared_ptr> action_client_; 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 f132f2953e..2b51e58b7f 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 @@ -120,6 +120,14 @@ class BtServiceNode : public BT::SyncActionNode } protected: + void increment_recovery_count() + { + int recovery_count = 0; + config().blackboard->get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->set("number_recoveries", recovery_count); // NOLINT + } + std::string service_name_, service_node_name_; typename std::shared_ptr> service_client_; std::shared_ptr request_; diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 21326342a3..c1eec645fb 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -51,6 +51,11 @@ class BackUpAction : public BtActionNode goal_.speed = speed; } + void on_tick() override + { + increment_recovery_count(); + } + 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 144244788f..8482e464c3 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -34,6 +34,11 @@ class ClearEntireCostmapService : public BtServiceNode(service_node_name, conf) { } + + void on_tick() override + { + increment_recovery_count(); + } }; } // namespace nav2_behavior_tree 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 0cff27603e..0000000000 --- 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 afa5af2bcd..ad668232c6 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -42,6 +42,11 @@ class SpinAction : public BtActionNode goal_.target_yaw = dist; } + void on_tick() override + { + increment_recovery_count(); + } + 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 85d0af0cd7..268a1f7e9b 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -46,6 +46,11 @@ class WaitAction : public BtActionNode goal_.time.sec = duration; } + void on_tick() override + { + increment_recovery_count(); + } + // Any BT node that accepts parameters must provide a requiredNodeParameters method static BT::PortsList providedPorts() { diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e3c2cd02b8..2e39eca9f2 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -80,11 +80,7 @@ class GoalReachedCondition : public BT::ConditionNode double dx = goal.pose.position.x - current_pose.pose.position.x; double dy = goal.pose.position.y - current_pose.pose.position.y; - if ( (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_) ) { - return true; - } else { - return false; - } + return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_); } static BT::PortsList providedPorts() diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index 239c42855b..e259026462 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -68,8 +68,8 @@ inline BT::NodeStatus DistanceController::tick() } // Get euclidean distance - auto travelled = nav2_util::geometry_utils::euclideanDistance( - start_pose_.pose.position, current_pose.pose.position); + auto travelled = nav2_util::geometry_utils::euclidean_distance( + start_pose_.pose, current_pose.pose); // The child gets ticked the first time through and every time the threshold // distance is crossed. In addition, once the child begins to run, it is diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index 55c9724879..b59a1e6493 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 560a26fc39..2ec38a0156 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 076a6ffffb..30aca9fe15 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 fe542f5f63..f9cdbb9f12 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_; @@ -125,7 +127,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_; @@ -133,6 +135,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 87cae45c53..db0843abad 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -21,6 +21,8 @@ #include #include +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" #include "nav2_behavior_tree/bt_conversions.hpp" #include "nav2_bt_navigator/ros_topic_logger.hpp" @@ -28,7 +30,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"); @@ -55,6 +58,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() @@ -98,6 +103,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_); @@ -111,6 +118,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; @@ -217,6 +225,7 @@ BtNavigator::navigateToPose() }; RosTopicLogger topic_logger(client_node_, tree_); + std::shared_ptr feedback_msg = std::make_shared(); auto on_loop = [&]() { if (action_server_->is_preempt_requested()) { @@ -225,6 +234,22 @@ BtNavigator::navigateToPose() initializeGoalPose(); } topic_logger.flush(); + + // action server feedback (pose, duration of task, + // number of recoveries, and distance remaining to goal) + nav2_util::getCurrentPose(feedback_msg->current_pose, *tf_); + + geometry_msgs::msg::PoseStamped goal_pose; + blackboard_->get("goal", goal_pose); + + feedback_msg->distance_remaining = nav2_util::geometry_utils::euclidean_distance( + feedback_msg->current_pose.pose, goal_pose.pose); + + 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 @@ -263,6 +288,10 @@ BtNavigator::initializeGoalPose() get_logger(), "Begin navigating from current location to (%.2f, %.2f)", goal->pose.pose.position.x, goal->pose.pose.position.y); + // Reset state for new action feedback + start_time_ = now(); + blackboard_->set("number_recoveries", 0); // NOLINT + // Update the goal pose on the blackboard blackboard_->set("goal", goal->pose); } diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index 3187655d56..d5e1b0366d 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 11dc15e670..ee9a162547 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -21,6 +21,7 @@ #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_controller/progress_checker.hpp" #include "nav2_controller/nav2_controller.hpp" @@ -37,8 +38,8 @@ ControllerServer::ControllerServer() declare_parameter("controller_frequency", 20.0); std::vector default_id, default_type; - default_type.push_back("dwb_core::DWBLocalPlanner"); - default_id.push_back("FollowPath"); + default_type.emplace_back("dwb_core::DWBLocalPlanner"); + default_id.emplace_back("FollowPath"); declare_parameter("controller_plugin_ids", default_id); declare_parameter("controller_plugin_types", default_type); @@ -292,7 +293,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) { RCLCPP_DEBUG( get_logger(), - "Providing path to the controller %s", current_controller_); + "Providing path to the controller %s", current_controller_.c_str()); if (path.poses.empty()) { throw nav2_core::PlannerException("Invalid path, Path is empty."); } @@ -303,6 +304,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() @@ -322,6 +324,11 @@ void ControllerServer::computeAndPublishVelocity() pose, nav_2d_utils::twist2Dto3D(twist)); + std::shared_ptr feedback = std::make_shared(); + feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y); + feedback->distance_to_goal = nav2_util::geometry_utils::euclidean_distance(end_pose_, pose.pose); + action_server_->publish_feedback(feedback); + RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds()); publishVelocity(cmd_vel_2d); } @@ -337,7 +344,7 @@ void ControllerServer::updateGlobalPath() } else { RCLCPP_INFO( get_logger(), "Terminating action, invalid controller %s requested.", - goal->controller_id); + goal->controller_id.c_str()); action_server_->terminate_current(); return; } diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index f324715b41..1ecdf58b0f 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -34,7 +34,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/BackUp.action b/nav2_msgs/action/BackUp.action index e8be2e5983..906bdd9fff 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/FollowPath.action b/nav2_msgs/action/FollowPath.action index e1241fe36c..8b9384492c 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 diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 10c7d5de2c..d047dad866 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 distance_remaining diff --git a/nav2_msgs/action/RandomCrawl.action b/nav2_msgs/action/RandomCrawl.action deleted file mode 100644 index 8077c1acaa..0000000000 --- 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 diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action index 021df0d6d7..d6c926695e 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_msgs/action/Wait.action b/nav2_msgs/action/Wait.action index 0855b72323..63dd38d411 100644 --- a/nav2_msgs/action/Wait.action +++ b/nav2_msgs/action/Wait.action @@ -5,3 +5,4 @@ builtin_interfaces/Duration time std_msgs/Empty result --- #feedback +builtin_interfaces/Duration time_left diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 9706e6e82b..d8d75052ae 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -26,7 +26,8 @@ namespace nav2_recoveries { BackUp::BackUp() -: Recovery() +: Recovery(), + feedback_(std::make_shared()) { } @@ -73,10 +74,14 @@ Status BackUp::onCycleUpdate() double diff_y = initial_pose_.pose.position.y - current_pose.pose.position.y; double distance = sqrt(diff_x * diff_x + diff_y * diff_y); + feedback_->distance_traveled = distance; + action_server_->publish_feedback(feedback_); + if (distance >= abs(command_x_)) { stopRobot(); return Status::SUCCEEDED; } + // TODO(mhpanah): cmd_vel value should be passed as a parameter auto cmd_vel = std::make_unique(); cmd_vel->linear.y = 0.0; diff --git a/nav2_recoveries/plugins/back_up.hpp b/nav2_recoveries/plugins/back_up.hpp index 4f04bd0352..ac219e65a7 100644 --- a/nav2_recoveries/plugins/back_up.hpp +++ b/nav2_recoveries/plugins/back_up.hpp @@ -51,6 +51,8 @@ class BackUp : public Recovery double command_x_; double command_speed_; double simulate_ahead_time_; + + BackUpAction::Feedback::SharedPtr feedback_; }; } // namespace nav2_recoveries diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 9afd6a2ac4..119fa09a56 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -35,9 +35,10 @@ namespace nav2_recoveries { Spin::Spin() -: Recovery() +: Recovery(), + feedback_(std::make_shared()), + prev_yaw_(0.0) { - prev_yaw_ = 0.0; } Spin::~Spin() @@ -103,6 +104,9 @@ Status Spin::onCycleUpdate() relative_yaw_ += delta_yaw; prev_yaw_ = current_yaw; + feedback_->angular_distance_traveled = relative_yaw_; + action_server_->publish_feedback(feedback_); + double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_); if (remaining_yaw <= 0) { stopRobot(); diff --git a/nav2_recoveries/plugins/spin.hpp b/nav2_recoveries/plugins/spin.hpp index 87b670e100..cf1d6b7d04 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_recoveries/plugins/spin.hpp @@ -43,6 +43,8 @@ class Spin : public Recovery geometry_msgs::msg::Twist * cmd_vel, geometry_msgs::msg::Pose2D & pose2d); + SpinAction::Feedback::SharedPtr feedback_; + double min_rotational_vel_; double max_rotational_vel_; double rotational_acc_lim_; diff --git a/nav2_recoveries/plugins/wait.cpp b/nav2_recoveries/plugins/wait.cpp index 10dcc19709..e9a27b4dda 100644 --- a/nav2_recoveries/plugins/wait.cpp +++ b/nav2_recoveries/plugins/wait.cpp @@ -22,7 +22,8 @@ namespace nav2_recoveries { Wait::Wait() -: Recovery() +: Recovery(), + feedback_(std::make_shared()) { } @@ -43,6 +44,9 @@ Status Wait::onCycleUpdate() auto time_left = std::chrono::duration_cast(wait_end_ - current_point).count(); + feedback_->time_left = rclcpp::Duration(time_left); + action_server_->publish_feedback(feedback_); + if (time_left > 0) { return Status::RUNNING; } else { diff --git a/nav2_recoveries/plugins/wait.hpp b/nav2_recoveries/plugins/wait.hpp index af337d3c94..34364ce174 100644 --- a/nav2_recoveries/plugins/wait.hpp +++ b/nav2_recoveries/plugins/wait.hpp @@ -38,6 +38,7 @@ class Wait : public Recovery protected: std::chrono::time_point wait_end_; + WaitAction::Feedback::SharedPtr feedback_; }; } // namespace nav2_recoveries diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 40466ad011..90fa6503ae 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -17,6 +17,8 @@ #include +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -32,13 +34,23 @@ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle) return tf2::toMsg(q); } -inline double euclideanDistance( - const geometry_msgs::msg::Point pos1, - const geometry_msgs::msg::Point pos2) +inline double euclidean_distance( + const geometry_msgs::msg::Point & pos1, + const geometry_msgs::msg::Point & pos2) { - const double dx = pos1.x - pos2.x; - const double dy = pos1.y - pos2.y; - const double dz = pos1.z - pos2.z; + double dx = pos1.x - pos2.x; + double dy = pos1.y - pos2.y; + double dz = pos1.z - pos2.z; + return std::sqrt(dx * dx + dy * dy + dz * dz); +} + +inline double euclidean_distance( + const geometry_msgs::msg::Pose & pos1, + const geometry_msgs::msg::Pose & pos2) +{ + double dx = pos1.position.x - pos2.position.x; + double dy = pos1.position.y - pos2.position.y; + double dz = pos1.position.z - pos2.position.z; return std::sqrt(dx * dx + dy * dy + dz * dz); } diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index b1feea6ad4..40f9672cbe 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -28,3 +28,7 @@ target_link_libraries(test_lifecycle_node ${library_name}) ament_add_gtest(test_lifecycle_cli_node test_lifecycle_cli_node.cpp) ament_target_dependencies(test_lifecycle_cli_node rclcpp_lifecycle) target_link_libraries(test_lifecycle_cli_node ${library_name}) + +ament_add_gtest(test_geometry_utils test_geometry_utils.cpp) +ament_target_dependencies(test_geometry_utils geometry_msgs) +target_link_libraries(test_geometry_utils ${library_name}) diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp new file mode 100644 index 0000000000..9c379575bb --- /dev/null +++ b/nav2_util/test/test_geometry_utils.cpp @@ -0,0 +1,51 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// 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 "nav2_util/geometry_utils.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "gtest/gtest.h" + +using nav2_util::geometry_utils::euclidean_distance; + +TEST(GeometryUtils, euclidean_distance_point) +{ + geometry_msgs::msg::Point point1; + point1.x = 3.0; + point1.y = 2.0; + point1.z = 1.0; + + geometry_msgs::msg::Point point2; + point2.x = 1.0; + point2.y = 2.0; + point2.z = 3.0; + + ASSERT_NEAR(euclidean_distance(point1, point2), 2.82843, 1e-5); +} + +TEST(GeometryUtils, euclidean_distance_pose) +{ + geometry_msgs::msg::Pose pose1; + pose1.position.x = 7.0; + pose1.position.y = 4.0; + pose1.position.z = 3.0; + + geometry_msgs::msg::Pose pose2; + pose2.position.x = 17.0; + pose2.position.y = 6.0; + pose2.position.z = 2.0; + + ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.24695, 1e-5); +}