enabling closed-loop control in spinning

nav2_motion_primitives/include/nav2_motion_primitives/spin.hpp

@@ -36,19 +36,16 @@ class Spin : public MotionPrimitive<nav2_tasks::SpinCommand, nav2_tasks::SpinRes
 
   nav2_tasks::TaskStatus onCycleUpdate(nav2_tasks::SpinResult & result) override;
 
-protected:
+private:
+  nav2_tasks::TaskStatus rotateSome();
+
   double min_rotational_vel_;
   double max_rotational_vel_;
   double rotational_acc_lim_;
   double goal_tolerance_angle_;
-
-  double start_yaw_;
-
-  std::chrono::system_clock::time_point start_time_;
-
-  nav2_tasks::TaskStatus timedSpin();
-
-  nav2_tasks::TaskStatus controlledSpin();
+  double direction_;
+  double commanded_dist_;
+  nav_msgs::msg::Odometry::SharedPtr initial_pose_;
 };
 
 }  // namespace nav2_motion_primitives

nav2_motion_primitives/src/back_up.cpp

@@ -55,15 +55,11 @@ nav2_tasks::TaskStatus BackUp::onRun(const nav2_tasks::BackUpCommand::SharedPtr
 nav2_tasks::TaskStatus BackUp::onCycleUpdate(nav2_tasks::BackUpResult & result)
 {
   TaskStatus status = controlledBackup();
-
-  // For now sending an empty task result
   nav2_tasks::BackUpResult empty_result;
   result = empty_result;
-
   return status;
 }
 
-
 nav2_tasks::TaskStatus BackUp::controlledBackup()
 {
   auto current_odom_pose = std::shared_ptr<nav_msgs::msg::Odometry>();

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,97 +51,69 @@ 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<geometry_msgs::msg::PoseWithCovarianceStamped>();
   if (!robot_->getCurrentPose(current_pose)) {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     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;
   }