added collision detection

toid_behaviors/include/toid_behaviors/move_coords.hpp

@@ -23,17 +23,24 @@ public:
     const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose & pose,
     const geometry_msgs::msg::Twist & vel) override;
 
+  double distanceToTarget(
+    const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point, const double target_theta, bool backwards);
+
+  double velocityTarget(const double dist_left);
+  
+  bool collisionDetection(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Pose &last_ok_pose);
+
   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;
+    return nav2_core::CostmapInfoType::LOCAL;
   }
 
 protected:
 
-  SmoothControlLaw scl;
+  SmoothControlLaw scl_;
 
   //Goal
   geometry_msgs::msg::Pose target_pose_;

toid_behaviors/include/toid_behaviors/scl.hpp

@@ -19,13 +19,17 @@ public:
   double v_angular_max = 2.0;
 
   void egocentric_polar(
-    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
+    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
-    double & r, double & phi, double & delta);
+    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,
+    const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current,
     geometry_msgs::msg::Twist & out_speed, bool backwards = false);
+
+  void step(
+    const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt,
+    bool backwards = false);
 };
 }  // namespace toid

toid_behaviors/include/toid_behaviors/simple_move.hpp

@@ -6,6 +6,7 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "toid_msgs/action/simple_rotate.hpp"
+#include "toid_msgs/msg/rival.hpp"
 
 namespace toid
 {
@@ -14,6 +15,8 @@ template <typename ActionT>
 class SimpleMove : public nav2_behaviors::TimedBehavior<ActionT>
 {
 public:
+  using Rival = toid_msgs::msg::Rival;
+
   virtual void configureCB() {}
   virtual void cleanupCB() {}
 
@@ -34,6 +37,18 @@ public:
     nav2_util::declare_parameter_if_not_declared(
       node, "odom_topic", rclcpp::ParameterValue("/odom"));
     std::string odom_topic_name = node->get_parameter("odom_topic").as_string();
+
+    nav2_util::declare_parameter_if_not_declared(node, "robot_width", rclcpp::ParameterValue(0.30));
+    node->get_parameter("robot_width", robot_width_);
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, "robot_length", rclcpp::ParameterValue(0.30));
+    node->get_parameter("robot_length", robot_length_);
+
+    nav2_util::declare_parameter_if_not_declared(
+      node, "rival_radius", rclcpp::ParameterValue(0.30));
+    node->get_parameter("rival_radius", rival_radius_);
+
     odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
       odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) {
         std::lock_guard lock(mut_);
@@ -52,12 +67,17 @@ public:
   void activate() override
   {
     nav2_behaviors::TimedBehavior<ActionT>::activate();
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock();
+    using namespace std::placeholders;
+    rivals_sub_ = node->create_subscription<Rival>(
+      "/dynamicObstacle", 1, std::bind(&SimpleMove<ActionT>::rival_cb, this, _1));
     activateCB();
   }
 
   void deactivate() override
   {
     nav2_behaviors::TimedBehavior<ActionT>::deactivate();
+    rivals_sub_.reset();
     deactivateCB();
   }
 
@@ -105,12 +125,51 @@ public:
     return r;
   }
 
+  bool check_rival_collision(geometry_msgs::msg::Pose2D & pose)
+  {
+    if (!rivals_) {
+      return false;
+    }
+    const double cosp = std::cos(pose.theta);
+    const double sinp = std::sin(pose.theta);
+    for (const auto & rival : rivals_->point) {
+      geometry_msgs::msg::Point local_rival;
+      const double dx = rival.x - pose.x;
+      const double dy = rival.y - pose.y;
+      local_rival.x = dx * cosp + dy * sinp;
+      local_rival.y = -dx * sinp + dy * cosp;
+
+      const double qx = std::abs(local_rival.x) - robot_length_ / 2.0;
+      const double qy = std::abs(local_rival.y) - robot_width_ / 2.0;
+
+      const double mqx = std::max(qx, 0.0);
+      const double mqy = std::max(qy, 0.0);
+
+      double length = std::sqrt(mqx * mqx + mqy * mqy);
+
+      double sdf = length + std::min(std::max(qx, qy), 0.0);
+      if (sdf < rival_radius_) {
+        return true;
+      }
+    }
+    return false;
+  }
+
+  void rival_cb(Rival::SharedPtr msg) { rivals_ = msg; }
+
 protected:
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
   geometry_msgs::msg::Pose current_pose_;
   geometry_msgs::msg::Twist current_vel_;
   std::recursive_mutex mut_;
   double control_duration_;
