fix(obstacle_avoidance_planner): adding missing functionality for sto…

…p margin due to out of drivable area

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

planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml

@@ -18,6 +18,9 @@
       # output
       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:

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -127,6 +127,10 @@ 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);
 
@@ -447,7 +454,19 @@ void ObstacleAvoidancePlanner::insertZeroVelocityOutsideDrivableArea(
 
   if (first_outside_idx) {
     for (size_t i = *first_outside_idx; i < optimized_traj_points.size(); ++i) {
-      optimized_traj_points.at(i).longitudinal_velocity_mps = 0.0;
+      size_t stop_idx = i;
+      const auto & op_target_point = motion_utils::calcLongitudinalOffsetPoint(
+        optimized_traj_points, optimized_traj_points.at(i).pose.position, -1.0 * vehicle_stop_margin_outside_drivable_area_);
+      if (op_target_point) {
+        const auto target_point = op_target_point.get();
+        // confirm that target point doesn't overlap with the stop point outside drivable area
+        const auto dist = tier4_autoware_utils::calcDistance2d(optimized_traj_points.at(i), target_point);
+        const double overlap_threshold = 1e-3;
+        if (dist > overlap_threshold) {
+          stop_idx = motion_utils::findNearestSegmentIndex(optimized_traj_points, target_point);
+        }
+      }
+      optimized_traj_points.at(stop_idx).longitudinal_velocity_mps = 0.0;
     }
   }