Restrict obstacle during recovery (#51)

amslabtech · Aug 18, 2024 · 8a49069 · 8a49069
1 parent c6c3d9f
commit 8a49069
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 1 deletion.
diff --git a/include/point_follow_planner/point_follow_planner.h b/include/point_follow_planner/point_follow_planner.h
@@ -94,6 +94,7 @@ class PointFollowPlanner
       const Window dynamic_window, const Eigen::Vector3d &goal);
   bool can_move();
   bool can_adjust_robot_direction(const Eigen::Vector3d &goal);
+  bool can_add_to_obs_list(const geometry_msgs::Point &point);
   bool check_collision(const std::vector<State> &traj);
   bool is_inside_of_triangle(const geometry_msgs::Point &target_point, const geometry_msgs::Polygon &triangle);
   bool is_inside_of_robot(const geometry_msgs::Pose &obstacle, const State &state);

diff --git a/src/point_follow_planner.cpp b/src/point_follow_planner.cpp
@@ -248,7 +248,9 @@ void PointFollowPlanner::create_obs_list(const nav_msgs::OccupancyGrid &map)
       {
         if (map.data[index_x + index_y * map.info.width] == 100)
         {
-          obs_list_.poses.push_back(pose);
+          if (can_add_to_obs_list(pose.position))
+            obs_list_.poses.push_back(pose);
+
           if (target_velocity_ >= 0.0)
           {
             if ((0.0 < pose.position.x && pose.position.x < dist_to_obj_th_x_) &&
@@ -268,6 +270,25 @@ void PointFollowPlanner::create_obs_list(const nav_msgs::OccupancyGrid &map)
   }
 }
 
+bool PointFollowPlanner::can_add_to_obs_list(const geometry_msgs::Point &point)
+{
+  if (0 < recovery_params_.recovery_count && fabs(point.y) < dist_to_obj_th_y_)
+  {
+    if (target_velocity_ >= 0.0)
+    {
+      if (point.x < 0.0)
+        return false;
+    }
+    else
+    {
+      if (0.0 < point.x)
+        return false;
+    }
+  }
+
+  return true;
+}
+
 PointFollowPlanner::Window PointFollowPlanner::calc_dynamic_window(const geometry_msgs::Twist &current_velocity)
 {
   Window window(min_velocity_, max_velocity_, -max_yawrate_, max_yawrate_);