From d6b02ac407a26d9394a23f76620f25f2274c8c37 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 9 Dec 2024 19:36:55 +0100 Subject: [PATCH 1/3] Improvements in getCostsCallback (#4767) * fix(simple-action-server): info log instead of warn on cancel (#4684) Cancelling a goal is nominal behavior and therefore it should not log warning. Signed-off-by: Rein Appeldoorn Signed-off-by: Tony Najjar * feat(controller-server): `publish_zero_velocity` parameter (#4675) * feat(controller-server): `publish_zero_velocity` parameter For optionally publishing a zero velocity command reference on goal exit. Publishing a zero velocity is not desired when we are following consecutive path segments that end with a velocity. Signed-off-by: Rein Appeldoorn * processed comments Signed-off-by: Rein Appeldoorn * comments Steve Signed-off-by: Rein Appeldoorn --------- Signed-off-by: Rein Appeldoorn Signed-off-by: Tony Najjar * Improvements in RemoveInCollisionGoals and adjacent features (#4676) * improvements Signed-off-by: Tony Najjar * ament_uncrustify Signed-off-by: Tony Najjar * Fix formatting Signed-off-by: Tony Najjar * fix building Signed-off-by: Tony Najjar * fixes Signed-off-by: Tony Najjar * add stamp Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar * Correct paper name for graceful controller Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar * Added missing action clients in robot_navigator(BasicNavigator) to destroy_node (#4698) * fix: added assisted_teleop_client to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo * fix: added other missing action clients to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo --------- Signed-off-by: Tiwa Ojo Signed-off-by: Tony Najjar * Adding disengagement threshold to rotation shim controller (#4699) * adding disengagement threshold to rotation shim controller Signed-off-by: Steve Macenski * change default to 22.5 deg Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar * Switch nav2_waypoint_follower to modern CMake idioms. (#4648) Signed-off-by: Chris Lalancette Signed-off-by: Tony Najjar * Fixing SGF in MPPI and Smoother (#4669) Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar * Feat/migrate gps nav2 system test (#4682) * include missing docking station parameters Signed-off-by: stevedan * fix crach RL Signed-off-by: stevedan * lintering Signed-off-by: stevedan * Change naming Signed-off-by: stevedan * update submodule Signed-off-by: stevedan * minor changes Signed-off-by: stevedan * fix issue with caching Signed-off-by: stevedan * fix issue with caching, increase version number Signed-off-by: stevedan * Pin git ref via sha to bust underlay workspace cache --------- Signed-off-by: stevedan Co-authored-by: ruffsl Signed-off-by: Tony Najjar * fix: handle transition failures in all servers (#4708) * fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas * adding support for rest of servers + review comments Signed-off-by: Steve Macenski * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski * fix vel smoother unit tests Signed-off-by: Steve Macenski * fixing docking server unit testing Signed-off-by: Steve Macenski * fixing last bits Signed-off-by: Steve Macenski --------- Signed-off-by: Kemal Bektas Signed-off-by: Steve Macenski Co-authored-by: Kemal Bektas Signed-off-by: Tony Najjar * [RotationShimController] fix: rotate on short paths (#4716) Add header data to goal for short paths. Commit d8ae3c1f9b8233e86ed54dfbe615b1ba56b51b6d added the possibility to the rotation shim controller to rotate towards the goal when the goal was closer that the `forward_sampling_distance`. This feature was not fully working as the goal was missing proper header data, causing the rotation shim to give back control to the main controller. Co-authored-by: agennart Signed-off-by: Tony Najjar * Improve Docking panel (#4717) * Added load and save panel Signed-off-by: Alberto Tudela * Improved dock_panel state machine Signed-off-by: Alberto Tudela * Added loading dock plugins log Signed-off-by: Alberto Tudela * Redo UI Signed-off-by: Alberto Tudela * Update tooltips Signed-off-by: Alberto Tudela * Fix null-dereference Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela Signed-off-by: Tony Najjar * Added parameter `rotate_to_heading_once` (#4721) Signed-off-by: Daniil Khaninaev Signed-off-by: Tony Najjar * [RotationShimController] fix: rotate to goal heading (#4724) Add frame_id to goal when rotating towards goal heading, otherwise the transform would fail. This bug was introduced in 30e2cde by not setting the frame_id. Signed-off-by: agennart Co-authored-by: agennart Signed-off-by: Tony Najjar * [loopback_sim] Publish clock, [nav2_costmap_2d] Fix Qos (#4726) * Publish /clock from loopback sim Signed-off-by: Adi Vardi * [nav2_costmap_2d] Fix obstacle_layer trying to use RELIABLE QoS Use QoS profile from rclcpp::SensorDataQoS() instead of rmw_qos_profile_t. This solves an issue where the subscriber uses RELIABLE setting even when initialized from rmw_qos_profile_sensor_data. In addition the Subscriber(..., rmw_qos_profile_t) constructor is deprecated in favor of Subscriber(..., rclcpp::QoS) Signed-off-by: Adi Vardi * [nav2_smac_planner] fix typos Signed-off-by: Adi Vardi * Use single quotes Signed-off-by: Adi Vardi --------- Signed-off-by: Adi Vardi Signed-off-by: Tony Najjar * Remove nav2_loopback_sim dependency on transforms3d. (#4738) The package never uses it, so don't declare it. Signed-off-by: Chris Lalancette Signed-off-by: Tony Najjar * Fix incorrect doxygen comment (#4741) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Tony Najjar * Fix missing dependency on nav2_costmap_2d (#4742) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Tony Najjar * Updating error logging in Smac collision detector object (#4743) * Updating error logging in Smac configs Signed-off-by: Steve Macenski * linting Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar * [map_io] Replace std logs by rclcpp logs (#4720) * replace std logs by rclcpp logs Signed-off-by: Guillaume Doisy * RCLCPP_DEBUG to RCLCPP_INFO for visibility Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Signed-off-by: Tony Najjar * Pass IDLE to on_tick, use that for initialize condition (#4744) * Pass IDLE to on_tick, use that for initialize condition Signed-off-by: redvinaa * Fix battery sub creation bug Signed-off-by: redvinaa --------- Signed-off-by: redvinaa Signed-off-by: Tony Najjar * nav2_costmap_2d: add missing default_value_ copy in Costmap2D operator= (#4753) default_value_ is an attribute of the Costmap2D class and should be copied along with the other attributes. Signed-off-by: Dylan De Coeyer Signed-off-by: Tony Najjar * improvements Signed-off-by: Tony Najjar * change back to NO_INFORMATION Signed-off-by: Tony Najjar * Revert "change back to NO_INFORMATION" This reverts commit 9f8c69cfb42a983efa535ccbe7a101a813616115. Signed-off-by: Tony Najjar * Add IsStoppedBTNode Signed-off-by: Tony Najjar * add topic name + reformat Signed-off-by: Tony Najjar * fix comment Signed-off-by: Tony Najjar * fix abs Signed-off-by: Tony Najjar * remove log Signed-off-by: Tony Najjar * add getter functions for raw twist Signed-off-by: Tony Najjar * remove unused code Signed-off-by: Tony Najjar * use odomsmoother Signed-off-by: Tony Najjar * fix formatting Signed-off-by: Tony Najjar * update groot Signed-off-by: Tony Najjar * Add test Signed-off-by: Tony Najjar * reset at success Signed-off-by: Tony Najjar * mppi parameters_handler: Improve verbose handling (#4704) (#4711) * mppi parameters_handler: Improve verbose handling (#4704) The "verbose" parameter of the parameters_handler is a special case that needs registration before the dynamic parameter handler callback is registered. In verbose mode make the parameter handler info/warn/debug messages more expressive. Signed-off-by: Mike Wake * Address review comments. (#4704) * remove comments. * Use RCLCPP_DEBUG instead of INFO for low level messages. * Add test for trying to access parameters that are not declared. Signed-off-by: Mike Wake * mppi parameters_handler: Improve static/dynamic/not defined logging (#4704) Attempts to change undefined parameters will not be successful and will log an error. Attempts to change static parameters will be ignored, a debug message is logged if a change in parameters is attempted. Signed-off-by: Mike Wake * mppi parameters_handler: populate SetParametersResult (#4704) Provide a mechanism to populate an rcl_interfaces::msg::SetParametersResult with the reasons for unsuccessful parameter setting, so that it may be propogated back to a set parameter client. The mechanism provides feedback when attempting to set undefined parameters, static parameters and could be used to validate dynamic parameters and provide a reason for rejection. NOTE: This changes public interface of ParametersHandler class. s/setDynamicParamCallback/setParamCallback s/addDynamicParamCallback/addParamCallback which takes a callback function that is able to populate a rcl_interfaces::msg::SetParametersResult. In order to indicate an unsuccessful parameter change and the reason, callback functions should append a "\n" to the reason before appending to if it is not empty. Signed-off-by: Mike Wake * mppi parameters_handler: fix reason handling and improve tests Signed-off-by: Mike Wake --------- Signed-off-by: Mike Wake Signed-off-by: Tony Najjar * Added collision detection for docking (#4752) * Added collision detection for docking Signed-off-by: Alberto Tudela * Minor fixes Signed-off-by: Alberto Tudela * Improve collision while undocking and test Signed-off-by: Alberto Tudela * Fix smoke testing Signed-off-by: Alberto Tudela * Rename dock_collision_threshold Signed-off-by: Alberto Tudela * Added docs and params Signed-off-by: Alberto Tudela * Minor changes in README Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela Signed-off-by: Tony Najjar * Use BT.CPP Tree::sleep (#4761) * Use BT.cpp sleep Signed-off-by: Tony Najjar * Implement BT Loop Rate Signed-off-by: Tony Najjar * Fix formatting Signed-off-by: Tony Najjar * Fix formatting Signed-off-by: Tony Najjar * move to nav2_behavior_tree Signed-off-by: Tony Najjar * fix Signed-off-by: Tony Najjar * fix lint Signed-off-by: Tony Najjar * cache Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar * FIX velocity_threshold_ Signed-off-by: Tony Najjar * Fix stopped Node Signed-off-by: Tony Najjar * Add tests to odometry_utils Signed-off-by: Tony Najjar * fix linting Signed-off-by: Tony Najjar * Simplify namespaced bringups and multirobot sim (#4715) * WIP single robot namespacing working Signed-off-by: Luca Della Vedova * Remove manual namespace substitution Signed-off-by: Luca Della Vedova * Remove all multirobot specific configs Signed-off-by: Luca Della Vedova * Refactor parsing function to common, add for rest of layers Signed-off-by: Luca Della Vedova * Fix dwb_critics test Signed-off-by: Luca Della Vedova * Add alternative API for costmap construction Signed-off-by: Luca Della Vedova * Address review feedback Signed-off-by: Luca Della Vedova * Remove remaining usage of `use_namespace` parameter Signed-off-by: Luca Della Vedova * Always join with parent namespace Signed-off-by: Luca Della Vedova * Use private parameter for parent namespace Signed-off-by: Luca Della Vedova * Fix integration test Signed-off-by: Luca Della Vedova * Revert "Use private parameter for parent namespace" This reverts commit 0c958dc994f5e618fef2471c0851a2bc3b19b1d5. Signed-off-by: Luca Della Vedova * Revert "Fix integration test" This reverts commit 137d57759b700f00c7548b997abcc8dc36d9cca9. Signed-off-by: Luca Della Vedova * Remove global map_topic parameter Signed-off-by: Luca Della Vedova * Simplify Costmap2DROS constructor Signed-off-by: Luca Della Vedova --------- Signed-off-by: Luca Della Vedova Signed-off-by: Luca Della Vedova Signed-off-by: Tony Najjar * Make RecoveryNode return Running (#4777) * Add IsStoppedBTNode Signed-off-by: Tony Najjar * add topic name + reformat Signed-off-by: Tony Najjar * fix comment Signed-off-by: Tony Najjar * fix abs Signed-off-by: Tony Najjar * remove log Signed-off-by: Tony Najjar * add getter functions for raw twist Signed-off-by: Tony Najjar * remove unused code Signed-off-by: Tony Najjar * use odomsmoother Signed-off-by: Tony Najjar * fix formatting Signed-off-by: Tony Najjar * update groot Signed-off-by: Tony Najjar * Add test Signed-off-by: Tony Najjar * reset at success Signed-off-by: Tony Najjar * FIX velocity_threshold_ Signed-off-by: Tony Najjar * Fix stopped Node Signed-off-by: Tony Najjar * Add tests to odometry_utils Signed-off-by: Tony Najjar * fix linting Signed-off-by: Tony Najjar * Make RecoveryNode return RUNNING Signed-off-by: Tony Najjar * PR review Signed-off-by: Tony Najjar * add halt at the end Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar * Migrating twist to twiststamped in simulations for recommended default bringups (#4779) * migrating from twist to twiststamped Signed-off-by: Steve Macenski * bump ci cache for updated TB4 sim files Signed-off-by: Steve Macenski * fixing collision monitor, velocity smoother unit tests Signed-off-by: Steve Macenski * fix assisted teleop test Signed-off-by: Steve Macenski * fixing docking server smoke test Signed-off-by: Steve Macenski * bust nav2 minimal tb sim cache --------- Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar * address comments Signed-off-by: Tony Najjar * set response to true Signed-off-by: Tony Najjar * fix test Signed-off-by: Tony Najjar * fail if out of bounds Signed-off-by: Tony Najjar --------- Signed-off-by: Rein Appeldoorn Signed-off-by: Tony Najjar Signed-off-by: Steve Macenski Signed-off-by: Tiwa Ojo Signed-off-by: Chris Lalancette Signed-off-by: stevedan Signed-off-by: Kemal Bektas Signed-off-by: Alberto Tudela Signed-off-by: Daniil Khaninaev Signed-off-by: agennart Signed-off-by: Adi Vardi Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Guillaume Doisy Signed-off-by: redvinaa Signed-off-by: Dylan De Coeyer Signed-off-by: Mike Wake Signed-off-by: Luca Della Vedova Signed-off-by: Luca Della Vedova Co-authored-by: Rein Appeldoorn Co-authored-by: Steve Macenski Co-authored-by: Tiwa Ojo <55967921+tiwaojo@users.noreply.github.com> Co-authored-by: Chris Lalancette Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Co-authored-by: ruffsl Co-authored-by: Kemal Bektas Co-authored-by: Saitama Co-authored-by: agennart Co-authored-by: Alberto Tudela Co-authored-by: Daniil Khaninaev Co-authored-by: Adi Vardi <57910756+adivardi@users.noreply.github.com> Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: Vince Reda <60265874+redvinaa@users.noreply.github.com> Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: aosmw <116058035+aosmw@users.noreply.github.com> Co-authored-by: Luca Della Vedova --- .../remove_in_collision_goals_action.cpp | 8 ++ .../test_remove_in_collision_goals_action.cpp | 107 +++++++++++++++--- nav2_costmap_2d/src/costmap_2d_ros.cpp | 22 ++-- nav2_msgs/srv/GetCosts.srv | 3 +- 4 files changed, 118 insertions(+), 22 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 d6f9ee662d0..78e6c2e35df 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 @@ -55,6 +55,14 @@ void RemoveInCollisionGoals::on_tick() BT::NodeStatus RemoveInCollisionGoals::on_completion( std::shared_ptr response) { + if (!response->success) { + RCLCPP_ERROR( + node_->get_logger(), + "GetCosts service call failed"); + setOutput("output_goals", input_goals_); + return BT::NodeStatus::FAILURE; + } + Goals valid_goal_poses; for (size_t i = 0; i < response->costs.size(); ++i) { if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) || 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..626d0b1b15d 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 @@ -25,10 +25,10 @@ #include "utils/test_behavior_tree_fixture.hpp" -class RemoveInCollisionGoalsService : public TestService +class RemoveInCollisionGoalsSucessService : public TestService { public: - RemoveInCollisionGoalsService() + RemoveInCollisionGoalsSucessService() : TestService("/global_costmap/get_cost_global_costmap") {} @@ -40,9 +40,28 @@ class RemoveInCollisionGoalsService : public TestServicecosts = {100, 50, 5, 254}; + response->success = true; } }; +class RemoveInCollisionGoalsFailureService : public TestService +{ +public: + RemoveInCollisionGoalsFailureService() + : TestService("/local_costmap/get_cost_local_costmap") + {} + + virtual void handle_service( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)request; + response->costs = {255, 50, 5, 254}; + response->success = false; + } +}; class RemoveInCollisionGoalsTestFixture : public ::testing::Test { @@ -86,7 +105,7 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test delete config_; config_ = nullptr; node_.reset(); - server_.reset(); + success_server_.reset(); factory_.reset(); } @@ -94,7 +113,8 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test { tree_.reset(); } - static std::shared_ptr server_; + static std::shared_ptr success_server_; + static std::shared_ptr failure_server_; protected: static rclcpp::Node::SharedPtr node_; @@ -106,12 +126,14 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test rclcpp::Node::SharedPtr RemoveInCollisionGoalsTestFixture::node_ = nullptr; BT::NodeConfiguration * RemoveInCollisionGoalsTestFixture::config_ = nullptr; -std::shared_ptr -RemoveInCollisionGoalsTestFixture::server_ = nullptr; +std::shared_ptr +RemoveInCollisionGoalsTestFixture::success_server_ = nullptr; +std::shared_ptr +RemoveInCollisionGoalsTestFixture::failure_server_ = nullptr; std::shared_ptr RemoveInCollisionGoalsTestFixture::factory_ = nullptr; std::shared_ptr RemoveInCollisionGoalsTestFixture::tree_ = nullptr; -TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) +TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_success) { // create tree std::string xml_txt = @@ -141,11 +163,13 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) config_->blackboard->set("goals", poses); - // tick until node succeeds - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + // tick until node is not running + tree_->rootNode()->executeTick(); + while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) { tree_->rootNode()->executeTick(); } + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); // check that it removed the point in range std::vector output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); @@ -156,6 +180,54 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) EXPECT_EQ(output_poses[2], poses[2]); } +TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fail) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + 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; + + poses[1].pose.position.x = 0.5; + poses[1].pose.position.y = 0.0; + + poses[2].pose.position.x = 1.0; + poses[2].pose.position.y = 0.0; + + poses[3].pose.position.x = 2.0; + poses[3].pose.position.y = 0.0; + + config_->blackboard->set("goals", poses); + + // tick until node is not running + tree_->rootNode()->executeTick(); + while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) { + tree_->rootNode()->executeTick(); + } + + // check that it failed and returned the original goals + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); + std::vector 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]); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); @@ -164,17 +236,24 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); // initialize service and spin on new thread - RemoveInCollisionGoalsTestFixture::server_ = - std::make_shared(); - std::thread server_thread([]() { - rclcpp::spin(RemoveInCollisionGoalsTestFixture::server_); + RemoveInCollisionGoalsTestFixture::success_server_ = + std::make_shared(); + std::thread success_server_thread([]() { + rclcpp::spin(RemoveInCollisionGoalsTestFixture::success_server_); + }); + + RemoveInCollisionGoalsTestFixture::failure_server_ = + std::make_shared(); + std::thread failure_server_thread([]() { + rclcpp::spin(RemoveInCollisionGoalsTestFixture::failure_server_); }); int all_successful = RUN_ALL_TESTS(); // shutdown ROS rclcpp::shutdown(); - server_thread.join(); + success_server_thread.join(); + failure_server_thread.join(); return all_successful; } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 5413fd3d2c1..a35a1786bb4 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -825,15 +825,14 @@ void Costmap2DROS::getCostsCallback( unsigned int mx, my; Costmap2D * costmap = layered_costmap_->getCostmap(); - + response->success = true; for (const auto & pose : request->poses) { geometry_msgs::msg::PoseStamped pose_transformed; - transformPoseToGlobalFrame(pose, pose_transformed); - bool in_bounds = costmap->worldToMap( - pose_transformed.pose.position.x, - pose_transformed.pose.position.y, mx, my); - - if (!in_bounds) { + if (!transformPoseToGlobalFrame(pose, pose_transformed)) { + RCLCPP_ERROR( + get_logger(), "Failed to transform, cannot get cost for pose (%.2f, %.2f)", + pose.pose.position.x, pose.pose.position.y); + response->success = false; response->costs.push_back(NO_INFORMATION); continue; } @@ -857,6 +856,15 @@ void Costmap2DROS::getCostsCallback( pose_transformed.pose.position.x, pose_transformed.pose.position.y); + bool in_bounds = costmap->worldToMap( + pose_transformed.pose.position.x, + pose_transformed.pose.position.y, mx, my); + + if (!in_bounds) { + response->success = false; + response->costs.push_back(LETHAL_OBSTACLE); + continue; + } // Get the cost at the map coordinates response->costs.push_back(static_cast(costmap->getCost(mx, my))); } diff --git a/nav2_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv index 55ebad4f0a2..c5ca48a3099 100644 --- a/nav2_msgs/srv/GetCosts.srv +++ b/nav2_msgs/srv/GetCosts.srv @@ -3,4 +3,5 @@ bool use_footprint geometry_msgs/PoseStamped[] poses --- -float32[] costs \ No newline at end of file +float32[] costs +bool success \ No newline at end of file From 4074d04d499d23318dd706d8e8207b80c951bfde Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 10 Dec 2024 19:29:15 +0100 Subject: [PATCH 2/3] Revert "Make RecoveryNode return Running (#4777)" (#4790) This reverts commit 9284c8a9969947b99e821e0c85768cca886920ce. --- .../plugins/control/recovery_node.cpp | 138 +++++++++--------- .../plugins/control/test_recovery_node.cpp | 9 +- 2 files changed, 71 insertions(+), 76 deletions(-) diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 1ab9d2ff34c..6eb3c6e99e5 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -37,83 +37,83 @@ BT::NodeStatus RecoveryNode::tick() throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children."); } - if (retry_count_ > number_of_retries_) { - halt(); - return BT::NodeStatus::FAILURE; - } setStatus(BT::NodeStatus::RUNNING); - TreeNode * child_node = children_nodes_[current_child_idx_]; - const BT::NodeStatus child_status = child_node->executeTick(); - - if (current_child_idx_ == 0) { - switch (child_status) { - case BT::NodeStatus::SKIPPED: - // If first child is skipped, the entire branch is considered skipped - halt(); - return BT::NodeStatus::SKIPPED; - - case BT::NodeStatus::SUCCESS: - // reset node and return success when first child returns success - halt(); - return BT::NodeStatus::SUCCESS; - - case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::FAILURE: - { - if (retry_count_ < number_of_retries_) { - // halt first child and tick second child in next iteration - ControlNode::haltChild(0); - current_child_idx_ = 1; - return BT::NodeStatus::RUNNING; - } else { - // reset node and return failure when max retries has been exceeded - halt(); + while (current_child_idx_ < children_count && retry_count_ <= number_of_retries_) { + TreeNode * child_node = children_nodes_[current_child_idx_]; + const BT::NodeStatus child_status = child_node->executeTick(); + + if (current_child_idx_ == 0) { + switch (child_status) { + case BT::NodeStatus::SKIPPED: + // If first child is skipped, the entire branch is considered skipped + halt(); + return BT::NodeStatus::SKIPPED; + + case BT::NodeStatus::SUCCESS: + // reset node and return success when first child returns success + halt(); + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + + case BT::NodeStatus::FAILURE: + { + if (retry_count_ < number_of_retries_) { + // halt first child and tick second child in next iteration + ControlNode::haltChild(0); + current_child_idx_++; + break; + } else { + // reset node and return failure when max retries has been exceeded + halt(); + return BT::NodeStatus::FAILURE; + } + } + + default: + throw BT::LogicError("A child node must never return IDLE"); + } // end switch + + } else if (current_child_idx_ == 1) { + switch (child_status) { + case BT::NodeStatus::SKIPPED: + { + // if we skip the recovery (maybe a precondition fails), then we + // should assume that no recovery is possible. For this reason, + // we should return FAILURE and reset the index. + // This does not count as a retry. + current_child_idx_ = 0; + ControlNode::haltChild(1); return BT::NodeStatus::FAILURE; } - } - - default: - throw BT::LogicError("A child node must never return IDLE"); - } // end switch - - } else if (current_child_idx_ == 1) { - switch (child_status) { - case BT::NodeStatus::SKIPPED: - { - // if we skip the recovery (maybe a precondition fails), then we - // should assume that no recovery is possible. For this reason, - // we should return FAILURE and reset the index. - // This does not count as a retry. - current_child_idx_ = 0; - ControlNode::haltChild(1); + case BT::NodeStatus::RUNNING: + return child_status; + + case BT::NodeStatus::SUCCESS: + { + // halt second child, increment recovery count, and tick first child in next iteration + ControlNode::haltChild(1); + retry_count_++; + current_child_idx_ = 0; + } + break; + + case BT::NodeStatus::FAILURE: + // reset node and return failure if second child fails + halt(); return BT::NodeStatus::FAILURE; - } - case BT::NodeStatus::RUNNING: - return child_status; - - case BT::NodeStatus::SUCCESS: - { - // halt second child, increment recovery count, and tick first child in next iteration - ControlNode::haltChild(1); - retry_count_++; - current_child_idx_ = 0; - return BT::NodeStatus::RUNNING; - } - case BT::NodeStatus::FAILURE: - // reset node and return failure if second child fails - halt(); - return BT::NodeStatus::FAILURE; + default: + throw BT::LogicError("A child node must never return IDLE"); + } // end switch + } + } // end while loop - default: - throw BT::LogicError("A child node must never return IDLE"); - } // end switch - } + // reset node and return failure halt(); - throw BT::LogicError("A recovery node has exactly 2 children and should never reach here"); + return BT::NodeStatus::FAILURE; } void RecoveryNode::halt() diff --git a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp index 532fcad45b2..82927031429 100644 --- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp @@ -105,7 +105,6 @@ TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child) first_child_->changeStatus(BT::NodeStatus::IDLE); EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); first_child_->changeStatus(BT::NodeStatus::FAILURE); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); second_child_->changeStatus(BT::NodeStatus::IDLE); EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); } @@ -120,9 +119,8 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry) first_child_->returnSuccessOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); } @@ -132,8 +130,8 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) // first child fails, second child fails first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::FAILURE); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); @@ -141,8 +139,6 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) first_child_->returnFailureOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); @@ -162,7 +158,6 @@ TEST_F(RecoveryNodeTestFixture, test_skipping) // first child fails, second child skipped first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SKIPPED); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); From 684a4d037241674550b756c5014711fa6c13e35e Mon Sep 17 00:00:00 2001 From: David Brown Date: Tue, 10 Dec 2024 20:51:40 +0100 Subject: [PATCH 3/3] new param to disable collision checking in DriveOnHeading and BackUp actions (#4785) * add parameter to the DriveOnHeading and BackUp actions to disable collision checking Signed-off-by: David Brown * update commander api Signed-off-by: David Brown * use a less verbose param name Signed-off-by: David Brown * fix flake8 error Signed-off-by: David Brown * rename param Signed-off-by: David Brown * set defaults in action messages and popular instance field for param Signed-off-by: David Brown * populate field in backup action as well Signed-off-by: David Brown * also add the new param to the spin action Signed-off-by: David Brown * fix incorrect type Signed-off-by: David Brown --------- Signed-off-by: David Brown Signed-off-by: David Brown Co-authored-by: David Brown --- .../plugins/action/back_up_action.hpp | 1 + .../plugins/action/drive_on_heading_action.hpp | 1 + .../nav2_behavior_tree/plugins/action/spin_action.hpp | 1 + nav2_behavior_tree/nav2_tree_nodes.xml | 3 +++ nav2_behavior_tree/plugins/action/back_up_action.cpp | 3 +++ .../plugins/action/drive_on_heading_action.cpp | 3 +++ .../test/plugins/action/test_back_up_action.cpp | 4 +++- .../plugins/action/test_drive_on_heading_action.cpp | 4 +++- .../test/plugins/action/test_spin_action.cpp | 4 +++- .../nav2_behaviors/plugins/drive_on_heading.hpp | 7 +++++++ .../include/nav2_behaviors/plugins/spin.hpp | 1 + nav2_behaviors/plugins/back_up.cpp | 1 + nav2_behaviors/plugins/spin.cpp | 5 +++++ nav2_msgs/action/BackUp.action | 1 + nav2_msgs/action/DriveOnHeading.action | 1 + nav2_msgs/action/Spin.action | 1 + nav2_simple_commander/README.md | 4 ++-- .../nav2_simple_commander/robot_navigator.py | 11 ++++++++--- 18 files changed, 48 insertions(+), 8 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp index 28722a1fcc0..37484cdbd40 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp @@ -80,6 +80,7 @@ class BackUpAction : public BtActionNode BT::InputPort("backup_dist", 0.15, "Distance to backup"), BT::InputPort("backup_speed", 0.025, "Speed at which to backup"), BT::InputPort("time_allowance", 10.0, "Allowed time for reversing"), + BT::InputPort("disable_collision_checks", false, "Disable collision checking"), BT::OutputPort( "error_code_id", "The back up behavior server error code") }); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp index dd1a6f29086..67730ac51cd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp @@ -59,6 +59,7 @@ class DriveOnHeadingAction : public BtActionNode("dist_to_travel", 0.15, "Distance to travel"), BT::InputPort("speed", 0.025, "Speed at which to travel"), BT::InputPort("time_allowance", 10.0, "Allowed time for driving on heading"), + BT::InputPort("disable_collision_checks", false, "Disable collision checking"), BT::OutputPort( "error_code_id", "The drive on heading behavior server error code") }); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index cad5e8ce4a8..711283fb076 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp @@ -64,6 +64,7 @@ class SpinAction : public BtActionNode BT::InputPort("spin_dist", 1.57, "Spin distance"), BT::InputPort("time_allowance", 10.0, "Allowed time for spinning"), BT::InputPort("is_recovery", true, "True if recovery"), + BT::InputPort("disable_collision_checks", false, "Disable collision checking"), BT::OutputPort( "error_code_id", "The spin behavior error code") }); diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 67059def0eb..5eae2516f74 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -13,6 +13,7 @@ Allowed time for reversing Server name Server timeout + Disable collision checking "Back up error code" @@ -22,6 +23,7 @@ Allowed time for reversing Server name Server timeout + Disable collision checking "Drive on heading error code" @@ -201,6 +203,7 @@ Allowed time for spinning Server name Server timeout + Disable collision checking Spin error code diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 3df77f98d7c..070cc4a88e5 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -36,6 +36,8 @@ void nav2_behavior_tree::BackUpAction::initialize() getInput("backup_speed", speed); double time_allowance; getInput("time_allowance", time_allowance); + bool disable_collision_checks; + getInput("disable_collision_checks", disable_collision_checks); // Populate the input message goal_.target.x = dist; @@ -43,6 +45,7 @@ void nav2_behavior_tree::BackUpAction::initialize() goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + goal_.disable_collision_checks = disable_collision_checks; } void BackUpAction::on_tick() diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp index 03c00344141..c5fa780a4c9 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -37,6 +37,8 @@ void DriveOnHeadingAction::initialize() getInput("speed", speed); double time_allowance; getInput("time_allowance", time_allowance); + bool disable_collision_checks; + getInput("disable_collision_checks", disable_collision_checks); // Populate the input message goal_.target.x = dist; @@ -44,6 +46,7 @@ void DriveOnHeadingAction::initialize() goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + goal_.disable_collision_checks = disable_collision_checks; initalized_ = true; } diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index 6974b351d38..98484bde8cb 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -131,18 +131,20 @@ TEST_F(BackUpActionTestFixture, test_ports) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(tree_->rootNode()->getInput("backup_dist"), 0.15); EXPECT_EQ(tree_->rootNode()->getInput("backup_speed"), 0.025); + EXPECT_EQ(tree_->rootNode()->getInput("disable_collision_checks"), false); xml_txt = R"( - + )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(tree_->rootNode()->getInput("backup_dist"), 2.0); EXPECT_EQ(tree_->rootNode()->getInput("backup_speed"), 0.26); + EXPECT_EQ(tree_->rootNode()->getInput("disable_collision_checks"), true); } TEST_F(BackUpActionTestFixture, test_tick) diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp index 99c3db72434..0c53fd54c8f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -128,18 +128,20 @@ TEST_F(DriveOnHeadingActionTestFixture, test_ports) EXPECT_EQ(tree_->rootNode()->getInput("dist_to_travel"), 0.15); EXPECT_EQ(tree_->rootNode()->getInput("speed"), 0.025); EXPECT_EQ(tree_->rootNode()->getInput("time_allowance"), 10.0); + EXPECT_EQ(tree_->rootNode()->getInput("disable_collision_checks"), false); xml_txt = R"( - + )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(tree_->rootNode()->getInput("dist_to_travel"), 2.0); EXPECT_EQ(tree_->rootNode()->getInput("speed"), 0.26); + EXPECT_EQ(tree_->rootNode()->getInput("disable_collision_checks"), true); } TEST_F(DriveOnHeadingActionTestFixture, test_tick) diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index 2d88d6e2092..134e9dc9c12 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -130,17 +130,19 @@ TEST_F(SpinActionTestFixture, test_ports) tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(tree_->rootNode()->getInput("spin_dist"), 1.57); + EXPECT_EQ(tree_->rootNode()->getInput("disable_collision_checks"), false); xml_txt = R"( - + )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(tree_->rootNode()->getInput("spin_dist"), 3.14); + EXPECT_EQ(tree_->rootNode()->getInput("disable_collision_checks"), true); } TEST_F(SpinActionTestFixture, test_tick) diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 0b74a044629..8a37f26a630 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -47,6 +47,7 @@ class DriveOnHeading : public TimedBehavior feedback_(std::make_shared()), command_x_(0.0), command_speed_(0.0), + command_disable_collision_checks_(false), simulate_ahead_time_(0.0) { } @@ -76,6 +77,7 @@ class DriveOnHeading : public TimedBehavior command_x_ = command->target.x; command_speed_ = command->speed; command_time_allowance_ = command->time_allowance; + command_disable_collision_checks_ = command->disable_collision_checks; end_time_ = this->clock_->now() + command_time_allowance_; @@ -168,6 +170,10 @@ class DriveOnHeading : public TimedBehavior const geometry_msgs::msg::Twist & cmd_vel, geometry_msgs::msg::Pose2D & pose2d) { + if (command_disable_collision_checks_) { + return true; + } + // Simulate ahead by simulate_ahead_time_ in this->cycle_frequency_ increments int cycle_count = 0; double sim_position_change; @@ -215,6 +221,7 @@ class DriveOnHeading : public TimedBehavior geometry_msgs::msg::PoseStamped initial_pose_; double command_x_; double command_speed_; + bool command_disable_collision_checks_; rclcpp::Duration command_time_allowance_{0, 0}; rclcpp::Time end_time_; double simulate_ahead_time_; diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp index eeb84462220..7ef1e05489f 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp @@ -89,6 +89,7 @@ class Spin : public TimedBehavior double max_rotational_vel_; double rotational_acc_lim_; double cmd_yaw_; + bool cmd_disable_collision_checks_; double prev_yaw_; double relative_yaw_; double simulate_ahead_time_; diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp index 7a194874c7e..137ebb1c3b3 100644 --- a/nav2_behaviors/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -30,6 +30,7 @@ ResultStatus BackUp::onRun(const std::shared_ptr comma command_x_ = -std::fabs(command->target.x); command_speed_ = -std::fabs(command->speed); command_time_allowance_ = command->time_allowance; + command_disable_collision_checks_ = command->disable_collision_checks; end_time_ = this->clock_->now() + command_time_allowance_; diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index 5965675f643..ce89eb34f23 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -91,6 +91,7 @@ ResultStatus Spin::onRun(const std::shared_ptr command) cmd_yaw_); command_time_allowance_ = command->time_allowance; + cmd_disable_collision_checks_ = command->disable_collision_checks; end_time_ = this->clock_->now() + command_time_allowance_; return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE}; @@ -164,6 +165,10 @@ bool Spin::isCollisionFree( const geometry_msgs::msg::Twist & cmd_vel, geometry_msgs::msg::Pose2D & pose2d) { + if (cmd_disable_collision_checks_) { + return true; + } + // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments int cycle_count = 0; double sim_position_change; diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action index 9be1d56988f..a3bd0cf1afa 100644 --- a/nav2_msgs/action/BackUp.action +++ b/nav2_msgs/action/BackUp.action @@ -3,6 +3,7 @@ geometry_msgs/Point target float32 speed builtin_interfaces/Duration time_allowance +bool disable_collision_checks False --- #result definition diff --git a/nav2_msgs/action/DriveOnHeading.action b/nav2_msgs/action/DriveOnHeading.action index a9bc79710d7..567fdc718ab 100644 --- a/nav2_msgs/action/DriveOnHeading.action +++ b/nav2_msgs/action/DriveOnHeading.action @@ -3,6 +3,7 @@ geometry_msgs/Point target float32 speed builtin_interfaces/Duration time_allowance +bool disable_collision_checks False --- #result definition diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action index 91288cbf205..5e0992ae9e0 100644 --- a/nav2_msgs/action/Spin.action +++ b/nav2_msgs/action/Spin.action @@ -1,6 +1,7 @@ #goal definition float32 target_yaw builtin_interfaces/Duration time_allowance +bool disable_collision_checks False --- #result definition diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 668fd9fe7c8..2b900437fec 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -23,8 +23,8 @@ New as of September 2023: the simple navigator constructor will accept a `namesp | goToPose(pose, behavior_tree='') | Requests the robot to drive to a pose (`PoseStamped`). | | followWaypoints(poses) | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose. | | followPath(path, controller_id='', goal_checker_id='') | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`. | -| spin(spin_dist=1.57, time_allowance=10) | Requests the robot to performs an in-place rotation by a given angle. | -| backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10) | Requests the robot to back up by a given distance. | +| spin(spin_dist=1.57, time_allowance=10, disable_collision_checks=False) | Requests the robot to performs an in-place rotation by a given angle. | +| backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10, disable_collision_checks=False) | Requests the robot to back up by a given distance. | | cancelTask() | Cancel an ongoing task request.| | isTaskComplete() | Checks if task is complete yet, times out at `100ms`. Returns `True` if completed and `False` if still going. | | getFeedback() | Gets feedback from task, returns action server feedback object. | diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index 4b604d55613..0e59df8814c 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -256,13 +256,14 @@ def followGpsWaypoints(self, gps_poses): self.result_future = self.goal_handle.get_result_async() return True - def spin(self, spin_dist=1.57, time_allowance=10): + def spin(self, spin_dist=1.57, time_allowance=10, disable_collision_checks=False): self.debug("Waiting for 'Spin' action server") while not self.spin_client.wait_for_server(timeout_sec=1.0): self.info("'Spin' action server not available, waiting...") goal_msg = Spin.Goal() goal_msg.target_yaw = spin_dist goal_msg.time_allowance = Duration(sec=time_allowance) + goal_msg.disable_collision_checks = disable_collision_checks self.info(f'Spinning to angle {goal_msg.target_yaw}....') send_goal_future = self.spin_client.send_goal_async( @@ -278,7 +279,8 @@ def spin(self, spin_dist=1.57, time_allowance=10): self.result_future = self.goal_handle.get_result_async() return True - def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): + def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10, + disable_collision_checks=False): self.debug("Waiting for 'Backup' action server") while not self.backup_client.wait_for_server(timeout_sec=1.0): self.info("'Backup' action server not available, waiting...") @@ -286,6 +288,7 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): goal_msg.target = Point(x=float(backup_dist)) goal_msg.speed = backup_speed goal_msg.time_allowance = Duration(sec=time_allowance) + goal_msg.disable_collision_checks = disable_collision_checks self.info(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....') send_goal_future = self.backup_client.send_goal_async( @@ -301,7 +304,8 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): self.result_future = self.goal_handle.get_result_async() return True - def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10): + def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10, + disable_collision_checks=False): self.debug("Waiting for 'DriveOnHeading' action server") while not self.backup_client.wait_for_server(timeout_sec=1.0): self.info("'DriveOnHeading' action server not available, waiting...") @@ -309,6 +313,7 @@ def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10): goal_msg.target = Point(x=float(dist)) goal_msg.speed = speed goal_msg.time_allowance = Duration(sec=time_allowance) + goal_msg.disable_collision_checks = disable_collision_checks self.info(f'Drive {goal_msg.target.x} m on heading at {goal_msg.speed} m/s....') send_goal_future = self.drive_on_heading_client.send_goal_async(