From 4323421b369387bf7cff1197eecda5e103c4dbe4 Mon Sep 17 00:00:00 2001 From: AnastasiaPittel Date: Thu, 4 May 2023 15:12:50 +0300 Subject: [PATCH 1/4] draft --- .../plugins/action/back_up_action.hpp | 2 - .../action/drive_on_heading_action.hpp | 5 ++- .../plugins/action/back_up_action.cpp | 6 --- .../action/drive_on_heading_action.cpp | 11 ++++++ nav2_behaviors/plugins/back_up.cpp | 7 ++-- nav2_behaviors/plugins/drive_on_heading.hpp | 39 +++++++++++++++++++ nav2_msgs/action/BackUp.action | 2 - nav2_msgs/action/DriveOnHeading.action | 2 + 8 files changed, 59 insertions(+), 15 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 2747ac23b12..af4a1e48f35 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 @@ -56,8 +56,6 @@ 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("free_goal_vel", false, "Don't stop when goal reached"), - BT::InputPort("check_local_costmap", true, "Check local costmap for collisions") }); } }; 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 5632a7551e6..d4a5ecf60c1 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 @@ -40,6 +40,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("time_allowance", 10.0, "Allowed time for driving on heading"), + BT::InputPort("free_goal_vel", false, "Don't stop when goal reached"), + BT::InputPort("check_local_costmap", true, "Check local costmap for collisions") }); } }; diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 8b30ead7869..b72afaf8106 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -32,10 +32,6 @@ BackUpAction::BackUpAction( getInput("backup_speed", speed); double time_allowance; getInput("time_allowance", time_allowance); - bool free_goal_vel; - getInput("free_goal_vel", free_goal_vel); - bool check_local_costmap; - getInput("check_local_costmap", check_local_costmap); // Populate the input message goal_.target.x = dist; @@ -43,8 +39,6 @@ BackUpAction::BackUpAction( goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); - goal_.free_goal_vel = free_goal_vel; - goal_.check_local_costmap = check_local_costmap; } 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 81a93a63628..3ff5beea4ff 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -32,6 +32,10 @@ DriveOnHeadingAction::DriveOnHeadingAction( getInput("speed", speed); double time_allowance; getInput("time_allowance", time_allowance); + bool free_goal_vel; + getInput("free_goal_vel", free_goal_vel); + bool check_local_costmap; + getInput("check_local_costmap", check_local_costmap); // Populate the input message goal_.target.x = dist; @@ -39,6 +43,13 @@ DriveOnHeadingAction::DriveOnHeadingAction( goal_.target.z = 0.0; goal_.speed = speed; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); + goal_.free_goal_vel = free_goal_vel; + goal_.check_local_costmap = check_local_costmap; +} + +void DriveOnHeadingAction::on_tick() +{ + increment_recovery_count(); } } // namespace nav2_behavior_tree diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp index ce1b8e20cf3..90a69a686ed 100644 --- a/nav2_behaviors/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -26,10 +26,9 @@ Status BackUp::onRun(const std::shared_ptr command) return Status::FAILED; } - command_x_ = command->target.x; - command_speed_ = std::fabs(command->speed); - free_goal_vel = command->free_goal_vel; - check_local_costmap = command->check_local_costmap; + // Silently ensure that both the speed and direction are negative. + command_x_ = -std::fabs(command->target.x); + command_speed_ = -std::fabs(command->speed); command_time_allowance_ = command->time_allowance; end_time_ = steady_clock_.now() + command_time_allowance_; diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/plugins/drive_on_heading.hpp index 3edfdb86403..45f501652d4 100644 --- a/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/plugins/drive_on_heading.hpp @@ -24,7 +24,9 @@ #include "nav2_msgs/action/drive_on_heading.hpp" #include "nav2_msgs/action/back_up.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_msgs/action/back_up.hpp" +using BackUpAction = nav2_msgs::action::BackUp; namespace nav2_behaviors { @@ -74,6 +76,9 @@ class DriveOnHeading : public TimedBehavior command_speed_ = command->speed; command_time_allowance_ = command->time_allowance; + free_goal_vel = command->free_goal_vel; + check_local_costmap = command->check_local_costmap; + end_time_ = this->steady_clock_.now() + command_time_allowance_; if (!nav2_util::getCurrentPose( @@ -214,6 +219,40 @@ class DriveOnHeading : public TimedBehavior bool check_local_costmap; }; + +template<> Status DriveOnHeading::onRun(const std::shared_ptr command) +{ + if (command->target.y != 0.0 || command->target.z != 0.0) { + RCLCPP_INFO( + this->logger_, + "DrivingOnHeading in Y and Z not supported, will only move in X."); + return Status::FAILED; + } + + // Ensure that both the speed and direction have the same sign + if (!((command->target.x > 0.0) == (command->speed > 0.0)) ) { + RCLCPP_ERROR(this->logger_, "Speed and command sign did not match"); + return Status::FAILED; + } + + command_x_ = command->target.x; + command_speed_ = command->speed; + command_time_allowance_ = command->time_allowance; + + + end_time_ = this->steady_clock_.now() + command_time_allowance_; + + if (!nav2_util::getCurrentPose( + initial_pose_, *this->tf_, this->global_frame_, this->robot_base_frame_, + this->transform_tolerance_)) + { + RCLCPP_ERROR(this->logger_, "Initial robot pose is not available."); + return Status::FAILED; + } + + return Status::SUCCEEDED; +} + } // namespace nav2_behaviors #endif // NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_ diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action index 85346321746..9937302578a 100644 --- a/nav2_msgs/action/BackUp.action +++ b/nav2_msgs/action/BackUp.action @@ -2,8 +2,6 @@ geometry_msgs/Point target float32 speed builtin_interfaces/Duration time_allowance -bool free_goal_vel -bool check_local_costmap --- #result definition builtin_interfaces/Duration total_elapsed_time diff --git a/nav2_msgs/action/DriveOnHeading.action b/nav2_msgs/action/DriveOnHeading.action index 9937302578a..85346321746 100644 --- a/nav2_msgs/action/DriveOnHeading.action +++ b/nav2_msgs/action/DriveOnHeading.action @@ -2,6 +2,8 @@ geometry_msgs/Point target float32 speed builtin_interfaces/Duration time_allowance +bool free_goal_vel +bool check_local_costmap --- #result definition builtin_interfaces/Duration total_elapsed_time From 3b3f4d15c1b50cb3f8b48a9e03aebb49dc725f5d Mon Sep 17 00:00:00 2001 From: AnastasiaPittel Date: Thu, 4 May 2023 15:32:49 +0300 Subject: [PATCH 2/4] draft --- .../plugins/action/drive_on_heading_action.cpp | 5 ----- 1 file changed, 5 deletions(-) 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 3ff5beea4ff..73f01259d33 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -47,11 +47,6 @@ DriveOnHeadingAction::DriveOnHeadingAction( goal_.check_local_costmap = check_local_costmap; } -void DriveOnHeadingAction::on_tick() -{ - increment_recovery_count(); -} - } // namespace nav2_behavior_tree #include "behaviortree_cpp_v3/bt_factory.h" From d5d2f860b00e22ff92c4ffd8c19b5754f3f156fb Mon Sep 17 00:00:00 2001 From: AnastasiaPittel Date: Thu, 4 May 2023 16:56:29 +0300 Subject: [PATCH 3/4] draft --- nav2_behaviors/plugins/drive_on_heading.hpp | 34 --------------------- nav2_msgs/action/BackUp.action | 2 ++ 2 files changed, 2 insertions(+), 34 deletions(-) diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/plugins/drive_on_heading.hpp index 45f501652d4..3a438616094 100644 --- a/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/plugins/drive_on_heading.hpp @@ -219,40 +219,6 @@ class DriveOnHeading : public TimedBehavior bool check_local_costmap; }; - -template<> Status DriveOnHeading::onRun(const std::shared_ptr command) -{ - if (command->target.y != 0.0 || command->target.z != 0.0) { - RCLCPP_INFO( - this->logger_, - "DrivingOnHeading in Y and Z not supported, will only move in X."); - return Status::FAILED; - } - - // Ensure that both the speed and direction have the same sign - if (!((command->target.x > 0.0) == (command->speed > 0.0)) ) { - RCLCPP_ERROR(this->logger_, "Speed and command sign did not match"); - return Status::FAILED; - } - - command_x_ = command->target.x; - command_speed_ = command->speed; - command_time_allowance_ = command->time_allowance; - - - end_time_ = this->steady_clock_.now() + command_time_allowance_; - - if (!nav2_util::getCurrentPose( - initial_pose_, *this->tf_, this->global_frame_, this->robot_base_frame_, - this->transform_tolerance_)) - { - RCLCPP_ERROR(this->logger_, "Initial robot pose is not available."); - return Status::FAILED; - } - - return Status::SUCCEEDED; -} - } // namespace nav2_behaviors #endif // NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_ diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action index 9937302578a..85346321746 100644 --- a/nav2_msgs/action/BackUp.action +++ b/nav2_msgs/action/BackUp.action @@ -2,6 +2,8 @@ geometry_msgs/Point target float32 speed builtin_interfaces/Duration time_allowance +bool free_goal_vel +bool check_local_costmap --- #result definition builtin_interfaces/Duration total_elapsed_time From 787fd971f9cb2e83ab4acdffd4bdf1b210a0eff0 Mon Sep 17 00:00:00 2001 From: AnastasiaPittel Date: Thu, 4 May 2023 16:59:12 +0300 Subject: [PATCH 4/4] draft --- nav2_behaviors/plugins/drive_on_heading.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/plugins/drive_on_heading.hpp index 3a438616094..dd495a02c71 100644 --- a/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/plugins/drive_on_heading.hpp @@ -24,9 +24,7 @@ #include "nav2_msgs/action/drive_on_heading.hpp" #include "nav2_msgs/action/back_up.hpp" #include "nav2_util/node_utils.hpp" -#include "nav2_msgs/action/back_up.hpp" -using BackUpAction = nav2_msgs::action::BackUp; namespace nav2_behaviors {