fix(obstacle_avoidance_planner): adding missing functionality for sto…

…p margin due to out of drivable area (autowarefoundation#4194)

* fix(obstacle_avoidance_planner): adding missing functionality for stop margin due to out of drivable area

Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>

---------

Signed-off-by: AhmedEbrahim <ahmed.a.d.ebrahim@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml

@@ -19,6 +19,9 @@
       output_delta_arc_length: 0.5     #  delta arc length for output trajectory [m]
       output_backward_traj_length: 5.0  # backward length for backward trajectory from base_link [m]
 
+      vehicle_stop_margin_outside_drivable_area: 1.5 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] .
+                                                     # This margin will be realized with delta_arc_length_for_mpt_points m precision.
+
     # replanning & trimming trajectory param outside algorithm
     replan:
       max_path_shape_around_ego_lat_dist: 2.0  # threshold of path shape change around ego [m]

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -127,6 +127,9 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
     const PlannerData & planner_data, std::vector<TrajectoryPoint> & traj_points) const;
   void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const;
   void publishDebugMarkerOfOptimization(const std::vector<TrajectoryPoint> & traj_points) const;
+
+private:
+  double vehicle_stop_margin_outside_drivable_area_;
 };
 }  // namespace obstacle_avoidance_planner
 

planning/obstacle_avoidance_planner/src/node.cpp

@@ -100,6 +100,9 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n
     time_keeper_ptr_->enable_calculation_time_info =
       declare_parameter<bool>("option.debug.enable_calculation_time_info");
 
+    vehicle_stop_margin_outside_drivable_area_ =
+      declare_parameter<double>("common.vehicle_stop_margin_outside_drivable_area");
+
     // parameters for ego nearest search
     ego_nearest_param_ = EgoNearestParam(this);
 
@@ -149,6 +152,10 @@ rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam(
     parameters, "option.debug.enable_calculation_time_info",
     time_keeper_ptr_->enable_calculation_time_info);
 
+  updateParam<double>(
+    parameters, "common.vehicle_stop_margin_outside_drivable_area",
+    vehicle_stop_margin_outside_drivable_area_);
+
   // parameters for ego nearest search
   ego_nearest_param_.onParam(parameters);
 
@@ -434,16 +441,29 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea(
         use_footprint_polygon_for_outside_drivable_area_check_);
 
       if (is_outside) {
-        publishVirtualWall(traj_point.pose);
         return i;
       }
     }
     return std::nullopt;
   }();
 
-  if (enable_outside_drivable_area_stop_) {
-    if (first_outside_idx) {
-      for (size_t i = *first_outside_idx; i < optimized_traj_points.size(); ++i) {
+  if (first_outside_idx) {
+    const auto stop_idx = [&]() {
+      const auto dist = tier4_autoware_utils::calcDistance2d(
+        optimized_traj_points.at(0), optimized_traj_points.at(*first_outside_idx));
+      const auto dist_with_margin = dist - vehicle_stop_margin_outside_drivable_area_;
+      const auto first_outside_idx_with_margin =
+        motion_utils::insertStopPoint(dist_with_margin, optimized_traj_points);
+      if (first_outside_idx_with_margin) {
+        return *first_outside_idx_with_margin;
+      }
+      return *first_outside_idx;
+    }();
+
+    publishVirtualWall(optimized_traj_points.at(stop_idx).pose);
+
+    if (enable_outside_drivable_area_stop_) {
+      for (size_t i = stop_idx; i < optimized_traj_points.size(); ++i) {
         optimized_traj_points.at(i).longitudinal_velocity_mps = 0.0;
       }
     }