feat(lane_change): use external velocity limit in safety check (autowarefoundation#6760)

* feat(lane_change): use external velocity limit in safety check

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

* style(pre-commit): autofix

* Minor refactoring

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>

* Fix spell check and remove headers

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Add warning

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -170,6 +170,8 @@ class NormalLaneChange : public LaneChangeBase
   bool isVehicleStuck(
     const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const;
 
+  double get_max_velocity_for_safety_check() const;
+
   bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const;
 
   bool check_prepare_phase() const;

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -1976,7 +1976,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
     for (const auto & obj_path : obj_predicted_paths) {
       const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons(
         path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0,
-        current_debug_data.second);
+        get_max_velocity_for_safety_check(), current_debug_data.second);
 
       if (collided_polygons.empty()) {
         utils::path_safety_checker::updateCollisionCheckDebugMap(
@@ -2071,6 +2071,17 @@ bool NormalLaneChange::isVehicleStuck(
   return false;
 }
 
+double NormalLaneChange::get_max_velocity_for_safety_check() const
+{
+  const auto external_velocity_limit_ptr = planner_data_->external_limit_max_velocity;
+  if (external_velocity_limit_ptr) {
+    return std::min(
+      static_cast<double>(external_velocity_limit_ptr->max_velocity), getCommonParam().max_vel);
+  }
+
+  return getCommonParam().max_vel;
+}
+
 bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const
 {
   if (current_lanes.empty()) {

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -38,6 +38,7 @@
 #include <tier4_planning_msgs/msg/reroute_availability.hpp>
 #include <tier4_planning_msgs/msg/scenario.hpp>
 #include <tier4_planning_msgs/msg/stop_reason_array.hpp>
+#include <tier4_planning_msgs/msg/velocity_limit.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
 #include <map>
@@ -101,6 +102,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Publisher<PoseWithUuidStamped>::SharedPtr modified_goal_publisher_;
   rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reason_publisher_;
   rclcpp::Publisher<RerouteAvailability>::SharedPtr reroute_availability_publisher_;
+  rclcpp::Subscription<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr
+    external_limit_max_velocity_subscriber_;
   rclcpp::TimerBase::SharedPtr timer_;
 
   std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_candidate_publishers_;
@@ -140,6 +143,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   void onRoute(const LaneletRoute::ConstSharedPtr route_msg);
   void onOperationMode(const OperationModeState::ConstSharedPtr msg);
   void onLateralOffset(const LateralOffset::ConstSharedPtr msg);
+  void on_external_velocity_limiter(
+    const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg);
+
   SetParametersResult onSetParam(const std::vector<rclcpp::Parameter> & parameters);
 
   OnSetParametersCallbackHandle::SharedPtr m_set_param_res;

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -112,6 +112,11 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
       current_scenario_ = std::make_shared<Scenario>(*msg);
     },
     createSubscriptionOptions(this));
+  external_limit_max_velocity_subscriber_ =
+    create_subscription<tier4_planning_msgs::msg::VelocityLimit>(
+      "/planning/scenario_planning/max_velocity", 1,
+      std::bind(&BehaviorPathPlannerNode::on_external_velocity_limiter, this, _1),
+      createSubscriptionOptions(this));
 
   // route_handler
   vector_map_subscriber_ = create_subscription<HADMapBin>(
@@ -815,6 +820,19 @@ void BehaviorPathPlannerNode::onOperationMode(const OperationModeState::ConstSha
   const std::lock_guard<std::mutex> lock(mutex_pd_);
   planner_data_->operation_mode = msg;
 }
+
+void BehaviorPathPlannerNode::on_external_velocity_limiter(
+  const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg)
+{
+  // Note: Using this parameter during path planning might cause unexpected deceleration or jerk.
+  // Therefore, do not use it for anything other than safety checks.
+  if (!msg) {
+    return;
+  }
+
+  const std::lock_guard<std::mutex> lock(mutex_pd_);
+  planner_data_->external_limit_max_velocity = msg;
+}
 void BehaviorPathPlannerNode::onLateralOffset(const LateralOffset::ConstSharedPtr msg)
 {
   std::lock_guard<std::mutex> lock(mutex_pd_);

planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp

@@ -37,6 +37,7 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <tier4_planning_msgs/msg/detail/velocity_limit__struct.hpp>
 #include <tier4_planning_msgs/msg/lateral_offset.hpp>
 
 #include <limits>
@@ -64,6 +65,7 @@ using route_handler::RouteHandler;
 using tier4_planning_msgs::msg::LateralOffset;
 using PlanResult = PathWithLaneId::SharedPtr;
 using lanelet::TrafficLight;
+using tier4_planning_msgs::msg::VelocityLimit;
 using unique_identifier_msgs::msg::UUID;
 
 struct TrafficSignalStamped
@@ -160,6 +162,7 @@ struct PlannerData
   std::map<int64_t, TrafficSignalStamped> traffic_light_id_map;
   BehaviorPathPlannerParameters parameters{};
   drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{};
+  VelocityLimit::ConstSharedPtr external_limit_max_velocity{};
 
   mutable std::vector<geometry_msgs::msg::Pose> drivable_area_expansion_prev_path_poses{};
   mutable std::vector<double> drivable_area_expansion_prev_curvatures{};

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp

@@ -141,7 +141,7 @@ std::vector<Polygon2d> getCollidedPolygons(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  const double hysteresis_factor, CollisionCheckDebug & debug);
+  const double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug);
 
 bool checkPolygonsIntersects(
   const std::vector<Polygon2d> & polys_1, const std::vector<Polygon2d> & polys_2);

planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

@@ -26,6 +26,8 @@
 #include <boost/geometry/algorithms/union.hpp>
 #include <boost/geometry/strategies/strategies.hpp>
 
+#include <limits>
+
 namespace behavior_path_planner::utils::path_safety_checker
 {
 
@@ -478,7 +480,7 @@ bool checkCollision(
 {
   const auto collided_polygons = getCollidedPolygons(
     planned_path, predicted_ego_path, target_object, target_object_path, common_parameters,
-    rss_parameters, hysteresis_factor, debug);
+    rss_parameters, hysteresis_factor, std::numeric_limits<double>::max(), debug);
   return collided_polygons.empty();
 }
 
@@ -488,7 +490,7 @@ std::vector<Polygon2d> getCollidedPolygons(
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
   const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
-  double hysteresis_factor, CollisionCheckDebug & debug)
+  double hysteresis_factor, const double max_velocity_limit, CollisionCheckDebug & debug)
 {
   {
     debug.ego_predicted_path = predicted_ego_path;
@@ -504,7 +506,7 @@ std::vector<Polygon2d> getCollidedPolygons(
     // get object information at current time
     const auto & obj_pose = obj_pose_with_poly.pose;
     const auto & obj_polygon = obj_pose_with_poly.poly;
-    const auto & object_velocity = obj_pose_with_poly.velocity;
+    const auto object_velocity = obj_pose_with_poly.velocity;
 
     // get ego information at current time
     // Note: we can create these polygons in advance. However, it can decrease the readability and
@@ -517,7 +519,7 @@ std::vector<Polygon2d> getCollidedPolygons(
     }
     const auto & ego_pose = interpolated_data->pose;
     const auto & ego_polygon = interpolated_data->poly;
-    const auto & ego_velocity = interpolated_data->velocity;
+    const auto ego_velocity = std::min(interpolated_data->velocity, max_velocity_limit);
 
     // check overlap
     if (boost::geometry::overlaps(ego_polygon, obj_polygon)) {