AMRNAV-1988 Migrate backup BT nodes to DriveOnHeading

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp

@@ -56,8 +56,6 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
         BT::InputPort<double>("backup_dist", 0.15, "Distance to backup"),
         BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing"),
-        BT::InputPort<bool>("free_goal_vel", false, "Don't stop when goal reached"),
-        BT::InputPort<bool>("check_local_costmap", true, "Check local costmap for collisions")
       });
   }
 };

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp

@@ -40,6 +40,7 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
     const std::string & action_name,
     const BT::NodeConfiguration & conf);
 
+  void on_tick() override;
   /**
    * @brief Creates list of BT ports
    * @return BT::PortsList Containing basic ports along with node-specific ports
@@ -50,7 +51,9 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
       {
         BT::InputPort<double>("dist_to_travel", 0.15, "Distance to travel"),
         BT::InputPort<double>("speed", 0.025, "Speed at which to travel"),
-        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading")
+        BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading"),
+        BT::InputPort<bool>("free_goal_vel", false, "Don't stop when goal reached"),
+        BT::InputPort<bool>("check_local_costmap", true, "Check local costmap for collisions")
       });
   }
 };

nav2_behavior_tree/plugins/action/back_up_action.cpp

@@ -32,19 +32,13 @@ 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;
   goal_.target.y = 0.0;
   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()

nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp

@@ -32,13 +32,19 @@ 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;
   goal_.target.y = 0.0;
   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

nav2_behaviors/plugins/back_up.cpp

@@ -26,10 +26,9 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> 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_;

nav2_behaviors/plugins/drive_on_heading.hpp

@@ -74,6 +74,9 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     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(

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