refactor(start_planner): separate start planner parameters and debug …

…data structure (autowarefoundation#6101) * Add data_structs.hpp and update include paths * Refactor start planner debug data structure * Update debug_data variable name in StartPlannerModule Signed-off-by: kyoichi-sugahara <[email protected]> --------- Signed-off-by: kyoichi-sugahara <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Interplai · May 28, 2024 · f0deba8 · f0deba8
1 parent 2877cbf
commit f0deba8
Show file tree

Hide file tree

Showing 7 changed files with 118 additions and 35 deletions.
diff --git a/...ommon/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/...ommon/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp
@@ -38,11 +38,6 @@ struct StartGoalPlannerData
   std::vector<PoseWithVelocityStamped> ego_predicted_path;
   // collision check debug map
   CollisionCheckDebugMap collision_check;
-
-  Pose refined_start_pose;
-  std::vector<Pose> start_pose_candidates;
-  size_t selected_start_pose_candidate_index;
-  double margin_for_start_pose_candidate;
 };
 
 }  // namespace behavior_path_planner

diff --git a/...anner_module/start_planner_parameters.hpp → ...ath_start_planner_module/data_structs.hpp b/...anner_module/start_planner_parameters.hpp → ...ath_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<PoseWithVelocityStamped> ego_predicted_path;
+  // collision check debug map
+  CollisionCheckDebugMap collision_check;
+
+  Pose refined_start_pose;
+  std::vector<Pose> 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/...g/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp b/...g/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 <string>
+#include <vector>
+
+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<PoseWithVelocityStamped> & 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/...start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/...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 <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>

diff --git a/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/..._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 <lane_departure_checker/lane_departure_checker.hpp>
 #include <vehicle_info_util/vehicle_info.hpp>
@@ -191,7 +191,7 @@ class StartPlannerModule : public SceneModuleInterface
 
   std::vector<std::shared_ptr<PullOutPlannerBase>> start_planners_;
   PullOutStatus status_;
-  mutable StartGoalPlannerData start_planner_data_;
+  mutable StartPlannerDebugData debug_data_;
 
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
 

diff --git a/planning/behavior_path_start_planner_module/src/debug.cpp 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<PoseWithVelocityStamped> & 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
@@ -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