diff --git a/nav2_motion_primitives/src/back_up.cpp b/nav2_motion_primitives/src/back_up.cpp index 00ead9ed4cb1adcebf2730e604357457a833090d..b93ce7860919b5c70025b0f145ccd83df2009525 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 b49495dc673c69deae6f7c7ca488af96195229c0..c395cbcfffa451b6011238f693789c8658da1003 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 a5cf8f3625341b7ace4dd10aaaaaedf46925aac3..939cfce668966454cf226cf00f6cc604588e3a43 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 ef3cff2aaa86c411b636ab74dd678bbdeeb5b712..f129b99265f0d50833f96f26b74d5c0dbc119196 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 5975b9596039efb47ef580f11b90e2fbf6e63ca8..13e40b971d23b47e2af160122ed15850cee2e360 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 7894800664ddf47fd58ecf92d1e5f56d972ae896..f1d27dc31f9e295eb22ecdffba93c7374cb8f4b9 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); } };