diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 3dc230f0e92ed..74f6b843803df 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -38,11 +38,6 @@ struct StartGoalPlannerData std::vector ego_predicted_path; // collision check debug map CollisionCheckDebugMap collision_check; - - Pose refined_start_pose; - std::vector start_pose_candidates; - size_t selected_start_pose_candidate_index; - double margin_for_start_pose_candidate; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp similarity index 80% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp rename to planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 66dbddebf99bb..8f8d2baf9c85e 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ #include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -29,10 +29,30 @@ namespace behavior_path_planner { +using autoware_auto_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; + using freespace_planning_algorithms::AstarParam; using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStarParam; +struct StartPlannerDebugData +{ + // filtered objects + PredictedObjects filtered_objects; + TargetObjectsOnLane target_objects_on_lane; + std::vector ego_predicted_path; + // collision check debug map + CollisionCheckDebugMap collision_check; + + Pose refined_start_pose; + std::vector start_pose_candidates; + size_t selected_start_pose_candidate_index; + double margin_for_start_pose_candidate; +}; + struct StartPlannerParameters { double th_arrived_distance{0.0}; @@ -105,4 +125,4 @@ struct StartPlannerParameters } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp new file mode 100644 index 0000000000000..6aa5584807d90 --- /dev/null +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp @@ -0,0 +1,39 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#define BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ + +#include "behavior_path_start_planner_module/data_structs.hpp" + +#include +#include + +namespace behavior_path_planner +{ +using behavior_path_planner::StartPlannerDebugData; + +void updateSafetyCheckDebugData( + StartPlannerDebugData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) +{ + data.filtered_objects = filtered_objects; + data.target_objects_on_lane = target_objects_on_lane; + data.ego_predicted_path = ego_predicted_path; +} + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index e345e3dc91337..0144fdd45ae50 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -16,8 +16,8 @@ #define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ #include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_start_planner_module/data_structs.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/start_planner_parameters.hpp" #include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 7b463f4fadf80..47cfa408162ca 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -21,11 +21,11 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "behavior_path_start_planner_module/data_structs.hpp" #include "behavior_path_start_planner_module/freespace_pull_out.hpp" #include "behavior_path_start_planner_module/geometric_pull_out.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" #include "behavior_path_start_planner_module/shift_pull_out.hpp" -#include "behavior_path_start_planner_module/start_planner_parameters.hpp" #include #include @@ -191,7 +191,7 @@ class StartPlannerModule : public SceneModuleInterface std::vector> start_planners_; PullOutStatus status_; - mutable StartGoalPlannerData start_planner_data_; + mutable StartPlannerDebugData debug_data_; std::deque odometry_buffer_; diff --git a/planning/behavior_path_start_planner_module/src/debug.cpp b/planning/behavior_path_start_planner_module/src/debug.cpp new file mode 100644 index 0000000000000..0f95470d86d40 --- /dev/null +++ b/planning/behavior_path_start_planner_module/src/debug.cpp @@ -0,0 +1,30 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_start_planner_module/debug.hpp" + +namespace behavior_path_planner +{ + +void updateSafetyCheckDebugData( + StartPlannerDebugData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) +{ + data.filtered_objects = filtered_objects; + data.target_objects_on_lane = target_objects_on_lane; + data.ego_predicted_path = ego_predicted_path; +} + +} // namespace behavior_path_planner 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 0e0a6b123be12..1b54b655fc424 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 @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_start_planner_module/debug.hpp" #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" @@ -126,7 +127,7 @@ void StartPlannerModule::initVariables() resetPathReference(); debug_marker_.markers.clear(); initializeSafetyCheckParameters(); - initializeCollisionCheckDebugMap(start_planner_data_.collision_check); + initializeCollisionCheckDebugMap(debug_data_.collision_check); } void StartPlannerModule::updateEgoPredictedPathParams( @@ -589,8 +590,8 @@ void StartPlannerModule::planWithPriority( if (findPullOutPath( start_pose_candidates[index], planner, refined_start_pose, goal_pose, collision_check_margin)) { - start_planner_data_.selected_start_pose_candidate_index = index; - start_planner_data_.margin_for_start_pose_candidate = collision_check_margin; + debug_data_.selected_start_pose_candidate_index = index; + debug_data_.margin_for_start_pose_candidate = collision_check_margin; return; } } @@ -848,8 +849,8 @@ void StartPlannerModule::updatePullOutStatus() start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); } - start_planner_data_.refined_start_pose = *refined_start_pose; - start_planner_data_.start_pose_candidates = start_pose_candidates; + debug_data_.refined_start_pose = *refined_start_pose; + debug_data_.start_pose_candidates = start_pose_candidates; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); @@ -1196,14 +1197,13 @@ bool StartPlannerModule::isSafePath() const const double hysteresis_factor = status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; - utils::parking_departure::updateSafetyCheckTargetObjectsData( - start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + behavior_path_planner::updateSafetyCheckDebugData( + debug_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane, - start_planner_data_.collision_check, planner_data_->parameters, - safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path, - hysteresis_factor); + debug_data_.collision_check, planner_data_->parameters, safety_check_params_->rss_params, + objects_filtering_params_->use_all_predicted_path, hysteresis_factor); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const @@ -1354,7 +1354,7 @@ void StartPlannerModule::setDebugData() add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createFootprintMarkerArray( - start_planner_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3)); + debug_data_.refined_start_pose, vehicle_info_, "refined_start_pose", 0, 0.9, 0.9, 0.3)); 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)); @@ -1399,14 +1399,13 @@ void StartPlannerModule::setDebugData() visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.3, 0.3, 0.3), purple); footprint_marker.lifetime = rclcpp::Duration::from_seconds(1.5); text_marker.lifetime = rclcpp::Duration::from_seconds(1.5); - for (size_t i = 0; i < start_planner_data_.start_pose_candidates.size(); ++i) { + for (size_t i = 0; i < debug_data_.start_pose_candidates.size(); ++i) { footprint_marker.id = i; text_marker.id = i; footprint_marker.points.clear(); text_marker.text = "idx[" + std::to_string(i) + "]"; - text_marker.pose = start_planner_data_.start_pose_candidates.at(i); - addFootprintMarker( - footprint_marker, start_planner_data_.start_pose_candidates.at(i), vehicle_info_); + text_marker.pose = debug_data_.start_pose_candidates.at(i); + addFootprintMarker(footprint_marker, debug_data_.start_pose_candidates.at(i), vehicle_info_); start_pose_footprint_marker_array.markers.push_back(footprint_marker); start_pose_text_marker_array.markers.push_back(text_marker); } @@ -1450,29 +1449,29 @@ void StartPlannerModule::setDebugData() // safety check if (parameters_->safety_check_params.enable_safety_check) { - if (start_planner_data_.ego_predicted_path.size() > 0) { + if (debug_data_.ego_predicted_path.size() > 0) { const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( - start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + debug_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); add(createPredictedPathMarkerArray( ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9)); } - if (start_planner_data_.filtered_objects.objects.size() > 0) { + if (debug_data_.filtered_objects.objects.size() > 0) { add(createObjectsMarkerArray( - start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + debug_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); } - add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info")); - add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path")); - add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation")); + add(showSafetyCheckInfo(debug_data_.collision_check, "object_debug_info")); + add(showPredictedPath(debug_data_.collision_check, "ego_predicted_path")); + add(showPolygon(debug_data_.collision_check, "ego_and_target_polygon_relation")); // set objects of interest - for (const auto & [uuid, data] : start_planner_data_.collision_check) { + for (const auto & [uuid, data] : debug_data_.collision_check) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - initializeCollisionCheckDebugMap(start_planner_data_.collision_check); + initializeCollisionCheckDebugMap(debug_data_.collision_check); } // Visualize planner type text