From 4016c05f1053cc105d77ba663661525aee653c05 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 27 Dec 2023 02:01:42 +0900 Subject: [PATCH] feat(start_planner): add static collision check end polygon marker (#5955) feat(start_planner): add static collision check end polygon Signed-off-by: kosuke55 --- .../src/start_planner_module.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index c9fa15d855268..30dd48ab383b9 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -1283,6 +1283,7 @@ void StartPlannerModule::setDebugData() const using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; + using visualization_msgs::msg::Marker; const auto life_time = rclcpp::Duration::from_seconds(1.5); auto add = [&](MarkerArray added) { @@ -1299,6 +1300,35 @@ void StartPlannerModule::setDebugData() const add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0)); + // visualize collision_check_end_pose and footprint + { + const auto local_footprint = createVehicleFootprint(vehicle_info_); + const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + getFullPath().points, status_.pull_out_path.end_pose.position, + parameters_->collision_check_distance_from_end); + if (collision_check_end_pose) { + add(createPoseMarkerArray( + *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0)); + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "static_collision_check_end_polygon", 0, + Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1), + tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + const auto footprint = transformVector( + local_footprint, tier4_autoware_utils::pose2transform(*collision_check_end_pose)); + const double ego_z = planner_data_->self_odometry->pose.pose.position.z; + for (size_t i = 0; i < footprint.size(); i++) { + const auto & current_point = footprint.at(i); + const auto & next_point = footprint.at((i + 1) % footprint.size()); + marker.points.push_back( + tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), ego_z)); + marker.points.push_back( + tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), ego_z)); + } + marker.lifetime = life_time; + debug_marker_.markers.push_back(marker); + } + } + // safety check if (parameters_->safety_check_params.enable_safety_check) { if (start_planner_data_.ego_predicted_path.size() > 0) {