fix(behavior_path_planner): add function to check the shift points in avoidance module

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -186,6 +186,7 @@ class AvoidanceModule : public SceneModuleInterface
   double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); }
   double getCurrentShift() const;
   double getCurrentLinearShift() const;
+  bool shiftedpathcheck(ShiftedPath shifted_path)const;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp

@@ -255,6 +255,8 @@ struct AvoidancePlanningData
 
   // current driving lanelet
   lanelet::ConstLanelets current_lanelets;
+  lanelet::ConstLanelets right_lanelets;
+  lanelet::ConstLanelets left_lanelets;
 
   // avoidance target objects
   ObjectDataArray objects;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp

@@ -28,6 +28,12 @@ bool isOnRight(const ObjectData & obj);
 lanelet::ConstLanelets calcLaneAroundPose(
   const std::shared_ptr<const PlannerData> & planner_data, const Pose & pose,
   const double backward_length);
+
+lanelet::ConstLanelets calcRightLaneAroundPose(
+  const std::shared_ptr<const PlannerData> & planner_data,lanelet::ConstLanelets curlanelets);
+
+lanelet::ConstLanelets calcLeftLaneAroundPose(
+  const std::shared_ptr<const PlannerData> & planner_data,lanelet::ConstLanelets curlanelets);
 
 size_t findPathIndexFromArclength(
   const std::vector<double> & path_arclength_arr, const double target_arc);

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -147,6 +147,8 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb
   // lanelet info
   data.current_lanelets = calcLaneAroundPose(
     planner_data_, reference_pose.pose, planner_data_->parameters.backward_path_length);
+  data.right_lanelets = calcRightLaneAroundPose(planner_data_,data.current_lanelets);
+  data.left_lanelets = calcLeftLaneAroundPose(planner_data_,data.current_lanelets);
 
   // target objects for avoidance
   data.objects = calcAvoidanceTargetObjects(data.current_lanelets, data.reference_path, debug);
@@ -2376,6 +2378,12 @@ ShiftedPath AvoidanceModule::generateAvoidancePath(PathShifter & path_shifter) c
     return toShiftedPath(avoidance_data_.reference_path);
   }
 
+  if(shiftedpathcheck(shifted_path))
+  {
+    RCLCPP_ERROR(getLogger(),"out of lanelet");
+    return toShiftedPath(avoidance_data_.reference_path);
+  }
+
   return shifted_path;
 }
 
@@ -2676,5 +2684,67 @@ void AvoidanceModule::setDebugData(const PathShifter & shifter, const DebugData
   addShiftPoint(shifter.getShiftPoints(), "path_shifter_registered_points", 0.99, 0.99, 0.0, 0.5);
   addAvoidPoint(debug.new_shift_points, "path_shifter_proposed_points", 0.99, 0.0, 0.0, 0.5);
 }
+bool AvoidanceModule::shiftedpathcheck(ShiftedPath shifted_path)const
+{
+  PathWithLaneId path_reference = shifted_path.path;
+   auto shift_length = shifted_path.shift_length;
+   lanelet::ConstLanelets current_lanelets;
+   lanelet::ConstLanelets left_lanelets;
+   lanelet::ConstLanelets right_lanelets;
+   lanelet::ConstLanelets check_lanelets;
+   left_lanelets=avoidance_data_.left_lanelets; 
+   right_lanelets=avoidance_data_.right_lanelets;
+   current_lanelets=avoidance_data_.current_lanelets;
+
+   check_lanelets.insert(
+    check_lanelets.end(), current_lanelets.begin(), current_lanelets.end());
+   if(!left_lanelets.empty())
+   { check_lanelets.insert(
+      check_lanelets.end(), left_lanelets.begin(), left_lanelets.end());
+   }
+
+   if(!right_lanelets.empty())
+   { check_lanelets.insert(
+      check_lanelets.end(), right_lanelets.begin(), right_lanelets.end());
+   }
+
+   bool out_flag=false;
+   size_t i=0;
+   auto with = planner_data_->parameters.vehicle_width*0.5;
+   for (auto & pt : path_reference.points)
+   {
+      if(fabs(shift_length.at(i))>1.0e-2)
+      {
+          double yaw = tf2::getYaw(pt.point.pose.orientation);
+
+         if(shift_length.at(i)>1.0e-2)
+         {
+           with=1.0*with;
+           pt.point.pose.position.x -= std::sin(yaw) * with;
+           pt.point.pose.position.y += std::cos(yaw) * with;
+         }
+         else
+         {
+           with=-1.0*with;
+           pt.point.pose.position.x -= std::sin(yaw) * with;
+           pt.point.pose.position.y += std::cos(yaw) * with;
+
+         }
+
+      for (const auto & clt : check_lanelets) {
+         if (lanelet::utils::isInLanelet(pt.point.pose, clt, 0.01)) {          
+             out_flag=false;
+             break;
+          }
+          out_flag=true;
+      }
+
+      if(out_flag==true)break;
+
+    }
+      i++;
+   }
+   return out_flag;
+}
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp

@@ -53,6 +53,37 @@ lanelet::ConstLanelets calcLaneAroundPose(
 
   return current_lanes;
 }
+lanelet::ConstLanelets calcRightLaneAroundPose(
+  const std::shared_ptr<const PlannerData> & planner_data,lanelet::ConstLanelets curlanelets)
+{
+  const auto & route_handler = planner_data->route_handler;
+  lanelet::ConstLanelets rightlanelets;
+  for(auto lane :curlanelets)
+  {        
+    auto right_lane = route_handler->getRightLanelet(lane);
+    if(right_lane)
+    {
+       rightlanelets.push_back(right_lane.get());
+    }
+  }
+  return rightlanelets;
+}
+
+lanelet::ConstLanelets calcLeftLaneAroundPose(
+  const std::shared_ptr<const PlannerData> & planner_data,lanelet::ConstLanelets curlanelets)
+{
+  const auto & route_handler = planner_data->route_handler;
+  lanelet::ConstLanelets leftlanelets;
+  for(auto lane :curlanelets)
+  {        
+    auto left_lane = route_handler->getLeftLanelet(lane);
+    if(left_lane)
+    {
+       leftlanelets.push_back(left_lane.get());
+    }
+  }
+  return leftlanelets;
+}
 
 ShiftedPath toShiftedPath(const PathWithLaneId & path)
 {