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;
  }