From 2384b200abbaaeda4825b4f15cd2264c5e8cef2f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 4 Jul 2024 17:07:23 +0900 Subject: [PATCH] fix(static_obstacle_avoidance): fix issues in target object filtering logic (#7830) * fix(static_obstacle_avoidance): check if object is inside/outside by its position point instead of its polygon Signed-off-by: satoshi-ota * refactor(static_obstacle_avoidance): add getter functions Signed-off-by: satoshi-ota * fix(static_obstacle_avoidance): check next lane without route if the current lane is not preferred Signed-off-by: satoshi-ota * fix(static_obstacle_avoidance): fix parked vehicle check Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota Signed-off-by: palas21 --- .../data_structs.hpp | 4 + .../src/debug.cpp | 10 +-- .../src/shift_line_generator.cpp | 16 ++-- .../src/utils.cpp | 84 ++++++++++--------- 4 files changed, 59 insertions(+), 55 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index d02b39047e71c..49599004e0952 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -352,6 +352,10 @@ struct ObjectData // avoidance target { } + Pose getPose() const { return object.kinematics.initial_pose_with_covariance.pose; } + + Point getPosition() const { return object.kinematics.initial_pose_with_covariance.pose.position; } + PredictedObject object; // object behavior. diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 9c771224e89eb..eb0d903f2e398 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -64,7 +64,7 @@ MarkerArray createObjectsCubeMarkerArray( } marker.id = uuidToInt32(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + marker.pose = object.getPose(); msg.markers.push_back(marker); } @@ -80,10 +80,8 @@ MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std: "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - for (const auto & p : object.envelope_poly.outer()) { - marker.points.push_back(createPoint(p.x(), p.y(), pos.z)); + marker.points.push_back(createPoint(p.x(), p.y(), object.getPosition().z)); } marker.points.push_back(marker.points.front()); @@ -142,7 +140,7 @@ MarkerArray createObjectInfoMarkerArray( for (const auto & object : objects) { if (verbose) { marker.id = uuidToInt32(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + marker.pose = object.getPose(); std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2) << std::boolalpha; string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" @@ -162,7 +160,7 @@ MarkerArray createObjectInfoMarkerArray( { marker.id = uuidToInt32(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + marker.pose = object.getPose(); marker.pose.position.z += 2.0; std::ostringstream string_stream; string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : ""); diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index a171ae9161041..c3a67eb074d73 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -165,9 +165,9 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // the avoidance path is already approved - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - const auto is_approved = (helper_->getShift(object_pos) > 0.0 && is_object_on_right) || - (helper_->getShift(object_pos) < 0.0 && !is_object_on_right); + const auto is_approved = + (helper_->getShift(object.getPosition()) > 0.0 && is_object_on_right) || + (helper_->getShift(object.getPosition()) < 0.0 && !is_object_on_right); if (is_approved) { return std::make_pair(desire_shift_length, avoidance_distance); } @@ -363,9 +363,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( if (is_return_shift_to_goal) { return true; } - const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position; const bool has_object_near_goal = - autoware::universe_utils::calcDistance2d(goal_pose.position, object_pos) < + autoware::universe_utils::calcDistance2d(goal_pose.position, o.getPosition()) < parameters_->object_check_goal_distance; return has_object_near_goal; }(); @@ -1027,8 +1026,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const auto has_object_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { return autoware::universe_utils::calcDistance2d( - data_->route_handler->getGoalPose().position, - o.object.kinematics.initial_pose_with_covariance.pose.position) < + data_->route_handler->getGoalPose().position, o.getPosition()) < parameters_->object_check_goal_distance; }); if (has_object_near_goal) { @@ -1097,9 +1095,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( if (utils::isAllowedGoalModification(data_->route_handler)) { const bool has_last_shift_near_goal = std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { - return autoware::universe_utils::calcDistance2d( - last_sl.end.position, - o.object.kinematics.initial_pose_with_covariance.pose.position) < + return autoware::universe_utils::calcDistance2d(last_sl.end.position, o.getPosition()) < parameters_->object_check_goal_distance; }); if (has_last_shift_near_goal) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 4272d0eaf9c5d..608dc209e4fce 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -293,8 +293,7 @@ bool isWithinCrosswalk( { using Point = boost::geometry::model::d2::point_xy; - const auto & p = object.object.kinematics.initial_pose_with_covariance.pose.position; - const Point p_object{p.x, p.y}; + const Point p_object{object.getPosition().x, object.getPosition().y}; // get conflicting crosswalk crosswalk constexpr int PEDESTRIAN_GRAPH_ID = 1; @@ -335,14 +334,16 @@ bool isWithinIntersection( route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str())); return boost::geometry::within( - object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), + lanelet::utils::to2D(polygon.basicPolygon())); } bool isOnEgoLane(const ObjectData & object, const std::shared_ptr & route_handler) { - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; if (boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), object.overhang_lanelet.polygon2d().basicPolygon())) { return true; } @@ -351,7 +352,8 @@ bool isOnEgoLane(const ObjectData & object, const std::shared_ptr lanelet::ConstLanelets prev_lanelet; if (route_handler->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &prev_lanelet)) { if (boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), prev_lanelet.front().polygon2d().basicPolygon())) { return true; } @@ -361,10 +363,20 @@ bool isOnEgoLane(const ObjectData & object, const std::shared_ptr lanelet::ConstLanelet next_lanelet; if (route_handler->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) { if (boost::geometry::within( - lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), next_lanelet.polygon2d().basicPolygon())) { return true; } + } else { + for (const auto & lane : route_handler->getNextLanelets(object.overhang_lanelet)) { + if (boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition())) + .basicPoint(), + lane.polygon2d().basicPolygon())) { + return true; + } + } } return false; @@ -372,20 +384,18 @@ bool isOnEgoLane(const ObjectData & object, const std::shared_ptr bool isParallelToEgoLane(const ObjectData & object, const double threshold) { - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto closest_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object_pose)); + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); + const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object.getPose())); return yaw_deviation < threshold || yaw_deviation > M_PI - threshold; } bool isMergingToEgoLane(const ObjectData & object) { - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto closest_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - const auto yaw_deviation = calcYawDeviation(closest_pose, object_pose); + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); + const auto yaw_deviation = calcYawDeviation(closest_pose, object.getPose()); if (isOnRight(object)) { if (yaw_deviation < 0.0 && -1.0 * M_PI_2 < yaw_deviation) { @@ -422,9 +432,8 @@ bool isParkedVehicle( return false; } - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; const auto centerline_pos = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position; + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()).position; bool is_left_side_parked_vehicle = false; if (!isOnRight(object)) { @@ -442,7 +451,8 @@ bool isParkedVehicle( return same_direction_lane; } - return static_cast(opposite_lanes.front().invert()); + return static_cast( + route_handler->getMostRightLanelet(opposite_lanes.front()).invert()); }(); const auto center_to_left_boundary = distance2d( @@ -457,7 +467,7 @@ bool isParkedVehicle( if (sub_type == "road_shoulder") { // assuming it's parked vehicle if its CoG is within road shoulder lanelet. if (boost::geometry::within( - to2D(toLaneletPoint(object_pos)).basicPoint(), + to2D(toLaneletPoint(object.getPosition())).basicPoint(), most_left_lanelet.polygon2d().basicPolygon())) { return true; } @@ -468,7 +478,7 @@ bool isParkedVehicle( const auto arc_coordinates = toArcCoordinates( to2D(object.overhang_lanelet.centerline().basicLineString()), - to2D(toLaneletPoint(object_pos)).basicPoint()); + to2D(toLaneletPoint(object.getPosition())).basicPoint()); object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; @@ -490,7 +500,8 @@ bool isParkedVehicle( return same_direction_lane; } - return static_cast(opposite_lanes.front().invert()); + return static_cast( + route_handler->getMostLeftLanelet(opposite_lanes.front()).invert()); }(); const auto center_to_right_boundary = distance2d( @@ -505,7 +516,7 @@ bool isParkedVehicle( if (sub_type == "road_shoulder") { // assuming it's parked vehicle if its CoG is within road shoulder lanelet. if (boost::geometry::within( - to2D(toLaneletPoint(object_pos)).basicPoint(), + to2D(toLaneletPoint(object.getPosition())).basicPoint(), most_right_lanelet.polygon2d().basicPolygon())) { return true; } @@ -516,7 +527,7 @@ bool isParkedVehicle( const auto arc_coordinates = toArcCoordinates( to2D(object.overhang_lanelet.centerline().basicLineString()), - to2D(toLaneletPoint(object_pos)).basicPoint()); + to2D(toLaneletPoint(object.getPosition())).basicPoint()); object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; is_right_side_parked_vehicle = @@ -527,9 +538,8 @@ bool isParkedVehicle( return false; } - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; object.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + lanelet::utils::getArcCoordinates(data.current_lanelets, object.getPose()).distance; if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { return false; } @@ -544,13 +554,13 @@ bool isCloseToStopFactor( { const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; // force avoidance for stopped vehicle bool is_close_to_stop_factor = false; // check traffic light - const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets); + const auto to_traffic_light = + getDistanceToNextTrafficLight(object.getPose(), data.extend_lanelets); { is_close_to_stop_factor = to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; @@ -792,9 +802,8 @@ bool isSatisfiedWithNonVehicleCondition( } // Object is on center line -> ignore. - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; object.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + lanelet::utils::getArcCoordinates(data.current_lanelets, object.getPose()).distance; if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { object.info = ObjectInfo::TOO_NEAR_TO_CENTERLINE; return false; @@ -844,9 +853,8 @@ bool isSatisfiedWithVehicleCondition( return false; } - const auto & current_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto is_moving_distance_longer_than_threshold = - calcDistance2d(object.init_pose, current_pose) > + calcDistance2d(object.init_pose, object.getPose()) > parameters->distance_threshold_for_ambiguous_vehicle; if (is_moving_distance_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; @@ -944,9 +952,8 @@ double getRoadShoulderDistance( using autoware::universe_utils::Point2d; using lanelet::utils::to2D; - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto object_closest_index = - autoware::motion_utils::findNearestIndex(data.reference_path.points, object_pose.position); + autoware::motion_utils::findNearestIndex(data.reference_path.points, object.getPosition()); const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; const auto rh = planner_data->route_handler; @@ -957,7 +964,7 @@ double getRoadShoulderDistance( std::vector> intersects; for (const auto & p1 : object.overhang_points) { const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()); const auto p_tmp = geometry_msgs::build().position(p1.second).orientation(centerline_pose.orientation); @@ -1326,8 +1333,7 @@ std::vector generateObstaclePolygonsForDrivableArea( object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = autoware::universe_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); - obstacles_for_drivable_area.push_back( - {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)}); + obstacles_for_drivable_area.push_back({object.getPose(), obj_poly, !isOnRight(object)}); } return obstacles_for_drivable_area; } @@ -1495,7 +1501,7 @@ void fillObjectMovingTime( object_data.last_stop = now; object_data.move_time = 0.0; if (is_new_object) { - object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + object_data.init_pose = object_data.getPose(); object_data.stop_time = 0.0; object_data.last_move = now; stopped_objects.push_back(object_data); @@ -1510,7 +1516,7 @@ void fillObjectMovingTime( } if (is_new_object) { - object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + object_data.init_pose = object_data.getPose(); object_data.move_time = std::numeric_limits::infinity(); object_data.stop_time = 0.0; object_data.last_move = now; @@ -1520,7 +1526,7 @@ void fillObjectMovingTime( object_data.last_stop = same_id_obj->last_stop; object_data.move_time = (now - same_id_obj->last_stop).seconds(); object_data.stop_time = 0.0; - object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + object_data.init_pose = object_data.getPose(); if (object_data.move_time > object_parameter.moving_time_threshold) { stopped_objects.erase(same_id_obj); @@ -1617,9 +1623,9 @@ void updateRegisteredObject( } constexpr auto POS_THR = 1.5; - const auto r_pos = registered_object.object.kinematics.initial_pose_with_covariance.pose; + const auto r_pos = registered_object.getPose(); const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) { - return calcDistance2d(r_pos, o.object.kinematics.initial_pose_with_covariance.pose) < POS_THR; + return calcDistance2d(r_pos, o.getPose()) < POS_THR; }); // same id object is not detected, but object is found around registered. update registered.