implemented controlled spin

nav2_recoveries/include/nav2_recoveries/spin.hpp

@@ -43,13 +43,9 @@ class Spin : public Recovery<SpinAction>
   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

nav2_recoveries/src/spin.cpp

@@ -49,50 +49,50 @@ Spin::~Spin()
 
 Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> 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<geometry_msgs::msg::PoseWithCovarianceStamped>();
-  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