From cd729c5904f29b5086a6983667b03a7e12be5a37 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 2 Dec 2022 09:39:01 +0900 Subject: [PATCH] feat(avoidance): improve avoidance target filter (#2329) * feat(route_handler): add getMostLeftLanelet() Signed-off-by: satoshi-ota * feat(avoidance): calc shiftable ratio in avoidance target filtering process Signed-off-by: satoshi-ota * feat(avoidance): output object's debug info for rviz Signed-off-by: satoshi-ota * fix(avoidance): use avoidance debug factor Signed-off-by: satoshi-ota * feat(tier4_planning_launch): add new params for avoidance Signed-off-by: satoshi-ota * fix(avoidance): reorder params for readability Signed-off-by: satoshi-ota * fix(tier4_planning_launch): reorder params for readability Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- .../avoidance/avoidance.param.yaml | 10 +- .../config/avoidance/avoidance.param.yaml | 12 +- .../avoidance/avoidance_module_data.hpp | 13 ++ .../scene_module/avoidance/debug.hpp | 1 + .../src/behavior_path_planner_node.cpp | 2 + .../avoidance/avoidance_module.cpp | 99 +++++++++++++- .../src/scene_module/avoidance/debug.cpp | 129 ++++++++++-------- .../include/route_handler/route_handler.hpp | 9 ++ planning/route_handler/src/route_handler.cpp | 11 ++ 9 files changed, 225 insertions(+), 61 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 93a1e498a757a..26f5b823d9f55 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -11,11 +11,19 @@ enable_avoidance_over_opposite_direction: true enable_update_path_when_object_is_gone: false - threshold_distance_object_is_on_center: 1.0 # [m] + # For target object filtering threshold_speed_object_is_stopped: 1.0 # [m/s] threshold_time_object_is_moving: 1.0 # [s] + object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] + + threshold_distance_object_is_on_center: 1.0 # [m] + + object_check_shiftable_ratio: 0.6 # [-] + object_check_min_road_shoulder_width: 0.5 # [m] + + # For lateral margin object_envelope_buffer: 0.3 # [m] lateral_collision_margin: 1.0 # [m] lateral_collision_safety_buffer: 0.7 # [m] diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 5cbd4301ea197..591e2991d6f92 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -11,10 +11,20 @@ enable_avoidance_over_opposite_direction: true enable_update_path_when_object_is_gone: false - threshold_distance_object_is_on_center: 1.0 # [m] + # For target object filtering threshold_speed_object_is_stopped: 1.0 # [m/s] + threshold_time_object_is_moving: 1.0 # [s] + object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] + + threshold_distance_object_is_on_center: 1.0 # [m] + + object_check_shiftable_ratio: 0.6 # [-] + object_check_min_road_shoulder_width: 0.5 # [m] + + # For lateral margin + object_envelope_buffer: 0.3 # [m] lateral_collision_margin: 1.0 # [m] lateral_collision_safety_buffer: 0.7 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 50fba4078d194..5968e8f344ca2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -35,6 +35,7 @@ namespace behavior_path_planner using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; @@ -80,6 +81,12 @@ struct AvoidanceParameters // continue to detect backward vehicles as avoidance targets until they are this distance away double object_check_backward_distance; + // use in judge whether the vehicle is parking object on road shoulder + double object_check_shiftable_ratio; + + // minimum road shoulder width. maybe 0.5 [m] + double object_check_min_road_shoulder_width; + // object's enveloped polygon double object_envelope_buffer; @@ -201,6 +208,9 @@ struct ObjectData // avoidance target // lateral distance to the closest footprint, in Frenet coordinate double overhang_dist; + // lateral shiftable ratio + double shiftable_ratio{0.0}; + // count up when object disappeared. Removed when it exceeds threshold. rclcpp::Time last_seen; double lost_time{0.0}; @@ -218,6 +228,9 @@ struct ObjectData // avoidance target // envelope polygon Polygon2d envelope_poly{}; + // envelope polygon centroid + Point2d centroid{}; + // lateral distance from overhang to the road shoulder double to_road_shoulder_distance{0.0}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp index ca2f806a493ab..60e31b0a3a416 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp @@ -37,6 +37,7 @@ namespace marker_utils::avoidance_marker using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::AvoidLineArray; +using behavior_path_planner::ObjectDataArray; using behavior_path_planner::ShiftLineArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 8a72a8da9061c..c0240a43644f3 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -285,6 +285,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.threshold_time_object_is_moving = dp("threshold_time_object_is_moving", 1.0); p.object_check_forward_distance = dp("object_check_forward_distance", 150.0); p.object_check_backward_distance = dp("object_check_backward_distance", 2.0); + p.object_check_shiftable_ratio = dp("object_check_shiftable_ratio", 1.0); + p.object_check_min_road_shoulder_width = dp("object_check_min_road_shoulder_width", 0.5); p.object_envelope_buffer = dp("object_envelope_buffer", 0.1); p.lateral_collision_margin = dp("lateral_collision_margin", 2.0); p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 79c692d1de4ad..e27bfd22c7997 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -191,7 +191,10 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb void AvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { + using boost::geometry::return_centroid; + using boost::geometry::within; using lanelet::geometry::distance2d; + using lanelet::geometry::toArcCoordinates; using lanelet::utils::getId; using lanelet::utils::to2D; @@ -224,7 +227,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( std::vector avoidance_debug_msg_array; for (const auto & i : lane_filtered_objects_index) { const auto & object = planner_data_->dynamic_object->objects.at(i); - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; AvoidanceDebugMsg avoidance_debug_msg; const auto avoidance_debug_array_false_and_push_back = [&avoidance_debug_msg, &avoidance_debug_msg_array](const std::string & failed_reason) { @@ -242,12 +245,15 @@ void AvoidanceModule::fillAvoidanceTargetObjects( object_data.object = object; avoidance_debug_msg.object_id = getUuidStr(object_data); - const auto object_closest_index = findNearestIndex(path_points, object_pos); + const auto object_closest_index = findNearestIndex(path_points, object_pose.position); const auto object_closest_pose = path_points.at(object_closest_index).point.pose; // Calc envelop polygon. fillObjectEnvelopePolygon(object_closest_pose, object_data); + // calc object centroid. + object_data.centroid = return_centroid(object_data.envelope_poly); + // calc longitudinal distance from ego to closest target object footprint point. fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path, ego_pos, object_data); avoidance_debug_msg.longitudinal_distance = object_data.longitudinal; @@ -256,7 +262,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( fillObjectMovingTime(object_data); if (object_data.move_time > parameters_->threshold_time_object_is_moving) { - avoidance_debug_array_false_and_push_back("MovingObject"); + avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::MOVING_OBJECT); data.other_objects.push_back(object_data); continue; } @@ -281,7 +287,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( } // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pos); + object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral; // Find the footprint point closest to the path, set to object_data.overhang_distance. @@ -332,6 +338,89 @@ void AvoidanceModule::fillAvoidanceTargetObjects( continue; } + lanelet::ConstLanelet object_closest_lanelet; + const auto lanelet_map = rh->getLaneletMapPtr(); + if (!lanelet::utils::query::getClosestLanelet( + lanelet::utils::query::laneletLayer(lanelet_map), object_pose, &object_closest_lanelet)) { + continue; + } + + lanelet::BasicPoint2d object_centroid(object_data.centroid.x(), object_data.centroid.y()); + + /** + * Is not object in adjacent lane? + * - Yes -> Is parking object? + * - Yes -> the object is avoidance target. + * - No -> ignore this object. + * - No -> the object is avoidance target no matter whether it is parking object or not. + */ + const auto is_in_ego_lane = + within(object_centroid, overhang_lanelet.polygon2d().basicPolygon()); + if (is_in_ego_lane) { + /** + * TODO(Satoshi Ota) use intersection area + * under the assumption that there is no parking vehicle inside intersection, + * ignore all objects that is in the ego lane as not parking objects. + */ + std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT); + data.other_objects.push_back(object_data); + continue; + } + + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object_closest_lanelet, object_pose.position); + lanelet::BasicPoint3d centerline_point( + centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); + + // ============================================ <- most_left_lanelet.leftBound() + // y road shoulder + // ^ ------------------------------------------ + // | x + + // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() + // + // -------------------------------------------- + // +: object position + // o: nearest point on centerline + + const auto most_left_road_lanelet = rh->getMostLeftLanelet(object_closest_lanelet); + const auto most_left_lanelet_candidates = + rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); + + lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; + + for (const auto & ll : most_left_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_left_lanelet = ll; + } + } + + const auto center_to_left_boundary = + distance2d(to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); + double object_shiftable_distance = center_to_left_boundary - 0.5 * object.shape.dimensions.y; + + const lanelet::Attribute sub_type = + most_left_lanelet.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() != "road_shoulder") { + object_shiftable_distance += parameters_->object_check_min_road_shoulder_width; + } + + const auto arc_coordinates = toArcCoordinates( + to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); + object_data.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; + + const auto is_parking_object = + object_data.shiftable_ratio > parameters_->object_check_shiftable_ratio; + + if (!is_parking_object) { + avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT); + data.other_objects.push_back(object_data); + continue; + } + } + object_data.last_seen = clock_->now(); // set data @@ -615,7 +704,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr const auto is_object_on_right = isOnRight(o); const auto shift_length = getShiftLength(o, is_object_on_right, avoid_margin); if (isSameDirectionShift(is_object_on_right, shift_length)) { - avoidance_debug_array_false_and_push_back("IgnoreSameDirectionShift"); + avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::SAME_DIRECTION_SHIFT); continue; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp index 5bfbf7ab8498b..c6ded01021c33 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp @@ -24,14 +24,72 @@ namespace marker_utils::avoidance_marker { + using behavior_path_planner::AvoidLine; using behavior_path_planner::util::shiftPose; +using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; +namespace +{ + +int32_t uuidToInt32(const unique_identifier_msgs::msg::UUID & uuid) +{ + int32_t ret = 0; + + for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) { + ret <<= sizeof(int8_t); + ret |= uuid.uuid.at(i); + } + + return ret; +} + +MarkerArray createObjectsCubeMarkerArray( + const ObjectDataArray & objects, std::string && ns, const Vector3 & scale, + const ColorRGBA & color) +{ + MarkerArray msg; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, scale, color); + for (const auto & object : objects) { + marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + for (const auto & object : objects) { + marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + std::ostringstream string_stream; + string_stream << std::fixed << std::setprecision(2); + string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" + << "lateral: " << object.lateral << " [-]"; + marker.text = string_stream.str(); + msg.markers.push_back(marker); + } + + return msg; +} + +} // namespace + MarkerArray createAvoidLineMarkerArray( const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g, const float & b, const double & w) @@ -85,35 +143,16 @@ MarkerArray createAvoidLineMarkerArray( MarkerArray createTargetObjectsMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns) { - const auto normal_color = tier4_autoware_utils::createMarkerColor(0.9, 0.0, 0.0, 0.8); - const auto disappearing_color = tier4_autoware_utils::createMarkerColor(0.9, 0.5, 0.9, 0.6); - - const auto uuid_to_id = [](const unique_identifier_msgs::msg::UUID & object_id) { - int32_t ret = 0; - - for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) { - ret <<= sizeof(int8_t); - ret |= object_id.uuid.at(i); - } - - return ret; - }; - MarkerArray msg; - msg.markers.reserve(objects.size() * 2); + msg.markers.reserve(objects.size() * 3); - { - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, - createMarkerScale(3.0, 1.5, 1.5), normal_color); - for (const auto & object : objects) { - marker.id = uuid_to_id(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; - marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.5, 1.5); - marker.color = std::fabs(object.lost_time) < 1e-2 ? normal_color : disappearing_color; - msg.markers.push_back(marker); - } - } + appendMarkerArray( + createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), + createMarkerColor(1.0, 0.0, 0.0, 0.8)), + &msg); + + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); { for (const auto & object : objects) { @@ -130,7 +169,7 @@ MarkerArray createTargetObjectsMarkerArray( } marker.points.push_back(marker.points.front()); - marker.id = uuid_to_id(object.object.object_id); + marker.id = uuidToInt32(object.object.object_id); msg.markers.push_back(marker); } } @@ -142,34 +181,16 @@ MarkerArray createTargetObjectsMarkerArray( MarkerArray createOtherObjectsMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns) { - const auto normal_color = tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.8); - - const auto uuid_to_id = [](const unique_identifier_msgs::msg::UUID & object_id) { - int32_t ret = 0; - - for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) { - ret <<= sizeof(int8_t); - ret |= object_id.uuid.at(i); - } - - return ret; - }; - MarkerArray msg; - msg.markers.reserve(objects.size()); + msg.markers.reserve(objects.size() * 2); - { - Marker marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, - createMarkerScale(3.0, 1.5, 1.5), normal_color); - for (const auto & object : objects) { - marker.id = uuid_to_id(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; - marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.5, 1.5); - marker.color = normal_color; - msg.markers.push_back(marker); - } - } + appendMarkerArray( + createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), + createMarkerColor(0.0, 1.0, 0.0, 0.8)), + &msg); + + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); return msg; } diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index b359e9dbd8753..59cfe17c88731 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -176,6 +176,15 @@ class RouteHandler const lanelet::ConstLanelet & current_lane, bool is_right = true, bool is_left = true, bool is_opposite = true, const bool & invert_opposite = false) const noexcept; + /** + * @brief Check if same-direction lane is available at the left side of the lanelet + * Searches for any lanes regardless of whether it is lane-changeable or not. + * Required the linestring to be shared(same line ID) between the lanelets. + * @param the lanelet of interest + * @return vector of lanelet having same direction if true + */ + lanelet::ConstLanelet getMostLeftLanelet(const lanelet::ConstLanelet & lanelet) const; + /** * @brief Searches the furthest linestring to the right side of the lanelet * Only lanelet with same direction is considered diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 85d34dd45fac5..f3bce09fa8615 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -999,6 +999,17 @@ lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLane return opposite_lanelets; } +lanelet::ConstLanelet RouteHandler::getMostLeftLanelet(const lanelet::ConstLanelet & lanelet) const +{ + // recursively compute the width of the lanes + const auto & same = getLeftLanelet(lanelet); + + if (same) { + return getMostLeftLanelet(same.get()); + } + return lanelet; +} + lanelet::ConstLineString3d RouteHandler::getRightMostSameDirectionLinestring( const lanelet::ConstLanelet & lanelet) const noexcept {