@@ -1296,28 +1296,32 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const
1296
1296
}
1297
1297
}
1298
1298
1299
- if (parameters_->use_object_recognition ) {
1300
- const auto pull_over_lanes = goal_planner_utils::getPullOverLanes (
1301
- *(planner_data_->route_handler ), left_side_parking_, parameters_->backward_goal_search_length ,
1302
- parameters_->forward_goal_search_length );
1303
- const auto [pull_over_lane_objects, others] =
1304
- utils::path_safety_checker::separateObjectsByLanelets (
1305
- *(planner_data_->dynamic_object ), pull_over_lanes);
1306
- const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity (
1307
- pull_over_lane_objects, parameters_->th_moving_object_velocity );
1308
- const auto common_parameters = planner_data_->parameters ;
1309
- const double base_link2front = common_parameters.base_link2front ;
1310
- const double base_link2rear = common_parameters.base_link2rear ;
1311
- const double vehicle_width = common_parameters.vehicle_width ;
1312
- if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin (
1313
- path, pull_over_lane_stop_objects, base_link2front, base_link2rear, vehicle_width,
1314
- parameters_->maximum_deceleration , parameters_->object_recognition_collision_check_margin ,
1315
- parameters_->object_recognition_collision_check_max_extra_stopping_margin )) {
1316
- return true ;
1317
- }
1299
+ if (!parameters_->use_object_recognition ) {
1300
+ return false ;
1318
1301
}
1319
1302
1320
- return false ;
1303
+ const auto pull_over_lanes = goal_planner_utils::getPullOverLanes (
1304
+ *(planner_data_->route_handler ), left_side_parking_, parameters_->backward_goal_search_length ,
1305
+ parameters_->forward_goal_search_length );
1306
+ const auto [pull_over_lane_objects, others] =
1307
+ utils::path_safety_checker::separateObjectsByLanelets (
1308
+ *(planner_data_->dynamic_object ), pull_over_lanes);
1309
+ const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity (
1310
+ pull_over_lane_objects, parameters_->th_moving_object_velocity );
1311
+ const auto common_parameters = planner_data_->parameters ;
1312
+ const double base_link2front = common_parameters.base_link2front ;
1313
+ const double base_link2rear = common_parameters.base_link2rear ;
1314
+ const double vehicle_width = common_parameters.vehicle_width ;
1315
+
1316
+ const auto ego_polygons_expanded =
1317
+ utils::path_safety_checker::generatePolygonsWithStoppingAndInertialMargin (
1318
+ path, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration ,
1319
+ parameters_->object_recognition_collision_check_max_extra_stopping_margin );
1320
+ debug_data_.ego_polygons_expanded = ego_polygons_expanded;
1321
+
1322
+ return utils::path_safety_checker::checkCollisionWithMargin (
1323
+ ego_polygons_expanded, pull_over_lane_stop_objects,
1324
+ parameters_->object_recognition_collision_check_margin );
1321
1325
}
1322
1326
1323
1327
bool GoalPlannerModule::hasEnoughDistance (const PullOverPath & pull_over_path) const
@@ -1686,6 +1690,24 @@ void GoalPlannerModule::setDebugData()
1686
1690
add (
1687
1691
createPathMarkerArray (partial_path, " partial_path_" + std::to_string (i), 0 , 0.9 , 0.5 , 0.9 ));
1688
1692
}
1693
+
1694
+ auto marker = tier4_autoware_utils::createDefaultMarker (
1695
+ " map" , rclcpp::Clock{RCL_ROS_TIME}.now (), " detection_polygons" , 0 , Marker::LINE_LIST,
1696
+ tier4_autoware_utils::createMarkerScale (0.01 , 0.0 , 0.0 ),
1697
+ tier4_autoware_utils::createMarkerColor (0.0 , 0.0 , 1.0 , 0.999 ));
1698
+
1699
+ for (const auto & ego_polygon : debug_data_.ego_polygons_expanded ) {
1700
+ for (size_t ep_idx = 0 ; ep_idx < ego_polygon.outer ().size (); ++ep_idx) {
1701
+ const auto & current_point = ego_polygon.outer ().at (ep_idx);
1702
+ const auto & next_point = ego_polygon.outer ().at ((ep_idx + 1 ) % ego_polygon.outer ().size ());
1703
+
1704
+ marker.points .push_back (
1705
+ tier4_autoware_utils::createPoint (current_point.x (), current_point.y (), 0.0 ));
1706
+ marker.points .push_back (
1707
+ tier4_autoware_utils::createPoint (next_point.x (), next_point.y (), 0.0 ));
1708
+ }
1709
+ }
1710
+ debug_marker_.markers .push_back (marker);
1689
1711
}
1690
1712
// safety check
1691
1713
if (parameters_->safety_check_params .enable_safety_check ) {
@@ -1695,7 +1717,6 @@ void GoalPlannerModule::setDebugData()
1695
1717
add (createPredictedPathMarkerArray (
1696
1718
ego_predicted_path, vehicle_info_, " ego_predicted_path_goal_planner" , 0 , 0.0 , 0.5 , 0.9 ));
1697
1719
}
1698
-
1699
1720
if (goal_planner_data_.filtered_objects .objects .size () > 0 ) {
1700
1721
add (createObjectsMarkerArray (
1701
1722
goal_planner_data_.filtered_objects , " filtered_objects" , 0 , 0.0 , 0.5 , 0.9 ));
0 commit comments