From a748d41d2d3a0f76ad463e344e4e8b54f9a8ca36 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 23 Jul 2023 23:19:05 +0900 Subject: [PATCH 1/2] feat(obstacle_cruise_planner): single slow down virtual wall Signed-off-by: Takayuki Murooka --- .../src/planner_interface.cpp | 22 ++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index e0480a4f89a9e..833f93c6af569 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -469,7 +469,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( std::min(traj_point.longitudinal_velocity_mps, static_cast(stable_slow_down_vel)); } - // add debug data and virtual wall + // add debug data slow_down_debug_multi_array_.data.push_back(obstacle.precise_lat_dist); slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_start); slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_end); @@ -477,12 +477,32 @@ std::vector PlannerInterface::generateSlowDownTrajectory( slow_down_debug_multi_array_.data.push_back(stable_slow_down_vel); slow_down_debug_multi_array_.data.push_back(slow_down_start_idx ? *slow_down_start_idx : -1.0); slow_down_debug_multi_array_.data.push_back(slow_down_end_idx ? *slow_down_end_idx : -1.0); + + // add virtual wall + if (slow_down_start_idx && slow_down_end_idx) { + const size_t ego_idx = + ego_nearest_param_.findIndex(slow_down_traj_points, planner_data.ego_pose); + const size_t slow_down_wall_idx = [&]() { + if (ego_idx < *slow_down_start_idx) return *slow_down_start_idx; + if (ego_idx < *slow_down_end_idx) return ego_idx; + return *slow_down_end_idx; + }(); + + const auto markers = motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", + planner_data.current_time, i, abs_ego_offset); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + } + + // add debug virtual wall + /* if (slow_down_start_idx) { add_slow_down_marker(i, slow_down_start_idx, true); } if (slow_down_end_idx) { add_slow_down_marker(i, slow_down_end_idx, false); } + */ debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle); From a036c3ec57ca10292aa11bcb4adb692fcd184508 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 23 Jul 2023 23:33:46 +0900 Subject: [PATCH 2/2] update Signed-off-by: Takayuki Murooka --- .../common_structs.hpp | 1 + planning/obstacle_cruise_planner/src/node.cpp | 4 +++ .../src/planner_interface.cpp | 27 ++++++++----------- 3 files changed, 16 insertions(+), 16 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index c41870ba808fc..b6a242609dfff 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -217,6 +217,7 @@ struct DebugData std::vector obstacles_to_stop; std::vector obstacles_to_cruise; std::vector obstacles_to_slow_down; + MarkerArray slow_down_debug_wall_marker; MarkerArray stop_wall_marker; MarkerArray cruise_wall_marker; MarkerArray slow_down_wall_marker; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index d5f7af0c86f72..ab2092a3120ca 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1342,6 +1342,10 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const debug_marker.markers.push_back(marker); } + // slow down debug wall marker + tier4_autoware_utils::appendMarkerArray( + debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); + debug_marker_pub_->publish(debug_marker); // 2. publish virtual wall for cruise and stop diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 833f93c6af569..1aaa897986bcd 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -411,17 +411,6 @@ std::vector PlannerInterface::generateSlowDownTrajectory( } return std::nullopt; }; - const auto add_slow_down_marker = - [&](const size_t obstacle_idx, const auto slow_down_traj_idx, const bool is_start) { - if (!slow_down_traj_idx) return; - - const int id = obstacle_idx * 2 + (is_start ? 0 : 1); - const auto text = is_start ? "obstacle slow down start" : "obstacle slow down end"; - const auto pose = slow_down_traj_points.at(*slow_down_traj_idx).pose; - const auto markers = motion_utils::createSlowDownVirtualWallMarker( - pose, text, planner_data.current_time, id, abs_ego_offset); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); - }; std::vector new_prev_slow_down_output; for (size_t i = 0; i < obstacles.size(); ++i) { @@ -433,7 +422,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego); if (!dist_vec_to_slow_down) { RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("ObstacleCruisePlanner::Plannerinterface"), enable_debug_info_, + rclcpp::get_logger("ObstacleCruisePlanner::PlannerInterface"), enable_debug_info_, "[SlowDown] Ignore obstacle (%s) since distance to slow down is not valid", obstacle.uuid.c_str()); continue; @@ -495,14 +484,20 @@ std::vector PlannerInterface::generateSlowDownTrajectory( } // add debug virtual wall - /* if (slow_down_start_idx) { - add_slow_down_marker(i, slow_down_start_idx, true); + const auto markers = motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", + planner_data.current_time, i * 2, abs_ego_offset); + tier4_autoware_utils::appendMarkerArray( + markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { - add_slow_down_marker(i, slow_down_end_idx, false); + const auto markers = motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", + planner_data.current_time, i * 2 + 1, abs_ego_offset); + tier4_autoware_utils::appendMarkerArray( + markers, &debug_data_ptr_->slow_down_debug_wall_marker); } - */ debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle);