enabling closed-loop control in spinning

nav2_motion_primitives/include/nav2_motion_primitives/back_up.hpp

@@ -40,7 +40,7 @@ class BackUp : public MotionPrimitive<nav2_tasks::BackUpCommand, nav2_tasks::Bac
  double linear_acc_lim_;
 
  nav_msgs::msg::Odometry::SharedPtr initial_pose_;
- double command_x_;
+ double command_x_2_;
 
  nav2_tasks::TaskStatus controlledBackup();
 };

nav2_motion_primitives/include/nav2_motion_primitives/spin.hpp

@@ -36,19 +36,14 @@ 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();
 };
 
 } // namespace nav2_motion_primitives

nav2_motion_primitives/src/back_up.cpp

@@ -43,7 +43,7 @@ nav2_tasks::TaskStatus BackUp::onRun(const nav2_tasks::BackUpCommand::SharedPtr
  RCLCPP_INFO(node_->get_logger(), "Backing up in Y and Z not supported, "
  "will only move in X.");
  }
- command_x_ = command->x;
+ command_x_2_ = command->x * command->x;
  if (!robot_->getOdometry(initial_pose_)) {
  RCLCPP_ERROR(node_->get_logger(), "initial robot odom pose is not available.");
  return nav2_tasks::TaskStatus::FAILED;
@@ -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>();
@@ -79,15 +75,15 @@ nav2_tasks::TaskStatus BackUp::controlledBackup()
 
  double diff_x = initial_pose_->pose.pose.position.x - current_odom_pose->pose.pose.position.x;
  double diff_y = initial_pose_->pose.pose.position.y - current_odom_pose->pose.pose.position.y;
- double distance = sqrt(diff_x * diff_x + diff_y * diff_y);
+ double distance_2 = diff_x * diff_x + diff_y * diff_y;
 
- if (distance >= abs(command_x_)) {
+ if (distance_2 >= abs(command_x_2_)) {
  cmd_vel.linear.x = 0;
  robot_->sendVelocity(cmd_vel);
  return TaskStatus::SUCCEEDED;
  }
  // TODO(mhpanah): cmd_vel value should be passed as a parameter
- command_x_ < 0 ? cmd_vel.linear.x = -0.025 : cmd_vel.linear.x = 0.025;
+ command_x_2_ < 0 ? cmd_vel.linear.x = -0.025 : cmd_vel.linear.x = 0.025;
  robot_->sendVelocity(cmd_vel);
 
  return TaskStatus::RUNNING;

nav2_motion_primitives/src/spin.cpp

@@ -49,86 +49,37 @@ Spin::~Spin()
 
 nav2_tasks::TaskStatus Spin::onRun(const nav2_tasks::SpinCommand::SharedPtr command)
 {
- double yaw, pitch, roll;
- tf2::getEulerYPR(command->quaternion, 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(), "Currently only supported spinning by a fixed amount");
-
- start_time_ = std::chrono::system_clock::now();
-
+ double pitch, roll;
+ tf2::getEulerYPR(command->quaternion, start_yaw_, pitch, roll);
  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()
+nav2_tasks::TaskStatus Spin::rotateSome()
 {
- // 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()
-{
- // 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);
+ // Get distance left
+ const double current_yaw = tf2::getYaw(current_pose->pose.pose.orientation);
+ const double current_angle_dist = current_yaw - start_yaw_;
+ const double dist_left = M_PI - current_angle_dist;
 
- double current_angle = current_yaw - start_yaw_;
+ // TODO(stevemacenski) #553 check for collisions
 
- 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
+ // 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;
@@ -138,8 +89,8 @@ nav2_tasks::TaskStatus Spin::controlledSpin()
 
  robot_->sendVelocity(cmd_vel);
 
- // check if we are done
- if (dist_left >= (0.0 - goal_tolerance_angle_)) {
+ // Termination conditions
+ if (fabs(dist_left) < goal_tolerance_angle_) {
  return TaskStatus::SUCCEEDED;
  }