Added simple translation behavior

toid_behaviors/src/simple_translate_x.cpp

@@ -0,0 +1,96 @@
+#include "toid_behaviors/simple_translate_x.hpp"
+
+#include <cmath>
+
+#include "angles/angles.h"
+#include "tf2/convert.hpp"
+
+namespace toid
+{
+
+SimpleTranslateXBehavior::SimpleTranslateXBehavior() : SimpleMove<TranslateAction>() {}
+SimpleTranslateXBehavior::~SimpleTranslateXBehavior() {}
+
+void SimpleTranslateXBehavior::configureCB()
+{
+  auto node = node_.lock();
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_vel_accel", rclcpp::ParameterValue(2.0));
+  node->get_parameter(behavior_name_ + ".max_vel_accel", max_vel_accel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_vel_decel", rclcpp::ParameterValue(1.0));
+  node->get_parameter(behavior_name_ + ".max_vel_decel", max_vel_decel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_vel_speed", rclcpp::ParameterValue(0.4));
+  node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".min_vel_speed", rclcpp::ParameterValue(0.007));
+  node->get_parameter(behavior_name_ + ".min_vel_speed", min_vel_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".max_angular_speed", rclcpp::ParameterValue(1.0));
+  node->get_parameter(behavior_name_ + ".max_angular_speed", max_angular_speed_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".kp", rclcpp::ParameterValue(5.0));
+  node->get_parameter(behavior_name_ + ".kp", kp_);
+}
+
+ResultStatus SimpleTranslateXBehavior::onStart(
+  const std::shared_ptr<const TranslateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+  const geometry_msgs::msg::Twist & vel)
+{
+  target_distance_ = command->distance;
+  target_angle_ = tf2::getYaw(pose.orientation);
+  target_sign_ = (target_distance_ < 0) ? -1.0 : 1.0;
+
+  target_distance_ *= target_sign_;
+
+  initial_pose_ = pose;
+  speed_ = vel.angular.x;
+
+  return ResultStatus{Status::SUCCEEDED};
+}
+
+ResultStatus SimpleTranslateXBehavior::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_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
+
+  const double dx = pose.position.x - initial_pose_.position.x;
+  const double dy = pose.position.y - initial_pose_.position.y;
+  const double dist_passed = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_));
+
+  const double lower_bound = speed_ - control_duration_ * max_vel_accel_;
+  const double upper_bound = speed_ + control_duration_ * max_vel_accel_;
+
+  float vel = std::clamp(max_vel_speed_, lower_bound, upper_bound);
+  float max_vel_to_stop =
+    0.8 * std::sqrt(2.0 * max_vel_decel_ * std::abs(target_distance_ - dist_passed));
+
+  vel = std::min(vel, max_vel_to_stop);
+  const double w = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
+
+  if (dist_passed >= target_distance_) {
+    out_vel.linear.x = 0;
+    out_vel.angular.z = 0;
+    return ResultStatus{Status::SUCCEEDED};
+  }
+
+  out_vel.linear.x = target_sign_ * vel;
+  out_vel.angular.z = w;
+
+  speed_ = vel;
+
+  return ResultStatus{Status::RUNNING};
+}
+
+}  // namespace toid
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::SimpleTranslateXBehavior, nav2_core::Behavior);