diff --git a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml index a0ddef6e363cc..a86443f5cabdb 100644 --- a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml +++ b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml @@ -3,6 +3,8 @@ transition_timeout: 10.0 frequency_hz: 10.0 check_engage_condition: false # set false if you do not want to care about the engage condition. + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index + nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. dist_threshold: 1.5 diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index 821190b62956b..3a2bc1a0cd1c0 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md @@ -513,25 +513,3 @@ To print the debug message, just run the following ```bash ros2 topic echo /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance_debug_message_array ``` - -### Visualizing drivable area boundary - -Sometimes, the developers might get a different result between two maps that may look identical during visual inspection. - -For example, in the same area, one can perform avoidance and another cannot. This might be related to the drivable area issues due to the non-compliance vector map design from the user. - -To debug the issue, the drivable area boundary can be visualized. - -![drivable_area_boundary_marker1](./image/avoidance_design/drivable_area_boundary_marker_example1.png) - -![drivable_area_boundary_marker2](./image/avoidance_design/drivable_area_boundary_marker_example2.png) - -The boundary can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_boundary` - -### Visualizing drivable area - -The drivable area can be visualize by adding the drivable area plugin - -![drivable_area_plugin](./image/drivable_area_plugin.png) - -and then add `/planning/scenario_planning/lane_driving/behavior_planning/path` as the topic. diff --git a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md index 88dcf30299a59..0d17c8c9fd509 100644 --- a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md +++ b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md @@ -76,3 +76,17 @@ std::vector right_bound; and each point of right bound and left bound has a position in the absolute coordinate system. ![drivable_lines](./image/drivable_area/drivable_lines.drawio.svg) + +### Visualizing maximum drivable area (Debug) + +Sometimes, the developers might get a different result between two maps that may look identical during visual inspection. + +For example, in the same area, one can perform avoidance and another cannot. This might be related to the maximum drivable area issues due to the non-compliance vector map design from the user. + +To debug the issue, the maximum drivable area boundary can be visualized. + +![drivable_area_boundary_marker1](./image/drivable_area/drivable_area_boundary_marker_example1.png) + +![drivable_area_boundary_marker2](./image/drivable_area/drivable_area_boundary_marker_example2.png) + +The maximum drivable area can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/maximum_drivable_area` diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index da2342f6782d4..6dce3ff06206b 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -21,7 +21,7 @@ path_interval: 2.0 - visualize_drivable_area_for_shared_linestrings_lanelet: true + visualize_maximum_drivable_area: true lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 diff --git a/planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example1.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example1.png rename to planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png diff --git a/planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example2.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example2.png rename to planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png 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 index 75d1f798d8b65..d4bc06a33d349 100644 --- 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 @@ -178,7 +178,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node const BehaviorModuleOutput & output); // debug - rclcpp::Publisher::SharedPtr debug_drivable_area_lanelets_publisher_; + rclcpp::Publisher::SharedPtr debug_maximum_drivable_area_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 8103092e23cdc..7caa25a9b4cf6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -58,8 +58,8 @@ struct BehaviorPathPlannerParameters double base_link2front; double base_link2rear; - // drivable area visualization - bool visualize_drivable_area_for_shared_linestrings_lanelet; + // maximum drivable area visualization + bool visualize_maximum_drivable_area; // collision check double lateral_distance_max_threshold; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 6b684998141c5..2de16855391ff 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -296,7 +296,7 @@ void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, const std::shared_ptr planner_data, const bool is_driving_forward = true); -lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( +lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data); /** diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index f9d98b0241629..5f5de72b26160 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -69,9 +69,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod debug_lane_change_msg_array_publisher_ = create_publisher("~/debug/lane_change_debug_message_array", 1); - if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { - debug_drivable_area_lanelets_publisher_ = - create_publisher("~/drivable_area_boundary", 1); + if (planner_data_->parameters.visualize_maximum_drivable_area) { + debug_maximum_drivable_area_publisher_ = + create_publisher("~/maximum_drivable_area", 1); } bound_publisher_ = create_publisher("~/debug/bound", 1); @@ -232,8 +232,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_signal_on_swerving = declare_parameter("turn_signal_on_swerving", true); p.path_interval = declare_parameter("path_interval"); - p.visualize_drivable_area_for_shared_linestrings_lanelet = - declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true); + p.visualize_maximum_drivable_area = declare_parameter("visualize_maximum_drivable_area", true); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); @@ -655,10 +654,10 @@ void BehaviorPathPlannerNode::run() publishSceneModuleDebugMsg(); - if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { - const auto drivable_area_lines = marker_utils::createFurthestLineStringMarkerArray( - util::getDrivableAreaForAllSharedLinestringLanelets(planner_data)); - debug_drivable_area_lanelets_publisher_->publish(drivable_area_lines); + if (planner_data->parameters.visualize_maximum_drivable_area) { + const auto maximum_drivable_area = + marker_utils::createFurthestLineStringMarkerArray(util::getMaximumDrivableArea(planner_data)); + debug_maximum_drivable_area_publisher_->publish(maximum_drivable_area); } mutex_bt_.unlock(); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index d335c6c13b582..c38b54086b3fd 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1665,7 +1665,7 @@ std::shared_ptr generateCenterLinePath( // TODO(Azu) Some parts of is the same with generateCenterLinePath. Therefore it might be better if // we can refactor some of the code for better readability -lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( +lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data) { const auto & p = planner_data->parameters;