feat(obstacle_avoidance_planner): some fix for narrow driving (autowa…

…refoundation#916)

* use car like constraints in mpt

* use not widest bounds for the first bounds

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* organized params

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix format

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* prepare rear_drive and uniform_circle constraints

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix param callback

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update config

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* remove unnecessary files

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update tier4_planning_launch params

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml

@@ -83,28 +83,6 @@
 
     # advanced parameters to improve performance as much as possible
     advanced:
-      option:
-        # check if planned trajectory is outside drivable area
-        drivability_check:
-          # true: vehicle shape is considered as a set of circles
-          # false: vehicle shape is considered as footprint (= rectangle)
-          use_vehicle_circles: false
-
-          # parameters only when use_vehicle_circles is true
-          vehicle_circles:
-            use_manual_vehicle_circles: false
-            num_for_constraints: 4
-
-            # parameters only when use_manual_vehicle_circles is true
-            longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95]
-            radius: 0.95
-
-            # parameters only when use_manual_vehicle_circles is false
-            radius_ratio: 0.9
-            # If this parameter is commented out, the parameter is calculated automatically
-            # based on the vehicle length and width
-            # num_for_radius: 4
-
       eb:
         common:
           num_joint_buffer_points: 3 # number of joint buffer points
@@ -161,15 +139,13 @@
             # two_step_soft_constraint: false
 
           vehicle_circles:
-            use_manual_vehicle_circles: false
-            num_for_constraints: 3
-
-            # parameters only when use_manual_vehicle_circles is true
-            longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95]
-            radius: 0.95
-
-            # parameters only when use_manual_vehicle_circles is false
-            radius_ratio: 0.8
-            # If this parameter is commented out, the parameter is calculated automatically
-            # based on the vehicle length and width
-            # num_for_radius: 4
+            method: "rear_drive"
+
+            uniform_circle:
+              num: 3
+              radius_ratio: 0.8
+
+            rear_drive:
+              num_for_calculation: 3
+              front_radius_ratio: 1.0
+              rear_radius_ratio: 1.0

planning/obstacle_avoidance_planner/config/obstacle_avoidance_planner.param.yaml

@@ -83,28 +83,6 @@
 
     # advanced parameters to improve performance as much as possible
     advanced:
-      option:
-        # check if planned trajectory is outside drivable area
-        drivability_check:
-          # true: vehicle shape is considered as a set of circles
-          # false: vehicle shape is considered as footprint (= rectangle)
-          use_vehicle_circles: false
-
-          # parameters only when use_vehicle_circles is true
-          vehicle_circles:
-            use_manual_vehicle_circles: false
-            num_for_constraints: 4
-
-            # parameters only when use_manual_vehicle_circles is true
-            longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95]
-            radius: 0.95
-
-            # parameters only when use_manual_vehicle_circles is false
-            radius_ratio: 0.9
-            # If this parameter is commented out, the parameter is calculated automatically
-            # based on the vehicle length and width
-            # num_for_radius: 4
-
       eb:
         common:
           num_joint_buffer_points: 3 # number of joint buffer points
@@ -161,15 +139,13 @@
             # two_step_soft_constraint: false
 
           vehicle_circles:
-            use_manual_vehicle_circles: false
-            num_for_constraints: 3
-
-            # parameters only when use_manual_vehicle_circles is true
-            longitudinal_offsets: [-0.15, 0.88, 1.9, 2.95]
-            radius: 0.95
-
-            # parameters only when use_manual_vehicle_circles is false
-            radius_ratio: 0.8
-            # If this parameter is commented out, the parameter is calculated automatically
-            # based on the vehicle length and width
-            # num_for_radius: 4
+            method: "rear_drive"
+
+            uniform_circle:
+              num: 3
+              radius_ratio: 0.8
+
+            rear_drive:
+              num_for_calculation: 3
+              front_radius_ratio: 1.0
+              rear_radius_ratio: 1.0

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp

