Completed implementation of initial rotate behavior

toid_behaviors/src/simple_rotate.cpp

@@ -8,7 +8,8 @@
 namespace toid
 {
 
-SimpleRotateBehavior::SimpleRotateBehavior() {}
+SimpleRotateBehavior::SimpleRotateBehavior() : SimpleMove<RotateAction>() {}
+SimpleRotateBehavior::~SimpleRotateBehavior() {}
 
 void SimpleRotateBehavior::configureCB()
 {
@@ -25,6 +26,10 @@ void SimpleRotateBehavior::configureCB()
   nav2_util::declare_parameter_if_not_declared(
     node, behavior_name_ + ".max_angular_accel", rclcpp::ParameterValue(4.0));
   node->get_parameter(behavior_name_ + ".max_angular_accel", max_angular_accel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(4.0));
+  node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_);
 }
 
 ResultStatus SimpleRotateBehavior::onStart(
@@ -38,6 +43,7 @@ ResultStatus SimpleRotateBehavior::onStart(
   last_angle_ = tf2::getYaw(pose.orientation);
 
   angular_speed_ = vel.angular.z;
+  last_time_ = clock_->now().seconds();
 
   return ResultStatus{Status::SUCCEEDED};
 }
@@ -70,7 +76,7 @@ ResultStatus SimpleRotateBehavior::updateVel(
     sign * fmax(std::fabs(anglular_distance_to_target), min_turn_angle_);
 
   const double speed_until_overshoot =
-    0.9 * std::sqrt(2.0 * max_angular_accel_ * std::fabs(angular_distance_to_heading));
+    std::sqrt(2.0 * max_angular_accel_ * std::fabs(angular_distance_to_heading));
 
   const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
   const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
@@ -79,9 +85,12 @@ ResultStatus SimpleRotateBehavior::updateVel(
     return ResultStatus{Status::SUCCEEDED};
   }
 
+  angular_speed_ = speed;
   out_vel.angular.z = speed;
-
   return ResultStatus{Status::RUNNING};
 }
 
-}  // namespace toid
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::SimpleRotateBehavior, nav2_core::Behavior);