+
+  double robot_width_ = 0.30;
+  double robot_length_ = 0.30;
+  double rival_radius_ = 0.30;
+
+  Rival::SharedPtr rivals_;
+  rclcpp::Subscription<Rival>::SharedPtr rivals_sub_;
 };
 
 }  // namespace toid

toid_behaviors/include/toid_behaviors/simple_rotate.hpp

@@ -2,6 +2,7 @@
 
 #include "toid_behaviors/simple_move.hpp"
 #include "toid_msgs/action/simple_rotate.h"
+#include "toid_msgs/msg/rival.hpp"
 
 namespace toid
 {
@@ -23,17 +24,25 @@ public:
   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 {
+  virtual nav2_core::CostmapInfoType getResourceInfo() override
-    return nav2_core::CostmapInfoType::NONE;
+  {
+    return nav2_core::CostmapInfoType::LOCAL;
   }
 
-protected:
+  void calc_err_and_sign(
+    double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign);
 
+  double check_space(const geometry_msgs::msg::Pose pose, const double e, const double sign);
+
+  double calc_speed(const double err, const double sign, const double angular_speed);
+
+protected:
   //Goal
   double target_angle_;
   double min_turn_angle_;
   double initial_direction_;
+  unsigned char mode_;
 
   //State
   double angular_speed_;
@@ -45,6 +54,7 @@ protected:
   double min_angular_speed_;
   double max_angular_accel_;
   double max_angular_decel_;
+  double lookahead_;
 };
 
 }  // namespace toid

toid_behaviors/src/move_coords.cpp

@@ -69,39 +69,83 @@ ResultStatus MoveCoords::onStart(
   target_sign_ = backwards_ ? -1.0 : 1.0;
   max_vel_speed_ = command->max_speed;
 
-  if(command->max_speed == 0) {
+  if (command->max_speed == 0) {
     auto node = node_.lock();
     node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);
   }
 
-  scl.k_phi = k_phi_;
+  scl_.k_phi = k_phi_;
-  scl.k_delta = k_delta_;
+  scl_.k_delta = k_delta_;
-  scl.bbeta = beta_;
+  scl_.bbeta = beta_;
-  scl.lam = lambda_;
+  scl_.lam = lambda_;
-  scl.slowdown_radius = slowdown_radius_;
+  scl_.slowdown_radius = slowdown_radius_;
-  scl.v_angular_max = max_angular_speed_;
+  scl_.v_angular_max = max_angular_speed_;
-  scl.v_linear_min = min_vel_speed_;
+  scl_.v_linear_min = min_vel_speed_;
-  scl.v_linear_max = max_vel_speed_;
+  scl_.v_linear_max = max_vel_speed_;
 
   last_speed_ = vel.angular.x;
 
   return ResultStatus{Status::SUCCEEDED};
 }
 
+double MoveCoords::distanceToTarget(
+  const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Point & target_point,
+  const double target_theta, bool backwards)
+{
+  const double dx = target_point.x - pose.position.x;
+  const double dy = target_point.y - pose.position.y;
+  const double target_sign = backwards? -1.0 : 1.0;
+
+  return target_sign * (dx * cos(target_theta) + dy * sin(target_theta));
+}
+
+
+double MoveCoords::velocityTarget(const double dist_left) {
+  const double lower_bound = last_speed_ - control_duration_ * max_vel_accel_;
+  const double upper_bound = last_speed_ + control_duration_ * max_vel_accel_;
+
+
+  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);
+  return std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+}
+
+bool MoveCoords::collisionDetection(const geometry_msgs::msg::Pose &pose, geometry_msgs::msg::Pose &last_ok_pose) {
+  const int samples = static_cast<int>(0.5/control_duration_);
+  geometry_msgs::msg::Pose current_pose = pose;
+  last_ok_pose = pose;
+  for(int i = 0; i < samples; i++) {
+    scl_.step(target_pose_, current_pose, control_duration_, backwards_);
+
+    geometry_msgs::msg::Pose2D p;
+    p.x = current_pose.position.x;
+    p.y = current_pose.position.y;
+    p.theta = tf2::getYaw(current_pose.orientation);
+
+    if(!local_collision_checker_->isCollisionFree(p, i==0)) {
+      return true;
+    }
+
+    if(check_rival_collision(p)) {
+      return true;
+    }
+
+    last_ok_pose = current_pose;
+    const double dist_left = distanceToTarget(current_pose, target_pose_.position, target_angle_, backwards_);
+    if(dist_left < 0.005) {
+      return false;
+    }
+  }
+  return false;
+}
+
 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_);
 
-  const double dx = target_pose_.position.x - pose.position.x;
+  double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_);
-  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;
@@ -109,30 +153,52 @@ ResultStatus MoveCoords::updateVel(
     return ResultStatus{Status::SUCCEEDED};
   }
 
