Completed implementation of initial rotate behavior

toid_behaviors/include/toid_behaviors/simple_move.hpp

@@ -41,6 +41,7 @@ public:
         current_vel_ = msg.twist.twist;
       });
     control_duration_ = 1.0 / this->cycle_frequency_;
+    configureCB();
   }
 
   void onCleanup() override

toid_behaviors/include/toid_behaviors/simple_rotate.hpp

@@ -12,6 +12,7 @@ class SimpleRotateBehavior : public SimpleMove<RotateAction>
 {
 public:
   SimpleRotateBehavior();
+  ~SimpleRotateBehavior();
 
   void configureCB() override;
 
@@ -22,6 +23,10 @@ public:
   ResultStatus updateVel(
     const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
     geometry_msgs::msg::Twist & out_vel) override;
+  
+  virtual nav2_core::CostmapInfoType getResourceInfo() {
+    return nav2_core::CostmapInfoType::NONE;
+  }
 
 protected:
 
@@ -33,11 +38,13 @@ protected:
   //State
   double angular_speed_;
   double last_angle_;
+  double last_time_;
 
   //Config
   double max_angular_speed_;
   double min_angular_speed_;
   double max_angular_accel_;
+  double max_angular_decel_;
 };
 
 }  // namespace toid

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

toid_navigation/params/toid_general_params.yaml

@@ -21,20 +21,22 @@ behavior_server:
     global_costmap_topic: global_costmap/costmap_raw
     local_footprint_topic: local_costmap/published_footprint
     global_footprint_topic: global_costmap/published_footprint
-    cycle_frequency: 10.0
-    behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait"]
+    cycle_frequency: 50.0
+    behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate"]
     spin:
       plugin: "nav2_behaviors::Spin"
     backup:
       plugin: "nav2_behaviors::BackUp"
-      enable_stamped_cmd_vel: true
-      simulate_ahead_time: 0.2
     drive_on_heading:
       plugin: "nav2_behaviors::DriveOnHeading"
     wait:
       plugin: "nav2_behaviors::Wait"
     assisted_teleop:
       plugin: "nav2_behaviors::AssistedTeleop"
+    rotate:
+      plugin: "toid::SimpleRotateBehavior"
+      max_angular_accel: 4.0
+      max_angular_decel: 1.0
     local_frame: map
     global_frame: map
     robot_base_frame: base_footprint