diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 72a153ed7d066..ad11204056846 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/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 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 0cffa379fd898..a07fa54863f92 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/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; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp index 14bbf6a37c4ae..13e9c46ef89af 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp +++ b/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 & planner_data, const Pose & pose, const double backward_length); + +lanelet::ConstLanelets calcRightLaneAroundPose( + const std::shared_ptr & planner_data,lanelet::ConstLanelets curlanelets); + +lanelet::ConstLanelets calcLeftLaneAroundPose( + const std::shared_ptr & planner_data,lanelet::ConstLanelets curlanelets); size_t findPathIndexFromArclength( const std::vector & path_arclength_arr, const double target_arc); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 2923996ca8697..29f60b3498e12 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/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 diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index e4b8afbe87770..ebf9ed74a0983 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/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 & 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 & 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) {