-  double vel = max_vel_speed_;
+  const double current_yaw = tf2::getYaw(pose.orientation);
-  double max_vel_to_stop = 0.8 * std::sqrt(2.0 * max_vel_decel_ * dist_left);
+  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
-  vel = std::min(vel, max_vel_to_stop);
 
+  geometry_msgs::msg::Pose last_ok_pose;
+  if(collisionDetection(pose, last_ok_pose)) {
+
+    dist_left = distanceToTarget(pose, last_ok_pose.position, tf2::getYaw(last_ok_pose.orientation), backwards_);
+    if(dist_left <= 0.02) {
+      out_vel.linear.x = 0;
+      out_vel.angular.z = 0;
+    } else {
+      scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
+      scl_.calculate_vel(last_ok_pose, pose, out_vel, backwards_);
+    }
+
+
+
+    last_speed_ = out_vel.linear.x;
+
+    RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
+    RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl_.v_linear_max);
+
+    return ResultStatus{Status::RUNNING};
+  }
 
   if (dist_left <= 0.02) {
-    out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound);
+    out_vel.linear.x = velocityTarget(dist_left);
     out_vel.angular.z = std::clamp(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_);
+  scl_.v_linear_max = target_sign_ * velocityTarget(dist_left);
+  scl_.calculate_vel(target_pose_, pose, out_vel, backwards_);
+
+
   last_speed_ = out_vel.linear.x;
 
-  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left); 
+  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Distance Left: %lf", dist_left);
-  RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 1000, "Max Speed: %lf", scl.v_linear_max); 
+  RCLCPP_DEBUG_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

@@ -3,20 +3,21 @@
 #include <cmath>
 
 #include "angles/angles.h"
+#include "nav2_util/geometry_utils.hpp"
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
-
+namespace toid
-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)
+  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;
+  curvature = backwards ? -curvature : curvature;
 
   double v = v_linear_max / (1.0 + bbeta * std::pow(fabs(curvature), lam));
 
@@ -44,8 +45,8 @@ double SmoothControlLaw::curvature(double r, double phi, double delta)
 }
 
 void SmoothControlLaw::egocentric_polar(
-  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards, double & r,
+  const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & current, bool backwards,
-  double & phi, double & delta)
+  double & r, double & phi, double & delta)
 {
   const double dx = target.position.x - current.position.x;
   const double dy = target.position.y - current.position.y;
@@ -59,4 +60,20 @@ void SmoothControlLaw::egocentric_polar(
   delta = angles::normalize_angle(ctheta + los);
 }
 
-}
+void SmoothControlLaw::step(
+  const geometry_msgs::msg::Pose & target, geometry_msgs::msg::Pose & current, double dt,
+  bool backwards)
+{
+  geometry_msgs::msg::Twist twist;
+  calculate_vel(target, current, twist, backwards);
+
+  double theta = tf2::getYaw(current.orientation);
+  double dx = twist.linear.x * dt * cos(theta);
+  double dy = twist.linear.x * dt * sin(theta);
+  current.orientation =
+    nav2_util::geometry_utils::orientationAroundZAxis(theta + twist.angular.z * dt);
+  current.position.x += dx;
+  current.position.y += dy;
+}
+
+}  // namespace toid

toid_behaviors/src/simple_rotate.cpp

