diff --git a/nav2_recoveries/include/nav2_recoveries/spin.hpp b/nav2_recoveries/include/nav2_recoveries/spin.hpp index a20274f511..1d7475e718 100644 --- a/nav2_recoveries/include/nav2_recoveries/spin.hpp +++ b/nav2_recoveries/include/nav2_recoveries/spin.hpp @@ -43,13 +43,9 @@ class Spin : public Recovery double rotational_acc_lim_; double goal_tolerance_angle_; + geometry_msgs::msg::Pose initial_pose_; double start_yaw_; - - std::chrono::system_clock::time_point start_time_; - - Status timedSpin(); - - Status controlledSpin(); + double command_yaw_; }; } // namespace nav2_recoveries diff --git a/nav2_recoveries/src/spin.cpp b/nav2_recoveries/src/spin.cpp index 247fc71bd0..adacfcb64d 100644 --- a/nav2_recoveries/src/spin.cpp +++ b/nav2_recoveries/src/spin.cpp @@ -49,50 +49,50 @@ Spin::~Spin() Status Spin::onRun(const std::shared_ptr command) { - double yaw, pitch, roll; - tf2::getEulerYPR(command->target.quaternion, yaw, pitch, roll); + 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 on Y and X not supported, " - "will only spin in Z."); + RCLCPP_INFO(node_->get_logger(), "Spinning around Y and X axes are not supported, " + "will only spin around Z axis."); } - RCLCPP_INFO(node_->get_logger(), "Currently only supported spinning by a fixed amount"); + if (!getRobotPose(initial_pose_)) { + RCLCPP_ERROR(node_->get_logger(), "initial robot pose is not available."); + return Status::FAILED; + } - start_time_ = std::chrono::system_clock::now(); + start_yaw_ = tf2::getYaw(initial_pose_.orientation); return Status::SUCCEEDED; } Status Spin::onCycleUpdate() { - // Currently only an open-loop time-based controller is implemented - // The closed-loop version 'controlledSpin()' has not been fully tested - return timedSpin(); -} + geometry_msgs::msg::Pose current_pose; + if (!getRobotPose(current_pose)) { + RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + return Status::FAILED; + } + + const double current_yaw = tf2::getYaw(current_pose.orientation); + const double yaw_diff = start_yaw_ - current_yaw; -Status Spin::timedSpin() -{ - // Output control command geometry_msgs::msg::Twist cmd_vel; + cmd_vel.linear.x = 0.0; + cmd_vel.linear.y = 0.0; + cmd_vel.angular.z = 0.0; - // TODO(orduno) #423 fixed time - auto current_time = std::chrono::system_clock::now(); - if (current_time - start_time_ >= 6s) { // almost 180 degrees + if (abs(yaw_diff) >= abs(command_yaw_)) { stopRobot(); return Status::SUCCEEDED; } - // TODO(orduno) #423 fixed speed - cmd_vel.linear.x = 0.0; - cmd_vel.linear.y = 0.0; - cmd_vel.angular.z = 0.5; - geometry_msgs::msg::Pose current_pose; - if (!getRobotPose(current_pose)) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); - return Status::FAILED; - } + 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; geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.position.x; @@ -111,49 +111,4 @@ Status Spin::timedSpin() return Status::RUNNING; } -Status Spin::controlledSpin() -{ - // TODO(orduno) #423 Test and tune controller - // check it doesn't abruptly start and stop - // or cause massive wheel slippage when accelerating - - // Get current robot orientation - auto current_pose = std::make_shared(); - if (!robot_state_->getCurrentPose(current_pose)) { - RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); - return Status::FAILED; - } - - double current_yaw = tf2::getYaw(current_pose->pose.pose.orientation); - - double current_angle = current_yaw - start_yaw_; - - double dist_left = M_PI - current_angle; - - // TODO(orduno) #379 forward simulation to check if future position is feasible - - // compute the velocity that will let us stop by the time we reach the goal - // v_f^2 == v_i^2 + 2 * a * d - // solving for v_i if v_f = 0 - double vel = sqrt(2 * rotational_acc_lim_ * dist_left); - - // limit velocity - vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); - - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.linear.x = 0.0; - cmd_vel.linear.y = 0.0; - cmd_vel.angular.z = vel; - - vel_publisher_->publishCommand(cmd_vel); - - // check if we are done - if (dist_left >= (0.0 - goal_tolerance_angle_)) { - stopRobot(); - return Status::SUCCEEDED; - } - - return Status::RUNNING; -} - } // namespace nav2_recoveries