diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index a59df6340ad00..fd21d18b98a64 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -70,6 +70,7 @@ struct Input lanelet::LaneletMapPtr lanelet_map{}; LaneletRoute::ConstSharedPtr route{}; lanelet::ConstLanelets route_lanelets{}; + lanelet::ConstLanelets shoulder_lanelets{}; Trajectory::ConstSharedPtr reference_trajectory{}; Trajectory::ConstSharedPtr predicted_trajectory{}; }; diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index 8869825890b8b..53b5461652970 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -66,6 +66,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; nav_msgs::msg::Odometry::ConstSharedPtr current_odom_; lanelet::LaneletMapPtr lanelet_map_; + lanelet::ConstLanelets shoulder_lanelets_; lanelet::traffic_rules::TrafficRulesPtr traffic_rules_; lanelet::routing::RoutingGraphPtr routing_graph_; LaneletRoute::ConstSharedPtr route_; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index db20dd7c8b981..e3e753d9e4023 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -123,7 +123,15 @@ Output LaneDepartureChecker::update(const Input & input) output.vehicle_passing_areas = createVehiclePassingAreas(output.vehicle_footprints); output.processing_time_map["createVehiclePassingAreas"] = stop_watch.toc(true); - output.candidate_lanelets = getCandidateLanelets(input.route_lanelets, output.vehicle_footprints); + const auto candidate_road_lanelets = + getCandidateLanelets(input.route_lanelets, output.vehicle_footprints); + const auto candidate_shoulder_lanelets = + getCandidateLanelets(input.shoulder_lanelets, output.vehicle_footprints); + output.candidate_lanelets = candidate_road_lanelets; + output.candidate_lanelets.insert( + output.candidate_lanelets.end(), candidate_shoulder_lanelets.begin(), + candidate_shoulder_lanelets.end()); + output.processing_time_map["getCandidateLanelets"] = stop_watch.toc(true); output.will_leave_lane = willLeaveLane(output.candidate_lanelets, output.vehicle_footprints); diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index d45fd2590a769..f0274e5c4712f 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -197,6 +197,10 @@ void LaneDepartureCheckerNode::onLaneletMapBin(const HADMapBin::ConstSharedPtr m { lanelet_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_); + + // get all shoulder lanes + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_); + shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); } void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) { route_ = msg; } @@ -313,6 +317,7 @@ void LaneDepartureCheckerNode::onTimer() input_.lanelet_map = lanelet_map_; input_.route = route_; input_.route_lanelets = route_lanelets_; + input_.shoulder_lanelets = shoulder_lanelets_; input_.reference_trajectory = reference_trajectory_; input_.predicted_trajectory = predicted_trajectory_; processing_time_map["Node: setInputData"] = stop_watch.toc(true);