[pull] main from autowarefoundation:main

map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp

@@ -68,7 +68,7 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt
       std::make_unique<PointcloudMapLoaderModule>(this, pcd_paths, publisher_name, true);
   }
 
-  if (enable_partial_load || enable_differential_load) {
+  if (enable_partial_load || enable_differential_load || enable_selected_load) {
     std::map<std::string, PCDFileMetadata> pcd_metadata_dict;
     try {
       pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths);

planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp

@@ -101,7 +101,7 @@ void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Po
     "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "dynamic_objects_to_avoid",
     marker_array.markers.size(), visualization_msgs::msg::Marker::CUBE,
     tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0),
-    tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8));
+    tier4_autoware_utils::createMarkerColor(0.7, 0.15, 0.9, 0.8));
   marker.pose = obj_pose;
 
   marker_array.markers.push_back(marker);
@@ -113,8 +113,8 @@ void appendExtractedPolygonMarker(
   auto marker = tier4_autoware_utils::createDefaultMarker(
     "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(),
     visualization_msgs::msg::Marker::LINE_STRIP,
-    tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0),
-    tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8));
+    tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0),
+    tier4_autoware_utils::createMarkerColor(0.7, 0.15, 0.9, 0.8));
 
   // NOTE: obj_poly.outer() has already duplicated points to close the polygon.
   for (size_t i = 0; i < obj_poly.outer().size(); ++i) {

planning/mission_planner/README.md

@@ -22,7 +22,6 @@ In current Autoware.universe, only Lanelet2 map format is supported.
 | `arrival_check_duration`   | double | Duration threshold for goal check                                                                                |
 | `goal_angle_threshold`     | double | Max goal pose angle for goal approve                                                                             |
 | `enable_correct_goal_pose` | bool   | Enabling correction of goal pose according to the closest lanelet orientation                                    |
-| `enable_correct_goal_pose` | bool   | Enabling correction of goal pose according to the closest lanelet orientation                                    |
 | `reroute_time_threshold`   | double | If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible |
 | `minimum_reroute_length`   | double | Minimum Length for publishing a new route                                                                        |
 

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -842,15 +842,38 @@ void MPTOptimizer::updateBounds(
 
 void MPTOptimizer::keepMinimumBoundsWidth(std::vector<ReferencePoint> & ref_points) const
 {
+  // calculate drivable area width considering the curvature
+  std::vector<double> min_dynamic_drivable_width_vec;
+  for (int i = 0; i < static_cast<int>(ref_points.size()); ++i) {
+    double curvature = std::abs(ref_points.at(i).curvature);
+    if (i != static_cast<int>(ref_points.size()) - 1) {
+      curvature = std::max(curvature, std::abs(ref_points.at(i + 1).curvature));
+    }
+    if (i != 0) {
+      curvature = std::max(curvature, std::abs(ref_points.at(i - 1).curvature));
+    }
+
+    const double max_longitudinal_length = std::max(
+      std::abs(vehicle_info_.max_longitudinal_offset_m),
+      std::abs(vehicle_info_.min_longitudinal_offset_m));
+    const double turning_radius = 1.0 / curvature;
+    const double additional_drivable_width_by_curvature =
+      std::hypot(max_longitudinal_length, turning_radius + vehicle_info_.vehicle_width_m / 2.0) -
+      turning_radius - vehicle_info_.vehicle_width_m / 2.0;
+    min_dynamic_drivable_width_vec.push_back(
+      mpt_param_.min_drivable_width + additional_drivable_width_by_curvature);
+  }
+
   // 1. calculate start and end sections which are out of bounds
   std::vector<std::pair<size_t, size_t>> out_of_upper_bound_sections;
   std::vector<std::pair<size_t, size_t>> out_of_lower_bound_sections;
   std::optional<size_t> out_of_upper_bound_start_idx = std::nullopt;
   std::optional<size_t> out_of_lower_bound_start_idx = std::nullopt;
   for (size_t i = 0; i < ref_points.size(); ++i) {
     const auto & b = ref_points.at(i).bounds;
+
     // const double drivable_width = b.upper_bound - b.lower_bound;
-    // const bool is_infeasible_to_drive = drivable_width < mpt_param_.min_drivable_width;
+    // const bool is_infeasible_to_drive = drivable_width < min_dynamic_drivable_width
 
     // NOTE: The following condition should be uncommented to see obstacles outside the path.
     //       However, on a narrow road, the ego may go outside the road border with this condition.
@@ -912,16 +935,16 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector<ReferencePoint> & ref_poin
         // It seems both bounds are cut out. Widen the bounds towards the both side.
         const double center_dist_to_bounds =
           (original_b.upper_bound + original_b.lower_bound) / 2.0;
-        b.upper_bound =
-          std::max(b.upper_bound, center_dist_to_bounds + mpt_param_.min_drivable_width / 2.0);
-        b.lower_bound =
-          std::min(b.lower_bound, center_dist_to_bounds - mpt_param_.min_drivable_width / 2.0);
+        b.upper_bound = std::max(
+          b.upper_bound, center_dist_to_bounds + min_dynamic_drivable_width_vec.at(p_idx) / 2.0);
+        b.lower_bound = std::min(
+          b.lower_bound, center_dist_to_bounds - min_dynamic_drivable_width_vec.at(p_idx) / 2.0);
         continue;
       }
       // Only the Lower bound is cut out. Widen the bounds towards the lower bound since cut out too
       // much.
       b.lower_bound =
-        std::min(b.lower_bound, original_b.upper_bound - mpt_param_.min_drivable_width);
+        std::min(b.lower_bound, original_b.upper_bound - min_dynamic_drivable_width_vec.at(p_idx));
       continue;
     }
     // extend longitudinal if it overlaps out_of_upper_bound_sections
@@ -962,16 +985,16 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector<ReferencePoint> & ref_poin
         // It seems both bounds are cut out. Widen the bounds towards the both side.
         const double center_dist_to_bounds =
           (original_b.upper_bound + original_b.lower_bound) / 2.0;
-        b.upper_bound =
-          std::max(b.upper_bound, center_dist_to_bounds + mpt_param_.min_drivable_width / 2.0);
-        b.lower_bound =
-          std::min(b.lower_bound, center_dist_to_bounds - mpt_param_.min_drivable_width / 2.0);
+        b.upper_bound = std::max(
+          b.upper_bound, center_dist_to_bounds + min_dynamic_drivable_width_vec.at(p_idx) / 2.0);
+        b.lower_bound = std::min(
+          b.lower_bound, center_dist_to_bounds - min_dynamic_drivable_width_vec.at(p_idx) / 2.0);
         continue;
       }
       // Only the Upper bound is cut out. Widen the bounds towards the upper bound since cut out too
       // much.
       b.upper_bound =
-        std::max(b.upper_bound, original_b.lower_bound + mpt_param_.min_drivable_width);
+        std::max(b.upper_bound, original_b.lower_bound + min_dynamic_drivable_width_vec.at(p_idx));
       continue;
     }
     // extend longitudinal if it overlaps out_of_lower_bound_sections

planning/obstacle_cruise_planner/src/node.cpp

@@ -1251,7 +1251,7 @@ void ObstacleCruisePlannerNode::publishDebugMarker() const
   for (size_t i = 0; i < debug_data_ptr_->obstacles_to_cruise.size(); ++i) {
     // obstacle
     const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker(
-      debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles_to_cruise", 1.0, 0.3, 0.0);
+      debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles_to_cruise", 1.0, 0.5, 0.5);
     debug_marker.markers.push_back(obstacle_marker);
 
     // collision points

planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

@@ -314,7 +314,7 @@ std::vector<TrajectoryPoint> PIDBasedPlanner::planCruise(
         stop_traj_points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, 0);
       // NOTE: use a different color from slow down one to visualize cruise and slow down
       // separately.
-      markers.markers.front().color = tier4_autoware_utils::createMarkerColor(1.0, 0.3, 0.0, 0.5);
+      markers.markers.front().color = tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.5, 0.5);
       tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker);
 
       // cruise obstacle