toid_behaviors/CMakeLists.txt

@@ -27,6 +27,8 @@ set(
 
 set(
   SOURCES
+  src/scl.cpp
+  src/move_coords.cpp
   src/simple_move.cpp
   src/simple_rotate.cpp
   src/simple_translate_x.cpp

toid_behaviors/include/toid_behaviors/move_coords.hpp

@@ -0,0 +1,64 @@
+#pragma once
+
+#include "toid_behaviors/simple_move.hpp"
+#include "toid_behaviors/scl.hpp"
+#include "toid_msgs/action/simple_move_coords.hpp"
+
+#include "geometry_msgs/msg/pose.hpp"
+
+namespace toid
+{
+using MoveAction = toid_msgs::action::SimpleMoveCoords;
+using namespace nav2_behaviors;
+
+class MoveCoords : public SimpleMove<MoveAction>
+{
+public:
+  MoveCoords();
+  ~MoveCoords();
+
+  void configureCB() override;
+
+  ResultStatus onStart(
+    const std::shared_ptr<const MoveAction::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:
+
+  SmoothControlLaw scl;
+
+  //Goal
+  geometry_msgs::msg::Pose target_pose_;
+  double target_angle_;
+  double target_sign_;
+  bool backwards_;
+
+  //State
+  double last_speed_;
+
+  //Config
+  double max_vel_accel_;
+  double max_vel_decel_;
+  double max_vel_speed_;
+  double min_vel_speed_;
+
+  double max_angular_speed_;
+
+  double kp_;
+  double k_phi_;
+  double k_delta_;
+  double beta_;
+  double lambda_;
+  double slowdown_radius_;
+};
+
+}  // namespace toid
+

toid_behaviors/include/toid_behaviors/scl.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace toid
+{
+class SmoothControlLaw
+{
+public:
+  double k_phi = 5.0;
+  double k_delta = 2.0;
+  double bbeta = 0.4;
+  double lam = 2.0;
+
+  double slowdown_radius = 0.20;
+  double v_linear_min = 0.05;
+  double v_linear_max = 0.1;
+  double v_angular_max = 2.0;
+
+  void egocentric_polar(
+    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
+    double & r, double & phi, double & delta);
+
+  double curvature(double r, double phi, double delta);
+
+  void calculate_vel(
+    const geometry_msgs::msg::Pose & target,const geometry_msgs::msg::Pose & current,
+    geometry_msgs::msg::Twist & out_speed, bool backwards = false);
+};
+}  // namespace toid

toid_behaviors/src/move_coords.cpp

@@ -0,0 +1,136 @@
+#include "toid_behaviors/move_coords.hpp"
+
+#include "angles/angles.h"
+
+namespace toid
+{
+
+MoveCoords::MoveCoords() : SimpleMove<MoveAction>() {}
+MoveCoords::~MoveCoords() {}
+
+void MoveCoords::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.10));
+  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_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".kphi", rclcpp::ParameterValue(5.0));
+  node->get_parameter(behavior_name_ + ".kphi", k_phi_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".kdelta", rclcpp::ParameterValue(2.0));
+  node->get_parameter(behavior_name_ + ".kdelta", k_delta_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".beta", rclcpp::ParameterValue(0.4));
+  node->get_parameter(behavior_name_ + ".beta", beta_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".lambda", rclcpp::ParameterValue(2.0));
+  node->get_parameter(behavior_name_ + ".lambda", lambda_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".slow_down_radius", rclcpp::ParameterValue(0.20));
+  node->get_parameter(behavior_name_ + ".slow_down_radius", slowdown_radius_);
+}
+
+ResultStatus MoveCoords::onStart(
+  const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose &,
+  const geometry_msgs::msg::Twist & vel)
+{
+  target_pose_.position.x = command->x;
+  target_pose_.position.y = command->y;
+  backwards_ = command->backwards;
+
+  target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta));
+  target_angle_ = command->theta;
+  target_sign_ = backwards_ ? -1.0 : 1.0;
+
+  scl.k_phi = k_phi_;
+  scl.k_delta = k_delta_;
+  scl.bbeta = beta_;
+  scl.lam = lambda_;
+  scl.slowdown_radius = slowdown_radius_;
+  scl.v_angular_max = max_angular_speed_;
+  scl.v_linear_min = min_vel_speed_;
+  scl.v_linear_max = max_vel_speed_;
+
+  last_speed_ = vel.angular.x;
+
+  return ResultStatus{Status::SUCCEEDED};
+}
+
+ResultStatus MoveCoords::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);
+  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
+
+  if (backwards_) {
+    angle_dist = angles::two_pi_complement(angle_dist);
+  }
+
+  const double dx = target_pose_.position.x - pose.position.x;
+  const double dy = target_pose_.position.y - pose.position.y;
+
+  const double dist_left = target_sign_ * (dx * cos(target_angle_) + dy * sin(target_angle_));
+
+  const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_;
+  const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_;
+
+  if (dist_left <= 0.001) {
+    out_vel.linear.x = 0;
+    out_vel.angular.z = 0;
+    return ResultStatus{Status::SUCCEEDED};
+  }
+
+  double vel = max_vel_speed_;
+  double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
+  vel = std::min(vel, max_vel_to_stop);
+
+
+  if (dist_left <= 0.02) {
+    out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+    out_vel.angular.z = std::clamp(target_sign_ * kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
+    last_speed_ = out_vel.linear.x;
+    return ResultStatus{Status::RUNNING};
+  }
+
+  scl.v_linear_max = target_sign_ * std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+  scl.calculate_vel(target_pose_, pose, out_vel, backwards_);
+  last_speed_ = out_vel.linear.x;
+
+  RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); 
+  RCLCPP_INFO_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl.v_linear_max); 
+
+  return ResultStatus{Status::RUNNING};
+}
+
+}  // namespace toid
+
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(toid::MoveCoords, nav2_core::Behavior);

