From a09b2c985d82674fb61b930e695b671bdcbbce9b Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 9 Jun 2023 16:12:05 +0900 Subject: [PATCH 1/5] fix(map_loader): handle enable_selected_load correctly (#3920) * fix(map_loader): update readme for metadata Signed-off-by: kminoda * fix(map_loader): handle enable_selected_load flag correctly Signed-off-by: kminoda * style(pre-commit): autofix * revert readme Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/pointcloud_map_loader/pointcloud_map_loader_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index dda25987629ce..a2d9307084545 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/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(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 pcd_metadata_dict; try { pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); From 21b64038779a2a929b9e409fb6bb13c2392ff789 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 9 Jun 2023 17:35:28 +0900 Subject: [PATCH 2/5] chore(obstacle_cruise_planner): change cruise color (#3925) Signed-off-by: Takayuki Murooka --- planning/obstacle_cruise_planner/src/node.cpp | 2 +- .../src/pid_based_planner/pid_based_planner.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 028c82047ed3a..bf705759e0422 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/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 diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index e729b97ac1deb..bca5ad3f3cf41 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -314,7 +314,7 @@ std::vector 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 From fc08b4f11770a6a2ac137ec8121c20f51179ca68 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 9 Jun 2023 20:33:00 +0900 Subject: [PATCH 3/5] chore(dynamic_avoidance): change debug marker's color (#3926) Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance/dynamic_avoidance_module.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 47c29be29c2a3..7ece399bf450c 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/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) { From 719a843fc63ff80ffff68d76bc863032857b6d36 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 10 Jun 2023 01:22:26 +0900 Subject: [PATCH 4/5] feat(obstacle_avoidance_planner): consider curvature for min_drivable_width (#3918) * feat(obstacle_avoidance_planner): consider curvature for min_drivable_width Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/mpt_optimizer.cpp | 45 ++++++++++++++----- 1 file changed, 34 insertions(+), 11 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 439b68e9032dd..eff2a653f39e2 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -842,6 +842,28 @@ void MPTOptimizer::updateBounds( void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_points) const { + // calculate drivable area width considering the curvature + std::vector min_dynamic_drivable_width_vec; + for (int i = 0; i < static_cast(ref_points.size()); ++i) { + double curvature = std::abs(ref_points.at(i).curvature); + if (i != static_cast(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> out_of_upper_bound_sections; std::vector> out_of_lower_bound_sections; @@ -849,8 +871,9 @@ void MPTOptimizer::keepMinimumBoundsWidth(std::vector & ref_poin std::optional 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 & 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 & 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 From e9f2f4fb4bd1b1dc105d5fd3964565028d426c1b Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Date: Sat, 10 Jun 2023 10:23:54 +0300 Subject: [PATCH 5/5] docs(mission_planner): remove duplicated param from readme (#3933) Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- planning/mission_planner/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index e1714420a45f0..4eb91e423c01f 100644 --- a/planning/mission_planner/README.md +++ b/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 |