Added simple translation behavior

toid_behaviors/CMakeLists.txt

@@ -29,6 +29,7 @@ set(
   SOURCES
   src/simple_move.cpp
   src/simple_rotate.cpp
+  src/simple_translate_x.cpp
 )
 
 find_package(ament_cmake REQUIRED)

toid_behaviors/include/toid_behaviors/simple_translate_x.hpp

@@ -0,0 +1,53 @@
+#pragma once
+
+#include "toid_behaviors/simple_move.hpp"
+#include "toid_msgs/action/simple_translate_x.hpp"
+
+namespace toid
+{
+using TranslateAction = toid_msgs::action::SimpleTranslateX;
+using namespace nav2_behaviors;
+
+class SimpleTranslateXBehavior : public SimpleMove<TranslateAction>
+{
+public:
+  SimpleTranslateXBehavior();
+  ~SimpleTranslateXBehavior();
+
+  void configureCB() override;
+
+  ResultStatus onStart(
+    const std::shared_ptr<const TranslateAction::Goal> command, const geometry_msgs::msg::Pose & pose,
+    const geometry_msgs::msg::Twist & vel) override;
+
+  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() override {
+    return nav2_core::CostmapInfoType::NONE;
+  }
+
+protected:
+
+  //Goal
+  double target_distance_;
+  double target_angle_;
+  double target_sign_;
+  geometry_msgs::msg::Pose initial_pose_;
+
+  //State
+  double speed_;
+
+  //Config
+  double max_vel_accel_;
+  double max_vel_decel_;
+  double max_vel_speed_;
+  double min_vel_speed_;
+  double max_angular_speed_;
+
+  double kp_;
+};
+
+}  // namespace toid
+

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

toid_behaviors/toid_behaviors.xml

@@ -3,5 +3,8 @@
         <class type="toid::SimpleRotateBehavior" base_class_type="nav2_core::Behavior">
             <description></description>
         </class>
+        <class type="toid::SimpleTranslateXBehavior" base_class_type="nav2_core::Behavior">
+            <description></description>
+        </class>
     </library>
 </class_libraries>