update vehicle circles

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp

@@ -151,8 +151,8 @@ struct DebugData
   std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> extended_non_fixed_traj;
 
   geometry_msgs::msg::Pose current_ego_pose;
-  std::vector<double> avoiding_circle_offsets;
-  double avoiding_circle_radius;
+  std::vector<double> vehicle_circle_longitudinal_offsets;
+  double vehicle_circle_radius;
 
   StreamWithPrint msg_stream;
 };
@@ -199,8 +199,8 @@ struct MPTParam
   bool fix_points_around_ego;
   int num_curvature_sampling_points;
 
-  std::vector<double> avoiding_circle_offsets;  // from base_link
-  double avoiding_circle_radius;
+  std::vector<double> vehicle_circle_longitudinal_offsets;  // from base_link
+  double vehicle_circle_radius;
 
   double delta_arc_length_for_mpt_points;
 

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/cv_utils.hpp

@@ -114,7 +114,8 @@ bool isOutsideDrivableAreaFromRectangleFootprint(
 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> avoiding_circle_offsets, const double avoiding_circle_radius);
+  const std::vector<double> vehicle_circle_longitudinal_offsets,
+  const double vehicle_circle_radius);
 }  // namespace cv_drivable_area_utils
 
 #endif  // OBSTACLE_AVOIDANCE_PLANNER__PROCESS_CV_HPP_

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -98,12 +98,23 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
   bool is_showing_debug_info_;
   bool is_showing_calculation_time_;
   bool is_stopping_if_outside_drivable_area_;
-  bool is_using_vehicle_config_;
   bool enable_avoidance_;
   bool enable_pre_smoothing_;
   bool skip_optimization_;
   bool reset_prev_info_;
-  bool use_footprint_for_drivability_;
+
+  // vehicle circles info for drivability check
+  bool use_vehicle_circles_for_drivability_;
+  bool use_manual_vehicle_circles_for_drivability_;
+  int vehicle_circle_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_num_for_mpt_;
+  double vehicle_circle_radius_ratio_for_mpt_;
 
   // params outside logic
   double min_ego_moving_dist_for_replan_;

planning/obstacle_avoidance_planner/src/cv_utils.cpp

@@ -444,14 +444,14 @@ bool isOutsideDrivableAreaFromRectangleFootprint(
 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> avoiding_circle_offsets, const double avoiding_circle_radius)
