fixed angle discontinuity issue

ros-navigation · Jul 29, 2019 · 6cbf137 · 6cbf137
1 parent bfed78f
commit 6cbf137
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 31 deletions.
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/spin_action.hpp
@@ -38,11 +38,7 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
 
   void on_init() override
   {
-    // TODO(orduno) #423 Fixed spin angle
-    // Rotate 90deg CCW
-    tf2::Quaternion quaternion;
-    quaternion.setRPY(0, 0, M_PI / 2);  // yaw, pitch and roll are rotation in z, y, x respectively
-    goal_.target.quaternion = tf2::toMsg(quaternion);
+    goal_.target_yaw = M_PI / 2;
   }
 };
 

diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action
@@ -1,5 +1,5 @@
 #goal definition
-geometry_msgs/QuaternionStamped target
+float32 target_yaw
 ---
 #result definition
 std_msgs/Empty result

diff --git a/nav2_recoveries/include/nav2_recoveries/spin.hpp b/nav2_recoveries/include/nav2_recoveries/spin.hpp
@@ -41,11 +41,10 @@ class Spin : public Recovery<SpinAction>
   double min_rotational_vel_;
   double max_rotational_vel_;
   double rotational_acc_lim_;
-  double goal_tolerance_angle_;
-
-  geometry_msgs::msg::Pose initial_pose_;
-  double start_yaw_;
-  double command_yaw_;
+  double cmd_yaw_;
+  double prev_yaw_;
+  double delta_yaw_;
+  double relative_yaw_;
 };
 
 }  // namespace nav2_recoveries

diff --git a/nav2_recoveries/src/spin.cpp b/nav2_recoveries/src/spin.cpp
@@ -39,8 +39,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;
+  prev_yaw_ = 0.0;
+  delta_yaw_ = 0.0;
+  relative_yaw_ = 0.0;
 }
 
 Spin::~Spin()
@@ -49,21 +50,7 @@ Spin::~Spin()
 
 Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)
 {
-  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 around Y and X axes are not supported, "
-      "will only spin around Z axis.");
-  }
-
-  if (!getRobotPose(initial_pose_)) {
-    RCLCPP_ERROR(node_->get_logger(), "initial robot pose is not available.");
-    return Status::FAILED;
-  }
-
-  start_yaw_ = tf2::getYaw(initial_pose_.orientation);
-
+  cmd_yaw_ = -command->target_yaw;
   return Status::SUCCEEDED;
 }
 
@@ -76,14 +63,17 @@ Status Spin::onCycleUpdate()
   }
 
   const double current_yaw = tf2::getYaw(current_pose.orientation);
-  const double yaw_diff = start_yaw_ - current_yaw;
+  delta_yaw_ = abs(abs(current_yaw) - abs(prev_yaw_));
+  relative_yaw_ += delta_yaw_;
+  const double yaw_diff = relative_yaw_ - cmd_yaw_;
 
   geometry_msgs::msg::Twist cmd_vel;
   cmd_vel.linear.x = 0.0;
   cmd_vel.linear.y = 0.0;
   cmd_vel.angular.z = 0.0;
 
-  if (abs(yaw_diff) >= abs(command_yaw_)) {
+  if (relative_yaw_ >= abs(cmd_yaw_)) {
+    relative_yaw_ = 0.0;
     stopRobot();
     return Status::SUCCEEDED;
   }
@@ -92,7 +82,7 @@ Status Spin::onCycleUpdate()
   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;
+  cmd_yaw_ < 0 ? cmd_vel.angular.z = -vel : cmd_vel.angular.z = vel;
 
   geometry_msgs::msg::Pose2D pose2d;
   pose2d.x = current_pose.position.x;
@@ -106,6 +96,7 @@ Status Spin::onCycleUpdate()
     return Status::SUCCEEDED;
   }
 
+  prev_yaw_ = current_yaw;
   vel_publisher_->publishCommand(cmd_vel);
 
   return Status::RUNNING;