refactor(behavior_path_planner): consolidate common function which is…

… used by start and goal planner (autowarefoundation#4804)

* refactor code

* consolidate function

* add utility

* util to utils

* define parameters

* fix typo

* fix arg name

* commonize calcFeasibleDecelDistance

* define modifyVelocityByDirection

* add description
---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_path_planner/CMakeLists.txt

@@ -32,6 +32,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/utils/path_utils.cpp
   src/utils/path_safety_checker/safety_check.cpp
   src/utils/path_safety_checker/objects_filtering.cpp
+  src/utils/start_goal_planner_common/utils.cpp
   src/utils/avoidance/utils.cpp
   src/utils/lane_change/utils.cpp
   src/utils/side_shift/util.cpp

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -65,7 +65,7 @@ enum class PathType {
   FREESPACE,
 };
 
-struct PUllOverStatus
+struct PullOverStatus
 {
   std::shared_ptr<PullOverPath> pull_over_path{};
   std::shared_ptr<PullOverPath> lane_parking_pull_over_path{};
@@ -76,7 +76,8 @@ struct PUllOverStatus
   lanelet::ConstLanelets pull_over_lanes{};
   std::vector<DrivableLanes> lanes{};  // current + pull_over
   bool has_decided_path{false};
-  bool is_safe{false};
+  bool is_safe_static_objects{false};   // current path is safe against static objects
+  bool is_safe_dynamic_objects{false};  // current path is safe against dynamic objects
   bool prev_is_safe{false};
   bool has_decided_velocity{false};
   bool has_requested_approval{false};
@@ -127,7 +128,8 @@ class GoalPlannerModule : public SceneModuleInterface
   CandidateOutput planCandidate() const override { return CandidateOutput{}; };
 
 private:
-  PUllOverStatus status_;
+
+  PullOverStatus status_;
 
   std::shared_ptr<GoalPlannerParameters> parameters_;
 
@@ -198,7 +200,6 @@ class GoalPlannerModule : public SceneModuleInterface
     const Pose & search_start_offset_pose, PathWithLaneId & path) const;
   PathWithLaneId generateStopPath();
   PathWithLaneId generateFeasibleStopPath();
-  boost::optional<double> calcFeasibleDecelDistance(const double target_velocity) const;
   void keepStoppedWithCurrentPath(PathWithLaneId & path);
   double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;
 

planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp

@@ -94,6 +94,10 @@ class GeometricParallelParking
 
   std::vector<PathWithLaneId> getArcPaths() const { return arc_paths_; }
   std::vector<PathWithLaneId> getPaths() const { return paths_; }
+  std::vector<std::pair<double, double>> getPairsTerminalVelocityAndAccel() const
+  {
+    return pairs_terminal_velocity_and_accel_;
+  }
   PathWithLaneId getPathByIdx(size_t const idx) const;
   PathWithLaneId getCurrentPath() const;
   PathWithLaneId getFullPath() const;
@@ -112,6 +116,7 @@ class GeometricParallelParking
 
   std::vector<PathWithLaneId> arc_paths_;
   std::vector<PathWithLaneId> paths_;
+  std::vector<std::pair<double, double>> pairs_terminal_velocity_and_accel_;
   size_t current_path_idx_ = 0;
 
   void clearPaths();

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp

@@ -17,6 +17,7 @@
 #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_
 
 #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 
 #include <freespace_planning_algorithms/abstract_algorithm.hpp>
 #include <freespace_planning_algorithms/astar_search.hpp>
@@ -101,6 +102,18 @@ struct GoalPlannerParameters
   AstarParam astar_parameters;
   RRTStarParam rrt_star_parameters;
 
+  // stop condition
+  double maximum_deceleration_for_stop;
+  double maximum_jerk_for_stop;
+
+  // hysteresis parameter
+  double hysteresis_factor_expand_rate;
+
+  // path safety checker
+  utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params;
+  utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params;
+  utils::path_safety_checker::SafetyCheckParams safety_check_params;
+
   // debug
   bool print_debug_info;
 };

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp

@@ -26,6 +26,7 @@
 #include <boost/optional.hpp>
 
 #include <memory>