+  const std::vector<double> vehicle_circle_longitudinal_offsets, const double vehicle_circle_radius)
 {
-  for (const double offset : avoiding_circle_offsets) {
+  for (const double offset : vehicle_circle_longitudinal_offsets) {
     const auto avoiding_pos =
       tier4_autoware_utils::calcOffsetPose(traj_point.pose, offset, 0.0, 0.0).position;
 
     const bool outside_drivable_area =
-      isOutsideDrivableArea(avoiding_pos, road_clearance_map, map_info, avoiding_circle_radius);
+      isOutsideDrivableArea(avoiding_pos, road_clearance_map, map_info, vehicle_circle_radius);
     if (outside_drivable_area) {
       return true;
     }

planning/obstacle_avoidance_planner/src/debug_visualization.cpp

@@ -68,7 +68,7 @@ visualization_msgs::msg::MarkerArray getPointsTextMarkerArray(
   auto marker = createDefaultMarker(
     "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
     createMarkerScale(0.0, 0.0, 0.15), createMarkerColor(r, g, b, 0.99));
-  marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+  marker.lifetime = rclcpp::Duration::from_seconds(1.5);
 
   visualization_msgs::msg::MarkerArray msg;
   for (size_t i = 0; i < points.size(); i++) {
@@ -351,7 +351,7 @@ visualization_msgs::msg::MarkerArray getObjectsMarkerArray(
   auto marker = createDefaultMarker(
     "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::CUBE,
     createMarkerScale(3.0, 1.0, 1.0), createMarkerColor(r, g, b, 0.8));
-  marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+  marker.lifetime = rclcpp::Duration::from_seconds(1.5);
 
   for (size_t i = 0; i < objects.size(); ++i) {
     const auto & object = objects.at(i);
@@ -378,7 +378,7 @@ visualization_msgs::msg::MarkerArray getRectanglesMarkerArray(
     auto marker = createDefaultMarker(
       "map", rclcpp::Clock().now(), ns, i, visualization_msgs::msg::Marker::LINE_STRIP,
       createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 1.0));
-    marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+    marker.lifetime = rclcpp::Duration::from_seconds(1.5);
 
     const double half_width = vehicle_param.width / 2.0;
     const double base_to_front = vehicle_param.length - vehicle_param.rear_overhang;
@@ -411,7 +411,7 @@ visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray(
   auto marker = createDefaultMarker(
     "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
     createMarkerScale(0.0, 0.0, 0.125), createMarkerColor(r, g, b, 0.99));
-  marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+  marker.lifetime = rclcpp::Duration::from_seconds(1.5);
 
   visualization_msgs::msg::MarkerArray msg;
   for (size_t i = 0; i < mpt_traj.size(); ++i) {
@@ -440,7 +440,7 @@ visualization_msgs::msg::MarkerArray getRectanglesNumMarkerArray(
 visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray(
   const std::vector<ReferencePoint> & ref_points,
   const SequentialBoundsCandidates & sequential_bounds_candidates, const double r, const double g,
-  const double b, const double avoiding_circle_radius, const size_t sampling_num)
+  const double b, const double vehicle_circle_radius, const size_t sampling_num)
 {
   const auto current_time = rclcpp::Clock().now();
   visualization_msgs::msg::MarkerArray msg;
@@ -467,11 +467,11 @@ visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray(
       pose.position = ref_points.at(p_idx).p;
       pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_points.at(p_idx).yaw);
 
-      const double lb_y = bounds_candidate.lower_bound - avoiding_circle_radius;
+      const double lb_y = bounds_candidate.lower_bound - vehicle_circle_radius;
       const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position;
       marker.points.push_back(lb);
 
-      const double ub_y = bounds_candidate.upper_bound + avoiding_circle_radius;
+      const double ub_y = bounds_candidate.upper_bound + vehicle_circle_radius;
       const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position;
       marker.points.push_back(ub);
     }
@@ -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 avoiding_circle_radius, const size_t sampling_num)
+  const double vehicle_circle_radius, const size_t sampling_num)
 {
   const auto current_time = rclcpp::Clock().now();
   visualization_msgs::msg::MarkerArray msg;
@@ -498,7 +498,7 @@ visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
       auto marker = createDefaultMarker(
         "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST,
         createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r + 0.5, g, b, 0.3));
-      marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+      marker.lifetime = rclcpp::Duration::from_seconds(1.5);
 
       for (size_t i = 0; i < ref_points.size(); i++) {
         if (i % sampling_num != 0) {
@@ -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 - avoiding_circle_radius;
+          ref_points.at(i).vehicle_bounds[bound_idx].lower_bound - vehicle_circle_radius;
         const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position;
 
         marker.points.push_back(pose.position);
@@ -520,7 +520,7 @@ visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
       auto marker = createDefaultMarker(
         "map", rclcpp::Clock().now(), ns, 1, visualization_msgs::msg::Marker::LINE_LIST,
         createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g + 0.5, b, 0.3));
-      marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+      marker.lifetime = rclcpp::Duration::from_seconds(1.5);
 
       for (size_t i = 0; i < ref_points.size(); i++) {
         if (i % sampling_num != 0) {
@@ -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 + avoiding_circle_radius;
+          ref_points.at(i).vehicle_bounds[bound_idx].upper_bound + vehicle_circle_radius;
         const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position;
 
         marker.points.push_back(pose.position);
@@ -544,7 +544,7 @@ visualization_msgs::msg::MarkerArray getBoundsLineMarkerArray(
 
 visualization_msgs::msg::MarkerArray getVehicleCircleLineMarkerArray(
   const std::vector<std::vector<geometry_msgs::msg::Pose>> & vehicle_circles_pose,
-  const double avoiding_circle_radius, const size_t sampling_num, const std::string & ns,
+  const double vehicle_circle_radius, 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();
@@ -558,14 +558,14 @@ visualization_msgs::msg::MarkerArray getVehicleCircleLineMarkerArray(
     auto marker = createDefaultMarker(
       "map", rclcpp::Clock().now(), ns, i, visualization_msgs::msg::Marker::LINE_LIST,
       createMarkerScale(0.1, 0, 0), createMarkerColor(r, g, b, 0.25));
-    marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+    marker.lifetime = rclcpp::Duration::from_seconds(1.5);
 
     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, avoiding_circle_radius, 0.0).position;
+        tier4_autoware_utils::calcOffsetPose(pose, 0.0, vehicle_circle_radius, 0.0).position;
       const auto lb =
-        tier4_autoware_utils::calcOffsetPose(pose, 0.0, -avoiding_circle_radius, 0.0).position;
+        tier4_autoware_utils::calcOffsetPose(pose, 0.0, -vehicle_circle_radius, 0.0).position;
 
       marker.points.push_back(ub);
       marker.points.push_back(lb);
@@ -583,7 +583,7 @@ visualization_msgs::msg::MarkerArray getLateralErrorsLineMarkerArray(
   auto marker = createDefaultMarker(
     "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST,
     createMarkerScale(0.1, 0, 0), createMarkerColor(r, g, b, 1.0));
-  marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+  marker.lifetime = rclcpp::Duration::from_seconds(1.5);
 
   for (size_t i = 0; i < mpt_ref_poses.size(); ++i) {
     if (i % sampling_num != 0) {
@@ -604,17 +604,18 @@ visualization_msgs::msg::MarkerArray getLateralErrorsLineMarkerArray(
 
 visualization_msgs::msg::MarkerArray getCurrentVehicleCirclesMarkerArray(
   const geometry_msgs::msg::Pose & current_ego_pose,
-  const std::vector<double> & avoiding_circle_offsets, const double avoiding_circle_radius,
-  const std::string & ns, const double r, const double g, const double b)
+  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)
 {
   visualization_msgs::msg::MarkerArray msg;
 
   size_t id = 0;
-  for (const double offset : avoiding_circle_offsets) {
+  for (const double offset : vehicle_circle_longitudinal_offsets) {
     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));
-    marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+    marker.lifetime = rclcpp::Duration::from_seconds(1.5);
     marker.pose = tier4_autoware_utils::calcOffsetPose(current_ego_pose, offset, 0.0, 0.0);
 
     constexpr size_t circle_dividing_num = 16;
@@ -623,8 +624,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 = avoiding_circle_radius * std::cos(edge_angle);
-      edge_pos.y = avoiding_circle_radius * std::sin(edge_angle);
+      edge_pos.x = vehicle_circle_radius * std::cos(edge_angle);
+      edge_pos.y = vehicle_circle_radius * std::sin(edge_angle);
 
       marker.points.push_back(edge_pos);
     }
@@ -637,8 +638,9 @@ 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> & avoiding_circle_offsets, const double avoiding_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_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)
 {
   visualization_msgs::msg::MarkerArray msg;
 
@@ -649,11 +651,11 @@ visualization_msgs::msg::MarkerArray getVehicleCirclesMarkerArray(
     }
     const auto & mpt_traj_point = mpt_traj_points.at(i);
 
-    for (const double offset : avoiding_circle_offsets) {
+    for (const double offset : vehicle_circle_longitudinal_offsets) {
       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));
-      marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+      marker.lifetime = rclcpp::Duration::from_seconds(1.5);
       marker.pose = tier4_autoware_utils::calcOffsetPose(mpt_traj_point.pose, offset, 0.0, 0.0);
 
       constexpr size_t circle_dividing_num = 16;
@@ -662,8 +664,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 = avoiding_circle_radius * std::cos(edge_angle);
-        edge_pos.y = avoiding_circle_radius * std::sin(edge_angle);
+        edge_pos.x = vehicle_circle_radius * std::cos(edge_angle);
+        edge_pos.y = vehicle_circle_radius * std::sin(edge_angle);
 
         marker.points.push_back(edge_pos);
       }
@@ -682,7 +684,7 @@ visualization_msgs::msg::MarkerArray getVirtualWallMarkerArray(
   auto marker = createDefaultMarker(
     "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::CUBE,
     createMarkerScale(0.1, 5.0, 2.0), createMarkerColor(r, g, b, 0.5));
-  marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+  marker.lifetime = rclcpp::Duration::from_seconds(1.5);
   marker.pose = pose;
 
   visualization_msgs::msg::MarkerArray msg;
@@ -698,7 +700,7 @@ visualization_msgs::msg::MarkerArray getVirtualWallTextMarkerArray(
   auto marker = createDefaultMarker(
     "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
     createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(r, g, b, 0.99));
-  marker.lifetime = rclcpp::Duration::from_seconds(1.0);
+  marker.lifetime = rclcpp::Duration::from_seconds(1.5);
   marker.pose = pose;
   marker.text = "drivable area";
 
@@ -762,7 +764,7 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
   // bounds
   appendMarkerArray(
     getBoundsLineMarkerArray(
-      debug_data_ptr->ref_points, 0.99, 0.99, 0.2, debug_data_ptr->avoiding_circle_radius,
+      debug_data_ptr->ref_points, 0.99, 0.99, 0.2, debug_data_ptr->vehicle_circle_radius,
       debug_data_ptr->visualize_sampling_num),
     &vis_marker_array);
 
@@ -771,14 +773,14 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
   appendMarkerArray(
     getBoundsCandidatesLineMarkerArray(
       debug_data_ptr->ref_points, debug_data_ptr->sequential_bounds_candidates, 0.2, 0.99, 0.99,
-      debug_data_ptr->avoiding_circle_radius, debug_data_ptr->visualize_sampling_num),
+      debug_data_ptr->vehicle_circle_radius, debug_data_ptr->visualize_sampling_num),
     &vis_marker_array);
   */
 
   // vehicle circle line
   appendMarkerArray(
     getVehicleCircleLineMarkerArray(
-      debug_data_ptr->vehicle_circles_pose, debug_data_ptr->avoiding_circle_radius,
+      debug_data_ptr->vehicle_circles_pose, debug_data_ptr->vehicle_circle_radius,
       debug_data_ptr->visualize_sampling_num, "vehicle_circle_lines", 0.99, 0.99, 0.2),
     &vis_marker_array);
 
@@ -792,15 +794,15 @@ visualization_msgs::msg::MarkerArray getDebugVisualizationMarker(
   // current vehicle circles
   appendMarkerArray(
     getCurrentVehicleCirclesMarkerArray(
-      debug_data_ptr->current_ego_pose, debug_data_ptr->avoiding_circle_offsets,
-      debug_data_ptr->avoiding_circle_radius, "current_vehicle_circles", 1.0, 0.3, 0.3),
+      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),
     &vis_marker_array);
 
   // vehicle circles
   appendMarkerArray(
     getVehicleCirclesMarkerArray(
-      debug_data_ptr->mpt_traj, debug_data_ptr->avoiding_circle_offsets,
-      debug_data_ptr->avoiding_circle_radius, debug_data_ptr->visualize_sampling_num,
+      debug_data_ptr->mpt_traj, debug_data_ptr->vehicle_circle_longitudinal_offsets,
+      debug_data_ptr->vehicle_circle_radius, debug_data_ptr->visualize_sampling_num,
       "vehicle_circles", 1.0, 0.3, 0.3),
     &vis_marker_array);