@@ -126,20 +126,20 @@ struct DebugData
   void init(
     const bool local_is_showing_calculation_time, const int local_mpt_visualize_sampling_num,
     const geometry_msgs::msg::Pose & local_current_ego_pose,
-    const double local_vehicle_circle_radius,
+    const std::vector<double> & local_vehicle_circle_radiuses,
     const std::vector<double> & local_vehicle_circle_longitudinal_offsets)
   {
     msg_stream.is_showing_calculation_time = local_is_showing_calculation_time;
     mpt_visualize_sampling_num = local_mpt_visualize_sampling_num;
     current_ego_pose = local_current_ego_pose;
-    vehicle_circle_radius = local_vehicle_circle_radius;
+    vehicle_circle_radiuses = local_vehicle_circle_radiuses;
     vehicle_circle_longitudinal_offsets = local_vehicle_circle_longitudinal_offsets;
   }
 
   StreamWithPrint msg_stream;
   size_t mpt_visualize_sampling_num;
   geometry_msgs::msg::Pose current_ego_pose;
-  double vehicle_circle_radius;
+  std::vector<double> vehicle_circle_radiuses;
   std::vector<double> vehicle_circle_longitudinal_offsets;
 
   boost::optional<geometry_msgs::msg::Pose> stop_pose_by_drivable_area = boost::none;
@@ -212,7 +212,7 @@ struct MPTParam
   int num_curvature_sampling_points;
 
   std::vector<double> vehicle_circle_longitudinal_offsets;  // from base_link
-  double vehicle_circle_radius;
+  std::vector<double> vehicle_circle_radiuses;
 
   double delta_arc_length_for_mpt_points;
 

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp

