From 8800bb2b80beae9af39b4319e2d2821f97bcf706 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 18 Aug 2022 07:06:14 +0900 Subject: [PATCH 01/16] added getExtendedAdjacentDirectionLane Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 20 ++++++ .../intersection/scene_intersection.cpp | 5 ++ .../src/scene_module/intersection/util.cpp | 66 +++++++++++++++++-- 3 files changed, 87 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 996e2d901d18c..2d646781f8fa5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -26,15 +26,34 @@ #include #include +#include +#include + #include #include #include #include #include +#include #include #include +namespace std +{ +template <> +struct hash +{ + size_t operator()(const unique_identifier_msgs::msg::UUID & uid) const + { + const auto & ids = uid.uuid; + boost::uuids::uuid u = {ids[0], ids[1], ids[2], ids[3], ids[4], ids[5], ids[6], ids[7], + ids[8], ids[9], ids[10], ids[11], ids[12], ids[13], ids[14], ids[15]}; + return boost::hash()(u); + } +}; +} // namespace std + namespace behavior_velocity_planner { // first: time, second: distance @@ -118,6 +137,7 @@ class IntersectionModule : public SceneModuleInterface std::string turn_direction_; bool has_traffic_light_; bool is_go_out_; + std::unordered_map> map_object_lane_; // Parameter PlannerParam planner_param_; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 2ff0ac7e8c848..b567cdd205185 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -310,6 +310,11 @@ bool IntersectionModule::checkCollision( continue; } + if (auto object_it = map_object_lane_.find(object.object_id); + object_it != map_object_lane_.end()) { + // push its corresponding lane + // map_object_lane_[object.object_id].push_back(0); + } // ignore vehicle in ego-lane && behind ego const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; const bool is_in_ego_lane = bg::within(to_bg2d(object_pose.position), ego_poly); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 2acd007f7e759..f070595b7dfeb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -575,11 +576,9 @@ bool generateStopLineBeforeIntersection( return false; } -geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p) +static std::string getTurnDirection(lanelet::ConstLanelet lane) { - geometry_msgs::msg::Pose pose; - pose.position = p; - return pose; + return lane.attributeOr("turn_direction", "else"); } bool isOverTargetIndex( @@ -604,5 +603,64 @@ bool isBeforeTargetIndex( return static_cast(target_idx > closest_idx); } +static std::vector getAllAdjacentLanelets( + const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane) +{ + std::set results; + + results.insert(lane.id()); + + auto it = routing_graph->adjacentRight(lane); + // take all lane on the right side + while (!!it) { + results.insert(it.get().id()); + it = routing_graph->adjacentRight(it.get()); + } + // take all lane on the left side + it = routing_graph->adjacentLeft(lane); + while (!!it) { + results.insert(it.get().id()); + it = routing_graph->adjacentLeft(it.get()); + } + + return std::vector(results.begin(), results.end()); +} + +std::vector extendedAdjacentDirectionLanes( + const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::ConstLanelet lane, const std::string & turn_direction) +{ + // some of the intersections are not well-formed, and "adjacent" turning + // lanelets are not sharing the LineStrings + if (turn_direction != "left" || turn_direction != "right" || turn_direction != "straight") + return std::vector(); + + // if lane's turn_direction does not match, return empty + if (getTurnDirection(lane) != turn_direction) return std::vector(); + + std::set previous_lanelet_ids; + for (auto && previous_lanelet : routing_graph->previous(lane)) + previous_lanelet_ids.insert(previous_lanelet.id()); + + std::set besides_previous_lanelet_ids; + for (auto && previous_lanelet_id : previous_lanelet_ids) { + lanelet::ConstLanelet previous_lanelet = map->laneletLayer.get(previous_lanelet_id); + for (auto && beside_lanelet : getAllAdjacentLanelets(routing_graph, previous_lanelet)) + besides_previous_lanelet_ids.insert(beside_lanelet); + } + + std::set following_turning_lanelets; + for (auto && besides_previous_lanelet_id : besides_previous_lanelet_ids) { + lanelet::ConstLanelet besides_previous_lanelet = + map->laneletLayer.get(besides_previous_lanelet_id); + for (auto && following_lanelet : routing_graph->following(besides_previous_lanelet)) { + // if this has {"turn_direction", "${turn_direction}"}, take this + if (getTurnDirection(following_lanelet) == turn_direction) + following_turning_lanelets.insert(following_lanelet.id()); + } + } + return std::vector(following_turning_lanelets.begin(), following_turning_lanelets.end()); +} + } // namespace util } // namespace behavior_velocity_planner From aecd62cb32bcbf19270a5acf93c4a53ecce20bbd Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 19 Aug 2022 11:17:11 +0900 Subject: [PATCH 02/16] added getIntersectionArea() -> optional Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 1 - .../include/scene_module/intersection/util.hpp | 4 ++++ .../intersection/scene_intersection.cpp | 6 +----- .../src/scene_module/intersection/util.cpp | 18 ++++++++++++++++++ 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 2d646781f8fa5..cc741574f7b9e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -137,7 +137,6 @@ class IntersectionModule : public SceneModuleInterface std::string turn_direction_; bool has_traffic_light_; bool is_go_out_; - std::unordered_map> map_object_lane_; // Parameter PlannerParam planner_param_; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 94a5d2fac2095..4adfae642ef3e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -27,6 +27,7 @@ #endif #include +#include #include #include @@ -143,6 +144,9 @@ bool isBeforeTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx); +std::optional getIntersectionArea( + lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index b567cdd205185..f87e3b97eba09 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -310,11 +310,6 @@ bool IntersectionModule::checkCollision( continue; } - if (auto object_it = map_object_lane_.find(object.object_id); - object_it != map_object_lane_.end()) { - // push its corresponding lane - // map_object_lane_[object.object_id].push_back(0); - } // ignore vehicle in ego-lane && behind ego const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; const bool is_in_ego_lane = bg::within(to_bg2d(object_pose.position), ego_poly); @@ -625,6 +620,7 @@ bool IntersectionModule::checkAngleForTargetLanelets( const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle); + // detection_areaのlaneletの内部にありかつ姿勢が一致するobjectは検知対象とする(からtrue) if (std::fabs(angle_diff) < planner_param_.detection_area_angle_thr) { return true; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index f070595b7dfeb..fd10aa8c3f881 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -576,6 +576,13 @@ bool generateStopLineBeforeIntersection( return false; } +geometry_msgs::msg::Pose toPose(const geometry_msgs::msg::Point & p) +{ + geometry_msgs::msg::Pose pose; + pose.position = p; + return pose; +} + static std::string getTurnDirection(lanelet::ConstLanelet lane) { return lane.attributeOr("turn_direction", "else"); @@ -662,5 +669,16 @@ std::vector extendedAdjacentDirectionLanes( return std::vector(following_turning_lanelets.begin(), following_turning_lanelets.end()); } +std::optional getIntersectionArea( + lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) +{ + const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); + if (area_id_str == "else") return std::nullopt; + + const int area_id = std::atoi(area_id_str.c_str()); + const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); + return std::make_optional(lanelet::utils::to2D(poly_3d).basicPolygon()); +} + } // namespace util } // namespace behavior_velocity_planner From aa745cc7543151b555bb7949721b08de488df8a4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 30 Aug 2022 15:54:16 +0900 Subject: [PATCH 03/16] remove detection_area_with_margin. target object is visualized even if it is deviated Signed-off-by: Mamoru Sobue --- .../scene_module/intersection/scene_intersection.hpp | 1 - .../include/scene_module/intersection/util.hpp | 8 +++----- .../src/scene_module/intersection/debug.cpp | 5 ----- .../scene_module/intersection/scene_intersection.cpp | 10 +--------- .../intersection/scene_merge_from_private_road.cpp | 5 +---- .../src/scene_module/intersection/util.cpp | 11 ++--------- 6 files changed, 7 insertions(+), 33 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index cc741574f7b9e..ec312237f4b8b 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -72,7 +72,6 @@ class IntersectionModule : public SceneModuleInterface geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; geometry_msgs::msg::Polygon ego_lane_polygon; - std::vector detection_area_with_margin; geometry_msgs::msg::Polygon stuck_vehicle_detect_area; geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; std::vector candidate_collision_object_polygons; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 4adfae642ef3e..ab8ef01f4d454 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -49,11 +49,9 @@ bool getDuplicatedPointIdx( */ bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const double detection_area_length, const double right_margin, - const double left_margin, std::vector * conflicting_lanelets_result, - lanelet::ConstLanelets * objective_lanelets_result, - std::vector * objective_lanelets_with_margin_result, - const rclcpp::Logger logger); + const int lane_id, const double detection_area_length, + std::vector * conflicting_lanelets_result, + lanelet::ConstLanelets * objective_lanelets_result, const rclcpp::Logger logger); struct StopLineIdx { diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 94364e928bbda..a94292ca46812 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -121,11 +121,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", module_id_), &debug_marker_array, now); - appendMarkerArray( - createLaneletPolygonsMarkerArray( - debug_data_.detection_area_with_margin, "detection_area_with_margin", module_id_), - &debug_marker_array, now); - appendMarkerArray( debug::createPolygonMarkerArray( debug_data_.ego_lane_polygon, "ego_lane", module_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7), diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index f87e3b97eba09..15cc0c2d7c7b9 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -94,20 +94,14 @@ bool IntersectionModule::modifyPathVelocity( /* get detection area and conflicting area */ lanelet::ConstLanelets detection_area_lanelets; std::vector conflicting_area_lanelets; - std::vector detection_area_lanelets_with_margin; util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin, - &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin, - logger_); + &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); std::vector detection_areas = util::getPolygon3dFromLanelets(detection_area_lanelets, planner_param_.detection_area_length); - std::vector detection_areas_with_margin = - util::getPolygon3dFromLaneletsVec( - detection_area_lanelets_with_margin, planner_param_.detection_area_length); std::vector conflicting_area_lanelet_ids = util::getLaneletIdsFromLaneletsVec(conflicting_area_lanelets); std::vector detection_area_lanelet_ids = @@ -120,7 +114,6 @@ bool IntersectionModule::modifyPathVelocity( return true; } debug_data_.detection_area = detection_areas; - debug_data_.detection_area_with_margin = detection_areas_with_margin; /* set stop-line and stop-judgement-line for base_link */ util::StopLineIdx stop_line_idxs; @@ -620,7 +613,6 @@ bool IntersectionModule::checkAngleForTargetLanelets( const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle); - // detection_areaのlaneletの内部にありかつ姿勢が一致するobjectは検知対象とする(からtrue) if (std::fabs(angle_diff) < planner_param_.detection_area_angle_thr) { return true; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 6e3d3169a6d8d..61f587396d444 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -68,13 +68,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( /* get detection area */ lanelet::ConstLanelets detection_area_lanelets; std::vector conflicting_area_lanelets; - std::vector detection_area_lanelets_with_margin; util::getObjectiveLanelets( lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - planner_param_.detection_area_right_margin, planner_param_.detection_area_left_margin, - &conflicting_area_lanelets, &detection_area_lanelets, &detection_area_lanelets_with_margin, - logger_); + &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); if (conflicting_areas.empty()) { diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index fd10aa8c3f881..95f0d7d279555 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -311,11 +311,9 @@ bool getStopPoseIndexFromMap( bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const double detection_area_length, double right_margin, double left_margin, + const int lane_id, const double detection_area_length, std::vector * conflicting_lanelets_result, - lanelet::ConstLanelets * objective_lanelets_result, - std::vector * objective_lanelets_with_margin_result, - const rclcpp::Logger logger) + lanelet::ConstLanelets * objective_lanelets_result, const rclcpp::Logger logger) { const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); @@ -356,7 +354,6 @@ bool getObjectiveLanelets( std::vector // conflicting lanes with "lane_id" conflicting_lanelets_ex_yield_ego; // excluding ego lanes and yield lanes std::vector objective_lanelets; // final objective lanelets - std::vector objective_lanelets_with_margin; // final objective lanelets // exclude yield lanelets and ego lanelets from objective_lanelets for (const auto & conflicting_lanelet : conflicting_lanelets) { @@ -366,11 +363,8 @@ bool getObjectiveLanelets( if (lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { continue; } - const auto objective_lanelet_with_margin = - generateOffsetLanelet(conflicting_lanelet, right_margin, left_margin); conflicting_lanelets_ex_yield_ego.push_back({conflicting_lanelet}); objective_lanelets.push_back({conflicting_lanelet}); - objective_lanelets_with_margin.push_back({objective_lanelet_with_margin}); } // get possible lanelet path that reaches conflicting_lane longer than given length @@ -397,7 +391,6 @@ bool getObjectiveLanelets( *conflicting_lanelets_result = conflicting_lanelets_ex_yield_ego; *objective_lanelets_result = objective_and_preceding_lanelets; - *objective_lanelets_with_margin_result = objective_lanelets_with_margin; // set this flag true when debugging const bool is_debug = false; From eb4e4e71114b70c4f6cfc25bd9af8e04a82dddee Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 30 Aug 2022 17:31:39 +0900 Subject: [PATCH 04/16] if intersection area is available, use it additionally Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 5 +++- .../intersection/scene_intersection.cpp | 30 ++++++++++++++++--- 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index ec312237f4b8b..50f31135d8515 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -147,6 +147,7 @@ class IntersectionModule : public SceneModuleInterface * @param path ego-car lane * @param detection_area_lanelet_ids angle check is performed for obstacles using this lanelet * ids + * @param intersection_area associated intersection_area if exists * @param objects_ptr target objects * @param closest_idx ego-car position index on the lane * @return true if collision is detected @@ -155,6 +156,7 @@ class IntersectionModule : public SceneModuleInterface lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, + const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area); @@ -236,7 +238,8 @@ class IntersectionModule : public SceneModuleInterface * @return true if the given pose belongs to any target lanelet */ bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids); + const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids, + const double margin = 0); /** * @brief Get lanes including ego lanelet and next lanelet diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 15cc0c2d7c7b9..7d9f6facf4679 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -115,6 +115,11 @@ bool IntersectionModule::modifyPathVelocity( } debug_data_.detection_area = detection_areas; + /* get interection area */ + const auto & assigned_lanelet = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + /* set stop-line and stop-judgement-line for base_link */ util::StopLineIdx stop_line_idxs; if (!util::generateStopLine( @@ -191,7 +196,7 @@ bool IntersectionModule::modifyPathVelocity( planner_param_.stuck_vehicle_detect_dist); bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_area_lanelet_ids, objects_ptr, closest_idx, + lanelet_map_ptr, *path, detection_area_lanelet_ids, intersection_area, objects_ptr, closest_idx, stuck_vehicle_detect_area); bool is_entry_prohibited = (has_collision || is_stuck); if (external_go) { @@ -277,6 +282,7 @@ bool IntersectionModule::checkCollision( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, + const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area) { @@ -319,7 +325,22 @@ bool IntersectionModule::checkCollision( // check direction of objects const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); - if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) { + if (intersection_area) { + const auto obj_poly = toFootprintPolygon(object); + const auto is_in_intersection_area = bg::within(obj_poly, intersection_area.value()); + if (is_in_intersection_area) { + target_objects.objects.push_back(object); + std::cout << "is_in_intersection_area: " << is_in_intersection_area << std::endl; + } else if (checkAngleForTargetLanelets( + object_direction, detection_area_lanelet_ids, + planner_param_.detection_area_margin)) { + target_objects.objects.push_back(object); + std::cout << "not in intersection_area, but in detection_area_with_margin" << std::endl; + } + } else if (checkAngleForTargetLanelets( + object_direction, detection_area_lanelet_ids, + planner_param_.detection_area_margin)) { + // intersection_area is not available, use detection_area_with_margin as before target_objects.objects.push_back(object); } } @@ -603,11 +624,12 @@ bool IntersectionModule::isTargetExternalInputStatus(const int target_status) } bool IntersectionModule::checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids) + const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids, + const double margin) { for (const int lanelet_id : target_lanelet_ids) { const auto ll = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lanelet_id); - if (!lanelet::utils::isInLanelet(pose, ll, planner_param_.detection_area_margin)) { + if (!lanelet::utils::isInLanelet(pose, ll, margin)) { continue; } const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); From 77afe9f1bd04e622e7695ba735c95bca1c992365 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 30 Aug 2022 23:26:09 +0900 Subject: [PATCH 05/16] toGeomMsg fails Signed-off-by: Mamoru Sobue --- .../scene_module/intersection/scene_intersection.hpp | 3 ++- .../include/scene_module/intersection/util.hpp | 2 +- .../scene_module/intersection/scene_intersection.cpp | 12 ++++++++---- .../src/scene_module/intersection/util.cpp | 4 ++-- 4 files changed, 13 insertions(+), 8 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 50f31135d8515..b190dc564388e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -77,6 +77,7 @@ class IntersectionModule : public SceneModuleInterface std::vector candidate_collision_object_polygons; std::vector intersection_detection_lanelets; std::vector detection_area; + geometry_msgs::msg::Polygon intersection_area; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; geometry_msgs::msg::Pose predicted_obj_pose; @@ -156,7 +157,7 @@ class IntersectionModule : public SceneModuleInterface lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, - const std::optional & intersection_area, + const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area); diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index ab8ef01f4d454..ec03b405529f5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -142,7 +142,7 @@ bool isBeforeTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx); -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); } // namespace util diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 7d9f6facf4679..ecb7237c4300d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -119,6 +119,10 @@ bool IntersectionModule::modifyPathVelocity( const auto & assigned_lanelet = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + if (intersection_area) { + const auto intersection_area_2d = lanelet::utils::to2D(intersection_area.value()); + debug_data_.intersection_area = toGeomMsg(intersection_area_2d); + } /* set stop-line and stop-judgement-line for base_link */ util::StopLineIdx stop_line_idxs; @@ -282,7 +286,7 @@ bool IntersectionModule::checkCollision( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, - const std::optional & intersection_area, + const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area) { @@ -327,15 +331,15 @@ bool IntersectionModule::checkCollision( const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); if (intersection_area) { const auto obj_poly = toFootprintPolygon(object); - const auto is_in_intersection_area = bg::within(obj_poly, intersection_area.value()); + const auto intersection_area_2d = + lanelet::utils::to2D(lanelet::utils::toHybrid(intersection_area.value())); + const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); if (is_in_intersection_area) { target_objects.objects.push_back(object); - std::cout << "is_in_intersection_area: " << is_in_intersection_area << std::endl; } else if (checkAngleForTargetLanelets( object_direction, detection_area_lanelet_ids, planner_param_.detection_area_margin)) { target_objects.objects.push_back(object); - std::cout << "not in intersection_area, but in detection_area_with_margin" << std::endl; } } else if (checkAngleForTargetLanelets( object_direction, detection_area_lanelet_ids, diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 95f0d7d279555..d7de8c902d914 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -662,7 +662,7 @@ std::vector extendedAdjacentDirectionLanes( return std::vector(following_turning_lanelets.begin(), following_turning_lanelets.end()); } -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); @@ -670,7 +670,7 @@ std::optional getIntersectionArea( const int area_id = std::atoi(area_id_str.c_str()); const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); - return std::make_optional(lanelet::utils::to2D(poly_3d).basicPolygon()); + return std::make_optional(poly_3d); } } // namespace util From aef5db4404e6bd80c8687f412f946602e18016b3 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 31 Aug 2022 00:00:57 +0900 Subject: [PATCH 06/16] use behavior_velocity_planner::Polygon Signed-off-by: Mamoru Sobue --- .../scene_module/intersection/scene_intersection.hpp | 2 +- .../include/scene_module/intersection/util.hpp | 2 +- .../src/scene_module/intersection/scene_intersection.cpp | 7 +++---- .../src/scene_module/intersection/util.cpp | 6 ++++-- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index b190dc564388e..d0305ce38b72c 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -157,7 +157,7 @@ class IntersectionModule : public SceneModuleInterface lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, - const std::optional & intersection_area, + const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area); diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index ec03b405529f5..5bb551f061c82 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -142,7 +142,7 @@ bool isBeforeTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx); -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); } // namespace util diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index ecb7237c4300d..c67754311f886 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -120,7 +120,7 @@ bool IntersectionModule::modifyPathVelocity( planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); if (intersection_area) { - const auto intersection_area_2d = lanelet::utils::to2D(intersection_area.value()); + const auto intersection_area_2d = intersection_area.value(); debug_data_.intersection_area = toGeomMsg(intersection_area_2d); } @@ -286,7 +286,7 @@ bool IntersectionModule::checkCollision( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, - const std::optional & intersection_area, + const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area) { @@ -331,8 +331,7 @@ bool IntersectionModule::checkCollision( const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); if (intersection_area) { const auto obj_poly = toFootprintPolygon(object); - const auto intersection_area_2d = - lanelet::utils::to2D(lanelet::utils::toHybrid(intersection_area.value())); + const auto intersection_area_2d = intersection_area.value(); const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); if (is_in_intersection_area) { target_objects.objects.push_back(object); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index d7de8c902d914..6d62d7a5f6dc7 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -662,7 +662,7 @@ std::vector extendedAdjacentDirectionLanes( return std::vector(following_turning_lanelets.begin(), following_turning_lanelets.end()); } -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); @@ -670,7 +670,9 @@ std::optional getIntersectionArea( const int area_id = std::atoi(area_id_str.c_str()); const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); - return std::make_optional(poly_3d); + Polygon2d poly{}; + for (const auto & p : poly_3d) poly.outer().emplace_back(p.x(), p.y()); + return std::make_optional(poly); } } // namespace util From b4e4adaedf39564e5f504c1bb89af51ea05bc122 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 31 Aug 2022 00:09:41 +0900 Subject: [PATCH 07/16] publish intersection area if exists Signed-off-by: Mamoru Sobue --- .../src/scene_module/intersection/debug.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index a94292ca46812..240adc3fe4038 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -122,9 +122,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); appendMarkerArray( - debug::createPolygonMarkerArray( - debug_data_.ego_lane_polygon, "ego_lane", module_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7), - &debug_marker_array, now); + createPolygonMarkerArray( + debug_data_.intersection_area, "intersection_area", lane_id_, 0.0, 1.0, 0.0), + &debug_marker_array, current_time); + + appendMarkerArray( + createPolygonMarkerArray( + debug_data_.ego_lane_polygon, "ego_lane", lane_id_, 0.3, 0.0, 0.00.0, 0.3, 0.7), + &debug_marker_array, current_time); appendMarkerArray( debug::createPolygonMarkerArray( From 426575641cdfe9f4a100a3b666269cc7ba13bde7 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 31 Aug 2022 00:36:42 +0900 Subject: [PATCH 08/16] remove unnecessary code Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 19 ------------------- .../intersection/scene_intersection.cpp | 2 +- 2 files changed, 1 insertion(+), 20 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index d0305ce38b72c..676a0a39483f1 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -26,34 +26,15 @@ #include #include -#include -#include - #include #include #include #include #include -#include #include #include -namespace std -{ -template <> -struct hash -{ - size_t operator()(const unique_identifier_msgs::msg::UUID & uid) const - { - const auto & ids = uid.uuid; - boost::uuids::uuid u = {ids[0], ids[1], ids[2], ids[3], ids[4], ids[5], ids[6], ids[7], - ids[8], ids[9], ids[10], ids[11], ids[12], ids[13], ids[14], ids[15]}; - return boost::hash()(u); - } -}; -} // namespace std - namespace behavior_velocity_planner { // first: time, second: distance diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index c67754311f886..cc5cb737c3154 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -115,7 +115,7 @@ bool IntersectionModule::modifyPathVelocity( } debug_data_.detection_area = detection_areas; - /* get interection area */ + /* get intersection area */ const auto & assigned_lanelet = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); From 047aa4721c4a9c36ab5165ca35193ca35a322f4e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 5 Sep 2022 17:25:53 +0900 Subject: [PATCH 09/16] visualizing adjacent lanes Signed-off-by: Mamoru Sobue --- .../intersection/scene_intersection.hpp | 3 +- .../scene_module/intersection/util.hpp | 4 +++ .../src/scene_module/intersection/debug.cpp | 4 +++ .../src/scene_module/intersection/manager.cpp | 2 -- .../intersection/scene_intersection.cpp | 32 +++++++++++++++---- .../src/scene_module/intersection/util.cpp | 3 +- 6 files changed, 38 insertions(+), 10 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 676a0a39483f1..fb5ed09b04689 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -59,6 +59,7 @@ class IntersectionModule : public SceneModuleInterface std::vector intersection_detection_lanelets; std::vector detection_area; geometry_msgs::msg::Polygon intersection_area; + std::vector adjacent_area; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; geometry_msgs::msg::Pose predicted_obj_pose; @@ -85,7 +86,6 @@ class IntersectionModule : public SceneModuleInterface double detection_area_left_margin; //! used for detecting objects in detection area only left //! direction double detection_area_length; //! used to create detection area polygon - double detection_area_angle_thr; //! threshold in checking the angle of detecting objects double min_predicted_path_confidence; //! minimum confidence value of predicted path to use for collision detection double external_input_timeout; //! used to disable external input @@ -138,6 +138,7 @@ class IntersectionModule : public SceneModuleInterface lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, + const std::vector & adjacent_lanelet_ids, const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area); diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 5bb551f061c82..1992adbf34c80 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -142,6 +142,10 @@ bool isBeforeTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx); +std::vector extendedAdjacentDirectionLanes( + const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::ConstLanelet lane); + std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 240adc3fe4038..12e49fcaa69ee 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -121,6 +121,10 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", module_id_), &debug_marker_array, now); + appendMarkerArray( + createLaneletPolygonsMarkerArray(debug_data_.adjacent_area, "adjacent_area", lane_id_), + &debug_marker_array, current_time); + appendMarkerArray( createPolygonMarkerArray( debug_data_.intersection_area, "intersection_area", lane_id_, 0.0, 1.0, 0.0), diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 08e39abedb6f3..32010b1d446f1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -46,8 +46,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.detection_area_right_margin = node.declare_parameter(ns + ".detection_area_right_margin", 0.5); ip.detection_area_left_margin = node.declare_parameter(ns + ".detection_area_left_margin", 0.5); ip.detection_area_length = node.declare_parameter(ns + ".detection_area_length", 200.0); - ip.detection_area_angle_thr = - node.declare_parameter(ns + ".detection_area_angle_threshold", M_PI / 4.0); ip.min_predicted_path_confidence = node.declare_parameter(ns + ".min_predicted_path_confidence", 0.05); ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index cc5cb737c3154..06799d04c2439 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -124,6 +125,16 @@ bool IntersectionModule::modifyPathVelocity( debug_data_.intersection_area = toGeomMsg(intersection_area_2d); } + /* get adjacent lanelets */ + const auto adjacent_lanelet_ids = + util::extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); + if (!adjacent_lanelet_ids.empty()) { + std::vector adjacent_lanes; + for (auto && adjacent_lanelet_id : adjacent_lanelet_ids) + adjacent_lanes.push_back(lanelet_map_ptr->laneletLayer.get(adjacent_lanelet_id).polygon3d()); + debug_data_.adjacent_area = adjacent_lanes; + } + /* set stop-line and stop-judgement-line for base_link */ util::StopLineIdx stop_line_idxs; if (!util::generateStopLine( @@ -200,8 +211,8 @@ bool IntersectionModule::modifyPathVelocity( planner_param_.stuck_vehicle_detect_dist); bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_area_lanelet_ids, intersection_area, objects_ptr, closest_idx, - stuck_vehicle_detect_area); + lanelet_map_ptr, *path, detection_area_lanelet_ids, adjacent_lanelet_ids, intersection_area, + objects_ptr, closest_idx, stuck_vehicle_detect_area); bool is_entry_prohibited = (has_collision || is_stuck); if (external_go) { is_entry_prohibited = false; @@ -286,7 +297,7 @@ bool IntersectionModule::checkCollision( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::vector & detection_area_lanelet_ids, - const std::optional & intersection_area, + const std::vector & adjacent_lanelet_ids, const std::optional & intersection_area, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const Polygon2d & stuck_vehicle_detect_area) { @@ -333,11 +344,19 @@ bool IntersectionModule::checkCollision( const auto obj_poly = toFootprintPolygon(object); const auto intersection_area_2d = intersection_area.value(); const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); + const auto is_in_adjacent_lanelets = checkAngleForTargetLanelets( + object_direction, adjacent_lanelet_ids, planner_param_.detection_area_margin); if (is_in_intersection_area) { - target_objects.objects.push_back(object); + if (!is_in_adjacent_lanelets) { + std::cout << "in intersection_area and not in adjacent lane, detecting" << std::endl; + target_objects.objects.push_back(object); + } else { + std::cout << "in intersection area, but in adjacent lane, not detecting" << std::endl; + } } else if (checkAngleForTargetLanelets( object_direction, detection_area_lanelet_ids, planner_param_.detection_area_margin)) { + std::cout << "not in intersection_area, but in detection_area, detecting" << std::endl; target_objects.objects.push_back(object); } } else if (checkAngleForTargetLanelets( @@ -630,6 +649,7 @@ bool IntersectionModule::checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids, const double margin) { + double min_angle = std::numeric_limits::infinity(); for (const int lanelet_id : target_lanelet_ids) { const auto ll = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lanelet_id); if (!lanelet::utils::isInLanelet(pose, ll, margin)) { @@ -638,8 +658,8 @@ bool IntersectionModule::checkAngleForTargetLanelets( const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle); - if (std::fabs(angle_diff) < planner_param_.detection_area_angle_thr) { - return true; + if (std::fabs(angle_diff) < min_angle) { + min_angle = std::fabs(angle_diff); } } return false; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 6d62d7a5f6dc7..ba2bf4f2c4b78 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -628,10 +628,11 @@ static std::vector getAllAdjacentLanelets( std::vector extendedAdjacentDirectionLanes( const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, - lanelet::ConstLanelet lane, const std::string & turn_direction) + lanelet::ConstLanelet lane) { // some of the intersections are not well-formed, and "adjacent" turning // lanelets are not sharing the LineStrings + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); if (turn_direction != "left" || turn_direction != "right" || turn_direction != "straight") return std::vector(); From 2e06814ae0aa06a7443855174ab12b981a949fa4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 5 Sep 2022 21:04:35 +0900 Subject: [PATCH 10/16] fixed for tier4_autoware_utisl Signed-off-by: Mamoru Sobue --- .../src/scene_module/intersection/scene_intersection.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 06799d04c2439..54d4a64e42751 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -122,7 +122,7 @@ bool IntersectionModule::modifyPathVelocity( const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); if (intersection_area) { const auto intersection_area_2d = intersection_area.value(); - debug_data_.intersection_area = toGeomMsg(intersection_area_2d); + debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } /* get adjacent lanelets */ @@ -341,7 +341,7 @@ bool IntersectionModule::checkCollision( // check direction of objects const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); if (intersection_area) { - const auto obj_poly = toFootprintPolygon(object); + const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); const auto is_in_adjacent_lanelets = checkAngleForTargetLanelets( From 6a85a01b6215c5bce39545d26475ce8e8f8bd0fa Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 5 Sep 2022 23:38:45 +0900 Subject: [PATCH 11/16] added missing parameter Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 1 + .../intersection/scene_intersection.hpp | 1 + .../src/scene_module/intersection/debug.cpp | 12 +++++++----- .../src/scene_module/intersection/manager.cpp | 2 ++ .../intersection/scene_intersection.cpp | 19 ++++++------------- .../src/scene_module/intersection/util.cpp | 14 +++++++------- 6 files changed, 24 insertions(+), 25 deletions(-) diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 0ea817ffefc81..79eb257de6889 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -14,6 +14,7 @@ detection_area_right_margin: 0.5 # [m] detection_area_left_margin: 0.5 # [m] detection_area_length: 200.0 # [m] + detection_area_angle_threshold: 0.785 # [rad] min_predicted_path_confidence: 0.05 collision_start_margin_time: 5.0 # [s] collision_end_margin_time: 2.0 # [s] diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index fb5ed09b04689..16a24641ae3b7 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -86,6 +86,7 @@ class IntersectionModule : public SceneModuleInterface double detection_area_left_margin; //! used for detecting objects in detection area only left //! direction double detection_area_length; //! used to create detection area polygon + double detection_area_angle_thr; //! threshold in checking the angle of detecting objects double min_predicted_path_confidence; //! minimum confidence value of predicted path to use for collision detection double external_input_timeout; //! used to disable external input diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 12e49fcaa69ee..d873f0d5b9dfa 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -32,7 +32,7 @@ using tier4_autoware_utils::createMarkerScale; visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, - const int64_t module_id) + const int64_t lane_id, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; @@ -49,7 +49,7 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( marker.action = visualization_msgs::msg::Marker::ADD; marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); marker.scale = createMarkerScale(0.1, 0.0, 0.0); - marker.color = createMarkerColor(0.0, 1.0, 0.0, 0.999); + marker.color = createMarkerColor(r, g, b, 0.999); for (const auto & p : polygon) { geometry_msgs::msg::Point point; point.x = p.x(); @@ -118,11 +118,13 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); appendMarkerArray( - createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", module_id_), - &debug_marker_array, now); + createLaneletPolygonsMarkerArray( + debug_data_.detection_area, "detection_area", lane_id_, 0.0, 1.0, 0.0), + &debug_marker_array, current_time); appendMarkerArray( - createLaneletPolygonsMarkerArray(debug_data_.adjacent_area, "adjacent_area", lane_id_), + createLaneletPolygonsMarkerArray( + debug_data_.adjacent_area, "adjacent_area", lane_id_, 0.913, 0.639, 0.149), &debug_marker_array, current_time); appendMarkerArray( diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 32010b1d446f1..08e39abedb6f3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -46,6 +46,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.detection_area_right_margin = node.declare_parameter(ns + ".detection_area_right_margin", 0.5); ip.detection_area_left_margin = node.declare_parameter(ns + ".detection_area_left_margin", 0.5); ip.detection_area_length = node.declare_parameter(ns + ".detection_area_length", 200.0); + ip.detection_area_angle_thr = + node.declare_parameter(ns + ".detection_area_angle_threshold", M_PI / 4.0); ip.min_predicted_path_confidence = node.declare_parameter(ns + ".min_predicted_path_confidence", 0.05); ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 54d4a64e42751..76e1fcea0729f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -27,7 +27,6 @@ #include #include -#include #include #include @@ -128,12 +127,11 @@ bool IntersectionModule::modifyPathVelocity( /* get adjacent lanelets */ const auto adjacent_lanelet_ids = util::extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); - if (!adjacent_lanelet_ids.empty()) { - std::vector adjacent_lanes; - for (auto && adjacent_lanelet_id : adjacent_lanelet_ids) - adjacent_lanes.push_back(lanelet_map_ptr->laneletLayer.get(adjacent_lanelet_id).polygon3d()); - debug_data_.adjacent_area = adjacent_lanes; + std::vector adjacent_lanes; + for (auto && adjacent_lanelet_id : adjacent_lanelet_ids) { + adjacent_lanes.push_back(lanelet_map_ptr->laneletLayer.get(adjacent_lanelet_id).polygon3d()); } + debug_data_.adjacent_area = adjacent_lanes; /* set stop-line and stop-judgement-line for base_link */ util::StopLineIdx stop_line_idxs; @@ -348,15 +346,11 @@ bool IntersectionModule::checkCollision( object_direction, adjacent_lanelet_ids, planner_param_.detection_area_margin); if (is_in_intersection_area) { if (!is_in_adjacent_lanelets) { - std::cout << "in intersection_area and not in adjacent lane, detecting" << std::endl; target_objects.objects.push_back(object); - } else { - std::cout << "in intersection area, but in adjacent lane, not detecting" << std::endl; } } else if (checkAngleForTargetLanelets( object_direction, detection_area_lanelet_ids, planner_param_.detection_area_margin)) { - std::cout << "not in intersection_area, but in detection_area, detecting" << std::endl; target_objects.objects.push_back(object); } } else if (checkAngleForTargetLanelets( @@ -649,7 +643,6 @@ bool IntersectionModule::checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids, const double margin) { - double min_angle = std::numeric_limits::infinity(); for (const int lanelet_id : target_lanelet_ids) { const auto ll = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lanelet_id); if (!lanelet::utils::isInLanelet(pose, ll, margin)) { @@ -658,8 +651,8 @@ bool IntersectionModule::checkAngleForTargetLanelets( const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle); - if (std::fabs(angle_diff) < min_angle) { - min_angle = std::fabs(angle_diff); + if (std::fabs(angle_diff) < planner_param_.detection_area_angle_thr) { + return true; } } return false; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index ba2bf4f2c4b78..2ea4c3b870b87 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -632,25 +632,25 @@ std::vector extendedAdjacentDirectionLanes( { // some of the intersections are not well-formed, and "adjacent" turning // lanelets are not sharing the LineStrings - const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - if (turn_direction != "left" || turn_direction != "right" || turn_direction != "straight") + const std::string turn_direction = getTurnDirection(lane); + if (turn_direction != "left" && turn_direction != "right" && turn_direction != "straight") return std::vector(); - // if lane's turn_direction does not match, return empty - if (getTurnDirection(lane) != turn_direction) return std::vector(); - std::set previous_lanelet_ids; - for (auto && previous_lanelet : routing_graph->previous(lane)) + for (auto && previous_lanelet : routing_graph->previous(lane)) { previous_lanelet_ids.insert(previous_lanelet.id()); + } std::set besides_previous_lanelet_ids; for (auto && previous_lanelet_id : previous_lanelet_ids) { lanelet::ConstLanelet previous_lanelet = map->laneletLayer.get(previous_lanelet_id); - for (auto && beside_lanelet : getAllAdjacentLanelets(routing_graph, previous_lanelet)) + for (auto && beside_lanelet : getAllAdjacentLanelets(routing_graph, previous_lanelet)) { besides_previous_lanelet_ids.insert(beside_lanelet); + } } std::set following_turning_lanelets; + following_turning_lanelets.insert(lane.id()); for (auto && besides_previous_lanelet_id : besides_previous_lanelet_ids) { lanelet::ConstLanelet besides_previous_lanelet = map->laneletLayer.get(besides_previous_lanelet_id); From 7dd5aa860f8ee61070ef21d2b647b4f436575089 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 5 Sep 2022 23:38:57 +0900 Subject: [PATCH 12/16] added missing parameter Signed-off-by: Mamoru Sobue --- .../behavior_velocity_planner/intersection.param.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 0ea817ffefc81..79eb257de6889 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -14,6 +14,7 @@ detection_area_right_margin: 0.5 # [m] detection_area_left_margin: 0.5 # [m] detection_area_length: 200.0 # [m] + detection_area_angle_threshold: 0.785 # [rad] min_predicted_path_confidence: 0.05 collision_start_margin_time: 5.0 # [s] collision_end_margin_time: 2.0 # [s] From 1329fc34b0464607c01cac8b01f295c5ce1fc2eb Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 6 Sep 2022 18:26:26 +0900 Subject: [PATCH 13/16] fixed order Signed-off-by: Mamoru Sobue --- .../src/scene_module/intersection/debug.cpp | 25 ++++++++++--------- .../intersection/scene_intersection.cpp | 5 ++-- 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index d873f0d5b9dfa..c7bbb10395af8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -30,14 +30,14 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerOrientation; using tier4_autoware_utils::createMarkerScale; -visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( +static visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, const int64_t lane_id, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; int32_t i = 0; - int32_t uid = planning_utils::bitShift(module_id); + int32_t uid = planning_utils::bitShift(lane_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -120,26 +120,27 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( appendMarkerArray( createLaneletPolygonsMarkerArray( debug_data_.detection_area, "detection_area", lane_id_, 0.0, 1.0, 0.0), - &debug_marker_array, current_time); + &debug_marker_array); appendMarkerArray( createLaneletPolygonsMarkerArray( debug_data_.adjacent_area, "adjacent_area", lane_id_, 0.913, 0.639, 0.149), - &debug_marker_array, current_time); + &debug_marker_array); appendMarkerArray( - createPolygonMarkerArray( - debug_data_.intersection_area, "intersection_area", lane_id_, 0.0, 1.0, 0.0), - &debug_marker_array, current_time); + debug::createPolygonMarkerArray( + debug_data_.intersection_area, "intersection_area", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 1.0, + 0.0), + &debug_marker_array); appendMarkerArray( - createPolygonMarkerArray( - debug_data_.ego_lane_polygon, "ego_lane", lane_id_, 0.3, 0.0, 0.00.0, 0.3, 0.7), - &debug_marker_array, current_time); + debug::createPolygonMarkerArray( + debug_data_.ego_lane_polygon, "ego_lane", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 0.3, 0.7), + &debug_marker_array); appendMarkerArray( debug::createPolygonMarkerArray( - debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", module_id_, now, 0.3, 0.0, + debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", lane_id_, now, 0.3, 0.0, 0.0, 0.0, 0.5, 0.5), &debug_marker_array, now); @@ -153,7 +154,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( for (const auto & p : debug_data_.candidate_collision_object_polygons) { appendMarkerArray( debug::createPolygonMarkerArray( - p, "candidate_collision_object_polygons", module_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, + p, "candidate_collision_object_polygons", lane_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, 0.5), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 76e1fcea0729f..d3bd968842adb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -344,10 +344,9 @@ bool IntersectionModule::checkCollision( const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); const auto is_in_adjacent_lanelets = checkAngleForTargetLanelets( object_direction, adjacent_lanelet_ids, planner_param_.detection_area_margin); + if (is_in_adjacent_lanelets) continue; if (is_in_intersection_area) { - if (!is_in_adjacent_lanelets) { - target_objects.objects.push_back(object); - } + target_objects.objects.push_back(object); } else if (checkAngleForTargetLanelets( object_direction, detection_area_lanelet_ids, planner_param_.detection_area_margin)) { From 998783b40bab41e892bc42715d2726e6210101a0 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 14 Sep 2022 10:22:26 +0900 Subject: [PATCH 14/16] Revert "feat(shift_decider): put the gear in park when vehicle reaches the goal (#1818)" This reverts commit 98a01c00a17061d9c90fed194445d10b51361a5f. Signed-off-by: Mamoru Sobue --- .../include/shift_decider/shift_decider.hpp | 6 ------ .../launch/shift_decider.launch.xml | 1 - control/shift_decider/package.xml | 1 - control/shift_decider/src/shift_decider.cpp | 16 ++-------------- .../launch/control.launch.py | 1 - 5 files changed, 2 insertions(+), 23 deletions(-) diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp index 02619bb2b11b7..a9d50d474c55a 100644 --- a/control/shift_decider/include/shift_decider/shift_decider.hpp +++ b/control/shift_decider/include/shift_decider/shift_decider.hpp @@ -18,7 +18,6 @@ #include #include -#include #include #include @@ -31,21 +30,16 @@ class ShiftDecider : public rclcpp::Node private: void onTimer(); void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg); - void onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg); void updateCurrentShiftCmd(); void initTimer(double period_s); rclcpp::Publisher::SharedPtr pub_shift_cmd_; rclcpp::Subscription::SharedPtr sub_control_cmd_; - rclcpp::Subscription::SharedPtr - sub_autoware_state_; rclcpp::TimerBase::SharedPtr timer_; autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr control_cmd_; - autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_; autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_; - bool park_on_goal_; }; #endif // SHIFT_DECIDER__SHIFT_DECIDER_HPP_ diff --git a/control/shift_decider/launch/shift_decider.launch.xml b/control/shift_decider/launch/shift_decider.launch.xml index 73ed434ade7f6..4fa7031ea8625 100644 --- a/control/shift_decider/launch/shift_decider.launch.xml +++ b/control/shift_decider/launch/shift_decider.launch.xml @@ -3,7 +3,6 @@ - diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml index c5bcfccb0ad43..a8c347b061472 100644 --- a/control/shift_decider/package.xml +++ b/control/shift_decider/package.xml @@ -14,7 +14,6 @@ autoware_cmake autoware_auto_control_msgs - autoware_auto_system_msgs autoware_auto_vehicle_msgs rclcpp rclcpp_components diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp index 5d9e1abe0015a..36a574c790e93 100644 --- a/control/shift_decider/src/shift_decider.cpp +++ b/control/shift_decider/src/shift_decider.cpp @@ -30,14 +30,10 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) rclcpp::QoS durable_qos(queue_size); durable_qos.transient_local(); - park_on_goal_ = declare_parameter("park_on_goal", true); - pub_shift_cmd_ = create_publisher("output/gear_cmd", durable_qos); sub_control_cmd_ = create_subscription( "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1)); - sub_autoware_state_ = create_subscription( - "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1)); initTimer(0.1); } @@ -48,14 +44,9 @@ void ShiftDecider::onControlCmd( control_cmd_ = msg; } -void ShiftDecider::onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg) -{ - autoware_state_ = msg; -} - void ShiftDecider::onTimer() { - if (!autoware_state_ || !control_cmd_) { + if (!control_cmd_) { return; } @@ -65,14 +56,11 @@ void ShiftDecider::onTimer() void ShiftDecider::updateCurrentShiftCmd() { - using autoware_auto_system_msgs::msg::AutowareState; using autoware_auto_vehicle_msgs::msg::GearCommand; shift_cmd_.stamp = now(); static constexpr double vel_threshold = 0.01; // to prevent chattering - if (autoware_state_->state == AutowareState::ARRIVED_GOAL && park_on_goal_) { - shift_cmd_.command = GearCommand::PARK; - } else if (control_cmd_->longitudinal.speed > vel_threshold) { + if (control_cmd_->longitudinal.speed > vel_threshold) { shift_cmd_.command = GearCommand::DRIVE; } else if (control_cmd_->longitudinal.speed < -vel_threshold) { shift_cmd_.command = GearCommand::REVERSE; diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 044c5e3a8136a..725ff1cab4217 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -146,7 +146,6 @@ def launch_setup(context, *args, **kwargs): name="shift_decider", remappings=[ ("input/control_cmd", "/control/trajectory_follower/control_cmd"), - ("input/state", "/autoware/state"), ("output/gear_cmd", "/control/shift_decider/gear_cmd"), ], parameters=[ From 0bf3bf8269eaa9f804b4daf1c8621fbf1bb0f9c3 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 14 Sep 2022 16:34:16 +0900 Subject: [PATCH 15/16] Revert "Revert "feat(shift_decider): put the gear in park when vehicle reaches the goal (#1818)"" This reverts commit e68f86ba1d4d6eb531dae2127c12fcc53533936c. Signed-off-by: Mamoru Sobue --- .../include/shift_decider/shift_decider.hpp | 6 ++++++ .../launch/shift_decider.launch.xml | 1 + control/shift_decider/package.xml | 1 + control/shift_decider/src/shift_decider.cpp | 16 ++++++++++++++-- .../launch/control.launch.py | 1 + 5 files changed, 23 insertions(+), 2 deletions(-) diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp index a9d50d474c55a..02619bb2b11b7 100644 --- a/control/shift_decider/include/shift_decider/shift_decider.hpp +++ b/control/shift_decider/include/shift_decider/shift_decider.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -30,16 +31,21 @@ class ShiftDecider : public rclcpp::Node private: void onTimer(); void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg); + void onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg); void updateCurrentShiftCmd(); void initTimer(double period_s); rclcpp::Publisher::SharedPtr pub_shift_cmd_; rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr + sub_autoware_state_; rclcpp::TimerBase::SharedPtr timer_; autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr control_cmd_; + autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_; autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_; + bool park_on_goal_; }; #endif // SHIFT_DECIDER__SHIFT_DECIDER_HPP_ diff --git a/control/shift_decider/launch/shift_decider.launch.xml b/control/shift_decider/launch/shift_decider.launch.xml index 4fa7031ea8625..73ed434ade7f6 100644 --- a/control/shift_decider/launch/shift_decider.launch.xml +++ b/control/shift_decider/launch/shift_decider.launch.xml @@ -3,6 +3,7 @@ + diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml index a8c347b061472..c5bcfccb0ad43 100644 --- a/control/shift_decider/package.xml +++ b/control/shift_decider/package.xml @@ -14,6 +14,7 @@ autoware_cmake autoware_auto_control_msgs + autoware_auto_system_msgs autoware_auto_vehicle_msgs rclcpp rclcpp_components diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp index 36a574c790e93..5d9e1abe0015a 100644 --- a/control/shift_decider/src/shift_decider.cpp +++ b/control/shift_decider/src/shift_decider.cpp @@ -30,10 +30,14 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) rclcpp::QoS durable_qos(queue_size); durable_qos.transient_local(); + park_on_goal_ = declare_parameter("park_on_goal", true); + pub_shift_cmd_ = create_publisher("output/gear_cmd", durable_qos); sub_control_cmd_ = create_subscription( "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1)); + sub_autoware_state_ = create_subscription( + "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1)); initTimer(0.1); } @@ -44,9 +48,14 @@ void ShiftDecider::onControlCmd( control_cmd_ = msg; } +void ShiftDecider::onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg) +{ + autoware_state_ = msg; +} + void ShiftDecider::onTimer() { - if (!control_cmd_) { + if (!autoware_state_ || !control_cmd_) { return; } @@ -56,11 +65,14 @@ void ShiftDecider::onTimer() void ShiftDecider::updateCurrentShiftCmd() { + using autoware_auto_system_msgs::msg::AutowareState; using autoware_auto_vehicle_msgs::msg::GearCommand; shift_cmd_.stamp = now(); static constexpr double vel_threshold = 0.01; // to prevent chattering - if (control_cmd_->longitudinal.speed > vel_threshold) { + if (autoware_state_->state == AutowareState::ARRIVED_GOAL && park_on_goal_) { + shift_cmd_.command = GearCommand::PARK; + } else if (control_cmd_->longitudinal.speed > vel_threshold) { shift_cmd_.command = GearCommand::DRIVE; } else if (control_cmd_->longitudinal.speed < -vel_threshold) { shift_cmd_.command = GearCommand::REVERSE; diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 725ff1cab4217..044c5e3a8136a 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -146,6 +146,7 @@ def launch_setup(context, *args, **kwargs): name="shift_decider", remappings=[ ("input/control_cmd", "/control/trajectory_follower/control_cmd"), + ("input/state", "/autoware/state"), ("output/gear_cmd", "/control/shift_decider/gear_cmd"), ], parameters=[ From 2640ddb9832e9976860b37211dc1d0256b362a00 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 20 Sep 2022 18:03:37 +0900 Subject: [PATCH 16/16] added document for intersection_area Signed-off-by: Mamoru Sobue --- .../docs/intersection/intersection.drawio.svg | 2 +- planning/behavior_velocity_planner/intersection-design.md | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg b/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg index a5cc48a88bd42..e6bd97a3cdebb 100644 --- a/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg +++ b/planning/behavior_velocity_planner/docs/intersection/intersection.drawio.svg @@ -1,4 +1,4 @@ -
Pass Judge Line
Pass Judge Line
Stop Line
Stop Line
Driving Lane
Driving Lane