@@ -4,6 +4,7 @@
 
 #include "angles/angles.h"
 #include "tf2/convert.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 namespace toid
 {
@@ -30,6 +31,10 @@ void SimpleRotateBehavior::configureCB()
   nav2_util::declare_parameter_if_not_declared(
     node, behavior_name_ + ".max_angular_decel", rclcpp::ParameterValue(4.0));
   node->get_parameter(behavior_name_ + ".max_angular_decel", max_angular_decel_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, behavior_name_ + ".lookahead", rclcpp::ParameterValue(0.5));
+  node->get_parameter(behavior_name_ + ".lookahead", lookahead_);
 }
 
 ResultStatus SimpleRotateBehavior::onStart(
@@ -40,12 +45,19 @@ ResultStatus SimpleRotateBehavior::onStart(
   min_turn_angle_ = abs(command->min_angle);
   initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
   max_angular_speed_ = command->max_speed;
+  mode_ = command->mode;
 
-  if(command->max_speed == 0) {
+  if (command->max_speed == 0) {
     auto node = node_.lock();
     node->get_parameter(behavior_name_ + ".max_angular_vel", max_angular_speed_);
   }
 
+  geometry_msgs::msg::Pose2D pose2d;
+  pose2d.x = pose.position.x;
+  pose2d.y = pose.position.y;
+  pose2d.theta = initial_direction_;
+  local_collision_checker_->isCollisionFree(pose2d, true);
+
   last_angle_ = tf2::getYaw(pose.orientation);
 
   angular_speed_ = vel.angular.z;
@@ -53,43 +65,108 @@ ResultStatus SimpleRotateBehavior::onStart(
   return ResultStatus{Status::SUCCEEDED};
 }
 
+void SimpleRotateBehavior::calc_err_and_sign(
+  double last_angle, double current_yaw, double & min_turn_angle, double & err, double & sign)
+{
+  err = angles::shortest_angular_distance(current_yaw, target_angle_);
+  sign = (err < 0) ? -1.0 : 1.0;
+  err = std::abs(err);
+
+  if (min_turn_angle > 0.0) {
+    const double angle_change = angles::shortest_angular_distance(last_angle, current_yaw);
+    min_turn_angle = std::max(0.0, min_turn_angle - initial_direction_ * angle_change);
+    err = std::max(initial_direction_ * sign * err, 0.0);
+    err = std::max(min_turn_angle, err);
+    sign = initial_direction_;
+  }
+}
+
+double SimpleRotateBehavior::calc_speed(
+  const double err, const double sign, const double angular_speed)
+{
+  const double upper_vel_ = angular_speed + max_angular_accel_ * control_duration_;
+  const double lower_vel_ = angular_speed - max_angular_accel_ * control_duration_;
+
+  const double speed_until_overshoot = std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
+
+  const double requested_speed = sign * std::min(speed_until_overshoot, max_angular_speed_);
+  const double speed = std::clamp(requested_speed, lower_vel_, upper_vel_);
+  return speed;
+}
+
 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::shortest_angular_distance(last_angle_ , current_yaw);
+  double min_turn_angle = min_turn_angle_;
-  last_angle_ = current_yaw;
+  double angular_speed = angular_speed_;
+  double err, sign;
 
-  double err = angles::shortest_angular_distance(current_yaw, target_angle_);
+  calc_err_and_sign(last_angle_, current_yaw, min_turn_angle, err, sign);
-  double sign = (err < 0)? -1.0 : 1.0;
-  err = std::abs(err);
 
-  if (min_turn_angle_ > 0.0) {
+  if (!(mode_ & RotateAction::Goal::IGNORE_OBSTACLES)) {
-    min_turn_angle_ = std::max(0.0, min_turn_angle_ - initial_direction_ * angle_change);
+    err = check_space(pose, err, sign);
-    err = std::max( initial_direction_ * sign * err, 0.0);
-    err = std::max(min_turn_angle_, err);
-    sign = initial_direction_;
   }
 
-  const double upper_vel_ = angular_speed_ + max_angular_accel_ * control_duration_;
+  double speed = 0.0;
-  const double lower_vel_ = angular_speed_ - max_angular_accel_ * control_duration_;
 
-  const double speed_until_overshoot =
+  if (err != 0.0) {
-    std::sqrt(2.0 * max_angular_accel_ * std::fabs(err));
+    speed = calc_speed(err, sign, angular_speed);
-
+  }
-  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};
   }
 
+  min_turn_angle_ = min_turn_angle;
+  last_angle_ = current_yaw;
   angular_speed_ = speed;
   out_vel.angular.z = speed;
   return ResultStatus{Status::RUNNING};
 }
 
+double SimpleRotateBehavior::check_space(
+  const geometry_msgs::msg::Pose pose, const double e, const double sign)
+{
+  geometry_msgs::msg::Pose2D pose2d;
+  pose2d.x = pose.position.x;
+  pose2d.y = pose.position.y;
+  double initial_theta = tf2::getYaw(pose.orientation);
+  pose2d.theta = initial_theta;
+  const double step_size = 0.1;
+  const double err = std::min(e, 1.0);
+  const bool check_map = !(mode_ & RotateAction::Goal::IGNORE_OBSTACLES);
+
+  for (int i = 1; i < err / step_size; i++) {
+    pose2d.theta += sign * step_size;
+
+    if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+      return step_size * (i - 1);
+    }
+
+    if (check_rival_collision(pose2d)) {
+      RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+      return step_size * (i - 1);
+    }
+  }
+
+  pose2d.theta = initial_theta + sign * err;
+
+  if (check_map && !local_collision_checker_->isCollisionFree(pose2d, false)) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+    return step_size * ((int)(err / step_size));
+  }
+
+  if (check_rival_collision(pose2d)) {
+    RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Rotation is blocked");
+    return step_size * ((int)(err / step_size));
+  }
+
+  return e;
+}
+
 }  // namespace toid
 
 #include "pluginlib/class_list_macros.hpp"