diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index 5db4b927103c0..1328f1fada9a7 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp @@ -28,15 +28,15 @@ using geometry_msgs::msg::Pose; visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); @@ -58,29 +58,32 @@ class VirtualWallMarkerCreator using create_wall_function = std::function; + const rclcpp::Time & now, const int32_t id, const double longitudinal_offset, + const std::string & ns_prefix)>; using delete_wall_function = std::function; visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const std::vector & stop_poses, const std::string & module_name, const rclcpp::Time & now, - const double longitudinal_offset = 0.0); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const std::vector & slow_down_poses, const std::string & module_name, - const rclcpp::Time & now, const double longitudinal_offset = 0.0); + const rclcpp::Time & now, const double longitudinal_offset = 0.0, + const std::string & ns_prefix = ""); visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const std::vector & dead_line_poses, const std::string & module_name, - const rclcpp::Time & now, const double longitudinal_offset = 0.0); + const rclcpp::Time & now, const double longitudinal_offset = 0.0, + const std::string & ns_prefix = ""); private: visualization_msgs::msg::MarkerArray handleVirtualWallMarker( const std::vector & poses, const std::string & module_name, const rclcpp::Time & now, create_wall_function function_create_wall_marker, delete_wall_function function_delete_wall_marker, size_t & previous_virtual_walls_nb, - const double longitudinal_offset = 0.0); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); size_t previous_stop_poses_nb_ = 0UL; size_t previous_slow_down_poses_nb_ = 0UL; diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 863a646c8c5c9..bc6a938400b4b 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -86,32 +86,35 @@ namespace motion_utils { visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) { const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); return createVirtualWallMarkerArray( - pose_with_offset, module_name, "stop_", now, id, createMarkerColor(1.0, 0.0, 0.0, 0.5)); + pose_with_offset, module_name, ns_prefix + "stop_", now, id, + createMarkerColor(1.0, 0.0, 0.0, 0.5)); } visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) { const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); return createVirtualWallMarkerArray( - pose_with_offset, module_name, "slow_down_", now, id, createMarkerColor(1.0, 1.0, 0.0, 0.5)); + pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, + createMarkerColor(1.0, 1.0, 0.0, 0.5)); } visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) { const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); return createVirtualWallMarkerArray( - pose_with_offset, module_name, "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5)); + pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, + createMarkerColor(0.0, 1.0, 0.0, 0.5)); } visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( @@ -136,7 +139,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWall const std::vector & poses, const std::string & module_name, const rclcpp::Time & now, create_wall_function function_create_wall_marker, delete_wall_function function_delete_wall_marker, size_t & previous_virtual_walls_nb, - const double longitudinal_offset) + const double longitudinal_offset, const std::string & ns_prefix) { visualization_msgs::msg::MarkerArray wall_marker; @@ -145,7 +148,8 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWall for (const auto & p : poses) { appendMarkerArray( - function_create_wall_marker(p, module_name, now, id++, longitudinal_offset), &wall_marker); + function_create_wall_marker(p, module_name, now, id++, longitudinal_offset, ns_prefix), + &wall_marker); } while (id < max_id) { @@ -158,37 +162,38 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWall visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createStopVirtualWallMarker( const std::vector & stop_poses, const std::string & module_name, const rclcpp::Time & now, - const double longitudinal_offset) + const double longitudinal_offset, const std::string & ns_prefix) { create_wall_function creator = motion_utils::createStopVirtualWallMarker; delete_wall_function deleter = motion_utils::createDeletedStopVirtualWallMarker; return handleVirtualWallMarker( - stop_poses, module_name, now, creator, deleter, previous_stop_poses_nb_, longitudinal_offset); + stop_poses, module_name, now, creator, deleter, previous_stop_poses_nb_, longitudinal_offset, + ns_prefix); } visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createSlowDownVirtualWallMarker( const std::vector & slow_down_poses, const std::string & module_name, - const rclcpp::Time & now, const double longitudinal_offset) + const rclcpp::Time & now, const double longitudinal_offset, const std::string & ns_prefix) { create_wall_function creator = motion_utils::createSlowDownVirtualWallMarker; delete_wall_function deleter = motion_utils::createDeletedSlowDownVirtualWallMarker; return handleVirtualWallMarker( slow_down_poses, module_name, now, creator, deleter, previous_slow_down_poses_nb_, - longitudinal_offset); + longitudinal_offset, ns_prefix); } visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createDeadLineVirtualWallMarker( const std::vector & dead_line_poses, const std::string & module_name, - const rclcpp::Time & now, const double longitudinal_offset) + const rclcpp::Time & now, const double longitudinal_offset, const std::string & ns_prefix) { create_wall_function creator = motion_utils::createDeadLineVirtualWallMarker; delete_wall_function deleter = motion_utils::createDeletedDeadLineVirtualWallMarker; return handleVirtualWallMarker( dead_line_poses, module_name, now, creator, deleter, previous_dead_line_poses_nb_, - longitudinal_offset); + longitudinal_offset, ns_prefix); } } // namespace motion_utils diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index b435e5e4f589f..78e47edb2cf48 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -526,6 +526,11 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d RCLCPP_INFO_SKIPFIRST_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "%s", s); }; + // if current operation mode is not autonomous mode, then change state to stopped + if (m_current_operation_mode.mode != OperationModeState::AUTONOMOUS) { + return changeState(ControlState::STOPPED); + } + // transit state // in DRIVE state if (m_control_state == ControlState::DRIVE) { diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 76a3305390da8..725c4fad19c48 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -19,7 +19,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 60c9609827589..5ad164d43e98c 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -26,7 +26,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index ddacff7f38f47..6150f48ba3ab3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -27,7 +27,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index ccc964d6de5ce..a86cfca405a7f 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -35,7 +35,7 @@ - + diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index bd33e66174429..ccc12518c7d2f 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: lane_change: - lane_changing_safety_check_duration: 8.0 # [s] + backward_lane_length: 200.0 #[m] prepare_duration: 4.0 # [s] backward_length_buffer_for_end_of_lane: 3.0 # [m] diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index d927546089159..d68eea697eefa 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -2,50 +2,54 @@ ## Purpose / Role -Search for a space where there are no objects and goal planner there. +Plan path around the goal. + +- Park at the designated goal. +- Modify the goal to avoid obstacles or to pull over at the side of tha lane. ## Design +If goal modification is not allowed, park at the designated fixed goal. (`fixed_goal_planner` in the figure below) +When allowed, park in accordance with the specified policy(e.g pull over on left/right side of the lane). (`rough_goal_planner` in the figure below). Currently rough goal planner only support pull_over feature, but it would be desirable to be able to accommodate various parking policies in the future. + ```plantuml @startuml package goal_planner{ - abstract class PullOverPlannerBase { - } - abstract class GoalSeacherBase { - } - package lane_parking <>{ - class ShiftPullOver { - } - class GeometricPullOver { + class GoalPlannerModule {} + + package rough_goal_planner <>{ + + package lane_parking <>{ + class ShiftPullOver {} + class GeometricPullOver {} } - } - package freespace_parking <>{ - class FreeSpacePullOver { + package freespace_parking <>{ + class FreeSpacePullOver {} } - } - class GoalSeacher { - } + class GoalSeacher {} - class GoalPlannerModule { - } + struct GoalCandidates {} + struct PullOverPath{} + abstract class PullOverPlannerBase {} + abstract class GoalSeacherBase {} - struct GoalCandidates { } - struct PullOverPath{} + package fixed_goal_planner <>{ + abstract class FixedGoalPlannerBase {} + class DefaultFixedPlanner{} + } } package utils{ - class PathShifter { - } + class PathShifter {} - class GeometricParallelParking { - } + class GeometricParallelParking {} } package freespace_planning_algorithms @@ -59,6 +63,7 @@ ShiftPullOver --|> PullOverPlannerBase GeometricPullOver --|> PullOverPlannerBase FreeSpacePullOver --|> PullOverPlannerBase GoalSeacher --|> GoalSeacherBase +DefaultFixedPlanner --|> FixedGoalPlannerBase PathShifter --o ShiftPullOver GeometricParallelParking --o GeometricPullOver @@ -67,6 +72,7 @@ RRTStar --o FreeSpacePullOver PullOverPlannerBase --o GoalPlannerModule GoalSeacherBase --o GoalPlannerModule +FixedGoalPlannerBase --o GoalPlannerModule PullOverPath --o PullOverPlannerBase GoalCandidates --o GoalSeacherBase @@ -74,11 +80,47 @@ GoalCandidates --o GoalSeacherBase @enduml ``` +## start condition + +Either one is activated when all conditions are met. + +### fixed_goal_planner + +- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- Route is set with `allow_goal_modification=false` by default. + + + +### rough_goal_planner + +#### pull over on road lane + +- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- Route is set with `allow_goal_modification=true` . + - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service. + - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz. +- ego-vehicle is in the same lane as the goal. + + + +#### pull over on shoulder lane + +- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- Goal is set in the `road_shoulder`. + + + +## finish condition + +- The distance to the goal from your vehicle is lower than threshold (default: < `1m`). +- The ego-vehicle is stopped. + - The speed is lower than threshold (default: < `0.01m/s`). + ## General parameters for goal_planner | Name | Unit | Type | Description | Default value | | :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 200.0 | +| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 100.0 | | th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | | th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | | th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md index 9e11f0b7eaf5a..61ce21d514623 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -13,15 +13,16 @@ The manager launches and executes scene modules in `behavior_path_planner` depen Support status: -| Name | Simple exclusive execution | Advanced simultaneous execution | -| :----------------------- | :------------------------: | :-----------------------------: | -| Avoidance | :heavy_check_mark: | :heavy_check_mark: | -| Avoidance By Lane Change | :heavy_check_mark: | :heavy_multiplication_x: | -| Lane Change | :heavy_check_mark: | :heavy_check_mark: | -| External Lane Change | :heavy_check_mark: | :heavy_multiplication_x: | -| Pull Over | :heavy_check_mark: | :heavy_multiplication_x: | -| Pull Out | :heavy_check_mark: | :heavy_multiplication_x: | -| Side Shift | :heavy_check_mark: | :heavy_multiplication_x: | +| Name | Simple exclusive execution | Advanced simultaneous execution | +| :--------------------------------------- | :------------------------: | :-----------------------------: | +| Avoidance | :heavy_check_mark: | :heavy_check_mark: | +| Avoidance By Lane Change | :heavy_check_mark: | :heavy_multiplication_x: | +| Lane Change | :heavy_check_mark: | :heavy_check_mark: | +| External Lane Change | :heavy_check_mark: | :heavy_multiplication_x: | +| Goal Planner (without goal modification) | :heavy_check_mark: | :heavy_check_mark: | +| Goal Planner (with goal modification) | :heavy_check_mark: | :heavy_multiplication_x: | +| Pull Out | :heavy_check_mark: | :heavy_check_mark: | +| Side Shift | :heavy_check_mark: | :heavy_multiplication_x: | Click [here](../README.md) for supported scene modules. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 07b8e86d34719..365a3a5021cf4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -238,9 +238,6 @@ class LaneChangeBase PathWithLaneId prev_approved_path_{}; - double lane_change_lane_length_{200.0}; - double check_length_{100.0}; - bool is_abort_path_approved_{false}; bool is_abort_approval_requested_{false}; bool is_activated_{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index 412bab01d6b9c..d596fde7d8b9c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -74,6 +74,7 @@ class SceneModuleManagerInterface SceneModulePtr getNewModule() { if (idling_module_ != nullptr) { + idling_module_->onEntry(); return idling_module_; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index e81f8821b25ed..28160d9000be3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -28,6 +28,7 @@ namespace behavior_path_planner struct LaneChangeParameters { // trajectory generation + double backward_lane_length{200.0}; double lane_change_finish_judge_buffer{3.0}; double prediction_time_resolution{0.5}; int lane_change_sampling_num{10}; 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 2a3c0c2cdbe6b..40ea8dc6f5240 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -709,6 +709,7 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() const auto parameter = [](std::string && name) { return "lane_change." + name; }; // trajectory generation + p.backward_lane_length = declare_parameter(parameter("backward_lane_length")); p.lane_change_finish_judge_buffer = declare_parameter(parameter("lane_change_finish_judge_buffer")); p.prediction_time_resolution = declare_parameter(parameter("prediction_time_resolution")); @@ -1460,9 +1461,9 @@ bool BehaviorPathPlannerNode::keepInputPoints( const std::vector> & statuses) const { #ifdef USE_OLD_ARCHITECTURE - const std::vector target_modules = {"PullOver", "Avoidance"}; + const std::vector target_modules = {"GoalPlanner", "Avoidance"}; #else - const std::vector target_modules = {"pull_over", "avoidance"}; + const std::vector target_modules = {"goal_planner", "avoidance"}; #endif const auto target_status = ModuleStatus::RUNNING; 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 6eec7ca3045b4..736f921f68285 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 @@ -106,6 +106,28 @@ void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Po marker_array.markers.push_back(marker); } + +void appendExtractedPolygonMarker( + MarkerArray & marker_array, const tier4_autoware_utils::Polygon2d & obj_poly) +{ + 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)); + + // NOTE: obj_poly.outer() has already duplicated points to close the polygon. + for (size_t i = 0; i < obj_poly.outer().size(); ++i) { + const auto & bound_point = obj_poly.outer().at(i); + + geometry_msgs::msg::Point bound_geom_point; + bound_geom_point.x = bound_point.x(); + bound_geom_point.y = bound_point.y(); + marker.points.push_back(bound_geom_point); + } + + marker_array.markers.push_back(marker); +} } // namespace #ifdef USE_OLD_ARCHITECTURE @@ -175,6 +197,7 @@ ModuleStatus DynamicAvoidanceModule::updateState() BehaviorModuleOutput DynamicAvoidanceModule::plan() { info_marker_.markers.clear(); + debug_marker_.markers.clear(); // 1. get reference path from previous module const auto prev_module_path = getPreviousModuleOutput().path; @@ -190,6 +213,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() obstacles_for_drivable_area.push_back({object.pose, obstacle_poly.value()}); appendObjectMarker(info_marker_, object.pose); + appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value()); } } @@ -222,7 +246,7 @@ DynamicAvoidanceModule::calcTargetObjects() const const auto prev_module_path = getPreviousModuleOutput().path; // 1. calculate target lanes to filter obstacles - const auto target_lanes = getAdjacentLanes(100.0, 10.0); + const auto target_lanes = getAdjacentLanes(100.0, 50.0); // 2. filter obstacles for dynamic avoidance const auto & predicted_objects = planner_data_->dynamic_object->objects; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 03844572d09d7..9be4778751fd1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -17,7 +17,9 @@ #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/utils.hpp" +#include #include #include @@ -247,30 +249,40 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( return {}; } // Get lane change lanes - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet(current_lanes, getEgoPose(), ¤t_lane); - - const auto minimum_prepare_length = planner_data_->parameters.minimum_prepare_length; - - const auto lane_change_prepare_length = std::max( - getEgoVelocity() * planner_data_->parameters.lane_change_prepare_duration, - minimum_prepare_length); - const auto & route_handler = getRouteHandler(); - const auto current_check_lanes = - route_handler->getLaneletSequence(current_lane, getEgoPose(), 0.0, lane_change_prepare_length); - const auto lane_change_lane = utils::lane_change::getLaneChangeTargetLane( *getRouteHandler(), current_lanes, type_, direction); - const auto lane_change_lane_length = std::max(lane_change_lane_length_, getEgoVelocity() * 10.0); - if (lane_change_lane) { - return route_handler->getLaneletSequence( - lane_change_lane.get(), getEgoPose(), lane_change_lane_length, lane_change_lane_length); + if (!lane_change_lane) { + return {}; } - return {}; + const auto front_pose = std::invoke([&lane_change_lane]() { + const auto & p = lane_change_lane->centerline().front(); + const auto front_point = lanelet::utils::conversion::toGeomMsgPt(p); + const auto front_yaw = lanelet::utils::getLaneletAngle(*lane_change_lane, front_point); + geometry_msgs::msg::Pose front_pose; + front_pose.position = front_point; + tf2::Quaternion quat; + quat.setRPY(0, 0, front_yaw); + front_pose.orientation = tf2::toMsg(quat); + return front_pose; + }); + + const auto forward_length = std::invoke([&]() { + const auto signed_distance = utils::getSignedDistance(front_pose, getEgoPose(), current_lanes); + const auto forward_path_length = planner_data_->parameters.forward_path_length; + if (signed_distance <= 0.0) { + return forward_path_length; + } + + return signed_distance + forward_path_length; + }); + const auto backward_length = lane_change_parameters_->backward_lane_length; + + return route_handler->getLaneletSequence( + lane_change_lane.get(), getEgoPose(), backward_length, forward_length); } int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const @@ -498,9 +510,10 @@ bool NormalLaneChange::getLaneChangePaths( if (candidate_paths->empty()) { // only compute dynamic object indices once + const auto backward_length = lane_change_parameters_->backward_lane_length; const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets( - route_handler, target_lanelets, getEgoPose(), check_length_); + route_handler, target_lanelets, getEgoPose(), backward_length); dynamic_object_indices = utils::lane_change::filterObjectIndices( {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, getEgoPose(), common_parameter.forward_path_length, *lane_change_parameters_, @@ -541,7 +554,8 @@ bool NormalLaneChange::isApprovedPathSafe(Pose & ego_pose_before_collision) cons // get lanes used for detection const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets( - *route_handler, path.target_lanelets, current_pose, check_length_); + *route_handler, path.target_lanelets, current_pose, + lane_change_parameters.backward_lane_length); CollisionCheckDebugMap debug_data; const auto lateral_buffer = diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 1fbdd6b542394..4babccc812dbe 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -169,14 +169,49 @@ boost::optional> intersectBound( return boost::none; } +double calcDistanceFromPointToSegment( + const geometry_msgs::msg::Point & segment_start_point, + const geometry_msgs::msg::Point & segment_end_point, + const geometry_msgs::msg::Point & target_point) +{ + const auto & a = segment_start_point; + const auto & b = segment_end_point; + const auto & p = target_point; + + const double dot_val = (b.x - a.x) * (p.x - a.x) + (b.y - a.y) * (p.y - a.y); + const double squared_segment_length = tier4_autoware_utils::calcSquaredDistance2d(a, b); + if (0 <= dot_val && dot_val <= squared_segment_length) { + const double numerator = std::abs((p.x - a.x) * (a.y - b.y) - (p.y - a.y) * (a.x - b.x)); + const double denominator = std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2)); + return numerator / denominator; + } + + // target_point is outside the segment. + return std::min( + tier4_autoware_utils::calcDistance2d(a, p), tier4_autoware_utils::calcDistance2d(b, p)); +} + PolygonPoint transformBoundFrenetCoordinate( - const std::vector & points, const geometry_msgs::msg::Point & point) + const std::vector & bound_points, + const geometry_msgs::msg::Point & target_point) { - const size_t seg_idx = motion_utils::findNearestSegmentIndex(points, point); + // NOTE: findNearestSegmentIndex cannot be used since a bound's interval is sometimes too large to + // find wrong nearest index. + std::vector dist_to_bound_segment_vec; + for (size_t i = 0; i < bound_points.size() - 1; ++i) { + const double dist_to_bound_segment = + calcDistanceFromPointToSegment(bound_points.at(i), bound_points.at(i + 1), target_point); + dist_to_bound_segment_vec.push_back(dist_to_bound_segment); + } + + const size_t min_dist_seg_idx = std::distance( + dist_to_bound_segment_vec.begin(), + std::min_element(dist_to_bound_segment_vec.begin(), dist_to_bound_segment_vec.end())); const double lon_dist_to_segment = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, point); - const double lat_dist = motion_utils::calcLateralOffset(points, point, seg_idx); - return PolygonPoint{point, seg_idx, lon_dist_to_segment, lat_dist}; + motion_utils::calcLongitudinalOffsetToSegment(bound_points, min_dist_seg_idx, target_point); + const double lat_dist_to_segment = + motion_utils::calcLateralOffset(bound_points, target_point, min_dist_seg_idx); + return PolygonPoint{target_point, min_dist_seg_idx, lon_dist_to_segment, lat_dist_to_segment}; } std::vector generatePolygonInsideBounds( @@ -190,6 +225,7 @@ std::vector generatePolygonInsideBounds( } std::vector inside_poly; + bool has_intersection = false; // NOTE: between obstacle polygon and bound for (int i = 0; i < static_cast(full_polygon.size()); ++i) { const auto & curr_poly = full_polygon.at(i); const auto & prev_poly = full_polygon.at(i == 0 ? full_polygon.size() - 1 : i - 1); @@ -214,6 +250,7 @@ std::vector generatePolygonInsideBounds( bound, intersection->first, intersection->second); const auto intersect_point = PolygonPoint{intersection->second, intersection->first, lon_dist, 0.0}; + has_intersection = true; if (is_prev_outside && !is_curr_outside) { inside_poly.push_back(intersect_point); @@ -226,7 +263,10 @@ std::vector generatePolygonInsideBounds( continue; } - return inside_poly; + if (has_intersection) { + return inside_poly; + } + return std::vector{}; } std::vector convertToGeometryPoints( @@ -1018,7 +1058,10 @@ bool isEgoOutOfRoute( } // If ego vehicle is over goal on goal lane, return true - if (lanelet::utils::isInLanelet(self_pose, goal_lane)) { + lanelet::ConstLanelet closest_lane; + const double yaw_threshold = tier4_autoware_utils::deg2rad(90); + if (lanelet::utils::query::getClosestLaneletWithConstrains( + {goal_lane}, self_pose, &closest_lane, 0.0, yaw_threshold)) { constexpr double buffer = 1.0; const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose); const auto goal_arc_coord = @@ -2854,8 +2897,7 @@ void extractObstaclesFromDrivableArea( } } - for (size_t i = 0; i < 2; ++i) { // for loop for right and left - const bool is_object_right = (i == 0); + for (const bool is_object_right : {true, false}) { const auto & polygons = is_object_right ? right_polygons : left_polygons; if (polygons.empty()) { continue; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp index 24d6f47e97ab1..62b597df1edcf 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp @@ -232,7 +232,7 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createVirtualWallMarkerArr } appendMarkerArray( virtual_wall_marker_creator_crosswalk_->createStopVirtualWallMarker( - stop_poses, "crosswalk", now), + stop_poses, "crosswalk", now, 0.0, std::to_string(module_id_) + "_"), &wall_marker); for (const auto & p : debug_data_.slow_poses) { const auto p_front = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); @@ -240,7 +240,7 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createVirtualWallMarkerArr } appendMarkerArray( virtual_wall_marker_creator_crosswalk_->createSlowDownVirtualWallMarker( - slow_down_poses, "crosswalk", now), + slow_down_poses, "crosswalk", now, 0.0, std::to_string(module_id_) + "_"), &wall_marker); return wall_marker; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index 0723d786f0d40..ed62e8c6e7350 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -207,19 +207,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker if (!activated_) { appendMarkerArray( virtual_wall_marker_creator_->createStopVirtualWallMarker( - {debug_data_.collision_stop_wall_pose}, "intersection", now), + {debug_data_.collision_stop_wall_pose}, "intersection", now, 0.0, + "intersection" + std::to_string(module_id_) + "_"), &wall_marker, now); } if (!activated_ && occlusion_first_stop_required_) { appendMarkerArray( virtual_wall_marker_creator_->createStopVirtualWallMarker( - {debug_data_.occlusion_first_stop_wall_pose}, "intersection", now), + {debug_data_.occlusion_first_stop_wall_pose}, "intersection", now, 0.0, + "intersection_occlusion_first_stop" + std::to_string(module_id_) + "_"), &wall_marker, now); } if (!occlusion_activated_) { appendMarkerArray( virtual_wall_marker_creator_->createStopVirtualWallMarker( - {debug_data_.occlusion_stop_wall_pose}, "intersection_occlusion", now), + {debug_data_.occlusion_stop_wall_pose}, "intersection_occlusion", now, 0.0, + "intersection_occlusion" + std::to_string(module_id_) + "_"), &wall_marker, now); } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 63ccab4ccb950..80ead93b5a77b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -328,7 +328,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_line_idx = occlusion_first_stop_line_idx; // insert creep velocity [default_stop_line, occlusion_stop_line) insert_creep_during_occlusion = - std::make_pair(default_stop_line_idx_opt.value(), occlusion_peeking_line_idx_opt.value()); + default_stop_line_idx_opt && occlusion_peeking_line_idx_opt + ? std::make_optional>( + default_stop_line_idx_opt.value(), occlusion_peeking_line_idx_opt.value()) + : std::nullopt; occlusion_state_ = OcclusionState::BEFORE_FIRST_STOP_LINE; } } else if (occlusion_state_ != OcclusionState::CLEARED) { diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index e982e2b106fcf..17a47be8c0596 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -196,7 +196,7 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio void SurroundObstacleCheckerNode::onTimer() { if (!odometry_ptr_) { - RCLCPP_WARN_THROTTLE( + RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current velocity..."); return; } @@ -206,13 +206,13 @@ void SurroundObstacleCheckerNode::onTimer() } if (node_param_.use_pointcloud && !pointcloud_ptr_) { - RCLCPP_WARN_THROTTLE( + RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for pointcloud info..."); return; } if (node_param_.use_dynamic_object && !object_ptr_) { - RCLCPP_WARN_THROTTLE( + RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic object info..."); return; } @@ -333,6 +333,9 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacle() cons boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCloud() const { + if (pointcloud_ptr_->data.empty()) { + return boost::none; + } const auto transform_stamped = getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5);