feat(behavior_path_planner): update pull_over module (#873)

* feat(behavior_path_planner): update pull_over module Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * use tf2_geometry_msgs/tf2_geometry_msgs.hpp for humble Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix werror of humble Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix test Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix goal change bug when starting drive Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
tier4 · Jul 8, 2022 · 56885ed · 56885ed
1 parent f8ab687
commit 56885ed
Show file tree

Hide file tree

Showing 17 changed files with 1,314 additions and 426 deletions.
diff --git a/...nning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/...nning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml
@@ -1,25 +1,54 @@
 /**:
   ros__parameters:
     pull_over:
+      request_length: 100.0
+      th_arrived_distance_m: 1.0
+      th_stopped_velocity_mps: 0.01
+      th_stopped_time_sec: 2.0  # It must be greater than the state_machine's.
+      pull_over_velocity: 2.0
+      pull_over_minimum_velocity: 0.3
+      margin_from_boundary: 0.5
+      decide_path_distance: 10.0
+      min_acc: -0.5
+      enable_shift_parking: true
+      enable_arc_forward_parking: true
+      enable_arc_backward_parking: false
+      # goal research
+      search_priority: "efficient_path" # "efficient_path" or "close_goal"
+      enable_goal_research: true
+      forward_goal_search_length: 20.0
+      backward_goal_search_length: 20.0
+      goal_search_interval: 1.0
+      goal_to_obj_margin: 2.0
+      # occupancy grid map
+      collision_check_margin: 0.5
+      theta_size: 360
+      obstacle_threshold: 60
+      # shift path
+      pull_over_sampling_num: 4
+      maximum_lateral_jerk: 2.0
+      minimum_lateral_jerk: 0.5
+      deceleration_interval: 15.0
+      maximum_deceleration: 1.0
+      after_pull_over_straight_distance: 5.0
+      before_pull_over_straight_distance: 5.0
+      # parallel parking path
+      after_forward_parking_straight_distance: 2.0
+      after_backward_parking_straight_distance: 2.0
+      forward_parking_velocity: 0.3
+      backward_parking_velocity: -0.3
+      arc_path_interval: 1.0
+      # hazard_on when parked
+      hazard_on_threshold_dis: 1.0
+      hazard_on_threshold_vel: 0.5
+      # check safety with dynamic objects. Not used now.
+      pull_over_duration: 2.0
+      pull_over_prepare_duration: 4.0
       min_stop_distance: 5.0
       stop_time: 2.0
       hysteresis_buffer_distance: 2.0
-      pull_over_prepare_duration: 4.0
-      pull_over_duration: 2.0
-      pull_over_finish_judge_buffer: 3.0
-      minimum_pull_over_velocity: 3.0
-      prediction_duration: 30.0
-      prediction_time_resolution: 0.5
-      static_obstacle_velocity_thresh: 1.5
-      maximum_deceleration: 1.0
       enable_collision_check_at_prepare_phase: false
       use_predicted_path_outside_lanelet: false
       use_all_predicted_path: false
-      enable_blocked_by_obstacle: false
-      pull_over_search_distance: 30.0
-      after_pull_over_straight_distance: 5.0
-      before_pull_over_straight_distance: 5.0
-      margin_from_boundary: 0.5
-      maximum_lateral_jerk: 2.0
-      minimum_lateral_jerk: 0.5
-      deceleration_interval: 15.0
+      # debug
+      print_debug_info: false
diff --git a/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp b/map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp
@@ -81,7 +81,8 @@ double getLaneletAngle(
 bool isInLanelet(
   const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet,
   const double radius = 0.0);
-
+geometry_msgs::msg::Pose getClosestCenterPose(
+  const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point);
 }  // namespace utils
 }  // namespace lanelet
 

diff --git a/map/lanelet2_extension/lib/utilities.cpp b/map/lanelet2_extension/lib/utilities.cpp
@@ -25,6 +25,12 @@
 #include <lanelet2_traffic_rules/TrafficRules.h>
 #include <lanelet2_traffic_rules/TrafficRulesFactory.h>
 
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
 #include <algorithm>
 #include <limits>
 #include <map>
@@ -685,5 +691,29 @@ bool isInLanelet(
   return false;
 }
 
+geometry_msgs::msg::Pose getClosestCenterPose(
+  const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point)
+{
+  lanelet::BasicPoint2d llt_search_point(search_point.x, search_point.y);
+  lanelet::ConstLineString3d segment = getClosestSegment(llt_search_point, lanelet.centerline());
+
+  const Eigen::Vector2d direction(
+    (segment.back().basicPoint2d() - segment.front().basicPoint2d()).normalized());
+  const Eigen::Vector2d xf(segment.front().basicPoint2d());
+  const Eigen::Vector2d x(search_point.x, search_point.y);
+  const Eigen::Vector2d p = xf + (x - xf).dot(direction) * direction;
+
+  geometry_msgs::msg::Pose closest_pose;
+  closest_pose.position.x = p.x();
+  closest_pose.position.y = p.y();
+  closest_pose.position.z = search_point.z;
+
+  const float lane_yaw = getLaneletAngle(lanelet, search_point);
+  tf2::Quaternion q;
+  q.setRPY(0, 0, lane_yaw);
+  closest_pose.orientation = tf2::toMsg(q);
+
+  return closest_pose;
+}
 }  // namespace utils
 }  // namespace lanelet
diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml
@@ -1,26 +1,54 @@
 /**:
   ros__parameters:
     pull_over:
+      request_length: 100.0
+      th_arrived_distance_m: 1.0
+      th_stopped_velocity_mps: 0.01
+      th_stopped_time_sec: 2.0  # It must be greater than the state_machine's.
+      pull_over_velocity: 2.0
+      pull_over_minimum_velocity: 0.3
+      margin_from_boundary: 0.5
+      decide_path_distance: 10.0
+      min_acc: -0.5
+      enable_shift_parking: true
+      enable_arc_forward_parking: true
+      enable_arc_backward_parking: false
+      # goal research
+      search_priority: "efficient_path" # "efficient_path" or "close_goal"
+      enable_goal_research: true
+      forward_goal_search_length: 20.0
+      backward_goal_search_length: 20.0
+      goal_search_interval: 1.0
+      goal_to_obj_margin: 2.0
+      # occupancy grid map
+      collision_check_margin: 0.0
+      theta_size: 360
+      obstacle_threshold: 60
+      # shift path
+      pull_over_sampling_num: 4
+      maximum_lateral_jerk: 2.0
+      minimum_lateral_jerk: 0.5
+      deceleration_interval: 15.0
+      maximum_deceleration: 1.0
+      after_pull_over_straight_distance: 5.0
+      before_pull_over_straight_distance: 5.0
+      # parallel parking path
+      after_forward_parking_straight_distance: 2.0
+      after_backward_parking_straight_distance: 2.0
+      forward_parking_velocity: 0.3
+      backward_parking_velocity: -0.3
+      arc_path_interval: 1.0
+      # hazard. Not used now.
+      hazard_on_threshold_dis: 1.0
+      hazard_on_threshold_vel: 0.5
+      # check safety with dynamic objects. Not used now.
+      pull_over_duration: 2.0
+      pull_over_prepare_duration: 4.0
       min_stop_distance: 5.0
       stop_time: 2.0
       hysteresis_buffer_distance: 2.0
-      pull_over_prepare_duration: 4.0
-      pull_over_duration: 2.0
-      pull_over_finish_judge_buffer: 3.0
-      minimum_pull_over_velocity: 3.0
-      prediction_duration: 30.0
-      prediction_time_resolution: 0.5
-      static_obstacle_velocity_thresh: 1.5
-      maximum_deceleration: 1.0
-      enable_abort_pull_over: false
       enable_collision_check_at_prepare_phase: false
       use_predicted_path_outside_lanelet: false
       use_all_predicted_path: false
-      enable_blocked_by_obstacle: false
-      pull_over_search_distance: 30.0
-      after_pull_over_straight_distance: 5.0
-      before_pull_over_straight_distance: 5.0
-      margin_from_boundary: 0.5
-      maximum_lateral_jerk: 2.0
-      minimum_lateral_jerk: 0.5
-      deceleration_interval: 15.0
+      # debug
+      print_debug_info: false
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -89,6 +89,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Subscription<Odometry>::SharedPtr velocity_subscriber_;
   rclcpp::Subscription<Scenario>::SharedPtr scenario_subscriber_;
   rclcpp::Subscription<PredictedObjects>::SharedPtr perception_subscriber_;
+  rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_subscriber_;
   rclcpp::Subscription<ApprovalMsg>::SharedPtr external_approval_subscriber_;
   rclcpp::Subscription<PathChangeModule>::SharedPtr force_approval_subscriber_;
   rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
@@ -106,6 +107,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   Scenario::SharedPtr current_scenario_{nullptr};
 
   std::string prev_ready_module_name_ = "NONE";
+  PathChangeModule ready_module_{};
+  PathChangeModuleArray running_modules_{};
 
   TurnSignalDecider turn_signal_decider_;
 
@@ -128,6 +131,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // callback
   void onVelocity(const Odometry::ConstSharedPtr msg);
   void onPerception(const PredictedObjects::ConstSharedPtr msg);
+  void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg);
   void onExternalApproval(const ApprovalMsg::ConstSharedPtr msg);
   void onForceApproval(const PathChangeModule::ConstSharedPtr msg);
   void onMap(const HADMapBin::ConstSharedPtr map_msg);
@@ -158,6 +162,12 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   PathWithLaneId::SharedPtr getPathCandidate(
     const BehaviorModuleOutput & bt_out, const std::shared_ptr<PlannerData> planner_data);
 
+  /**
+   * @brief skip smooth goal connection
+   */
+  bool skipSmoothGoalConnection(
+    const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses) const;
+
   // debug
 
 private:

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
@@ -39,6 +39,7 @@ using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::PoseStamped;
 using geometry_msgs::msg::TwistStamped;
+using nav_msgs::msg::OccupancyGrid;
 using nav_msgs::msg::Odometry;
 using route_handler::RouteHandler;
 struct BoolStamped
@@ -65,6 +66,7 @@ struct PlannerData
   PoseStamped::ConstSharedPtr self_pose{};
   Odometry::ConstSharedPtr self_odometry{};
   PredictedObjects::ConstSharedPtr dynamic_object{};
+  OccupancyGrid::ConstSharedPtr occupancy_grid{};
   PathWithLaneId::SharedPtr reference_path{std::make_shared<PathWithLaneId>()};
   PathWithLaneId::SharedPtr prev_output_path{std::make_shared<PathWithLaneId>()};
   BehaviorPathPlannerParameters parameters{};

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp
@@ -15,6 +15,8 @@
 #ifndef BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
 #define BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_
 
+#include <vehicle_info_util/vehicle_info_util.hpp>
+
 struct BehaviorPathPlannerParameters
 {
   double backward_path_length;
@@ -38,6 +40,7 @@ struct BehaviorPathPlannerParameters
   double turn_light_on_threshold_time;
 
   // vehicle info
+  vehicle_info_util::VehicleInfo vehicle_info;
   double wheel_base;
   double front_overhang;
   double rear_overhang;