Skip to content

Commit 188d100

Browse files
committed
feat(goal_planne): check objects within the area between ego edge and boudary of pull_over_lanes
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent 81605c1 commit 188d100

File tree

7 files changed

+155
-17
lines changed

7 files changed

+155
-17
lines changed

planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml

+2
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,8 @@
3939
object_recognition_collision_check_max_extra_stopping_margin: 1.0
4040
th_moving_object_velocity: 1.0
4141
detection_bound_offset: 15.0
42+
outer_road_detection_offset: 1.0
43+
inner_road_detection_offset: 0.0
4244

4345
# pull over
4446
pull_over:

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -221,6 +221,7 @@ struct GoalPlannerDebugData
221221
{
222222
FreespacePlannerDebugData freespace_planner{};
223223
std::vector<Polygon2d> ego_polygons_expanded{};
224+
lanelet::ConstLanelet expanded_pull_over_lane_between_ego{};
224225
};
225226

226227
struct LastApprovalData

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,8 @@ struct GoalPlannerParameters
7575
double object_recognition_collision_check_max_extra_stopping_margin{0.0};
7676
double th_moving_object_velocity{0.0};
7777
double detection_bound_offset{0.0};
78+
double outer_road_detection_offset{0.0};
79+
double inner_road_detection_offset{0.0};
7880

7981
// pull over general params
8082
double pull_over_minimum_request_length{0.0};

planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp

+14-1
Original file line numberDiff line numberDiff line change
@@ -45,9 +45,19 @@ using visualization_msgs::msg::MarkerArray;
4545
lanelet::ConstLanelets getPullOverLanes(
4646
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
4747
const double forward_distance);
48+
49+
/*
50+
* @brief expand pull_over_lanes to the opposite side of drivable roads by bound_offset.
51+
* bound_offset must be positive regardless of left_side is true/false
52+
*/
4853
lanelet::ConstLanelets generateExpandedPullOverLanes(
4954
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
50-
const double forward_distance, double bound_offset);
55+
const double forward_distance, const double bound_offset);
56+
57+
lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes(
58+
const lanelet::ConstLanelets & pull_over_lanes, const bool left_side,
59+
const geometry_msgs::msg::Pose ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info,
60+
const double outer_road_offset, const double inner_road_offset);
5161
PredictedObjects extractObjectsInExpandedPullOverLanes(
5262
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
5363
const double forward_distance, double bound_offset, const PredictedObjects & objects);
@@ -85,6 +95,9 @@ MarkerArray createTextsMarkerArray(
8595
const std::vector<Pose> & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color);
8696
MarkerArray createGoalCandidatesMarkerArray(
8797
const GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color);
98+
MarkerArray createLaneletPolygonMarkerArray(
99+
const lanelet::CompoundPolygon3d & polygon, const std_msgs::msg::Header & header,
100+
const std::string & ns, const std_msgs::msg::ColorRGBA & color);
88101
MarkerArray createNumObjectsToAvoidTextsMarkerArray(
89102
const GoalCandidates & goal_candidates, std::string && ns,
90103
const std_msgs::msg::ColorRGBA & color);

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

+65-14
Original file line numberDiff line numberDiff line change
@@ -1867,6 +1867,51 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData(
18671867
goal_planner_data_.ego_predicted_path = ego_predicted_path;
18681868
}
18691869