@@ -219,6 +219,7 @@ class MPTOptimizer
 
   void calcBounds(
     std::vector<ReferencePoint> & ref_points, const bool enable_avoidance, const CVMaps & maps,
+    const std::unique_ptr<Trajectories> & prev_trajs,
     std::shared_ptr<DebugData> debug_data_ptr) const;
 
   void calcVehicleBounds(

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -44,6 +44,7 @@
 
 #include <algorithm>
 #include <memory>
+#include <string>
 #include <vector>
 
 namespace
@@ -158,20 +159,10 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
   bool skip_optimization_;
   bool reset_prev_optimization_;
 
-  // vehicle circles info for drivability check
-  bool use_vehicle_circles_for_drivability_;
-  bool use_manual_vehicle_circles_for_drivability_;
-  int vehicle_circle_constraints_num_for_drivability_;
-  int vehicle_circle_radius_num_for_drivability_;
-  double vehicle_circle_radius_ratio_for_drivability_;
-  double vehicle_circle_radius_for_drivability_;
-  std::vector<double> vehicle_circle_longitudinal_offsets_for_drivability_;
-
   // vehicle circles info for for mpt constraints
-  bool use_manual_vehicle_circles_for_mpt_;
-  int vehicle_circle_constraints_num_for_mpt_;
-  int vehicle_circle_radius_num_for_mpt_;
-  double vehicle_circle_radius_ratio_for_mpt_;
+  std::string vehicle_circle_method_;
+  int vehicle_circle_num_for_calculation_;
+  std::vector<double> vehicle_circle_radius_ratios_;
 
   // params for replan
   double max_path_shape_change_dist_for_replan_;

planning/obstacle_avoidance_planner/src/cv_utils.cpp

@@ -441,7 +441,7 @@ bool isOutsideDrivableAreaFromRectangleFootprint(
   return false;
 }
 
-bool isOutsideDrivableAreaFromCirclesFootprint(
+[[maybe_unused]] bool isOutsideDrivableAreaFromCirclesFootprint(
   const autoware_auto_planning_msgs::msg::TrajectoryPoint & traj_point,
   const cv::Mat & road_clearance_map, const nav_msgs::msg::MapMetaData & map_info,
   const std::vector<double> vehicle_circle_longitudinal_offsets, const double vehicle_circle_radius)

planning/obstacle_avoidance_planner/src/debug_visualization.cpp

@@ -484,7 +484,7 @@ visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray(
 
 visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
   const std::vector<ReferencePoint> & ref_points, const double r, const double g, const double b,
-  const double vehicle_circle_radius, const size_t sampling_num)
+  const double vehicle_width, const size_t sampling_num)
 {
   const auto current_time = rclcpp::Clock().now();
   visualization_msgs::msg::MarkerArray msg;
@@ -507,7 +507,7 @@ visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
 
         const geometry_msgs::msg::Pose & pose = ref_points.at(i).vehicle_bounds_poses.at(bound_idx);
         const double lb_y =
-          ref_points.at(i).vehicle_bounds[bound_idx].lower_bound - vehicle_circle_radius;
+          ref_points.at(i).vehicle_bounds[bound_idx].lower_bound - vehicle_width / 2.0;
         const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position;
 
         marker.points.push_back(pose.position);
@@ -529,7 +529,7 @@ visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
 
         const geometry_msgs::msg::Pose & pose = ref_points.at(i).vehicle_bounds_poses.at(bound_idx);
         const double ub_y =
-          ref_points.at(i).vehicle_bounds[bound_idx].upper_bound + vehicle_circle_radius;
+          ref_points.at(i).vehicle_bounds[bound_idx].upper_bound + vehicle_width / 2.0;
         const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position;
 
         marker.points.push_back(pose.position);
@@ -544,8 +544,8 @@ visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
 
 visualization_msgs::msg::MarkerArray getVehicleCircleLineMarkerArray(
   const std::vector<std::vector<geometry_msgs::msg::Pose>> & vehicle_circles_pose,
-  const double vehicle_circle_radius, const size_t sampling_num, const std::string & ns,
-  const double r, const double g, const double b)
+  const double vehicle_width, const size_t sampling_num, const std::string & ns, const double r,
+  const double g, const double b)
 {
   const auto current_time = rclcpp::Clock().now();
   visualization_msgs::msg::MarkerArray msg;
@@ -563,9 +563,9 @@ visualization_msgs::msg::MarkerArray getVehicleCircleLineMarkerArray(
     for (size_t j = 0; j < vehicle_circles_pose.at(i).size(); ++j) {
       const geometry_msgs::msg::Pose & pose = vehicle_circles_pose.at(i).at(j);
       const auto ub =
-        tier4_autoware_utils::calcOffsetPose(pose, 0.0, vehicle_circle_radius, 0.0).position;
+        tier4_autoware_utils::calcOffsetPose(pose, 0.0, vehicle_width / 2.0, 0.0).position;
       const auto lb =
-        tier4_autoware_utils::calcOffsetPose(pose, 0.0, -vehicle_circle_radius, 0.0).position;
+        tier4_autoware_utils::calcOffsetPose(pose, 0.0, -vehicle_width / 2.0, 0.0).position;
 
       marker.points.push_back(ub);
       marker.points.push_back(lb);
@@ -605,13 +605,15 @@ visualization_msgs::msg::MarkerArray getLateralErrorsLineMarkerArray(
 visualization_msgs::msg::MarkerArray getCurrentVehicleCirclesMarkerArray(
   const geometry_msgs::msg::Pose & current_ego_pose,
   const std::vector<double> & vehicle_circle_longitudinal_offsets,
-  const double vehicle_circle_radius, const std::string & ns, const double r, const double g,
-  const double b)
+  const std::vector<double> & vehicle_circle_radiuses, const std::string & ns, const double r,
+  const double g, const double b)
 {
   visualization_msgs::msg::MarkerArray msg;
 
   size_t id = 0;
-  for (const double offset : vehicle_circle_longitudinal_offsets) {
+  for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) {
+    const double offset = vehicle_circle_longitudinal_offsets.at(v_idx);
+
     auto marker = createDefaultMarker(
       "map", rclcpp::Clock().now(), ns, id, visualization_msgs::msg::Marker::LINE_STRIP,
       createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8));
@@ -624,8 +626,8 @@ visualization_msgs::msg::MarkerArray getCurrentVehicleCirclesMarkerArray(
 
       const double edge_angle =
         static_cast<double>(e_idx) / static_cast<double>(circle_dividing_num) * 2.0 * M_PI;
-      edge_pos.x = vehicle_circle_radius * std::cos(edge_angle);
-      edge_pos.y = vehicle_circle_radius * std::sin(edge_angle);
+      edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle);
+      edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle);
 
       marker.points.push_back(edge_pos);
     }
@@ -639,8 +641,8 @@ visualization_msgs::msg::MarkerArray getCurrentVehicleCirclesMarkerArray(
 visualization_msgs::msg::MarkerArray getVehicleCirclesMarkerArray(
   const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & mpt_traj_points,
   const std::vector<double> & vehicle_circle_longitudinal_offsets,
-  const double vehicle_circle_radius, const size_t sampling_num, const std::string & ns,
-  const double r, const double g, const double b)
+  const std::vector<double> & vehicle_circle_radiuses, const size_t sampling_num,
+  const std::string & ns, const double r, const double g, const double b)
 {
   visualization_msgs::msg::MarkerArray msg;
 
@@ -651,7 +653,9 @@ visualization_msgs::msg::MarkerArray getVehicleCirclesMarkerArray(
     }
     const auto & mpt_traj_point = mpt_traj_points.at(i);
 
-    for (const double offset : vehicle_circle_longitudinal_offsets) {
+    for (size_t v_idx = 0; v_idx < vehicle_circle_longitudinal_offsets.size(); ++v_idx) {
+      const double offset = vehicle_circle_longitudinal_offsets.at(v_idx);
+
       auto marker = createDefaultMarker(
         "map", rclcpp::Clock().now(), ns, id, visualization_msgs::msg::Marker::LINE_STRIP,
         createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.8));
@@ -664,8 +668,8 @@ visualization_msgs::msg::MarkerArray getVehicleCirclesMarkerArray(
 
         const double edge_angle =
           static_cast<double>(e_idx) / static_cast<double>(circle_dividing_num) * 2.0 * M_PI;
-        edge_pos.x = vehicle_circle_radius * std::cos(edge_angle);
-        edge_pos.y = vehicle_circle_radius * std::sin(edge_angle);
+        edge_pos.x = vehicle_circle_radiuses.at(v_idx) * std::cos(edge_angle);
+        edge_pos.y = vehicle_circle_radiuses.at(v_idx) * std::sin(edge_angle);
 
         marker.points.push_back(edge_pos);
       }
@@ -754,7 +758,7 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
   // bounds
   appendMarkerArray(
     getBoundsLineMarkerArray(
-      debug_data_ptr->ref_points, 0.99, 0.99, 0.2, debug_data_ptr->vehicle_circle_radius,
+      debug_data_ptr->ref_points, 0.99, 0.99, 0.2, vehicle_param.width,
       debug_data_ptr->mpt_visualize_sampling_num),
     &vis_marker_array);
 
@@ -770,7 +774,7 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
   // vehicle circle line
   appendMarkerArray(
     getVehicleCircleLineMarkerArray(
-      debug_data_ptr->vehicle_circles_pose, debug_data_ptr->vehicle_circle_radius,
+      debug_data_ptr->vehicle_circles_pose, vehicle_param.width,
       debug_data_ptr->mpt_visualize_sampling_num, "vehicle_circle_lines", 0.99, 0.99, 0.2),
     &vis_marker_array);
 
@@ -785,14 +789,14 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
   appendMarkerArray(
     getCurrentVehicleCirclesMarkerArray(
       debug_data_ptr->current_ego_pose, debug_data_ptr->vehicle_circle_longitudinal_offsets,
-      debug_data_ptr->vehicle_circle_radius, "current_vehicle_circles", 1.0, 0.3, 0.3),
+      debug_data_ptr->vehicle_circle_radiuses, "current_vehicle_circles", 1.0, 0.3, 0.3),
     &vis_marker_array);
 
   // vehicle circles
   appendMarkerArray(
     getVehicleCirclesMarkerArray(
       debug_data_ptr->mpt_traj, debug_data_ptr->vehicle_circle_longitudinal_offsets,
-      debug_data_ptr->vehicle_circle_radius, debug_data_ptr->mpt_visualize_sampling_num,
+      debug_data_ptr->vehicle_circle_radiuses, debug_data_ptr->mpt_visualize_sampling_num,
       "vehicle_circles", 1.0, 0.3, 0.3),
     &vis_marker_array);