diff --git a/nav2_motion_primitives/include/nav2_motion_primitives/spin.hpp b/nav2_motion_primitives/include/nav2_motion_primitives/spin.hpp index 55a3fddc42..26838958cf 100644 --- a/nav2_motion_primitives/include/nav2_motion_primitives/spin.hpp +++ b/nav2_motion_primitives/include/nav2_motion_primitives/spin.hpp @@ -36,19 +36,16 @@ class Spin : public MotionPrimitive(); diff --git a/nav2_motion_primitives/src/spin.cpp b/nav2_motion_primitives/src/spin.cpp index ff93e7b297..895bbb7eaf 100644 --- a/nav2_motion_primitives/src/spin.cpp +++ b/nav2_motion_primitives/src/spin.cpp @@ -40,8 +40,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; + goal_tolerance_angle_ = 0.17; + direction_ = 1.0; + commanded_dist_ = 0.0; } Spin::~Spin() @@ -50,65 +51,41 @@ Spin::~Spin() nav2_tasks::TaskStatus Spin::onRun(const nav2_tasks::SpinCommand::SharedPtr command) { - double yaw, pitch, roll; - tf2::getEulerYPR(command->quaternion, yaw, pitch, roll); + double pitch_holder, roll_holder; + if (!robot_->getOdometry(initial_pose_)) { + RCLCPP_ERROR(node_->get_logger(), "initial robot odom pose is not available."); + return nav2_tasks::TaskStatus::FAILED; + } + + // getEulerYPR are normalized angles (-M_PI, M_PI] + tf2::getEulerYPR(command->quaternion, commanded_dist_, pitch_holder, roll_holder); - if (roll != 0.0 || pitch != 0.0) { - RCLCPP_INFO(node_->get_logger(), "Spinning on Y and X not supported, " + if (roll_holder != 0.0 || pitch_holder != 0.0) { + RCLCPP_WARN(node_->get_logger(), "Spinning on Y and X not supported, " "will only spin in Z."); } - RCLCPP_INFO(node_->get_logger(), "Currently only supported spinning by a fixed amount"); - - start_time_ = std::chrono::system_clock::now(); + // gets unsigned distance and turning direction + if (commanded_dist_ < 0.0) { + commanded_dist_ += 2 * M_PI; + direction_ = -1.0; + } else { + direction_ = 1.0; + } return nav2_tasks::TaskStatus::SUCCEEDED; } nav2_tasks::TaskStatus Spin::onCycleUpdate(nav2_tasks::SpinResult & result) { - // Currently only an open-loop controller is implemented - // TODO(orduno) #423 Create a base class for open-loop controlled motion_primitives - // controlledSpin() has not been fully tested - TaskStatus status = timedSpin(); - - // For now sending an empty task result + TaskStatus status = rotateSome(); nav2_tasks::SpinResult empty_result; result = empty_result; - return status; } -nav2_tasks::TaskStatus Spin::timedSpin() -{ - // Output control command - geometry_msgs::msg::Twist cmd_vel; - - // TODO(orduno) #423 fixed speed - cmd_vel.linear.x = 0.0; - cmd_vel.linear.y = 0.0; - cmd_vel.angular.z = 0.5; - robot_->sendVelocity(cmd_vel); - - // TODO(orduno) #423 fixed time - auto current_time = std::chrono::system_clock::now(); - if (current_time - start_time_ >= 6s) { // almost 180 degrees - // Stop the robot - cmd_vel.angular.z = 0.0; - robot_->sendVelocity(cmd_vel); - - return TaskStatus::SUCCEEDED; - } - - return TaskStatus::RUNNING; -} - -nav2_tasks::TaskStatus Spin::controlledSpin() +nav2_tasks::TaskStatus Spin::rotateSome() { - // 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_->getCurrentPose(current_pose)) { @@ -116,31 +93,27 @@ nav2_tasks::TaskStatus Spin::controlledSpin() return TaskStatus::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; + // Get distance left + const double current_angle_dist = tf2::angleShortestPath(current_pose, initial_pose_); + const double dist_left = commanded_dist_ - fabs(current_angle_dist); - // TODO(orduno) #379 forward simulation to check if future position is feasible + // TODO(stevemacenski) #533 check for collisions - // 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 + // Compute the velocity, clip, and send 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; + cmd_vel.angular.z = vel * direction_; robot_->sendVelocity(cmd_vel); - // check if we are done - if (dist_left >= (0.0 - goal_tolerance_angle_)) { + // Termination conditions + if (dist_left < goal_tolerance_angle_) { + cmd_vel.angular.z = 0.0; + robot_->sendVelocity(cmd_vel); return TaskStatus::SUCCEEDED; }