From d8be0c08b170b00e79014eff264ee1c12dceb379 Mon Sep 17 00:00:00 2001
From: "Carlos A. Orduno" <39749557+orduno@users.noreply.github.com>
Date: Thu, 16 May 2019 10:41:13 -0700
Subject: [PATCH] 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 | 6 ++++--
nav2_motion_primitives/src/spin.cpp | 2 +-
nav2_msgs/action/BackUp.action | 2 +-
nav2_msgs/action/Spin.action | 2 +-
nav2_tasks/include/nav2_tasks/back_up_action.hpp | 13 ++++++-------
nav2_tasks/include/nav2_tasks/spin_action.hpp | 9 ++++-----
6 files changed, 17 insertions(+), 17 deletions(-)
diff --git a/nav2_motion_primitives/src/back_up.cpp b/nav2_motion_primitives/src/back_up.cpp
index 00ead9ed..b93ce786 100644
--- a/nav2_motion_primitives/src/back_up.cpp
+++ b/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;
diff --git a/nav2_motion_primitives/src/spin.cpp b/nav2_motion_primitives/src/spin.cpp
index b49495dc..c395cbcf 100644
--- a/nav2_motion_primitives/src/spin.cpp
+++ b/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, "
diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action
index a5cf8f36..939cfce6 100644
--- a/nav2_msgs/action/BackUp.action
+++ b/nav2_msgs/action/BackUp.action
@@ -1,5 +1,5 @@
#goal definition
-geometry_msgs/Point target_location
+geometry_msgs/Point target
---
#result definition
std_msgs/Empty result
diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action
index ef3cff2a..f129b992 100644
--- a/nav2_msgs/action/Spin.action
+++ b/nav2_msgs/action/Spin.action
@@ -1,5 +1,5 @@
#goal definition
-geometry_msgs/QuaternionStamped goal
+geometry_msgs/QuaternionStamped target
---
#result definition
std_msgs/Empty result
diff --git a/nav2_tasks/include/nav2_tasks/back_up_action.hpp b/nav2_tasks/include/nav2_tasks/back_up_action.hpp
index 5975b959..13e40b97 100644
--- a/nav2_tasks/include/nav2_tasks/back_up_action.hpp
+++ b/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;
}
};
diff --git a/nav2_tasks/include/nav2_tasks/spin_action.hpp b/nav2_tasks/include/nav2_tasks/spin_action.hpp
index 78948006..f1d27dc3 100644
--- a/nav2_tasks/include/nav2_tasks/spin_action.hpp
+++ b/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);
}
};
--
GitLab