1870+
static std::vector<utils::path_safety_checker::ExtendedPredictedObject> filterObjectsByWithinPolicy(
1871+
const std::shared_ptr<const PredictedObjects> & objects,
1872+
const lanelet::ConstLanelets & target_lanes,
1873+
const std::shared_ptr<behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams> &
1874+
params)
1875+
{
1876+
// implanted part of behavior_path_planner::utils::path_safety_checker::filterObjects() and
1877+
// createTargetObjectsOnLane()
1878+
1879+
// Guard
1880+
if (objects->objects.empty()) {
1881+
return {};
1882+
}
1883+
1884+
const double ignore_object_velocity_threshold = params->ignore_object_velocity_threshold;
1885+
const auto & target_object_types = params->object_types_to_check;
1886+
1887+
PredictedObjects filtered_objects = utils::path_safety_checker::filterObjectsByVelocity(
1888+
*objects, ignore_object_velocity_threshold, false);
1889+
1890+
utils::path_safety_checker::filterObjectsByClass(filtered_objects, target_object_types);
1891+
1892+
std::vector<PredictedObject> within_filtered_objects;
1893+
for (const auto & target_lane : target_lanes) {
1894+
const auto lane_poly = target_lane.polygon2d().basicPolygon();
1895+
for (const auto & filtered_object : filtered_objects.objects) {
1896+
const auto object_bbox = tier4_autoware_utils::toPolygon2d(filtered_object);
1897+
if (boost::geometry::within(object_bbox, lane_poly)) {
1898+
within_filtered_objects.push_back(filtered_object);
1899+
break;
1900+
}
1901+
}
1902+
}
1903+
1904+
const double safety_check_time_horizon = params->safety_check_time_horizon;
1905+
const double safety_check_time_resolution = params->safety_check_time_resolution;
1906+
1907+
std::vector<utils::path_safety_checker::ExtendedPredictedObject> refined_filtered_objects;
1908+
for (const auto & within_filtered_object : within_filtered_objects) {
1909+
refined_filtered_objects.push_back(utils::path_safety_checker::transform(
1910+
within_filtered_object, safety_check_time_horizon, safety_check_time_resolution));
1911+
}
1912+
return refined_filtered_objects;
1913+
}
1914+
18701915
std::pair<bool, bool> GoalPlannerModule::isSafePath() const
18711916
{
18721917
if (!thread_safe_data_.get_pull_over_path()) {
@@ -1904,31 +1949,30 @@ std::pair<bool, bool> GoalPlannerModule::isSafePath() const
19041949
ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity,
19051950
ego_seg_idx, is_object_front, limit_to_max_velocity);
19061951

1907-
// filtering objects with velocity, position and class
1908-
const auto filtered_objects = utils::path_safety_checker::filterObjects(
1909-
dynamic_object, route_handler, pull_over_lanes, current_pose.position,
1910-
objects_filtering_params_);
1911-
19121952
// filtering objects based on the current position's lane
1913-
const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane(
1914-
pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_);
1953+
const auto expanded_pull_over_lanes_between_ego =
1954+
goal_planner_utils::generateBetweenEgoAndExpandedPullOverLanes(
1955+
pull_over_lanes, left_side_parking_, current_pose, planner_data_->parameters.vehicle_info,
1956+
parameters_->outer_road_detection_offset, parameters_->inner_road_detection_offset);
1957+
const auto merged_expanded_pull_over_lanes =
1958+
lanelet::utils::combineLaneletsShape(expanded_pull_over_lanes_between_ego);
1959+
debug_data_.expanded_pull_over_lane_between_ego = merged_expanded_pull_over_lanes;
19151960

1916-
utils::parking_departure::updateSafetyCheckTargetObjectsData(
1917-
goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
1961+
const auto filtered_objects = filterObjectsByWithinPolicy(
1962+
dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params_);
19181963

19191964
const double hysteresis_factor =
19201965
prev_data_.safety_status.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate;
19211966

19221967
const bool current_is_safe = std::invoke([&]() {
19231968
if (parameters_->safety_check_params.method == "RSS") {
19241969
return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS(
1925-
pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane,
1926-
goal_planner_data_.collision_check, planner_data_->parameters,
1927-
safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path,
1928-
hysteresis_factor);
1970+
pull_over_path, ego_predicted_path, filtered_objects, goal_planner_data_.collision_check,
1971+
planner_data_->parameters, safety_check_params_->rss_params,
1972+
objects_filtering_params_->use_all_predicted_path, hysteresis_factor);
19291973
} else if (parameters_->safety_check_params.method == "integral_predicted_polygon") {
19301974
return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon(
1931-
ego_predicted_path, vehicle_info_, target_objects_on_lane.on_current_lane,
1975+
ego_predicted_path, vehicle_info_, filtered_objects,
19321976
objects_filtering_params_->check_all_predicted_path,
19331977
parameters_->safety_check_params.integral_predicted_polygon_params,
19341978
goal_planner_data_.collision_check);
@@ -2046,6 +2090,13 @@ void GoalPlannerModule::setDebugData()
20462090
}
20472091
debug_marker_.markers.push_back(marker);
20482092

2093+
tier4_autoware_utils::appendMarkerArray(
2094+
goal_planner_utils::createLaneletPolygonMarkerArray(
2095+
debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header,
2096+
"expanded_pull_over_lane_between_ego",
2097+
tier4_autoware_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)),
2098+
&debug_marker_);
2099+
20492100
// Visualize debug poses
20502101
const auto & debug_poses = thread_safe_data_.get_pull_over_path()->debug_poses;
20512102
for (size_t i = 0; i < debug_poses.size(); ++i) {

planning/behavior_path_goal_planner_module/src/manager.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -102,6 +102,10 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node)
102102
ns + "object_recognition_collision_check_max_extra_stopping_margin");
103103
p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
104104
p.detection_bound_offset = node->declare_parameter<double>(ns + "detection_bound_offset");
105+
p.outer_road_detection_offset =
106+
node->declare_parameter<double>(ns + "outer_road_detection_offset");
107+
p.inner_road_detection_offset =
108+
node->declare_parameter<double>(ns + "inner_road_detection_offset");
105109
}
106110

107111
// pull over general params

planning/behavior_path_goal_planner_module/src/util.cpp

+67-2
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
1818
#include "behavior_path_planner_common/utils/utils.hpp"
1919

