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,
-    double & r, double & phi, double & delta);
+    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,
+    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 {
-    return nav2_core::CostmapInfoType::NONE;
+
+  virtual nav2_core::CostmapInfoType getResourceInfo() override
+  {
+    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