Update spin and backup bt actions to use ROS2 actions (#725)

* Updating the spin & backup behavior tree action nodes to use ROS2 actions.

* Deleting the stop primitive.

nav2_motion_primitives/src/back_up.cpp

@@ -38,11 +38,13 @@ BackUp::~BackUp()
 
 Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
 {
-  if (command->target_location.y != 0.0 || command->target_location.z != 0.0) {
+  if (command->target.y != 0.0 || command->target.z != 0.0) {
     RCLCPP_INFO(node_->get_logger(), "Backing up in Y and Z not supported, "
       "will only move in X.");
   }
-  command_x_ = command->target_location.x;
+  
+  command_x_ = command->target.x;
+
   if (!robot_->getOdometry(initial_pose_)) {
     RCLCPP_ERROR(node_->get_logger(), "initial robot odom pose is not available.");
     return Status::FAILED;

nav2_motion_primitives/src/spin.cpp

@@ -50,7 +50,7 @@ Spin::~Spin()
 Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)
 {
   double yaw, pitch, roll;
-  tf2::getEulerYPR(command->goal.quaternion, yaw, pitch, roll);
+  tf2::getEulerYPR(command->target.quaternion, yaw, pitch, roll);
 
   if (roll != 0.0 || pitch != 0.0) {
     RCLCPP_INFO(node_->get_logger(), "Spinning on Y and X not supported, "

nav2_msgs/action/BackUp.action

 #goal definition
-geometry_msgs/Point target_location
+geometry_msgs/Point target
 ---
 #result definition
 std_msgs/Empty result

nav2_msgs/action/Spin.action

 #goal definition
-geometry_msgs/QuaternionStamped goal
+geometry_msgs/QuaternionStamped target
 ---
 #result definition
 std_msgs/Empty result

nav2_tasks/include/nav2_tasks/back_up_action.hpp

@@ -19,27 +19,26 @@
 #include <memory>
 #include <cmath>
 
-#include "nav2_tasks/bt_conversions.hpp"
 #include "nav2_tasks/bt_action_node.hpp"
-#include "nav2_tasks/back_up_task.hpp"
+#include "nav2_msgs/action/back_up.hpp"
 
 namespace nav2_tasks
 {
 
-class BackUpAction : public BtActionNode<BackUpCommand, BackUpResult>
+class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
 {
 public:
   explicit BackUpAction(const std::string & action_name)
-  : BtActionNode<BackUpCommand, BackUpResult>(action_name)
+  : BtActionNode<nav2_msgs::action::BackUp>(action_name)
   {
   }
 
   void on_init() override
   {
     // Populate the input message
-    goal->x = -0.15;
-    goal->y = 0.0;
-    goal->z = 0.0;
+    goal_.target.x = -0.15;
+    goal_.target.y = 0.0;
+    goal_.target.z = 0.0;
   }
 };
 

nav2_tasks/include/nav2_tasks/spin_action.hpp

@@ -19,9 +19,8 @@
 #include <memory>
 #include <cmath>
 
-#include "nav2_tasks/bt_conversions.hpp"
 #include "nav2_tasks/bt_action_node.hpp"
-#include "nav2_tasks/spin_task.hpp"
+#include "nav2_msgs/action/spin.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "tf2/LinearMath/Quaternion.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
@@ -29,11 +28,11 @@
 namespace nav2_tasks
 {
 
-class SpinAction : public BtActionNode<SpinCommand, SpinResult>
+class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
 {
 public:
   explicit SpinAction(const std::string & action_name)
-  : BtActionNode<SpinCommand, SpinResult>(action_name)
+  : BtActionNode<nav2_msgs::action::Spin>(action_name)
   {
   }
 
@@ -43,7 +42,7 @@ public:
     // Rotate 90deg CCW
     tf2::Quaternion quaternion;
     quaternion.setRPY(0, 0, M_PI / 2);  // yaw, pitch and roll are rotation in z, y, x respectively
-    goal_->quaternion = tf2::toMsg(quaternion);
+    goal_.target.quaternion = tf2::toMsg(quaternion);
   }
 };