20+
#include <lanelet2_extension/utility/message_conversion.hpp>
2021
#include <lanelet2_extension/utility/query.hpp>
2122
#include <lanelet2_extension/utility/utilities.hpp>
2223
#include <rclcpp/rclcpp.hpp>
@@ -78,13 +79,62 @@ lanelet::ConstLanelets getPullOverLanes(
7879

7980
lanelet::ConstLanelets generateExpandedPullOverLanes(
8081
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
81-
const double forward_distance, double bound_offset)
82+
const double forward_distance, const double bound_offset)
8283
{
8384
const auto pull_over_lanes =
8485
getPullOverLanes(route_handler, left_side, backward_distance, forward_distance);
8586

8687
return left_side ? lanelet::utils::getExpandedLanelets(pull_over_lanes, bound_offset, 0.0)
87-
: lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, bound_offset);
88+
: lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, -bound_offset);
89+
}
90+
91+
static double getOffsetToLanesBoundary(
92+
const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose target_pose,
93+
const bool left_side)
94+
{
95+
lanelet::ConstLanelet closest_lanelet;
96+
lanelet::utils::query::getClosestLanelet(lanelet_sequence, target_pose, &closest_lanelet);
97+
98+
// the boundary closer to ego. if left_side, take right boundary
99+
const auto & boundary3d = left_side ? closest_lanelet.rightBound() : closest_lanelet.leftBound();
100+
const auto boundary = lanelet::utils::to2D(boundary3d);
101+
using lanelet::utils::conversion::toLaneletPoint;
102+
const auto arc_coords = lanelet::geometry::toArcCoordinates(
103+
boundary, lanelet::utils::to2D(toLaneletPoint(target_pose.position)).basicPoint());
104+
return arc_coords.distance;
105+
}
106+
107+
lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes(
108+
const lanelet::ConstLanelets & pull_over_lanes, const bool left_side,
109+
const geometry_msgs::msg::Pose ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info,
110+
const double outer_road_offset, const double inner_road_offset)
111+
{
112+
const double front_overhang = vehicle_info.front_overhang_m,
113+
wheel_base = vehicle_info.wheel_base_m, wheel_tread = vehicle_info.wheel_tread_m;
114+
const double side_overhang =
115+
left_side ? vehicle_info.left_overhang_m : vehicle_info.right_overhang_m;
116+
const double ego_length_to_front = wheel_base + front_overhang;
117+
const double ego_width_to_front =
118+
!left_side ? (-wheel_tread / 2.0 - side_overhang) : (wheel_tread / 2.0 + side_overhang);
119+
tier4_autoware_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front};
120+
const auto front_edge_glob = tier4_autoware_utils::transformPoint(
121+
front_edge_local, tier4_autoware_utils::pose2transform(ego_pose));
122+
geometry_msgs::msg::Pose ego_front_pose;
123+
ego_front_pose.position =
124+
createPoint(front_edge_glob.x(), front_edge_glob.y(), ego_pose.position.z);
125+
126+
// ==========================================================================================
127+
// NOTE: the point which is on the right side of a directed line has negative distance
128+
// getExpandedLanelet(1.0, -2.0) expands a lanelet by 1.0 to the left and by 2.0 to the right
129+
// ==========================================================================================
130+
const double ego_offset_to_closer_boundary =
131+
getOffsetToLanesBoundary(pull_over_lanes, ego_front_pose, left_side);
132+
return left_side ? lanelet::utils::getExpandedLanelets(
133+
pull_over_lanes, outer_road_offset,
134+
ego_offset_to_closer_boundary - inner_road_offset)
135+
: lanelet::utils::getExpandedLanelets(
136+
pull_over_lanes, ego_offset_to_closer_boundary + inner_road_offset,
137+
-outer_road_offset);
88138
}
89139

90140
PredictedObjects extractObjectsInExpandedPullOverLanes(
@@ -233,6 +283,21 @@ MarkerArray createGoalCandidatesMarkerArray(
233283
return marker_array;
234284
}
235285

286+
MarkerArray createLaneletPolygonMarkerArray(
287+
const lanelet::CompoundPolygon3d & polygon, const std_msgs::msg::Header & header,
288+
const std::string & ns, const std_msgs::msg::ColorRGBA & color)
289+
{
290+
visualization_msgs::msg::MarkerArray marker_array{};
291+
auto marker = createDefaultMarker(
292+
header.frame_id, header.stamp, ns, 0, visualization_msgs::msg::Marker::LINE_STRIP,
293+
createMarkerScale(0.1, 0.0, 0.0), color);
294+
for (const auto & p : polygon) {
295+
marker.points.push_back(createPoint(p.x(), p.y(), p.z()));
296+
}
297+
marker_array.markers.push_back(marker);
298+
return marker_array;
299+
}
300+
236301
double calcLateralDeviationBetweenPaths(
237302
const PathWithLaneId & reference_path, const PathWithLaneId & target_path)
238303
{

0 commit comments

Comments
 (0)