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..73f01259d33 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,8 @@ 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; } } // 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..dd495a02c71 100644 --- a/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/plugins/drive_on_heading.hpp @@ -74,6 +74,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( 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