Added move coords behavior

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