Attention lane (Yellow)

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Attention lane (Yellow)...

Stuck Vehicle

If there is a stuck vehicle on an exit place in the intersection, wait at the entrance of the intersection

Stuck Vehicle...

Front vehicle in a same lane

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Front vehicle in a same lane...

Consider collisions with vehicles on the attention area

Consider collisions with vehicles on the at...
Behind Vehicle in a same lane
Behind Vehicle in a s...

stuck_vehicle_detect_dist_(5.0m)

Detection distance for the stuck vehicle

stuck_vehicle_detect_dist_(5.0m)...

Stop Line Margin

Distance between stop line and crossing lane  when stop line is generated.

Stop Line Margin...
Ignore None Attention Area Collision
Ignore None Attention Area Collis...Text is not SVG - cannot display \ No newline at end of file +
Pass Judge Line
Pass Judge Line
Stop Line
Stop Line
Driving Lane
Driving Lane

Attention lane (Yellow)

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Attention lane (Yellow)...

Stuck Vehicle

If there is a stuck vehicle on an exit place in the intersection, wait at the entrance of the intersection

Stuck Vehicle...

Front vehicle in a same lane

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Front vehicle in a same lane...

Consider collisions with vehicles on the attention area

Consider collisions with vehicles on the at...
Behind Vehicle in a same lane
Behind Vehicle in a sa...

