diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp index fe3abd98c6..54681f929f 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp @@ -38,11 +38,7 @@ class SpinAction : public BtActionNode void on_init() override { - // TODO(orduno) #423 Fixed spin angle - // 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_.target.quaternion = tf2::toMsg(quaternion); + goal_.target_yaw = M_PI / 2; } }; diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action index f129b99265..021df0d6d7 100644 --- a/nav2_msgs/action/Spin.action +++ b/nav2_msgs/action/Spin.action @@ -1,5 +1,5 @@ #goal definition -geometry_msgs/QuaternionStamped target +float32 target_yaw --- #result definition std_msgs/Empty result diff --git a/nav2_recoveries/include/nav2_recoveries/spin.hpp b/nav2_recoveries/include/nav2_recoveries/spin.hpp index 1d7475e718..4d0af3b697 100644 --- a/nav2_recoveries/include/nav2_recoveries/spin.hpp +++ b/nav2_recoveries/include/nav2_recoveries/spin.hpp @@ -41,11 +41,10 @@ class Spin : public Recovery double min_rotational_vel_; double max_rotational_vel_; double rotational_acc_lim_; - double goal_tolerance_angle_; - - geometry_msgs::msg::Pose initial_pose_; - double start_yaw_; - double command_yaw_; + double cmd_yaw_; + double prev_yaw_; + double delta_yaw_; + double relative_yaw_; }; } // namespace nav2_recoveries diff --git a/nav2_recoveries/src/spin.cpp b/nav2_recoveries/src/spin.cpp index adacfcb64d..4d1f3e508d 100644 --- a/nav2_recoveries/src/spin.cpp +++ b/nav2_recoveries/src/spin.cpp @@ -39,8 +39,9 @@ Spin::Spin(rclcpp::Node::SharedPtr & node) max_rotational_vel_ = 1.0; min_rotational_vel_ = 0.4; rotational_acc_lim_ = 3.2; - goal_tolerance_angle_ = 0.10; - start_yaw_ = 0.0; + prev_yaw_ = 0.0; + delta_yaw_ = 0.0; + relative_yaw_ = 0.0; } Spin::~Spin() @@ -49,21 +50,7 @@ Spin::~Spin() Status Spin::onRun(const std::shared_ptr command) { - double pitch, roll; - tf2::getEulerYPR(command->target.quaternion, command_yaw_, pitch, roll); - - if (roll != 0.0 || pitch != 0.0) { - RCLCPP_INFO(node_->get_logger(), "Spinning around Y and X axes are not supported, " - "will only spin around Z axis."); - } - - if (!getRobotPose(initial_pose_)) { - RCLCPP_ERROR(node_->get_logger(), "initial robot pose is not available."); - return Status::FAILED; - } - - start_yaw_ = tf2::getYaw(initial_pose_.orientation); - + cmd_yaw_ = -command->target_yaw; return Status::SUCCEEDED; } @@ -76,14 +63,17 @@ Status Spin::onCycleUpdate() } const double current_yaw = tf2::getYaw(current_pose.orientation); - const double yaw_diff = start_yaw_ - current_yaw; + delta_yaw_ = abs(abs(current_yaw) - abs(prev_yaw_)); + relative_yaw_ += delta_yaw_; + const double yaw_diff = relative_yaw_ - cmd_yaw_; geometry_msgs::msg::Twist cmd_vel; cmd_vel.linear.x = 0.0; cmd_vel.linear.y = 0.0; cmd_vel.angular.z = 0.0; - if (abs(yaw_diff) >= abs(command_yaw_)) { + if (relative_yaw_ >= abs(cmd_yaw_)) { + relative_yaw_ = 0.0; stopRobot(); return Status::SUCCEEDED; } @@ -92,7 +82,7 @@ Status Spin::onCycleUpdate() double vel = sqrt(2 * rotational_acc_lim_ * abs(yaw_diff)); vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); - command_yaw_ < 0 ? cmd_vel.angular.z = -vel : cmd_vel.angular.z = vel; + cmd_yaw_ < 0 ? cmd_vel.angular.z = -vel : cmd_vel.angular.z = vel; geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.position.x; @@ -106,6 +96,7 @@ Status Spin::onCycleUpdate() return Status::SUCCEEDED; } + prev_yaw_ = current_yaw; vel_publisher_->publishCommand(cmd_vel); return Status::RUNNING;