toid_behaviors/src/scl.cpp

@@ -0,0 +1,62 @@
+#include "toid_behaviors/scl.hpp"
+
+#include <cmath>
+
+#include "angles/angles.h"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+
+namespace toid {
+void SmoothControlLaw::calculate_vel(
+  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
+  geometry_msgs::msg::Twist &out_speed, bool backwards)
+{
+  double r, phi, delta;
+  egocentric_polar(target, current, backwards, r, phi, delta);
+
+  double curvature = this->curvature(r, phi, delta);
+  curvature = backwards? -curvature : curvature;
+
+  double v = v_linear_max / (1.0 + bbeta * std::pow(fabs(curvature), lam));
+
+  v = std::min(v_linear_max * (r / slowdown_radius), v);
+  v = std::clamp(v, v_linear_min, v_linear_max);
+  v = backwards ? -v : v;
+
+  const double w = curvature * v;
+  const double w_bound = std::clamp(w, -v_angular_max, v_angular_max);
+
+  v = (curvature != 0.0) ? (w_bound / curvature) : v;
+  out_speed.linear.x = v;
+  out_speed.angular.z = w_bound;
+}
+
+double SmoothControlLaw::curvature(double r, double phi, double delta)
+{
+  if (r < 1e-6) {
+    return 0.0;
+  }
+  const float prop = k_delta * (delta - std::atan(-k_phi * phi));
+  const float feedback = (1 + k_phi / (1 + (k_phi * phi) * (k_phi * phi))) * std::sin(delta);
+
+  return -1.0 / r * (prop + feedback);
+}
+
+void SmoothControlLaw::egocentric_polar(
+  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards, double & r,
+  double & phi, double & delta)
+{
+  const double dx = target.position.x - current.position.x;
+  const double dy = target.position.y - current.position.y;
+  const double los = backwards ? (std::atan2(-dy, dx) + M_PI) : std::atan2(-dy, dx);
+
+  const double ttheta = tf2::getYaw(target.orientation);
+  const double ctheta = tf2::getYaw(current.orientation);
+
+  r = std::hypot(dx, dy);
+  phi = angles::normalize_angle(ttheta + los);
+  delta = angles::normalize_angle(ctheta + los);
+}
+
+}

toid_behaviors/toid_behaviors.xml

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

toid_msgs/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/SendDouble.srv"
+  "action/SimpleMoveCoords.action"
   "action/SimpleRotate.action"
   "action/SimpleTranslateX.action"
 )

toid_msgs/action/SimpleMoveCoords.action

@@ -0,0 +1,15 @@
+uint8 IGNORE_OBSTACLES=1
+
+float64 x
+float64 y
+float64 theta
+uint8 backwards 0
+uint8 mode 0
+---
+uint16 NONE=0
+uint16 TF_ERROR=1
+
+builtin_interfaces/Duration total_elapsed_time
+uint16 error_code
+string error_msg
+---

toid_navigation/params/toid_general_params.yaml

@@ -22,7 +22,7 @@ behavior_server:
     local_footprint_topic: local_costmap/published_footprint
     global_footprint_topic: global_costmap/published_footprint
     cycle_frequency: 50.0
-    behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate", "translateX"]
+    behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate", "translateX", "moveCoords"]
     spin:
       plugin: "nav2_behaviors::Spin"
     backup:
@@ -39,6 +39,8 @@ behavior_server:
       max_angular_decel: 1.0
     translateX:
       plugin: "toid::SimpleTranslateXBehavior"
+    moveCoords:
+      plugin: "toid::MoveCoords"
     local_frame: map
     global_frame: map
     robot_base_frame: base_footprint