Added simple rotate behavior

toid_behaviors/src/simple_rotate.cpp

@@ -0,0 +1,87 @@
+#include "toid_behaviors/simple_rotate.hpp"
+
+#include <cmath>
+
+#include "angles/angles.h"
+#include "tf2/convert.hpp"
+
+namespace toid
+{
+
+SimpleRotateBehavior::SimpleRotateBehavior() {}
+
+void SimpleRotateBehavior::configureCB()
+{
+  auto node = node_.lock();
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_vel", rclcpp::ParameterValue(2.0));
+
+  node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".min_angular_vel", rclcpp::ParameterValue(0.2));
+  node->get_parameter(behavior_name_ + ".min_angular_vel", min_angular_speed_);
+
+  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_);
+}
+
+ResultStatus SimpleRotateBehavior::onStart(
+  const std::shared_ptr<const RotateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+  const geometry_msgs::msg::Twist & vel)
+{
+  target_angle_ = command->angle;
+  min_turn_angle_ = abs(command->min_angle);
+  initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
+
+  last_angle_ = tf2::getYaw(pose.orientation);
+
+  angular_speed_ = vel.angular.z;
+
+  return ResultStatus{Status::SUCCEEDED};
+}
+
+ResultStatus SimpleRotateBehavior::updateVel(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist &,
+  geometry_msgs::msg::Twist & out_vel)
+{
+  const double current_yaw = tf2::getYaw(pose.orientation);
+  const double angle_change = angles::normalize_angle(current_yaw - last_angle_);
+  last_angle_ = current_yaw;
+
+  if (min_turn_angle_ > 0.0) {
+    min_turn_angle_ = fmax(0.0, min_turn_angle_ - initial_direction_ * angle_change);
+  }
+
+  const double upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_;
+  const double lower_vel_ = angular_speed_ - max_angular_accel_ * control_duration_;
+
+  double anglular_distance_to_target =
+    angles::shortest_angular_distance(current_yaw, target_angle_);
+  double sign = (anglular_distance_to_target >= 0.0) ? 1.0 : -1.0;
+
+  if (min_turn_angle_ > 0.0 && initial_direction_ != sign) {
+    anglular_distance_to_target = angles::two_pi_complement(anglular_distance_to_target);
+    sign = initial_direction_;
+  }
+
+  const double angular_distance_to_heading =
+    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));
+
+  const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
+  const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
+
+  if (min_turn_angle_ == 0 && std::fabs(current_yaw - target_angle_) < 0.01) {
+    return ResultStatus{Status::SUCCEEDED};
+  }
+
+  out_vel.angular.z = speed;
+
+  return ResultStatus{Status::RUNNING};
+}
+
+}  // namespace toid