stuck_vehicle_detect_dist_(5.0m)

Detection distance for the stuck vehicle

stuck_vehicle_detect_dist_(5.0m)...

Stop Line Margin

Distance between stop line and crossing lane  when stop line is generated.

Stop Line Margin...
Ignore None Attention Area Collision
Ignore None Attention Area Collis...

Intersection Area (Green)

Objects exist in this area is also the target for attention

Intersection Area (Green)...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/intersection-design.md b/planning/behavior_velocity_planner/intersection-design.md index f499a1ed194eb..3959a5ebfd83d 100644 --- a/planning/behavior_velocity_planner/intersection-design.md +++ b/planning/behavior_velocity_planner/intersection-design.md @@ -25,11 +25,12 @@ Objects that satisfy all of the following conditions are considered as target ob - The type of object type is **car**, **truck**, **bus** or **motorbike**. (Bicycle, pedestrian, animal, unknown are not.) - The center of gravity of object is **located within a certain distance** from the attention lane (threshold = `detection_area_margin`) . - - In the past, the decision was made based on whether any points of the object's polygon exists in the attention lane, but since there were many false positives, the logic has changed to the current one. + - (Optional condition) The center of gravity is in the **intersection area**. + - To deal with objects that is in the area not covered by the lanelets in the intersection. - The posture of object is **the same direction as the attention lane** (threshold = `detection_area_angle_threshold`). - The orientation of the target is recalculated in this module according to the `orientation_reliable` and the sign of the velocity of the target. -- Not being **in the same lane as the ego vehicle**. - - To avoid judging the vehicle ahead as a collision target. This logic needs to be improved. +- Not being **in the neighbouring lanes of the ego vehicle**. + - neighbouring lanes include the ego lane of the vehicle and the adjacent lanes of it with turn_direction as the ego lane. #### How to Define Attention Lanes