+#include <utility>
 #include <vector>
 
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
@@ -46,6 +47,8 @@ struct PullOverPath
 {
   PullOverPlannerType type{PullOverPlannerType::NONE};
   std::vector<PathWithLaneId> partial_paths{};
+  // accelerate with constant acceleration to the target velocity
+  std::vector<std::pair<double, double>> pairs_terminal_velocity_and_accel{};
   Pose start_pose{};
   Pose end_pose{};
   std::vector<Pose> debug_poses{};

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp

@@ -170,7 +170,7 @@ struct ObjectsFilteringParams
 struct SafetyCheckParams
 {
   bool enable_safety_check;          ///< Enable safety checks.
-  double backward_lane_length;       ///< Length of the backward lane for path generation.
+  double backward_path_length;       ///< Length of the backward lane for path generation.
   double forward_path_length;        ///< Length of the forward path lane for path generation.
   RSSparams rss_params;              ///< Parameters related to the RSS model.
   bool publish_debug_marker{false};  ///< Option to publish debug markers.

planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp

@@ -0,0 +1,45 @@
+// 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_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_
+
+#include "behavior_path_planner/marker_utils/utils.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+
+#include <vector>
+
+namespace behavior_path_planner
+{
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
+using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
+/*
+ * Common data for start/goal_planner module
+ */
+struct StartGoalPlannerData
+{
+  // filtered objects
+  PredictedObjects filtered_objects;
+  TargetObjectsOnLane target_objects_on_lane;
+  std::vector<PoseWithVelocityStamped> ego_predicted_path;
+};
+
+}  // namespace behavior_path_planner
+
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp

@@ -0,0 +1,94 @@
+// 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_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_
+#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_
+
+#include "behavior_path_planner/data_manager.hpp"
+#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
+#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp"
+#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp"
+
+#include <motion_utils/distance/distance.hpp>
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+namespace behavior_path_planner::utils::start_goal_planner_common
+{
+
+using behavior_path_planner::StartPlannerParameters;
+using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams;
+using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams;
+using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams;
+
+boost::optional<double> calcFeasibleDecelDistance(
+  std::shared_ptr<const PlannerData> planner_data, const double acc_lim, const double jerk_lim,
+  const double target_velocity);
+
+/**
+ * @brief Update path velocities based on driving direction.
+ *
+ * This function updates the longitudinal velocity of each point in the provided paths,
+ * based on whether the vehicle is driving forward or backward. It also sets the terminal
+ * velocity and acceleration for each path.
+ *
+ * @param paths A vector of paths with lane IDs to be updated.
+ * @param terminal_vel_acc_pairs A vector of pairs, where each pair contains the terminal
+ *                               velocity and acceleration for a corresponding path.
+ * @param target_velocity The target velocity for ego vehicle predicted path.
+ * @param acceleration The acceleration for ego vehicle predicted path.
+ */
+void modifyVelocityByDirection(
+  std::vector<PathWithLaneId> & paths,
+  std::vector<std::pair<double, double>> & terminal_vel_acc_pairs, const double target_velocity,
+  const double acceleration);
+
+void updateEgoPredictedPathParams(
+  std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
+  const std::shared_ptr<StartPlannerParameters> & start_planner_params);
+
+void updateEgoPredictedPathParams(
+  std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
+  const std::shared_ptr<GoalPlannerParameters> & start_planner_params);
+
+void updateSafetyCheckParams(
+  std::shared_ptr<SafetyCheckParams> & safety_check_params,
+  const std::shared_ptr<StartPlannerParameters> & start_planner_params);
+
+void updateSafetyCheckParams(
+  std::shared_ptr<SafetyCheckParams> & safety_check_params,
+  const std::shared_ptr<GoalPlannerParameters> & start_planner_params);
+
+void updateObjectsFilteringParams(
+  std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
+  const std::shared_ptr<StartPlannerParameters> & start_planner_params);
+
+void updateObjectsFilteringParams(
+  std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
+  const std::shared_ptr<GoalPlannerParameters> & start_planner_params);
+
+void updatePathProperty(
+  std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
+  const std::pair<double, double> & pairs_terminal_velocity_and_accel);
+
+std::pair<double, double> getPairsTerminalVelocityAndAccel(
+  const std::vector<std::pair<double, double>> & pairs_terminal_velocity_and_accel,
+  const size_t current_path_idx);
+
+}  // namespace behavior_path_planner::utils::start_goal_planner_common
+
+#endif  // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp

@@ -17,6 +17,7 @@
 #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_
 
 #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 
 #include <freespace_planning_algorithms/abstract_algorithm.hpp>
 #include <freespace_planning_algorithms/astar_search.hpp>
@@ -76,6 +77,18 @@ struct StartPlannerParameters
   PlannerCommonParam freespace_planner_common_parameters;
   AstarParam astar_parameters;
   RRTStarParam rrt_star_parameters;
+
+  // stop condition
+  double maximum_deceleration_for_stop;
+  double maximum_jerk_for_stop;
+
+  // hysteresis parameter
+  double hysteresis_factor_expand_rate;
+
+  // path safety checker
+  utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params;
+  utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params;
+  utils::path_safety_checker::SafetyCheckParams safety_check_params;
 };
 
 }  // namespace behavior_path_planner