From 33e2956a6f4734cb9f48814d46dfefd8921ba358 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Wed, 30 Aug 2023 03:27:25 +0200 Subject: [PATCH 01/14] build: remove Boost from CMakeLists.txt (#4786) Signed-off-by: Esteve Fernandez --- common/perception_utils/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/common/perception_utils/CMakeLists.txt b/common/perception_utils/CMakeLists.txt index 302bb63512937..d4bbe928c0be8 100644 --- a/common/perception_utils/CMakeLists.txt +++ b/common/perception_utils/CMakeLists.txt @@ -4,6 +4,4 @@ project(perception_utils) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(Boost REQUIRED) - ament_auto_package() From d6748e62f1b501e704d77dbc44442f32e3dca853 Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Wed, 30 Aug 2023 12:32:31 +0900 Subject: [PATCH 02/14] chore: add TLR model args to launch files (#4805) Signed-off-by: Shunsuke Miura --- .../launch/perception.launch.xml | 13 +++++++++++++ .../traffic_light.launch.xml | 16 ++++++++++++---- 2 files changed, 25 insertions(+), 4 deletions(-) diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 6634c36ff3768..c41177e51116b 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -75,6 +75,19 @@ + + + + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 1f86a89ddfc7a..ebe3ff6af1893 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -3,10 +3,12 @@ - - - - + + + + + + @@ -17,6 +19,12 @@ + + + + + + From 8f9da612d166785f9f01bf7a4618226fc5e290e3 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 30 Aug 2023 12:58:31 +0900 Subject: [PATCH 03/14] feat(crosswalk): stabler collision point (#4781) * feat(crosswalk): stabler collision point Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/debug.cpp | 6 +- .../src/scene_crosswalk.cpp | 134 +++++++++--------- 2 files changed, 72 insertions(+), 68 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index a5bc3ebf72179..eae7491872cfd 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -97,7 +97,7 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( for (size_t i = 0; i < debug_data.obj_polygons.size(); ++i) { const auto & points = debug_data.obj_polygons.at(i); auto marker = createDefaultMarker( - "map", now, "object polygon", uid + i++, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), + "map", now, "object polygon", uid + i, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 1.0, 0.999)); marker.points = points; // marker.points.push_back(marker.points.front()); @@ -107,8 +107,8 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( for (size_t i = 0; i < debug_data.ego_polygons.size(); ++i) { const auto & points = debug_data.ego_polygons.at(i); auto marker = createDefaultMarker( - "map", now, "vehicle polygon", uid + i++, Marker::LINE_STRIP, - createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + "map", now, "vehicle polygon", uid + i, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), + createMarkerColor(1.0, 1.0, 0.0, 0.999)); marker.points = points; // marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 3b69b74c53d80..b7b2c9c594647 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -68,39 +68,34 @@ std::vector toGeometryPointVector( return points; } -Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back, - const geometry_msgs::msg::Polygon & base_polygon) +void offsetPolygon2d( + const geometry_msgs::msg::Pose & origin_point, const geometry_msgs::msg::Polygon & polygon, + Polygon2d & offset_polygon) { - Polygon2d one_step_polygon{}; - - { - geometry_msgs::msg::Polygon out_polygon{}; - geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_front); - tf2::doTransform(base_polygon, out_polygon, geometry_tf); - - for (const auto & p : out_polygon.points) { - one_step_polygon.outer().push_back(Point2d(p.x, p.y)); - } + for (const auto & polygon_point : polygon.points) { + const auto offset_pos = + tier4_autoware_utils::calcOffsetPose(origin_point, polygon_point.x, polygon_point.y, 0.0) + .position; + offset_polygon.outer().push_back(Point2d(offset_pos.x, offset_pos.y)); } +} - { - geometry_msgs::msg::Polygon out_polygon{}; - geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_back); - tf2::doTransform(base_polygon, out_polygon, geometry_tf); - - for (const auto & p : out_polygon.points) { - one_step_polygon.outer().push_back(Point2d(p.x, p.y)); - } +template +Polygon2d createMultiStepPolygon( + const T & obj_path_points, const geometry_msgs::msg::Polygon & polygon, const size_t start_idx, + const size_t end_idx) +{ + Polygon2d multi_step_polygon{}; + for (size_t i = start_idx; i <= end_idx; ++i) { + offsetPolygon2d( + tier4_autoware_utils::getPose(obj_path_points.at(i)), polygon, multi_step_polygon); } - Polygon2d hull_polygon{}; - bg::convex_hull(one_step_polygon, hull_polygon); - bg::correct(hull_polygon); + Polygon2d hull_multi_step_polygon{}; + bg::convex_hull(multi_step_polygon, hull_multi_step_polygon); + bg::correct(hull_multi_step_polygon); - return hull_polygon; + return hull_multi_step_polygon; } void sortCrosswalksByDistance( @@ -127,24 +122,12 @@ void sortCrosswalksByDistance( std::sort(crosswalks.begin(), crosswalks.end(), compare); } -std::vector calcOverlappingPoints(const Polygon2d & polygon1, const Polygon2d & polygon2) +std::vector calcOverlappingPoints(const Polygon2d & polygon1, const Polygon2d & polygon2) { // NOTE: If one polygon is fully inside the other polygon, the result is empty. - std::vector intersection{}; - bg::intersection(polygon1, polygon2, intersection); - - if (bg::within(polygon1, polygon2)) { - for (const auto & p : polygon1.outer()) { - intersection.push_back(Point2d{p.x(), p.y()}); - } - } - if (bg::within(polygon2, polygon1)) { - for (const auto & p : polygon2.outer()) { - intersection.push_back(Point2d{p.x(), p.y()}); - } - } - - return intersection; + std::vector intersection_polygons{}; + bg::intersection(polygon1, polygon2, intersection_polygons); + return intersection_polygons; } StopFactor createStopFactor( @@ -602,37 +585,56 @@ std::optional CrosswalkModule::getCollisionPoint( double minimum_stop_dist = std::numeric_limits::max(); std::optional nearest_collision_point{std::nullopt}; for (const auto & obj_path : object.kinematics.predicted_paths) { - for (size_t i = 0; i < obj_path.path.size() - 1; ++i) { - const auto & p_obj_front = obj_path.path.at(i); - const auto & p_obj_back = obj_path.path.at(i + 1); - const auto obj_one_step_polygon = createOneStepPolygon(p_obj_front, p_obj_back, obj_polygon); + size_t start_idx{0}; + bool is_start_idx_initialized{false}; + for (size_t i = 0; i < obj_path.path.size(); ++i) { + // For effective computation, the point and polygon intersection is calculated first. + const auto obj_one_step_polygon = createMultiStepPolygon(obj_path.path, obj_polygon, i, i); + const auto one_step_intersection_polygons = + calcOverlappingPoints(obj_one_step_polygon, attention_area); + if (!one_step_intersection_polygons.empty()) { + if (!is_start_idx_initialized) { + start_idx = i; + is_start_idx_initialized = true; + } + if (i != obj_path.path.size() - 1) { + // NOTE: Even if the object path does not fully cross the ego path, the path should be + // considered. + continue; + } + } + + if (!is_start_idx_initialized) { + continue; + } + + // Calculate multi-step object polygon, and reset start_idx + const size_t start_idx_with_margin = std::max(static_cast(start_idx) - 1, 0); + const size_t end_idx_with_margin = std::min(i + 1, obj_path.path.size() - 1); + const auto obj_multi_step_polygon = createMultiStepPolygon( + obj_path.path, obj_polygon, start_idx_with_margin, end_idx_with_margin); + is_start_idx_initialized = false; // Calculate intersection points between object and attention area - const auto tmp_intersection = calcOverlappingPoints(obj_one_step_polygon, attention_area); - if (tmp_intersection.empty()) { + const auto multi_step_intersection_polygons = + calcOverlappingPoints(obj_multi_step_polygon, attention_area); + if (multi_step_intersection_polygons.empty()) { continue; } // Calculate nearest collision point among collisions with a predicted path - double local_minimum_stop_dist = std::numeric_limits::max(); - geometry_msgs::msg::Point local_nearest_collision_point{}; - for (const auto & p : tmp_intersection) { - const auto cp = createPoint(p.x(), p.y(), ego_pos.z); - const auto dist_ego2cp = calcSignedArcLength(ego_path.points, ego_pos, cp); - - if (dist_ego2cp < local_minimum_stop_dist) { - local_minimum_stop_dist = dist_ego2cp; - local_nearest_collision_point = cp; - } - } + Point2d boost_intersection_center_point; + bg::centroid(multi_step_intersection_polygons.front(), boost_intersection_center_point); + const auto intersection_center_point = createPoint( + boost_intersection_center_point.x(), boost_intersection_center_point.y(), ego_pos.z); const auto dist_ego2cp = - calcSignedArcLength(ego_path.points, ego_pos, local_nearest_collision_point); + calcSignedArcLength(ego_path.points, ego_pos, intersection_center_point); constexpr double eps = 1e-3; const auto dist_obj2cp = calcArcLength(obj_path.path) < eps ? 0.0 - : calcSignedArcLength(obj_path.path, size_t(0), local_nearest_collision_point); + : calcSignedArcLength(obj_path.path, size_t(0), intersection_center_point); if ( dist_ego2cp < crosswalk_attention_range.first || @@ -644,8 +646,9 @@ std::optional CrosswalkModule::getCollisionPoint( if (dist_ego2cp < minimum_stop_dist) { minimum_stop_dist = dist_ego2cp; nearest_collision_point = createCollisionPoint( - local_nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); - debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z)); + intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); + debug_data_.obj_polygons.push_back( + toGeometryPointVector(obj_multi_step_polygon, ego_pos.z)); } break; @@ -753,7 +756,8 @@ Polygon2d CrosswalkModule::getAttentionArea( break; } - const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, ego_polygon); + const auto ego_one_step_polygon = + createMultiStepPolygon(sparse_resample_path.points, ego_polygon, j, j + 1); debug_data_.ego_polygons.push_back(toGeometryPointVector(ego_one_step_polygon, ego_pos.z)); From 56c458533f81b5c3201c887aeb52c06232854360 Mon Sep 17 00:00:00 2001 From: Tao Zhong <55872497+tzhong518@users.noreply.github.com> Date: Wed, 30 Aug 2023 13:47:48 +0900 Subject: [PATCH 04/14] fix(crosswalk_traffic_light_estimator): move crosswalk after fusion (#4734) * fix: move crosswalk after fusion Signed-off-by: tzhong518 * Update launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> * Rename TrafficLight to TrafficSignal Signed-off-by: Shunsuke Miura * change input to be considered as the regulatory-element Signed-off-by: Shunsuke Miura --------- Signed-off-by: tzhong518 Signed-off-by: Shunsuke Miura Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: Shunsuke Miura --- .../traffic_light.launch.xml | 23 +++++- .../traffic_light_node_container.launch.py | 43 ----------- .../node.hpp | 10 ++- .../package.xml | 1 + .../src/node.cpp | 72 +++++++++++-------- 5 files changed, 72 insertions(+), 77 deletions(-) diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index ebe3ff6af1893..a9301f31d87aa 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -11,6 +11,7 @@ + @@ -70,7 +71,7 @@ - + @@ -120,7 +121,7 @@ - + @@ -132,9 +133,27 @@ + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index 8b3d15f2cff95..a6bcb40e81252 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -73,7 +73,6 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("classifier_mean", "[123.675, 116.28, 103.53]") add_launch_arg("classifier_std", "[58.395, 57.12, 57.375]") - add_launch_arg("use_crosswalk_traffic_light_estimator", "True") add_launch_arg("use_intra_process", "False") add_launch_arg("use_multithread", "False") @@ -137,46 +136,6 @@ def create_parameter_dict(*args): output="both", ) - estimator_loader = LoadComposableNodes( - composable_node_descriptions=[ - ComposableNode( - package="crosswalk_traffic_light_estimator", - plugin="traffic_light::CrosswalkTrafficLightEstimatorNode", - name="crosswalk_traffic_light_estimator", - namespace="classification", - remappings=[ - ("~/input/vector_map", "/map/vector_map"), - ("~/input/route", "/planning/mission_planning/route"), - ("~/input/classified/traffic_signals", "classified/traffic_signals"), - ("~/output/traffic_signals", "estimated/traffic_signals"), - ], - extra_arguments=[{"use_intra_process_comms": False}], - ), - ], - target_container=container, - condition=IfCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")), - ) - - relay_loader = LoadComposableNodes( - composable_node_descriptions=[ - ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="classified_signals_relay", - namespace="classification", - parameters=[ - {"input_topic": "classified/traffic_signals"}, - {"output_topic": "estimated/traffic_signals"}, - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ) - ], - target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")), - ) - decompressor_loader = LoadComposableNodes( composable_node_descriptions=[ ComposableNode( @@ -251,7 +210,5 @@ def create_parameter_dict(*args): container, decompressor_loader, fine_detector_loader, - estimator_loader, - relay_loader, ] ) diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp index 0850436851e6a..3b4855f1f38e9 100644 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -47,9 +48,9 @@ using autoware_planning_msgs::msg::LaneletRoute; using tier4_autoware_utils::DebugPublisher; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; -using tier4_perception_msgs::msg::TrafficLightElement; -using tier4_perception_msgs::msg::TrafficSignal; -using tier4_perception_msgs::msg::TrafficSignalArray; +using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; +using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray; +using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; using TrafficSignalAndTime = std::pair; using TrafficLightIdMap = std::unordered_map; @@ -89,6 +90,9 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, const TrafficLightIdMap & traffic_light_id_map) const; + boost::optional getHighestConfidenceTrafficSignal( + const lanelet::Id & id, const TrafficLightIdMap & traffic_light_id_map) const; + // Node param bool use_last_detect_color_; double last_detect_color_hold_time_; diff --git a/perception/crosswalk_traffic_light_estimator/package.xml b/perception/crosswalk_traffic_light_estimator/package.xml index 435ecf6e6fa3d..f029972f7124f 100644 --- a/perception/crosswalk_traffic_light_estimator/package.xml +++ b/perception/crosswalk_traffic_light_estimator/package.xml @@ -12,6 +12,7 @@ autoware_auto_mapping_msgs autoware_auto_planning_msgs + autoware_perception_msgs autoware_planning_msgs lanelet2_extension rclcpp diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index 55d272cb71cfe..dd7a9ea04ae38 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -160,7 +160,7 @@ void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray( TrafficLightIdMap traffic_light_id_map; for (const auto & traffic_signal : msg->signals) { - traffic_light_id_map[traffic_signal.traffic_light_id] = + traffic_light_id_map[traffic_signal.traffic_signal_id] = std::pair(traffic_signal, get_clock()->now()); } @@ -191,11 +191,11 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal( continue; } - if (elements.front().color == TrafficLightElement::UNKNOWN) { + if (elements.front().color == TrafficSignalElement::UNKNOWN) { continue; } - const auto & id = input_traffic_signal.second.first.traffic_light_id; + const auto & id = input_traffic_signal.second.first.traffic_signal_id; if (last_detect_color_.count(id) == 0) { last_detect_color_.insert(std::make_pair(id, input_traffic_signal.second)); @@ -207,7 +207,7 @@ void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal( std::vector erase_id_list; for (auto & last_traffic_signal : last_detect_color_) { - const auto & id = last_traffic_signal.second.first.traffic_light_id; + const auto & id = last_traffic_signal.second.first.traffic_signal_id; if (traffic_light_id_map.count(id) == 0) { // hold signal recognition results for [last_detect_color_hold_time_] seconds. @@ -227,19 +227,13 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( const auto tl_reg_elems = crosswalk.regulatoryElementsAs(); for (const auto & tl_reg_elem : tl_reg_elems) { - const auto crosswalk_traffic_lights = tl_reg_elem->trafficLights(); - - for (const auto & traffic_light : crosswalk_traffic_lights) { - const auto ll_traffic_light = static_cast(traffic_light); - - TrafficSignal output_traffic_signal; - TrafficLightElement output_traffic_light; - output_traffic_light.color = color; - output_traffic_light.confidence = 1.0; - output_traffic_signal.elements.push_back(output_traffic_light); - output_traffic_signal.traffic_light_id = ll_traffic_light.id(); - msg.signals.push_back(output_traffic_signal); - } + TrafficSignal output_traffic_signal; + TrafficSignalElement output_traffic_signal_element; + output_traffic_signal_element.color = color; + output_traffic_signal_element.confidence = 1.0; + output_traffic_signal.elements.push_back(output_traffic_signal_element); + output_traffic_signal.traffic_signal_id = tl_reg_elem->id(); + msg.signals.push_back(output_traffic_signal); } } @@ -256,34 +250,32 @@ lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets( } const auto tl_reg_elem = tl_reg_elems.front(); - const auto traffic_lights_for_vehicle = tl_reg_elem->trafficLights(); - const auto current_detected_signal = - getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, traffic_light_id_map); + getHighestConfidenceTrafficSignal(tl_reg_elem->id(), traffic_light_id_map); if (!current_detected_signal && !use_last_detect_color_) { continue; } const auto current_is_not_red = - current_detected_signal ? current_detected_signal.get() == TrafficLightElement::GREEN || - current_detected_signal.get() == TrafficLightElement::AMBER + current_detected_signal ? current_detected_signal.get() == TrafficSignalElement::GREEN || + current_detected_signal.get() == TrafficSignalElement::AMBER : true; const auto current_is_unknown_or_none = - current_detected_signal ? current_detected_signal.get() == TrafficLightElement::UNKNOWN + current_detected_signal ? current_detected_signal.get() == TrafficSignalElement::UNKNOWN : true; const auto last_detected_signal = - getHighestConfidenceTrafficSignal(traffic_lights_for_vehicle, last_detect_color_); + getHighestConfidenceTrafficSignal(tl_reg_elem->id(), last_detect_color_); if (!last_detected_signal) { continue; } const auto was_not_red = current_is_unknown_or_none && - (last_detected_signal.get() == TrafficLightElement::GREEN || - last_detected_signal.get() == TrafficLightElement::AMBER) && + (last_detected_signal.get() == TrafficSignalElement::GREEN || + last_detected_signal.get() == TrafficSignalElement::AMBER) && use_last_detect_color_; if (!current_is_not_red && !was_not_red) { @@ -324,13 +316,13 @@ uint8_t CrosswalkTrafficLightEstimatorNode::estimateCrosswalkTrafficSignal( } if (has_straight_non_red_lane || has_related_non_red_tl) { - return TrafficLightElement::RED; + return TrafficSignalElement::RED; } const auto has_merge_lane = hasMergeLane(non_red_lanelets, routing_graph_ptr_); return !has_merge_lane && has_left_non_red_lane && has_right_non_red_lane - ? TrafficLightElement::RED - : TrafficLightElement::UNKNOWN; + ? TrafficSignalElement::RED + : TrafficSignalElement::UNKNOWN; } boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal( @@ -367,6 +359,28 @@ boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenc return ret; } + +boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal( + const lanelet::Id & id, const TrafficLightIdMap & traffic_light_id_map) const +{ + boost::optional ret{boost::none}; + + double highest_confidence = 0.0; + if (traffic_light_id_map.count(id) == 0) { + return ret; + } + + for (const auto & element : traffic_light_id_map.at(id).first.elements) { + if (element.confidence < highest_confidence) { + continue; + } + + highest_confidence = element.confidence; + ret = element.color; + } + + return ret; +} } // namespace traffic_light #include From 58a54633edf01dc2f383acbd4237a2ade54a926a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 30 Aug 2023 14:50:17 +0900 Subject: [PATCH 05/14] feat(crosswalk): no stop decision with braking distance (#4802) * feat(crosswalk): no stop decision with bracking distance Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 12 +++++++++--- .../src/manager.cpp | 10 ++++++++-- .../src/scene_crosswalk.cpp | 12 +++++++++++- .../src/scene_crosswalk.hpp | 3 +++ 4 files changed, 31 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 4b0a1ffc4317b..b1e5bcb32bd18 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -42,12 +42,18 @@ ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering - max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. + + no_stop_decision: + max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding - disable_stop_for_yield_cancel: false # for the crosswalk where there is a traffic signal - disable_yield_for_new_stopped_object: false # for the crosswalk where there is a traffic signal + disable_stop_for_yield_cancel: true # for the crosswalk where there is a traffic signal + disable_yield_for_new_stopped_object: true # for the crosswalk where there is a traffic signal timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 30a1a81a1b50a..5a9de20d79477 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -88,8 +88,14 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_later_margin_y"); cp.ego_pass_later_additional_margin = getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_later_additional_margin"); - cp.max_offset_to_crosswalk_for_yield = - getOrDeclareParameter(node, ns + ".pass_judge.max_offset_to_crosswalk_for_yield"); + cp.max_offset_to_crosswalk_for_yield = getOrDeclareParameter( + node, ns + ".pass_judge.no_stop_decision.max_offset_to_crosswalk_for_yield"); + cp.min_acc_for_no_stop_decision = + getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.min_acc"); + cp.max_jerk_for_no_stop_decision = + getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.max_jerk"); + cp.min_jerk_for_no_stop_decision = + getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.min_jerk"); cp.stop_object_velocity = getOrDeclareParameter(node, ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index b7b2c9c594647..7adde028f59c0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -309,6 +309,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const std::optional & default_stop_pose) { const auto & ego_pos = planner_data_->current_odometry->pose.position; + const double ego_vel = planner_data_->current_velocity->twist.linear.x; + const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; + const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second; @@ -324,11 +327,18 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area); // Check if ego moves forward enough to ignore yield. + const auto & p = planner_param_; if (!path_intersects.empty()) { const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const double dist_ego2crosswalk = calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()); - if (dist_ego2crosswalk - base_link2front < planner_param_.max_offset_to_crosswalk_for_yield) { + const auto braking_distance_opt = motion_utils::calcDecelDistWithJerkAndAccConstraints( + ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, + p.min_jerk_for_no_stop_decision); + const double braking_distance = braking_distance_opt ? *braking_distance_opt : 0.0; + if ( + dist_ego2crosswalk - base_link2front - braking_distance < + p.max_offset_to_crosswalk_for_yield) { return {}; } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index edff62371f6b4..e0851366d7f71 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -106,6 +106,9 @@ class CrosswalkModule : public SceneModuleInterface std::vector ego_pass_later_margin_y; double ego_pass_later_additional_margin; double max_offset_to_crosswalk_for_yield; + double min_acc_for_no_stop_decision; + double max_jerk_for_no_stop_decision; + double min_jerk_for_no_stop_decision; double stop_object_velocity; double min_object_velocity; bool disable_stop_for_yield_cancel; From 1afe476a1923669cbf14cfcac978050eacaeb5ec Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 30 Aug 2023 15:38:54 +0900 Subject: [PATCH 06/14] feat(intersection): suppress intersection occlusion chattering (#4788) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 1 + .../src/manager.cpp | 2 + .../src/scene_intersection.cpp | 80 ++++++++++++------- .../src/scene_intersection.hpp | 2 + 4 files changed, 55 insertions(+), 30 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 2b3ca11bbe750..fb822cdc5cf4d 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -48,6 +48,7 @@ denoise_kernel: 1.0 # [m] possible_object_bbox: [1.0, 2.0] # [m x m] ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h + stop_release_margin_time: 1.0 # [s] enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval intersection: true diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 9d6359ebe9b9b..8a4a3e046022d 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -123,6 +123,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + ip.occlusion.stop_release_margin_time = + getOrDeclareParameter(node, ns + ".occlusion.stop_release_margin_time"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 7ae549ce8b274..c5fcaca40c24f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -82,10 +82,19 @@ IntersectionModule::IntersectionModule( turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); collision_state_machine_.setMarginTime( planner_param_.collision_detection.state_transit_margin_time); - before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); - before_creep_state_machine_.setState(StateMachine::State::STOP); - stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); - stuck_private_area_timeout_.setState(StateMachine::State::STOP); + { + before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); + before_creep_state_machine_.setState(StateMachine::State::STOP); + } + { + occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time); + occlusion_stop_state_machine_.setState(StateMachine::State::GO); + } + { + stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); + stuck_private_area_timeout_.setState(StateMachine::State::STOP); + } + decision_state_pub_ = node_.create_publisher("~/debug/intersection/decision_state", 1); } @@ -639,6 +648,35 @@ void reactRTCApproval( return; } +static std::string formatDecisionResult(const IntersectionModule::DecisionResult & decision_result) +{ + if (std::holds_alternative(decision_result)) { + return "Indecisive"; + } + if (std::holds_alternative(decision_result)) { + return "Safe"; + } + if (std::holds_alternative(decision_result)) { + return "StuckStop"; + } + if (std::holds_alternative(decision_result)) { + return "NonOccludedCollisionStop"; + } + if (std::holds_alternative(decision_result)) { + return "FirstWaitBeforeOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "PeekingTowardOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "OccludedCollisionStop"; + } + if (std::holds_alternative(decision_result)) { + return "TrafficLightArrowSolidOn"; + } + return ""; +} + bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { debug_data_ = util::DebugData(); @@ -650,31 +688,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); - std::string decision_type = "intersection" + std::to_string(module_id_) + " : "; - if (std::get_if(&decision_result)) { - decision_type += "Indecisive"; - } - if (std::get_if(&decision_result)) { - decision_type += "StuckStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "NonOccludedCollisionStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "FirstWaitBeforeOcclusion"; - } - if (std::get_if(&decision_result)) { - decision_type += "PeekingTowardOcclusion"; - } - if (std::get_if(&decision_result)) { - decision_type += "OccludedCollisionStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "Safe"; - } - if (std::get_if(&decision_result)) { - decision_type += "TrafficLightArrowSolidOn"; - } + const std::string decision_type = + "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); std_msgs::msg::String decision_result_msg; decision_result_msg.data = decision_type; decision_state_pub_->publish(decision_result_msg); @@ -898,10 +913,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( first_attention_area.value(), interpolated_path_info, occlusion_attention_divisions_.value(), parked_attention_objects, occlusion_dist_thr) : true; + occlusion_stop_state_machine_.setStateWithMarginTime( + is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, + logger_.get_child("occlusion_stop"), *clock_); // check safety const bool ext_occlusion_requested = (is_occlusion_cleared && !occlusion_activated_); - if (!is_occlusion_cleared || ext_occlusion_requested) { + if ( + occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || + ext_occlusion_requested) { const double dist_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(closest_idx).point.pose.position, path->points.at(default_stop_line_idx).point.pose.position); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 774b68710be7f..6684ae5ae0b45 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -109,6 +109,7 @@ class IntersectionModule : public SceneModuleInterface double denoise_kernel; std::vector possible_object_bbox; double ignore_parked_vehicle_speed_threshold; + double stop_release_margin_time; } occlusion; }; @@ -223,6 +224,7 @@ class IntersectionModule : public SceneModuleInterface // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop + StateMachine occlusion_stop_state_machine_; // NOTE: uuid_ is base member // for stuck vehicle detection From 198587c9a254940ba072e110e3fb1ab36644265e Mon Sep 17 00:00:00 2001 From: Phoebe Wu Date: Wed, 30 Aug 2023 16:35:22 +0800 Subject: [PATCH 07/14] refactor(radar_fusion_to_detected_object): rework parameters (#4663) * refactor(radar_fusion_to_detected_object): rework parameters Signed-off-by: PhoebeWu21 * style(pre-commit): autofix * style(pre-commit): autofix * style(pre-commit): autofix --------- Signed-off-by: PhoebeWu21 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> --- .../radar_fusion_to_detected_object/README.md | 10 +- ...adar_fusion_to_detected_object.schema.json | 114 ++++++++++++++++++ ..._object_fusion_to_detected_object_node.cpp | 26 ++-- 3 files changed, 131 insertions(+), 19 deletions(-) create mode 100644 perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json diff --git a/perception/radar_fusion_to_detected_object/README.md b/perception/radar_fusion_to_detected_object/README.md index b22938cbf0e5a..24eb4eb0e93c1 100644 --- a/perception/radar_fusion_to_detected_object/README.md +++ b/perception/radar_fusion_to_detected_object/README.md @@ -13,11 +13,11 @@ The document of core algorithm is [here](docs/algorithm.md) ### Parameters for sensor fusion -| Name | Type | Description | Default value | -| :----------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | -| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | -| threshold_yaw_diff | double | The yaw orientation threshold. If $ \vert \theta _{ob} - \theta_ {ra} \vert < threshold*yaw_diff $ attached to radar information include estimated velocity, where $ \theta*{ob} $ is yaw angle from 3d detected object, $ \theta\_ {ra} $ is yaw angle from radar object. [rad] | 0.35 | +| Name | Type | Description | Default value | +| :----------------------- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| bounding_box_margin | double | The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m] | 2.0 | +| split_threshold_velocity | double | The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s] | 5.0 | +| threshold_yaw_diff | double | The yaw orientation threshold. If ∣*θob* − *θra*∣ < threshold × yaw*diff attached to radar information include estimated velocity, where*θob*is yaw angle from 3d detected object,*θra\_ is yaw angle from radar object. [rad] | 0.35 | ### Weight parameters for velocity estimation diff --git a/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json b/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json new file mode 100644 index 0000000000000..688414df56c1e --- /dev/null +++ b/perception/radar_fusion_to_detected_object/schema/radar_fusion_to_detected_object.schema.json @@ -0,0 +1,114 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Radar Fusion to Detected Object Node", + "type": "object", + "definitions": { + "radar_fusion_to_detected_object": { + "type": "object", + "properties": { + "node_params": { + "type": "object", + "properties": { + "update_rate_hz": { + "type": "number", + "description": "Update rate for parameters. [hz]", + "default": "10.0", + "minimum": 0.0 + } + }, + "required": ["update_rate_hz"] + }, + "core_params": { + "type": "object", + "properties": { + "bounding_box_margin": { + "type": "number", + "description": "The distance to extend the 2D bird's-eye view Bounding Box on each side. This distance is used as a threshold to find radar centroids falling inside the extended box. [m]", + "default": "2.0", + "minimum": 0.0 + }, + "split_threshold_velocity": { + "type": "number", + "description": "The object's velocity threshold to decide to split for two objects from radar information (currently not implemented) [m/s]", + "default": "5.0", + "minimum": 0.0 + }, + "threshold_yaw_diff": { + "type": "number", + "description": "The yaw orientation threshold. If ∣θob−θra∣("node_params.update_rate_hz", 10.0); + node_param_.update_rate_hz = declare_parameter("node_params.update_rate_hz"); // Core Parameter - core_param_.bounding_box_margin = - declare_parameter("core_params.bounding_box_margin", 0.5); + core_param_.bounding_box_margin = declare_parameter("core_params.bounding_box_margin"); core_param_.split_threshold_velocity = - declare_parameter("core_params.split_threshold_velocity", 0.0); - core_param_.threshold_yaw_diff = - declare_parameter("core_params.threshold_yaw_diff", 0.35); + declare_parameter("core_params.split_threshold_velocity"); + core_param_.threshold_yaw_diff = declare_parameter("core_params.threshold_yaw_diff"); core_param_.velocity_weight_min_distance = - declare_parameter("core_params.velocity_weight_min_distance", 1.0); + declare_parameter("core_params.velocity_weight_min_distance"); core_param_.velocity_weight_average = - declare_parameter("core_params.velocity_weight_average", 0.0); + declare_parameter("core_params.velocity_weight_average"); core_param_.velocity_weight_median = - declare_parameter("core_params.velocity_weight_median", 0.0); + declare_parameter("core_params.velocity_weight_median"); core_param_.velocity_weight_target_value_average = - declare_parameter("core_params.velocity_weight_target_value_average", 0.0); + declare_parameter("core_params.velocity_weight_target_value_average"); core_param_.velocity_weight_target_value_top = - declare_parameter("core_params.velocity_weight_target_value_top", 1.0); + declare_parameter("core_params.velocity_weight_target_value_top"); core_param_.convert_doppler_to_twist = - declare_parameter("core_params.convert_doppler_to_twist", false); + declare_parameter("core_params.convert_doppler_to_twist"); core_param_.threshold_probability = - declare_parameter("core_params.threshold_probability", 0.0); + declare_parameter("core_params.threshold_probability"); core_param_.compensate_probability = - declare_parameter("core_params.compensate_probability", false); + declare_parameter("core_params.compensate_probability"); // Core radar_fusion_to_detected_object_ = std::make_unique(get_logger()); From 14deed5b2d3cfb11d3adffb504d2c799dc25bffc Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 30 Aug 2023 17:43:43 +0900 Subject: [PATCH 08/14] feat(intersection): strict definition of stuck vehicle detection area (#4782) Signed-off-by: Mamoru Sobue --- .../README.md | 3 +- .../config/intersection.param.yaml | 1 - .../docs/stuck-vehicle.drawio.svg | 443 +++++++++++------- .../src/manager.cpp | 4 - .../src/scene_intersection.cpp | 34 +- .../src/scene_intersection.hpp | 6 +- .../src/util.cpp | 130 +++-- .../src/util.hpp | 14 +- .../src/util_type.hpp | 3 + 9 files changed, 376 insertions(+), 262 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 3e62ab75d23cd..5f26ef34c1186 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -62,7 +62,7 @@ Objects that satisfy all of the following conditions are considered as target ob #### Stuck Vehicle Detection -If there is any object in a certain distance (`stuck_vehicle_ignore_dist` and `stuck_vehicle_detect_dist`) before and after the exit of the intersection lane and the object velocity is less than a threshold (=`stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path +If there is any object on the path in inside the intersection and at the exit of the intersection (up to `stuck_vehicle_detect_dist`) lane and its velocity is less than a threshold (`stuck_vehicle.stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg) @@ -107,7 +107,6 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav | `common.stop_overshoot_margin` | double | [m] margin for the overshoot from stopline | | `common.path_interpolation_ds` | double | [m] path interpolation interval | | `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection | -| `stuck_vehicle.stuck_vehicle_ignore_dist` | double | [m] length behind the exit of intersection for stuck vehicle detection | | `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection | | `collision_detection.state_transit_margin_time` | double | [m] time margin to change state | | `collision_detection.min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection | diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index fb822cdc5cf4d..c30c5d70f8c60 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -15,7 +15,6 @@ stuck_vehicle: use_stuck_stopline: true # stopline generated before the first conflicting area stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h # enable_front_car_decel_prediction: false # By default this feature is disabled # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning diff --git a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg index d2a8286122e54..8802dc786b617 100644 --- a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg @@ -5,28 +5,28 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="2831px" - height="1073px" - viewBox="-0.5 -0.5 2831 1073" - content="<mxfile host="Electron" modified="2023-06-07T05:26:53.508Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="B7vxgoBz0X2z8hjEAQoV" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="2842px" + height="1201px" + viewBox="-0.5 -0.5 2842 1201" + content="<mxfile host="Electron" modified="2023-08-28T08:00:31.600Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="CbgId97oWJwBUVWni98L" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">7V1rc9s21v41nul+MAd3gB8TJ267230nu912237JyLJsq1EsV5Jz2V//AiRB4sareJOjtJ1GIAVSOM85OHdc4KuPX77fLZ4e/rm9XW0uELj9coHfXCCEIEY4QvJvavBrOhgTQtKR+936Nh2DxcDP6/+tskGQjT6vb1d768bDdrs5rJ/sweX28XG1PFhji91u+9m+7W67sZ/6tLhfeQM/LxcbPap/gRr/7/r28JCOC8SL8R9W6/sH/WzI4vTKx4W+OZt6/7C43X5Oh5Kfh99e4KvddntI//bxy9Vqo5ZQr8yvV9+Tr3/9cvufV7dP19dXb37/44fry3Rhrtt8Jf9pu9XjofPUV89vf3n6/PbygV79+f7dl5/f/veP15c0nfrTYvOcrVj2Ww9f9RLuts+Ptys1CbjArz8/rA+rn58WS3X1swSOHHs4fNzIT1D+9W692VxtN9td8l38OvlHju+2h8VhvX2Uw5dQqInuto8H4870jxzfH3bbD6vQHFv51PVBIZGqCW7XO4mYdM7Pq71cmNcN1ylbz0+r3WH1xSBxtm7fr7YfV4fdV3lLdvUScowzRGeccEmZyNbqcwEriGDGHw8GplgcxRrTGZ7v82cUFJN/yYjWgoCinoCrx9tXipXkp8ft48omWLrcmjNw60Vc3VoM6C+hsToU+Gujx3arjQTIJ5uZQ6uVPeHddi1fr45CsT3Pfvu8W66yr5qs4s3GYuTMxgAR9myHxe5+dfBmk2u9+Grc9qRu2Fe9uv8w+erCgUY6bQGUfIm7Yyc+YyfHDnQIgBk4AjvubADz4bDjvTrlw2NHb/Bn8DAIUY+ChyJntiEFD/XAM4rggXoH7g88t4v9Q6KkwKAOca3+OWGMcSpckUJg5AiVFijz56PQEXglKOsNA+gsQLQA8bZ/LcK7CRB3NoDZcALEf3UWUTKCDMGt8LPcLPb79dKGkCU0FB5+zr673R0etvfbx8XmbTH6erW5SeZShsNa2piepMlAaltMLweVFNKuqKRCeLMBZ7b+UOm/OiK0/Rs6XxoIx6Qex32b4PEpGuDa3C4AJDIAmQY4AHFEsc8XHEWMlHPBUTY4nMCLcpIkBHi2JGS9ayMvXCNl1BPo7jwtlBYY+7ONrI/y8Zn4RF2hKHZoRXCAjRHgPv6GdIWiBhSs0whtCpcTwGZ34pNk9WV9+E1rlvLvv6spI5p9evMle0Ly4av+8CiX4Tfzg/Et9bH4WvJJf68drVOubKCRpAzX4Mb5CCUhmANNcIQlJbA7GwTN/Hh9CSXUzr1/hnSNhlYPaTo3SPOYRHFMXVgTFBFOYv2nM8aD0zNAcQQa2md9YR23cwqesV4d0q2F+tyQLs0DLwoGHc93c2BzzwfAgBgZ0O08nOMAGnQBNJxWePOmwpvPDNIMcdfixcIxa1rIaoTc2Qge10iiDTwdZzPZMpOFSzPmxoJbIMCbTe7UZFwEnB0lrXMLuIMAijr70Zng/mwjI+DsKKkEVpWjhIFsHttRwnz8DekooQ2syjMFwxSk2ukxqauLtkvdOquWNZxQbyyJmW0rIdVSh1d6US0xdWYbeFthZ+9tP5Buai3RuVlLUtoiV9oyWninuoSVkKcthWYcGNqigbS+l9h9uvBDvEdubnllxeJGPwmEqWwYGZhEcmtG0lLBlHM3UYwwFJGYUYiT5FPsbYeEYOv72qdqggYLHglaTOKKrt52StLAq6jzeX5a3Kw277b7dUaAm+3hsP0oiaBveLVZ36sLh62jAO0fFk9qso9f7lUtTXSzkLIpklSTys7j8u1ms37arxLBI4dePd4nTwaRkFJcQMwJgDiW/yWy4ba4DiHAjFPAY84ZJbG8vtgttdwCkZSZjZQvSHx5mFlmXdSrapA31q80aTJkYR9JUEAfOpDHQ2GlgcOub714joSBuJYyiAUo46bM90eYBrl+vRsscVPSSJa/Xa8Kuya7a0yKXTJbfUN63zNJxgOb92Ak4z2kZ9okVaZjVmCIC9KYjqTGRmRXVU2+8PV6o18vt3siDoSlKKLEpq1QFeWHd6vdWi71aleqBlZio1YP1NK1Vg/UUm8meiByokAw7uguxdyeyJ1nYKWPN8jrXD7vPhX5x1Ve026WDQ6wTSkrRBwimx3UDl3HEPKTi+SCLxCmNl+w9nxR8Ny0XDIvvzKzsc078giEDrOhYerUHFZkIxSo8WNDVcOxnMEfzOEP1IE/+vKkjcVJeM6cBElfrDTyfqMTaOudDON7FEQcR3EsAJf/QobcZKaYqssUZH/8EIn8UsRE8f1AvIQjyT40T4gSQ/kTWAN/wnSmSHebI4dPc5sDS6pJmKt/BeZuaBoSEomCILEmmkFWLOnOA3YkH8ooYQ0M/OmcQZx6/h8Ja46J4DjGlMfU9f8g0QxAhM4QPxDQiAACcAwwFRBD29EIgYgApzEDMSNQIsm3aKmcIStXdnYGHjE+FIam8EWobgTzo6DEpKRgLpaJHSulUgAAI0VVePQLeiQGY37ewAs/tQ3W0h2RK5ICEEuRhHE8lANCw6RWIdTCtlYhZPMyrSCENqptZ6hQqM51FcA7xlwhgxW8g6sfMnS8qoEqOSfXRUvLx2Qbx/4axzsxAAvNy6aCtArchDro7sZBmFQ9hLos1IiD2vo1YId3GNTrIRqYJ8NkUbRwfIyRVWG76ntnTHyajInjKrwy2AtjkmqmYF0YswN/8H+Jzzc//sn+/uHd0+bD7pfFX29+u+zKHqmt1pxrqnOa018eZKEgXBpDdzJciSpLAPF+BL5glhcJ2NWZ2HIigWY9s3rzvjWwCueT4sMRINbaXdKYI7l8FVk9OGZGVo+01wMdGhQMpGarJ3GDCb2ZcrhBbO3FJ/VcjpDUk8O6uVeH297d/PN0aT14gvYsabb73IhjMz2KfdKMm9eDGxQTDdI4Z5K0ng7M5PgeoM6NmCytRzdhmqETbdKU7UqC15oTmg9qzQl940zMCQQcYe963ZuqdojaEyE4bvBUtEsWOAWz2cpUmwvOm/YmwPNqTuDiHHc1YZgzz8g5AqJBjc0cHLvHFJSpPDZqwxwRVAP0cB5bJ99u/zJ/Xl2WdFPMQovsKvOdrjPYzbzpyUeLofMcNIIfNp6gq+flKfaEhMTtsUVFoMJ2ip6QcQ9KQZe9e4LE9nYErpVtGv21si2el2wLtrWEbkZCm34dXksjNHJby3gCE/skBRHgLqmoCFSkTSKIJiuNHiF5pY1k7FtMNVXB4nmZ3arvvtcvNybdxRQl3mzxyGKqQUuSibKXOfaKyKWwiCiIdeqi70blhERI5LmNoQYvTk4zc0rS+6udbJD2NpmP9QhnqiihcLnzm8IIs9hpmalzUjiJOMopSvxkRQxoBANtXgbLV9TCYp5BrlCycuf85EuIR8hQbo2YPMaZu9+JyfaEziQlmU5RkzBKhKs5yXISsToSYTEqFzdwBLzItjbdErBEQ9VNQ76+y828kosFdUQKi4N4bavExc7RGhBWzzt08fOxXSLHqHeWipcNaRh3chN38Fb3zQ5ayatnBzArdmDAVq0ZtRRr4pxb1jjM4mQ6MIKrph2aGRrYNnOKukjGsNtZQNG+mqRzcn6noEvjbQOc5raR+8TzWAnuZdvAzrwqn73DttE2JENj57Es+HP6itBcf/n3r+ubP8mnffyPq4+72+c///fusoF15dpGe2lD7aPDbvG4f9ruUlU4Wi5275HNosnPX+3eflqpVQiq1ABQsLoLMTAA8M3rK8N0a2nbLbJPy9Vjwo2W+i1xodDq+mbvkj+D+lelZHcDPSjgXw1lleZZT8co5yv2+h+/XsW/3PzxdRF/+vHX9eKnyzYYOCT/qzOc9ql4BRFAQZe4DYLl7epG3NRbUayHLOEq0sRc1c471GEMRIHcTQFjn0JMSo2BSIR6ZFPcmk1XFBIQ3mdRlsLfF5s65J4N18aRiwwe52NWfngU6PPI2WDI6LsO2yFAHfPeieVquQxB40ZQQgdmWYFJpLOoC5blJGJ+mwVKRAQDclU3ke6dMiGXB9sov8+NRR/21/P2kOH6MhWdr+QNiD59SVZPX5d/u8/+v9H3HzXR/vC8/JAs+cN6KedA4Faqrim7yfXYrRbqJ0oFBtyvHle7xUFiCIHk4uFhldBULq66UwEsH1V6sLrvLh9JBMw+nTnKF2Hn/qBi5Mf0Gcmzl4v9Kp/p/95d6Xf681kqpbcp4fRlCULrJ1U8TJI0XUBv+KYYKxGxUoxtpDCqZ6YnwxzIvmRYCC6vPWViE6V7Jyl2UmxeTo0RZEjPzeou+ZFqpaQZE3A3Xyd/pmJTqdTaPMog9ZUeggJ7am469M6eE5RDnGT3fyqA2/uY6fx3q/u/PhJggO7/QQL2fwRLu0WciVFcQiFnV2txRjmOndkYIM3KCNufUe4/TL66cKBxnNkbxE67VJKXjR33bA/sls20wo47G3DPeeoTO96r9+wyCWKn3WGkLxg7xE+/OUbuADdLcEC5Q9zMh5HkzrHNl761Y8Mo8g4fBgRGnY+OC8xH4WA5XkEIwAbutm9GfribP+5+TjWJvdkAbhbi6iQ//FdnUZbmMKgIgROkuZxgJjMVzN2cqAgU4Q+cyRwmITqTsAkJuefbmw8J2x3scN7JKUSuxKTuPC2EPWP+bCPv41PUtp2oA8nNHCc4wMZDHh8ZpuBk5e4TJyW2o7WZTlKpkZjpJJU3zkcoYb1BFDpjdw2UYujOBodr7BZe4XZO0TOkazS0ekijuUEaJS3MSLjAQ5nPKCLcvdzFkq58DgMURwA4Ue+h0X8+PLUX9Af6k1TdNx/s+8d0Mui4EFsg3HNPMCBGBnQ7X/P5gOsaXb1enM+tEp9w6trAWHSOzxAO3NkIHtlsOrvA2xrOzKUZc2NqLYSaN5vcqcmoCED9O8BfOAIEdJMPKaKdEYCpP9vICJiuUvPkXScscDAYRKHS+iFdJ5pgZwq2pyDVSsakzi/U+8nE36ZqqZetVrXUN85mWwmplrp+thfVElNntqG3lW+1yLxvSDf156L5aUpepzHKqDoQoTOsGfC0pdCMQ0O7QahiPgcrUAiRcUqC55QhDEWk+SkLPA6U1KhDFgQt5nAlV38bZcilnhYL7J8WjxehCoxlKjFU9cXu/mbxnVwwVbfg/O9vCXFAUq5xt/i43nxNv/Jx+7jdp5pSfr0o5wBZNQeQZDpcZiUAr5LXWOxSZQbcLJYf7hNRdmm/yndIpdSppyenkyR/odmL+HUmNCGwqib75/Z2tVEDb5PR3VbemX/S60iTlZQjb9TfFUqoWisql7/uXpjfq4HaaRpUTJOSK79SXEiplV/QAl8NqEdZmqoaTHRVNZ7sEmoEJh8LjVXPpXcHdTVnQnUx0VyT7+S6a/6dTH9VV83dJjSn3mHUNZpOWOiyyasqbVYNG8uQsnOThc4v5Wtc8DZV3J3fqbqB4YIu4KtxKS0O0FdSni4ekRQJ6IsZO+dXM3VXX1Z1PfrSvfEmLhaSjzkgqgGC2wMk1xjUSOKMCIDBVAfUKHbIkOyQPRDBWU9qEMFdTfNavrGGXqFqnfO3yHa2VjiIw8+wNt8Qa5c8S4r5kmelicKhZ1nbsv+sLPHP+ZJO/2u2BOWvlaYUlwBWP9oGsUHsDug2eOuM7n7QbcxkkTYpkugZ3WXPSkoopkN36RJQHnrqYOimZ3T3im4GobFyA8tui0rzkd20FN1jy242KrqzgIl7m6t7poGTb4QdODWAaAvgtOSkV4Yof1pSkNKBIXqBofEzz0K2FyFbqh2a+2dfQrbsWUklzHRCtnwJ0pqZEeWswXVzchHEL85BYLoBbPkmDE3OdRDoWo4yps1qOpqwZlcfgaH6nBEyJEKAQeeTQoi5QZ+VtUm2VUbL9hRa9ozO2yqMy581oaoGjd85J0H1Er3d5qZgISCpZCoTVUlSRxmfjODtNu37TnJKh8gDgPCRVEdQV7iRMjKnofPiUWn4PHmQaqNH8xB68ewsjG4MpKF0Z8CZJQupOzdZ8xTrVCpKs3h6fsnQMLMIeujSqQtgIQyLwrY1erdrBC57FgTdXKMXvQjgY6NJZ/bqwF6GJHbZy7h06uzFY5ImmYRZzCvi6pXfKh9eVHZNxnfHxrnOfNeB74y1dfnOuHTifCeN0tJgLiyJU3RlM17qGkzrzCZjr2MDbVOzV/H5CPaCI7OXac25/AVfkOGOeJnXJ6lr63Ufs/J9HLNtSsP9HOubGoVMlCGDlSVbdEZh6bPSyrvJUDhuqO+MQg8ZHJtYcxyLJQpAdxualz9rShTONB73TTkxk5pCfcV3Yhrb1RROzJnG474phFDT5p6bm9s0o84Gy2gGS4Ujzrx24pt0lcFiBrAHNlgwLXnWGJu0uZYnyWAn6XAzVSOPwV5OJEnuOgaK7F1HV1r2ymQQlWrCVc8bhdGMlWjKaGnZJ3LVk9jFXYXSYBZ45tcN2h+jVDCGiVMLGlx7vya0RN0I1IaW4TdUIzqoItIhIBg+OE9dzo7OU1S0D89TF9XxeQFpmp5QpT7rYwBvFlLwRpKCUkd9XL7dbNZP+9RsT+pFXz3eZ28DIiH3OgExJwDiGMBU2io5btwDIcCMU8BjzhlVNaTynsVuWYhjEDGejLbQpSEJi/7cxTCwllzQpxbRJFwwgMsBC4XxFReikJcE73rDZIdg2Rjm08kQHJbUgFVQHLEKipcVjvVG8A7hm1Hs5bg9yaUIu12vTPM6v3siNFyysIZuVU64cOAVOtbgcDg22NBKd88PV1dD2CS46w/u4BfpSxfXh6sXP8QwxSOuDlNzrAWUOXMa2AvWIfDFeLHWjb3ZFoHcYLyBQi8an0v8UzAAUEnAHca9xmEwDz+m7CmjaPsdIjDL590nK4zSKCRzvJGNS9m7hlEjnmShOsyaqlFN2VWOhHjK5FqEC4s+fwg7lmtNSTE+H+fXToGPWZi/eK9cDGGJsEDd2Pj48rYSscLGLY3vUtU2S8FhcTTLxwqORvlYV44ezWHemxzIFfoTlgOQjCMIJt3PO0QqtfduYiedYDAiQAAu/4UMlaf+kghwdVJn8qc8aiwniZgo5qsIIXMk+ZrmucxmsswgHaA6BAvnah73agPn4K3HC46jWLKa+ldgXpbfBAmJREHY2ASB69OlsdQTKzAysJFsunhO0XHL9cZl+2olb3FMBMcxpqmj3PPVItEWqISeCk4hoFKoEYBjgKmA2AyamTgFQgm1mIGYESgRW+7JoXJGoy9MYH/lESvZgXrDqgHJWQmwtCXWSSBDsoO53ZFw2gslarcrBJhB+DYevsGF17FlaHNR/vty7xlGhAAkH9MzwTjOx8Zz5hkQc5V/c/dxlf9CXTkF5R9CaPNVOHQiLC0S8BKVr6NpwGAD3sbNXmEMswF3iNCdmhvwKFvbZuiAV+BIdu7Hy9eVwXMinwSD0ybMRajDXX3yNyZNXoG6DN6Jv4/3D8Ij33YM7yGedcFfRwfi9Cl/fpixWM1+xIpBntMWKzhuwiUMDihWSDNGZX2IlX64tkNgf+a5ghwBQxey0gPj5LTZ1umB0uqvaBWm8gOlvqgnLQsi9WaodYj3zsnLNFl64OXE6YEFq9X7nHjYd26NzypBEM+0Ak+XV50A0cNCC8XlJJ80QxB3iFON2OFyhgmCbQRAic8DmnlOc0oQJB0CYrP0NsxI9S8WrrGib6pznqJvMKyn6Nd4veel6CNQsj2WRU26KfOIhh9jNT0YW2E3KwfOZnYoC3em3FdRh1dcO2Xuw/2a0qzkKVPm5hCDx16Wk73fKnKVr2twWpGvixqzX3m+bh9+9s77ZA6+k+BUUrKB9ZsVj0qaPOKybL3B/eUYlrwRGtcnTjoE1cdME3s5TUcgKWvuS0VF05EZHANAOgRumyp0fWhYsyu8KlalcRMEkw9dkW5eO/EmCBXnGcCy/KHuDelKe5xanvzRVbSZF66+HJELSqpWlMitqGOfg8gdtZr1xNLduu8Uxdo1F8wGt3qC2bh26oKZlh7+EpO+BTMlpc+KpxTMHeKW86hr4bi0148UghEFsU4OLw/VcEIiJPIs8qpuiE7Vi+kHGUQYzjRyN3ElC6mx9M3AHYURZnHgHAUzm42TiKMcKeaqu7kHgEamh3nsTHAzaeUUEwzKylh6qly5hHja2pUWyLTyVqwwIjHFFqHzLlYx9+g5iajLqbML6qBgkZw1JTkWE0ofOuv4Fimj6pzixr2kjJrqmqu1m/zoau3FtVNQzQUtEZAsLuWW45X0uOTMUgibPXUMdZ0a7HbSCR1mVEoA7jMWjFtwVn0Tmc7s3zOTmhqmZ1qfVLoHA2Hzi1HL+CIlh5N3DD+X5OMxgps8dBQW7RBdOrVotGRZv5EaFHFDnhq6gUsvwehKLq4IRp/WVmvF4awoMR5wq8UlT1UVVT1stceHqmlc8oKsdFnGiFybZwo1lS2+2b6Xhv4+OuwWj/un7S61nKLlYvc+WRdXyiTLttq9/bRKV6/CIgOAgtXdRakkAgC+eX2VXD/SNbHIR5arx0yIODacRGbKZ6Hgzl3yxxENA4Vk5IZYFgVHFSGZquIWK+F4EHuvQ7yuANoh+0tT436vNx8QgRSD4SCdj7jl7epG3AQRF7T2mbsdDEXzmKvORyVkZwxEFTUjwjxiw0sZh9HArmfaIQ7XQsjgEAJaCJkVhQRUqTtIFe9dDCVkAmiar9yJozII8ti65hXamRc9ZxMbHIIdolKdXYsBijYVP3diuVouy7F4IyihY4kcJknKjPWx6C1AHIlygiPOLYnkaaKGRBqE4NXhLrZJ3bE3yYf7wjerLyhWC4CC/fW8LW641PvMq+RWudxfionyO81H7A/Pyw8Z0R7Wy2RmBG6l2aL5XS7HbrVIf0+iW4L71eNqtzgkSEQgu+nwsMoInKy6+paCq3VNWUXpd+6s8UQy7osn6t+crK/+4aFFSW7wlqxGfktJuJESrA0DPVmmXDaBZd+FeOwpk8foItv5Uy/u3jFEn0yxrQJGF7Zo3qzuMmws0tjTG1oaPLpO/syEXaUZUcKrsdkA0wtDVugGsOzk29ZsmgzutilL2AaM2t3/ub1d6bsV2tR6Xe+fFpIkCmrqo3xMBjL1DiWhyHwjdrfhdBPOkKZwpsOPdcFHK/QYrGw2YpDBqmYjGJlVNJtYUj9Ge1SMfSI95sSGVaaCFuBPPCmmupAFmvRyZXBK1mvFXv/j16v4l5s/vi7iTz/+ul78dHmpFEp1YwbJ5EaohiQdbLSB7CqB6Te+ph+VTaE+fs5+3kVWoKzGMjClg6owWQ1KECUDBYDk6HX6vIscNtnjUwgpuV7xE3j2ExIhn9znYUXv5Goft4RQJoIsWDQgzRwJk5w8Uk2ZpI7YpYwSnsMQRoxPGKVZNSONXQuc3zUmxZLTQQyKIQ1lk2QcjEmyuJ5kuSO5CMFaVLJJmnuCk+3X9hcrb7FLLJOuDsUyX246b+bHTb24hQ/XuPjma/4p9d2mr5f6nHF2dIf2N+Ps2I7c15x9NfUzFx8KHSQda4mNzOtbcSNLb8xcwPVSL3E+V2HNwBINQEmP5c7gYq4wvrInZD5ODeXkXA4DyklnbXMKy6tboNSbCHN7Incey0/roD3/1d0ZAIN6BshCKxmcCm7IJEgpK5QD3WcKl21KWSE5MMNiB7VD1zGEFd0s0P1bxgfU5gvWni8KnpuWS9isuITZ2OYdeSRpZ28yG2rGJFn8IL9Nhy/KXtdhRcYdjkvn65f/YKsNaEyWM/iDOfyBOvBHxsDwotVeNh0niTlzUnJeRC+sNPZ+g+rxniXHu4T/9ep78vWvX27/8+r26fr66s3vf/xwfanZx9VB7cT5ZByE6ad1U0gxjp1THoyFCp3u4KiugVMdXDQETnMYRK/VttA8TZHuNkcOn+Y2R+A0BouR/FMYHLLq0xc8Wg5llKiOx3XEm84ZxKnn/3EPUnD8P0g0AxChM8RP6JQEEz+B0xEc/OhEc39nSBPMh8EQrcdQ7wJAHXEwPwoGTjMwCBg4xaCJR2I45mezVQi7uiNyRVIdN2AqkuqogYEcEBomtQqhFra1CqHeUmeiEIbOEjBgHThDoJO66J8WYHpcqx8ytCrZwA8+J9dFS8vHZBvH/hrHOzEAC9F5sZDfrd8Ad6BLfyf/nt8429x9/M72A/g1Al32a99hWK9Hg0hJO7f7AI4PaDEdrWa6Xlz1vTMmPU3GDPS7N91yfp/7LowZ6GhvMoXfyX7Mra1BWCr1klgKeByW7oM7TphA1uKF+tG7RnWgD72LpkD/+UFUbtIgBvLiky8uR0i+yGHd3Prmthcu/zxd+gVp4LHv27JW5djzIw6xSJM0S3RN51HzLwganzLTpV90YCbHRoQ6hj1Z+gVp4KaeNvo1hhLYkuC1ap/mg1q1T984E7Uv6b9s4tP1jjZV7ZLaG1M0wXHVN9LAgX9i5o2VUTQXnMOmOIezxjnuasIwZ56RY7mkQYxhDg447UjrJMqTht9OvhGqAXo436iTD65/mQ/mxQvEEdVd8+hy9TP3TQ/jS0s6cJvPQSP4y0iDoNAw4fyiAC+5Mevo2iKJtZGC28pDUemJcIySrEu2azHqVq0uqLMWrcPoug3CFXVKQZe9e4IE5HYErpdtTXO2yLyyHyUeIRIOHqEbOW4q4ORsgLuzZc658bb8yUocTkwQUeKSiopA5dAkgqj3mocZJRm0kYx9iynRVEzNK7VUCRbmojUmR4gp6M0WjyumaAOP/lRZpsKx/kK9lh0xEeix7AkMv7fyIMKDTuD9HiGvNAdMc+e31xvZoGmgJ7JDUt0L2aPjUK5WiuoJN6+k0s55pEkj4/khJo9x5u53v1exg5JJUkfpFLnjY0S4WpAsJ5HfW9hlZDEqF0/mWSbTxkg6JcpogteqbhrytaobnVcSqKCOSAl09+2ixCV9fM15A/17x1TnGjiap86JTlrwWpCGcSc3cQdvde/s0DSwopWKmbAD04EUnSnm99Htwg2uUzHQKXdMZmjgC55T1CVpdGtl/Yv2Wf+dk6i7BF2a8wk6zW0j94nnsRK/JWsXRsHOvKFetA04pW1IJuk0a+2CwZ8zaISGNvDuu8ZRRRNZi0ftzo5BnTprGxvg4KxhbFknx1rjbpF9yvo32vp32qjRc85mLRoHdbAC6kZ6UMDBGkorzdOe+tfOG3hWG4MAtwZB1tYzJMbThp79gaAQ9sy1yVaLxCYrg8TdZv30Q6ka3xc8SAyT/psWQEgxZjrXmB60co/zW3tHCQu50NIWcDdF87d0IO1NaOAnbbJoNmNUrRgBok9f0q9l1/VEWfvFovmi2Xoxa7yYtF00my6Cu53q7Jq2T8waLl4lG6K6+TG5knxnmTQMzO77v3dXeq5H1fUO/Pkst7vbdAn1Td775L3v0p9q//yLpPliST88zUm63WKtH+LJUCq8FosepovmijhtrZg1VszUnaeCeZKWigWTJO0Us2aKSStFj1GzJooBRs3aJwa9HKNxjss3WOezGVxDQk5oqOOTvfOM6Lcq1tBdUeEqNF0dHmWur7OAY7uln4nGp481tukKnHmax3iY3GrqZhvYDhJd8ylS3/MRHrK0IsiDDQoA61TBQp1OfVK70iGOLmDxZsNugGposLQrONTJ+Yv9Q25GG/hQ4+8WB7ltJMUZchrfREYtcPRSUMO0CCincysR44aRx0ZNHAojl2hq60Lj2j6936wfV+8/yrdT2lKuy6xb6Tdyyz7YuPPUcFdb/7i+vVVff71bSV0lC0oDbT0ki0ZfX9A3aq7nwzbTZ6AHzgz/ptqih46CahtFhFEfTTD2FBGEA5B2kxd600N4g9jYUFFUpRCaREEFVWzRAwtVFSnir6UQSqhDHQICXzW9evP29Wvha8PN+jwNhwYKffUFBRKqND0sNAxVCsgaxCWmjKnHQlDEMeBIoPjCCbADwkEsQAwFhOq/xIQxw+2U+vC4W9zyxdLBA0J+uF05HbQtU0CJj5+IB5irqBIiqmuTRy4o5SF/wGxQRJi06hAQlFICMXdRJDjkgDMgBOaQuRDCAS9VEEKQhv1Ws4CQagHGAiDS7n/T34jiSDvBLXcSQBEayqHEA0kBumTzW9qXKnJIhkMHEU7nCImNONCJPuSJJj24S56v/vWDAO9u3j+//1US7/fLV+jrJZqzRIEwjgUhVKrzgEAxP4mSebCHlCixq9pyML4wCUJnQgX3BUBnBH0GeluR8K2iSaAz77alAAsBAaEYMIC9NiqxCiNLm5PGHENOyUsFD/E1GRFPocnwf4nPNz/+yf7+4d3T5sPul8Vfb367bJC239xL56esdPXHuXS/W7HlMqeU439rSrpZ+emY6NNP583Wn58uCJuQujNUQDXg73tefnifxTPfp9HV97fr/aHG8dc63jlLf2CJEOrOBAWuSOziSmhfn5XhMZD/L4i0kHbUDli4DljpGXCBiZbpyqtJdvc3i+9AGpR3/ve3ZCqQPPVu8XG9+Zp+5eP2cbtPQ+P59eKtQOlLpWfwuSfWyVH7aLuSQylB6Li8knsrTgVsMw0yzh0sPwazy2mXCemqT7s0H2EdG/lZvzwCh93i7m69TP6+yU4fRMnZgGCnP1afP5k/Ydfhinnhu59Wd4fLh4U+RjN9s781f4OLugM0UzS3XOf6U0VLf1GzRWjwo/SL1577mYjlwJGegfM1/XOSMwGtLhYiGuXH7+rMx/RZVL0EfZPOrsV18cDS07vtYzzzYYNPhjmfk+Oy47s5Ms/gdM/npMYRnN75nGQ+53M6KoZc/0X6sqHNPLt512BED3z3bzVdxp4Gc5ZPX6JWlG4p7t5Uvg2UqFP+25e8VuXJpuWaTZZL9ZL0Go6pE8liUJpgIFAjSgKqDRxMtwmZ72fd5qzbnHWbs25z1m2+bd0mJTM4PO8eqzb6s3rzzas3VCDuqjcxGFS9uciwbvgUC5Tjt/8P</diagram></mxfile>" > - - - - - - - - - - - - - - + + + + + + + + + + + + + - + - + - + - - - - - + + + + + - + - - - + + + - - - + + + - - - - + + + + + + + - - - - + + + + - - - + + + - - + + - + - - - - - + + + + + - +
@@ -259,22 +262,22 @@
- stuck vehicle detection area is generated on the path around the exit o... + stuck vehicle detection area is generated on the path around the exit o... - - - - - - - - - - - - + + + + + + + + + + + - + - + - + - - - - - + + + + + - + - +
@@ -366,14 +369,14 @@
- %3CmxGraphModel%3E%3Croot%3E%3... + %3CmxGraphModel%3E%3Croot%3E%3... - - - + + + - - - + + + - - - - + + + + - - - - + + + + - - - + + + - - + + - + - - + + - - + + @@ -510,7 +513,7 @@
@@ -521,18 +524,18 @@
- stuck vehicle detection area is generated from the path, so in this... + stuck vehicle detection area is generated from the path, so in this... - - - - - + + + + + -
+
@@ -542,76 +545,158 @@
- stop_line_margin + stop_line_margin - + - - + +
`
 - ` + ` + + + + +
+
+
+ + + stuck_vehicle_detect_dist + + +
+
+
+ + stuck_vehicle_detect_dist + + + + + +
+
+
+ + + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bfont-size%3A%2035px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3Bw%2F%20traffic%20light%2C%20right%26lt%3B%2Fb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bb%26gt%3B(Left-hand%20traffic)%26lt%3B%2Fb%26gt%3B%26lt%3B%2Ffont%26gt%3B%26lt%3Bspan%20style%3D%26quot%3Bfont-size%3A%2030px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3B%2Fb%26gt%3B%26lt%3B%2Fspan%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dleft%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22-17325%22%20y%3D%22-7209%22%20width%3D%22350%22%20height%3D%22140%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + + straight +
+
+ (Right-hand traffic) + + + +
+ + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%... + + + + + +
+
+
+ + + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bfont-size%3A%2035px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3Bw%2F%20traffic%20light%2C%20right%26lt%3B%2Fb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bb%26gt%3B(Left-hand%20traffic)%26lt%3B%2Fb%26gt%3B%26lt%3B%2Ffont%26gt%3B%26lt%3Bspan%20style%3D%26quot%3Bfont-size%3A%2030px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3B%2Fb%26gt%3B%26lt%3B%2Fspan%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dleft%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22-17325%22%20y%3D%22-7209%22%20width%3D%22350%22%20height%3D%22140%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + + left turn +
+
+ (Right-hand traffic) + + + +
+ + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%... + + diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 8a4a3e046022d..bf0970f3640d1 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -43,7 +43,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) { const std::string ns(getModuleName()); auto & ip = intersection_param_; - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.common.attention_area_margin = getOrDeclareParameter(node, ns + ".common.attention_area_margin"); ip.common.attention_area_length = @@ -68,9 +67,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); - ip.stuck_vehicle.stuck_vehicle_ignore_dist = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") + - vehicle_info.max_longitudinal_offset_m; ip.stuck_vehicle.stuck_vehicle_vel_thr = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_vel_thr"); /* diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index c5fcaca40c24f..0dfadf2f00d7a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -771,22 +771,18 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( [closest_idx, stuck_stop_line_idx, default_stop_line_idx, occlusion_peeking_stop_line_idx, pass_judge_line_idx] = intersection_stop_lines; + const auto & conflicting_area = intersection_lanelets_.value().conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, *path, associative_ids_, closest_idx, - planner_data_->vehicle_info_.vehicle_width_m); + lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area.value(), + conflicting_area, closest_idx, planner_data_->vehicle_info_.vehicle_width_m); if (!path_lanelets_opt.has_value()) { RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); return IntersectionModule::Indecisive{}; } const auto path_lanelets = path_lanelets_opt.value(); - const auto ego_lane_with_next_lane = - path_lanelets.next.has_value() - ? std::vector< - lanelet::ConstLanelet>{path_lanelets.ego_or_entry2exit, path_lanelets.next.value()} - : std::vector{path_lanelets.ego_or_entry2exit}; - const bool stuck_detected = - checkStuckVehicle(planner_data_, ego_lane_with_next_lane, *path, intersection_stop_lines); + const bool stuck_detected = checkStuckVehicle( + planner_data_, path_lanelets, interpolated_path_info, intersection_stop_lines); if (stuck_detected) { const double dist_stopline = motion_utils::calcSignedArcLength( @@ -967,9 +963,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, - const lanelet::ConstLanelets & ego_lane_with_next_lane, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, + const util::InterpolatedPathInfo & interpolated_path_info, const util::IntersectionStopLines & intersection_stop_lines) { const auto & objects_ptr = planner_data->predicted_objects; @@ -978,20 +973,16 @@ bool IntersectionModule::checkStuckVehicle( const auto stuck_line_idx = intersection_stop_lines.stuck_stop_line; // considering lane change in the intersection, these lanelets are generated from the path - const auto ego_lane = ego_lane_with_next_lane.front(); - debug_data_.ego_lane = ego_lane.polygon3d(); + const auto & path = interpolated_path_info.path; const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( - input_path, ego_lane_with_next_lane, closest_idx, - planner_param_.stuck_vehicle.stuck_vehicle_detect_dist, - planner_param_.stuck_vehicle.stuck_vehicle_ignore_dist, - planner_data->vehicle_info_.vehicle_length_m); + path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); const double dist_stuck_stopline = motion_utils::calcSignedArcLength( - input_path.points, input_path.points.at(stuck_line_idx).point.pose.position, - input_path.points.at(closest_idx).point.pose.position); + path.points, path.points.at(stuck_line_idx).point.pose.position, + path.points.at(closest_idx).point.pose.position); const bool is_over_stuck_stopline = - util::isOverTargetIndex(input_path, closest_idx, current_pose, stuck_line_idx) && + util::isOverTargetIndex(path, closest_idx, current_pose, stuck_line_idx) && (dist_stuck_stopline > planner_param_.common.stop_overshoot_margin); bool is_stuck = false; @@ -1079,6 +1070,7 @@ bool IntersectionModule::checkCollision( const auto closest_arc_coords = getArcCoordinates( concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); const auto & ego_lane = path_lanelets.ego_or_entry2exit; + debug_data_.ego_lane = ego_lane.polygon3d(); const auto ego_poly = ego_lane.polygon2d().basicPolygon(); // check collision between predicted_path and ego_area diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 6684ae5ae0b45..245503683e128 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -65,8 +65,6 @@ class IntersectionModule : public SceneModuleInterface { bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check - double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle - //! check double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped /* double @@ -248,8 +246,8 @@ class IntersectionModule : public SceneModuleInterface bool checkStuckVehicle( const std::shared_ptr & planner_data, - const lanelet::ConstLanelets & ego_lane_with_next_lane, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const util::PathLanelets & path_lanelets, + const util::InterpolatedPathInfo & interpolated_path_info, const util::IntersectionStopLines & intersection_stop_lines); autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index a987ce9af9dfe..cd6d941b43de2 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -324,14 +324,25 @@ std::optional generateIntersectionStopLines( std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon) + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, + const bool search_forward) { const auto polygon_2d = lanelet::utils::to2D(polygon); - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - auto p = path.points.at(i).point.pose.position; - const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional(i); + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + const auto & p = path.points.at(i).point.pose.position; + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional(i); + } + } + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + const auto & p = path.points.at(i).point.pose.position; + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional(i); + } } } return std::nullopt; @@ -341,21 +352,39 @@ static std::optional> getFirstPointInsidePolygons( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, - const std::vector & polygons) + const std::vector & polygons, const bool search_forward = true) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - auto p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } if (is_in_lanelet) { - return std::make_optional>( - i, polygon); + break; } } - if (is_in_lanelet) { - break; + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } } } return std::nullopt; @@ -919,35 +948,29 @@ bool checkStuckVehicleInIntersection( } Polygon2d generateStuckVehicleDetectAreaPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double stuck_vehicle_detect_dist, const double stuck_vehicle_ignore_dist, - const double vehicle_length_m) + const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; using lanelet::utils::getPolygonFromArcLength; using lanelet::utils::to2D; - const double extra_dist = stuck_vehicle_detect_dist + vehicle_length_m; - const double ignore_dist = stuck_vehicle_ignore_dist + vehicle_length_m; - - const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front()); - - const auto closest_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - - const double start_arc_length = intersection_exit_length - ignore_dist > closest_arc_coords.length - ? intersection_exit_length - ignore_dist - : closest_arc_coords.length; - - const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist; + Polygon2d polygon{}; + if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { + return polygon; + } + double target_polygon_length = + getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); + lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; + if (path_lanelets.next) { + targets.push_back(path_lanelets.next.value()); + const double next_arc_length = + std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); + target_polygon_length += next_arc_length; + } const auto target_polygon = - to2D(getPolygonFromArcLength(ego_lane_with_next_lane, start_arc_length, end_arc_length)) - .basicPolygon(); - - Polygon2d polygon{}; + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); if (target_polygon.empty()) { return polygon; @@ -1130,13 +1153,17 @@ static lanelet::ConstLanelets getPrevLanelets( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const size_t closest_idx, const double width) + const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, const size_t closest_idx, + const double width) { - const auto assigned_lane_interval_opt = findLaneIdsInterval(path, associative_ids); + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; if (!assigned_lane_interval_opt) { return std::nullopt; } + const auto assigned_lane_interval = assigned_lane_interval_opt.value(); + const auto & path = interpolated_path_info.path; PathLanelets path_lanelets; // prev @@ -1144,7 +1171,7 @@ std::optional generatePathLanelets( path_lanelets.all = path_lanelets.prev; // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval_opt.value(); + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; if (closest_idx > assigned_lane_start) { path_lanelets.all.push_back( planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width)); @@ -1162,12 +1189,27 @@ std::optional generatePathLanelets( const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); if (next_lane_interval_opt) { const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = - planning_utils::generatePathLanelet(path, next_start + 1, next_end, width); + path_lanelets.next = planning_utils::generatePathLanelet(path, next_start, next_end, width); path_lanelets.all.push_back(path_lanelets.next.value()); } } + const auto first_inside_conflicting_idx_opt = + getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + const auto last_inside_conflicting_idx_opt = + getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { + const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); + const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; + lanelet::ConstLanelet conflicting_interval = planning_utils::generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = planning_utils::generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } return path_lanelets; } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 287a75979927b..1361847b4b980 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -83,7 +83,8 @@ std::optional generateIntersectionStopLines( std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon); + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, + const bool search_forward = true); /** * @brief check if ego is over the target_idx. If the index is same, compare the exact pose @@ -132,10 +133,7 @@ bool checkStuckVehicleInIntersection( DebugData * debug_data); Polygon2d generateStuckVehicleDetectAreaPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double stuck_vehicle_detect_dist, const double stuck_vehicle_ignore_dist, - const double vehicle_length_m); + const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); bool checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, @@ -158,8 +156,10 @@ double calcDistanceUntilIntersectionLanelet( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const size_t closest_idx, const double width); + const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, const size_t closest_idx, + const double width); } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 26187d75ff53c..0af797b35b2cc 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -142,6 +142,9 @@ struct PathLanelets std::optional next = std::nullopt; // this is nullopt is the goal is inside intersection lanelet::ConstLanelets all; + lanelet::ConstLanelets + conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with + // conflicting lanelets plus the next lane part of the path }; } // namespace behavior_velocity_planner::util From 741ab23abfdf0b62f6c8848feb9c8d90ab696753 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Wed, 30 Aug 2023 12:03:51 +0200 Subject: [PATCH 09/14] build(behavior_velocity_intersection_module): add libopencv-dev dependency (#4814) Signed-off-by: Esteve Fernandez --- planning/behavior_velocity_intersection_module/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 1d6067d4be322..f22e9d788fc65 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -23,6 +23,7 @@ geometry_msgs interpolation lanelet2_extension + libopencv-dev magic_enum motion_utils nav_msgs From 812910abf1c8d21de0337d64cd29835ca8c400d1 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 30 Aug 2023 19:48:12 +0900 Subject: [PATCH 10/14] feat(geography_utils): add geography_utils package (#4807) * first commit Signed-off-by: kminoda * update Signed-off-by: kminoda * style(pre-commit): autofix * update height.cpp Signed-off-by: kminoda * style(pre-commit): autofix * updat Signed-off-by: kminoda * style(pre-commit): autofix * add test Signed-off-by: kminoda * revert tier4_autoware_utils Signed-off-by: kminoda * style(pre-commit): autofix * fix cspell Signed-off-by: kminoda * remove and revert convert.cpp Signed-off-by: kminoda * remove boost Signed-off-by: kminoda * update comment Signed-off-by: kminoda * remove maybe_unsued Signed-off-by: kminoda * style(pre-commit): autofix * rename from tier4_geography_utils to geography_utils Signed-off-by: kminoda * rename from tier4_geography_utils to geography_utils Signed-off-by: kminoda * style(pre-commit): autofix * add some test Signed-off-by: kminoda * style(pre-commit): autofix * edit test Signed-off-by: kminoda * rename namespace Signed-off-by: kminoda * style(pre-commit): autofix * rename namespace complete Signed-off-by: kminoda * style(pre-commit): autofix * use angle brackets inclusion Signed-off-by: kminoda * style(pre-commit): autofix --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/geography_utils/CMakeLists.txt | 35 +++++++ common/geography_utils/README.md | 5 + .../include/geography_utils/height.hpp | 36 ++++++++ common/geography_utils/package.xml | 22 +++++ common/geography_utils/src/height.cpp | 63 +++++++++++++ common/geography_utils/test/test_height.cpp | 92 +++++++++++++++++++ .../gnss_poser/include/gnss_poser/convert.hpp | 47 +++++----- sensing/gnss_poser/package.xml | 1 + system/default_ad_api/package.xml | 1 + system/default_ad_api/src/vehicle.cpp | 10 +- 10 files changed, 284 insertions(+), 28 deletions(-) create mode 100644 common/geography_utils/CMakeLists.txt create mode 100644 common/geography_utils/README.md create mode 100644 common/geography_utils/include/geography_utils/height.hpp create mode 100644 common/geography_utils/package.xml create mode 100644 common/geography_utils/src/height.cpp create mode 100644 common/geography_utils/test/test_height.cpp diff --git a/common/geography_utils/CMakeLists.txt b/common/geography_utils/CMakeLists.txt new file mode 100644 index 0000000000000..5cc2290636157 --- /dev/null +++ b/common/geography_utils/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.14) +project(geography_utils) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# GeographicLib +find_package(PkgConfig) +find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h + PATH_SUFFIXES GeographicLib +) +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) +find_library(GeographicLib_LIBRARIES NAMES Geographic) + +ament_auto_add_library(geography_utils SHARED + src/height.cpp +) + +target_link_libraries(geography_utils + ${GeographicLib_LIBRARIES} +) + +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + + file(GLOB_RECURSE test_files test/*.cpp) + + ament_add_ros_isolated_gtest(test_geography_utils ${test_files}) + + target_link_libraries(test_geography_utils + geography_utils + ) +endif() + +ament_auto_package() diff --git a/common/geography_utils/README.md b/common/geography_utils/README.md new file mode 100644 index 0000000000000..fb4c2dc3a8312 --- /dev/null +++ b/common/geography_utils/README.md @@ -0,0 +1,5 @@ +# geography_utils + +## Purpose + +This package contains geography-related functions used by other packages, so please refer to them as needed. diff --git a/common/geography_utils/include/geography_utils/height.hpp b/common/geography_utils/include/geography_utils/height.hpp new file mode 100644 index 0000000000000..1921e79699970 --- /dev/null +++ b/common/geography_utils/include/geography_utils/height.hpp @@ -0,0 +1,36 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOGRAPHY_UTILS__HEIGHT_HPP_ +#define GEOGRAPHY_UTILS__HEIGHT_HPP_ + +#include +#include +#include +#include + +namespace geography_utils +{ + +typedef double (*HeightConversionFunction)( + const double height, const double latitude, const double longitude); +double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude); +double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude); +double convert_height( + const double height, const double latitude, const double longitude, + const std::string & source_vertical_datum, const std::string & target_vertical_datum); + +} // namespace geography_utils + +#endif // GEOGRAPHY_UTILS__HEIGHT_HPP_ diff --git a/common/geography_utils/package.xml b/common/geography_utils/package.xml new file mode 100644 index 0000000000000..61ac473498632 --- /dev/null +++ b/common/geography_utils/package.xml @@ -0,0 +1,22 @@ + + + + geography_utils + 0.1.0 + The geography_utils package + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + geographiclib + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/geography_utils/src/height.cpp b/common/geography_utils/src/height.cpp new file mode 100644 index 0000000000000..fe69557f25a76 --- /dev/null +++ b/common/geography_utils/src/height.cpp @@ -0,0 +1,63 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include + +namespace geography_utils +{ + +double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude) +{ + GeographicLib::Geoid egm2008("egm2008-1"); + // cSpell: ignore ELLIPSOIDTOGEOID + return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); +} + +double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude) +{ + GeographicLib::Geoid egm2008("egm2008-1"); + // cSpell: ignore GEOIDTOELLIPSOID + return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); +} + +double convert_height( + const double height, const double latitude, const double longitude, + const std::string & source_vertical_datum, const std::string & target_vertical_datum) +{ + if (source_vertical_datum == target_vertical_datum) { + return height; + } + std::map, HeightConversionFunction> conversion_map; + conversion_map[{"WGS84", "EGM2008"}] = convert_wgs84_to_egm2008; + conversion_map[{"EGM2008", "WGS84"}] = convert_egm2008_to_wgs84; + + auto key = std::make_pair(source_vertical_datum, target_vertical_datum); + if (conversion_map.find(key) != conversion_map.end()) { + return conversion_map[key](height, latitude, longitude); + } else { + std::string error_message = + "Invalid conversion types: " + std::string(source_vertical_datum.c_str()) + " to " + + std::string(target_vertical_datum.c_str()); + + throw std::invalid_argument(error_message); + } +} + +} // namespace geography_utils diff --git a/common/geography_utils/test/test_height.cpp b/common/geography_utils/test/test_height.cpp new file mode 100644 index 0000000000000..3d8a4c9c203fe --- /dev/null +++ b/common/geography_utils/test/test_height.cpp @@ -0,0 +1,92 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +// Test case to verify if same source and target datums return original height +TEST(Tier4GeographyUtils, SameSourceTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + const std::string datum = "WGS84"; + + double converted_height = + geography_utils::convert_height(height, latitude, longitude, datum, datum); + + EXPECT_DOUBLE_EQ(height, converted_height); +} + +// Test case to verify valid source and target datums +TEST(Tier4GeographyUtils, ValidSourceTargetDatum) +{ + // Calculated with + // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + const double target_height = -30.18; + + double converted_height = + geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); + + EXPECT_NEAR(target_height, converted_height, 0.1); +} + +// Test case to verify invalid source and target datums +TEST(Tier4GeographyUtils, InvalidSourceTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), + std::invalid_argument); +} + +// Test case to verify invalid source datums +TEST(Tier4GeographyUtils, InvalidSourceDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), + std::invalid_argument); +} + +// Test case to verify invalid target datums +TEST(Tier4GeographyUtils, InvalidTargetDatum) +{ + const double height = 10.0; + const double latitude = 35.0; + const double longitude = 139.0; + + EXPECT_THROW( + geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), + std::invalid_argument); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp index 74c9734833232..4dda5d1276c54 100644 --- a/sensing/gnss_poser/include/gnss_poser/convert.hpp +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -38,23 +39,7 @@ enum class MGRSPrecision { _1_MIllI_METER = 8, _100MICRO_METER = 9, }; -// EllipsoidHeight:height above ellipsoid -// OrthometricHeight:height above geoid -double EllipsoidHeight2OrthometricHeight( - const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger) -{ - double OrthometricHeight{0.0}; - try { - GeographicLib::Geoid egm2008("egm2008-1"); - OrthometricHeight = egm2008.ConvertHeight( - nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude, - GeographicLib::Geoid::ELLIPSOIDTOGEOID); - } catch (const GeographicLib::GeographicErr & err) { - RCLCPP_ERROR_STREAM( - logger, "Failed to convert Height from Ellipsoid to Orthometric" << err.what()); - } - return OrthometricHeight; -} + GNSSStat NavSatFix2UTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger, int height_system) @@ -65,11 +50,16 @@ GNSSStat NavSatFix2UTM( GeographicLib::UTMUPS::Forward( nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x, utm.y); + + std::string target_height_system; if (height_system == 0) { - utm.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger); + target_height_system = "EGM2008"; } else { - utm.z = nav_sat_fix_msg.altitude; + target_height_system = "WGS84"; } + utm.z = geography_utils::convert_height( + nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, "WGS84", + target_height_system); utm.latitude = nav_sat_fix_msg.latitude; utm.longitude = nav_sat_fix_msg.longitude; utm.altitude = nav_sat_fix_msg.altitude; @@ -78,6 +68,7 @@ GNSSStat NavSatFix2UTM( } return utm; } + GNSSStat NavSatFix2LocalCartesianUTM( const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system) @@ -89,11 +80,15 @@ GNSSStat NavSatFix2LocalCartesianUTM( GeographicLib::UTMUPS::Forward( nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone, utm_origin.east_north_up, utm_origin.x, utm_origin.y); + std::string target_height_system; if (height_system == 0) { - utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_origin, logger); + target_height_system = "EGM2008"; } else { - utm_origin.z = nav_sat_fix_origin.altitude; + target_height_system = "WGS84"; } + utm_origin.z = geography_utils::convert_height( + nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, + "WGS84", target_height_system); // individual coordinates of global coordinate system double global_x = 0.0; @@ -107,17 +102,17 @@ GNSSStat NavSatFix2LocalCartesianUTM( // individual coordinates of local coordinate system utm_local.x = global_x - utm_origin.x; utm_local.y = global_y - utm_origin.y; - if (height_system == 0) { - utm_local.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger) - utm_origin.z; - } else { - utm_local.z = nav_sat_fix_msg.altitude - utm_origin.z; - } + utm_local.z = geography_utils::convert_height( + nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, + "WGS84", target_height_system) - + utm_origin.z; } catch (const GeographicLib::GeographicErr & err) { RCLCPP_ERROR_STREAM( logger, "Failed to convert from LLH to UTM in local coordinates" << err.what()); } return utm_local; } + GNSSStat UTM2MGRS( const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger) { diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 1488f329edecb..77380bf5492a1 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -15,6 +15,7 @@ autoware_sensing_msgs geographiclib + geography_utils geometry_msgs rclcpp rclcpp_components diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index dfceeabe41122..fd61d3c62e397 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -23,6 +23,7 @@ autoware_planning_msgs component_interface_specs component_interface_utils + geography_utils lanelet2_extension motion_utils nav_msgs diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index fb434adbb0e40..c8f7df315c487 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -14,6 +14,8 @@ #include "vehicle.hpp" +#include + #include namespace default_ad_api @@ -153,7 +155,9 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.header.frame_id = "global"; vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; + vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( + projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, + map_projector_info_->vertical_datum, "WGS84"); } else if (map_projector_info_->projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { lanelet::GPSPoint position{ map_projector_info_->map_origin.latitude, map_projector_info_->map_origin.longitude}; @@ -165,7 +169,9 @@ void VehicleNode::publish_kinematics() vehicle_kinematics.geographic_pose.header.frame_id = "global"; vehicle_kinematics.geographic_pose.position.latitude = projected_gps_point.lat; vehicle_kinematics.geographic_pose.position.longitude = projected_gps_point.lon; - vehicle_kinematics.geographic_pose.position.altitude = projected_gps_point.ele; + vehicle_kinematics.geographic_pose.position.altitude = geography_utils::convert_height( + projected_gps_point.ele, projected_gps_point.lat, projected_gps_point.lon, + map_projector_info_->vertical_datum, "WGS84"); } else { vehicle_kinematics.geographic_pose.position.latitude = std::numeric_limits::quiet_NaN(); vehicle_kinematics.geographic_pose.position.longitude = From 5f04a370b2712aa9c42943e4d2866a1577169d6f Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Wed, 30 Aug 2023 20:59:28 +0900 Subject: [PATCH 11/14] fix(lane_change): fix lane_change document (#4817) Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> --- .../docs/behavior_path_planner_lane_change_design.md | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index fddd535931d94..2a04580aef5b5 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -55,12 +55,12 @@ where `common_param` is vehicle common parameter, which defines vehicle common m The `longitudinal_acceleration_resolution` is determine by the following ```C++ -longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - minimum_longitudinal_deceleration) / longitudinal_acceleration_sampling_num +longitudinal_acceleration_resolution = (maximum_longitudinal_acceleration - minimum_longitudinal_acceleration) / longitudinal_acceleration_sampling_num ``` Note that when the `current_velocity` is lower than `minimum_lane_changing_velocity`, the vehicle needs to accelerate its velocity to `minimum_lane_changing_velocity`. Therefore, longitudinal acceleration becomes positive value (not decelerate). -The following figure illustrates when `lane_change_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. +The following figure illustrates when `longitudinal_acceleration_sampling_num = 4`. Assuming that `maximum_deceleration = 1.0` then `a0 == 0.0 == no deceleration`, `a1 == 0.25`, `a2 == 0.5`, `a3 == 0.75` and `a4 == 1.0 == maximum_deceleration`. `a0` is the expected lane change trajectories should ego vehicle do not decelerate, and `a1`'s path is the expected lane change trajectories should ego vehicle decelerate at `0.25 m/s^2`. ![path_samples](../image/lane_change/lane_change-candidate_path_samples.png) @@ -79,12 +79,12 @@ The maximum and minimum lateral accelerations are defined in the lane change par | 4.0 | 0.3 | 0.4 | | 6.0 | 0.3 | 0.5 | -In this case, when the current velocity of the ego vehicle is 3.0, the minimum and maximum lateral accelerations are 0.2 and 0.35 respectively. These values are obtained by linearly interpolating the second and third rows of the map, which provide the minimum and maximum lateral acceleration values. +In this case, when the current velocity of the ego vehicle is 3.0, the minimum and maximum lateral accelerations are 0.25 and 0.4 respectively. These values are obtained by linearly interpolating the second and third rows of the map, which provide the minimum and maximum lateral acceleration values. Within this range, we sample the lateral acceleration for the ego vehicle. Similar to the method used for sampling longitudinal acceleration, the resolution of lateral acceleration (lateral_acceleration_resolution) is determined by the following: ```C++ -lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_deceleration) / lateral_acceleration_sampling_num +lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_acceleration) / lateral_acceleration_sampling_num ``` #### Candidate Path's validity check @@ -280,7 +280,6 @@ The following parameters are configurable in `lane_change.param.yaml`. | `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | | `min_length_for_turn_signal_activation` | [m] | double | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length | 10.0 | | `length_ratio_for_turn_signal_deactivation` | [-] | double | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | -| `object_shiftable_ratio_threshold` | [-] | double | Vehicles around the center line within this distance ratio will be excluded from parking objects | 0.6 | | `max_longitudinal_acc` | [-] | double | maximum longitudinal acceleration for lane change | 1.0 | | `min_longitudinal_acc` | [-] | double | maximum longitudinal deceleration for lane change | -1.0 | | `lateral_acceleration.velocity` | [m/s] | double | Reference velocity for lateral acceleration calculation (look up table) | [0.0, 4.0, 10.0] | From 058680d2eb17df82d3ca79961c0c5e2b483b842e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 30 Aug 2023 23:12:54 +0900 Subject: [PATCH 12/14] fix(goal_planner): set correct reference path (#4809) Signed-off-by: satoshi-ota --- .../scene_module/goal_planner/goal_planner_module.cpp | 10 ++++++++++ .../utils/goal_planner/default_fixed_goal_planner.cpp | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f37149f706436..3574abe7b1185 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -520,8 +520,13 @@ void GoalPlannerModule::generateGoalCandidates() BehaviorModuleOutput GoalPlannerModule::plan() { + resetPathCandidate(); + resetPathReference(); + generateGoalCandidates(); + path_reference_ = getPreviousModuleOutput().reference_path; + if (goal_planner_utils::isAllowedGoalModification( planner_data_->route_handler, left_side_parking_)) { return planWithGoalModification(); @@ -840,6 +845,11 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { + resetPathCandidate(); + resetPathReference(); + + path_reference_ = getPreviousModuleOutput().reference_path; + if (goal_planner_utils::isAllowedGoalModification( planner_data_->route_handler, left_side_parking_)) { return planWaitingApprovalWithGoalModification(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp index 5d77027efa5d0..7efdbdf1552d5 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp @@ -35,7 +35,7 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const PathWithLaneId smoothed_path = modifyPathForSmoothGoalConnection(*(output.path), planner_data); output.path = std::make_shared(smoothed_path); - output.reference_path = std::make_shared(smoothed_path); + output.reference_path = getPreviousModuleOutput().reference_path; return output; } From 54b3958489b8f527f49ed4addcc57431be796e2f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 31 Aug 2023 01:04:35 +0900 Subject: [PATCH 13/14] feat(dynamic_avoidance): generate drivable area by ego/object-path-based depending on the case (#4813) * feat(dynamic_avoidance): generate drivable area by ego/object-path-based depending on the case Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * apply clang-format Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance_module.hpp | 18 +++-- .../dynamic_avoidance_module.cpp | 74 +++++++++++++------ .../dynamic_avoidance/manager.cpp | 5 -- 3 files changed, 65 insertions(+), 32 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 3c302d934735c..fccc27051380e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -74,7 +74,6 @@ struct DynamicAvoidanceParameters double max_oncoming_crossing_object_angle{0.0}; // drivable area generation - std::string polygon_generation_method{}; double min_obj_path_based_lon_polygon_margin{0.0}; double lat_offset_from_obstacle{0.0}; double max_lat_offset_to_avoid{0.0}; @@ -94,6 +93,10 @@ struct DynamicAvoidanceParameters class DynamicAvoidanceModule : public SceneModuleInterface { public: + enum class PolygonGenerationMethod { + EGO_PATH_BASE = 0, + OBJECT_PATH_BASE, + }; struct DynamicAvoidanceObject { DynamicAvoidanceObject( @@ -128,15 +131,18 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional lat_offset_to_avoid{std::nullopt}; bool is_collision_left; bool should_be_avoided{false}; + PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::OBJECT_PATH_BASE}; void update( const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, - const bool arg_is_collision_left, const bool arg_should_be_avoided) + const bool arg_is_collision_left, const bool arg_should_be_avoided, + const PolygonGenerationMethod & arg_polygon_generation_method) { lon_offset_to_avoid = arg_lon_offset_to_avoid; lat_offset_to_avoid = arg_lat_offset_to_avoid; is_collision_left = arg_is_collision_left; should_be_avoided = arg_should_be_avoided; + polygon_generation_method = arg_polygon_generation_method; } }; @@ -230,11 +236,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface void updateObject( const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, - const bool should_be_avoided) + const bool should_be_avoided, const PolygonGenerationMethod & polygon_generation_method) { if (object_map_.count(uuid) != 0) { object_map_.at(uuid).update( - lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, + polygon_generation_method); } } @@ -312,7 +319,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface void updateTargetObjects(); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset, + PolygonGenerationMethod & polygon_generation_method) const; DecisionWithReason willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 0bfda71a09079..da98c0478e580 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -63,7 +63,7 @@ void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Po } void appendExtractedPolygonMarker( - MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly) + MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly, const double obj_z) { auto marker = tier4_autoware_utils::createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(), @@ -78,6 +78,7 @@ void appendExtractedPolygonMarker( geometry_msgs::msg::Point bound_geom_point; bound_geom_point.x = bound_point.x(); bound_geom_point.y = bound_point.y(); + bound_geom_point.z = obj_z; marker.points.push_back(bound_geom_point); } @@ -151,6 +152,25 @@ double calcDiffAngleAgainstPath( return diff_yaw; } +double calcDiffAngleBetweenPaths( + const std::vector & path_points, const PredictedPath & predicted_path) +{ + const size_t nearest_idx = + motion_utils::findNearestSegmentIndex(path_points, predicted_path.path.front().position); + const double ego_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); + + constexpr size_t max_predicted_path_size = 5; + double signed_max_angle{0.0}; + for (size_t i = 0; i < std::min(max_predicted_path_size, predicted_path.path.size()); ++i) { + const double obj_yaw = tf2::getYaw(predicted_path.path.at(i).orientation); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_yaw - ego_yaw); + if (std::abs(signed_max_angle) < std::abs(diff_yaw)) { + signed_max_angle = diff_yaw; + } + } + return signed_max_angle; +} + double calcDistanceToPath( const std::vector & path_points, const geometry_msgs::msg::Point & target_pos) @@ -281,10 +301,10 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() std::vector obstacles_for_drivable_area; for (const auto & object : target_objects_) { const auto obstacle_poly = [&]() { - if (parameters_->polygon_generation_method == "ego_path_base") { + if (object.polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) { return calcEgoPathBasedDynamicObstaclePolygon(object); } - if (parameters_->polygon_generation_method == "object_path_base") { + if (object.polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) { return calcObjectPathBasedDynamicObstaclePolygon(object); } throw std::logic_error("The polygon_generation_method's string is invalid."); @@ -294,7 +314,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() {object.pose, obstacle_poly.value(), object.is_collision_left}); appendObjectMarker(info_marker_, object.pose); - appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value()); + appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value(), object.pose.position.z); } } @@ -375,6 +395,10 @@ void DynamicAvoidanceModule::updateTargetObjects() predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); + const auto obj_path = *std::max_element( + predicted_object.kinematics.predicted_paths.begin(), + predicted_object.kinematics.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); // 1.a. check label const bool is_label_target_obstacle = @@ -391,7 +415,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.c. check if object is not crossing ego's path - const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose); + const double obj_angle = calcDiffAngleBetweenPaths(prev_module_path->points, obj_path); const double max_crossing_object_angle = 0.0 <= obj_tangent_vel ? parameters_->max_overtaking_crossing_object_angle : parameters_->max_oncoming_crossing_object_angle; @@ -448,6 +472,7 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2. Precise filtering of target objects and check if they should be avoided for (const auto & object : target_objects_manager_.getValidObjects()) { + PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::EGO_PATH_BASE}; const auto obj_uuid = object.uuid; const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); const auto obj_path = *std::max_element( @@ -472,8 +497,8 @@ void DynamicAvoidanceModule::updateTargetObjects() getLateralLongitudinalOffset(prev_module_path->points, object.pose, object.shape); // 2.c. check if object will not cut in - const bool will_object_cut_in = - willObjectCutIn(prev_module_path->points, obj_path, object.vel, lat_lon_offset); + const bool will_object_cut_in = willObjectCutIn( + prev_module_path->points, obj_path, object.vel, lat_lon_offset, polygon_generation_method); if (will_object_cut_in) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -551,7 +576,8 @@ void DynamicAvoidanceModule::updateTargetObjects() const bool should_be_avoided = true; target_objects_manager_.updateObject( - obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); + obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, + polygon_generation_method); } } @@ -627,16 +653,26 @@ bool DynamicAvoidanceModule::isObjectFarFromPath( bool DynamicAvoidanceModule::willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset, + PolygonGenerationMethod & polygon_generation_method) const { - constexpr double epsilon_path_lat_diff = 0.3; - - // Ignore oncoming object - if (obj_tangent_vel < 0) { + // Check if ego's path and object's path are close. + const bool will_object_cut_in = [&]() { + for (const auto & predicted_path_point : predicted_path.path) { + const double paths_lat_diff = + motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); + if (std::abs(paths_lat_diff) < planner_data_->parameters.vehicle_width / 2.0) { + return true; + } + } + return false; + }(); + if (!will_object_cut_in) { + // The object's path will not cut in return false; } - // Ignore object close to the ego + // Ignore object longitudinally close to the ego const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); const double relative_velocity = getEgoSpeed() - obj_tangent_vel; const double lon_offset_ego_to_obj = @@ -647,17 +683,11 @@ bool DynamicAvoidanceModule::willObjectCutIn( lon_offset_ego_to_obj < std::max( parameters_->min_lon_offset_ego_to_cut_in_object, relative_velocity * parameters_->min_time_to_start_cut_in)) { + polygon_generation_method = PolygonGenerationMethod::EGO_PATH_BASE; return false; } - for (const auto & predicted_path_point : predicted_path.path) { - const double paths_lat_diff = - motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); - if (std::abs(paths_lat_diff) < epsilon_path_lat_diff) { - return true; - } - } - return false; + return true; } DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCutOut( diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index c77d81457c15d..82ebf59ea5bb2 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -81,8 +81,6 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( { // drivable_area_generation std::string ns = "dynamic_avoidance.drivable_area_generation."; - p.polygon_generation_method = - node->declare_parameter(ns + "polygon_generation_method"); p.min_obj_path_based_lon_polygon_margin = node->declare_parameter(ns + "object_path_base.min_longitudinal_polygon_margin"); p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); @@ -181,9 +179,6 @@ void DynamicAvoidanceModuleManager::updateModuleParams( { // drivable_area_generation const std::string ns = "dynamic_avoidance.drivable_area_generation."; - - updateParam( - parameters, ns + "polygon_generation_method", p->polygon_generation_method); updateParam( parameters, ns + "object_path_base.min_longitudinal_polygon_margin", p->min_obj_path_based_lon_polygon_margin); From 0b2afec2610c46aa363672f11ad0a88f660e9c1b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 31 Aug 2023 01:04:51 +0900 Subject: [PATCH 14/14] fix(crosswalk): fix plotter data (#4822) Signed-off-by: Takayuki Murooka --- .../scripts/time_to_collision_plotter.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py b/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py index 5093683e17c22..8a9de1a563249 100755 --- a/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py +++ b/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py @@ -109,7 +109,9 @@ def on_collision_info(self, msg): return for i in range(int(len(collision_info_str_vec) / 5)): - collision_info_data_vec.append(CollisionInfo(*collision_info_str_vec[i : i + 5])) + collision_info_data_vec.append( + CollisionInfo(*collision_info_str_vec[i * 5 : i * 5 + 5]) + ) # memorize data in the history dictionary for collision_info_data in collision_info_data_vec: