diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp index ee642c93a378f..2b89fc19d1878 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp @@ -40,6 +40,11 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", const bool is_driving_forward = true); +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward); + visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp index 07fcbd9840c88..fd86c54d65be9 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -29,12 +29,13 @@ namespace autoware::motion_utils { /// @brief type of virtual wall associated with different marker styles and namespace -enum VirtualWallType { stop, slowdown, deadline }; +enum VirtualWallType { stop, slowdown, deadline, pass }; /// @brief virtual wall to be visualized in rviz struct VirtualWall { geometry_msgs::msg::Pose pose{}; std::string text{}; + std::string detail{}; std::string ns{}; VirtualWallType style = stop; double longitudinal_offset{}; diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp index 91ad1c41eecc3..44b621a53c5f1 100644 --- a/common/autoware_motion_utils/src/marker/marker_helper.cpp +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -54,7 +54,7 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( { auto marker = createDefaultMarker( "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + createMarkerScale(0.0, 0.0, 1.0 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); marker.pose = vehicle_front_pose; marker.pose.position.z += 2.0; @@ -85,6 +85,41 @@ inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( return marker_array; } + +inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray( + const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, + const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // Arrow + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "direction", id, visualization_msgs::msg::Marker::ARROW, + createMarkerScale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color); + + marker.pose = vehicle_front_pose; + + marker_array.markers.push_back(marker); + } + + // Factor Text + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.4 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 2.0; + marker.text = module_name; + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + } // namespace namespace autoware::motion_utils @@ -125,6 +160,18 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( createMarkerColor(0.0, 1.0, 0.0, 0.5)); } +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) +{ + const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + return createIntendedPassArrowMarkerArray( + pose_with_offset, module_name, ns_prefix + "intended_pass_", now, id, + createMarkerColor(0.77, 0.77, 0.77, 0.5)); +} + visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id) { diff --git a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp index 4fecaea1bb838..fb8708b5b7baa 100644 --- a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -63,10 +63,16 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( case deadline: create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker; break; + case pass: + create_fn = autoware::motion_utils::createIntendedPassVirtualMarker; + break; } + const auto wall_text = virtual_wall.detail.empty() + ? virtual_wall.text + : virtual_wall.text + "(" + virtual_wall.detail + ")"; auto markers = create_fn( - virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, - virtual_wall.ns, virtual_wall.is_driving_forward); + virtual_wall.pose, wall_text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns, + virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { marker.id = static_cast(marker_count_per_namespace_[marker.ns].current++); marker_array.markers.push_back(marker); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index 44dca13ccfd66..b7b4ad83de362 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -29,8 +29,12 @@ GeometricPullOver::GeometricPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward) : PullOverPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, - lane_departure_checker_{ - lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_}, + lane_departure_checker_{[&]() { + auto lane_departure_checker_params = lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; + }()}, is_forward_{is_forward}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 8350252369cc6..c8e0788e39b6f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -32,8 +32,12 @@ namespace autoware::behavior_path_planner { ShiftPullOver::ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) : PullOverPlannerBase{node, parameters}, - lane_departure_checker_{ - lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, vehicle_info_}, + lane_departure_checker_{[&]() { + auto lane_departure_checker_params = lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; + }()}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 20fd564049133..601fb893f82c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -1149,7 +1149,8 @@ The following parameters are used to configure terminal lane change path feature | `collision_check.check_other_lanes` | [-] | boolean | If true, the lane change module includes objects in other lanes when performing collision assessment. | false | | `collision_check.use_all_predicted_paths` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | | `collision_check.prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | +| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between predicted ego pose and predicted object pose when executing rss-based collision checking | 3.1416 | +| `collision_check.th_incoming_object_yaw` | [rad] | double | Maximum yaw difference between current ego pose and current object pose. Objects with a yaw difference exceeding this value are excluded from the safety check. | 2.3562 | #### safety constraints during lane change path is computed @@ -1162,6 +1163,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | | `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.execution.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.execution.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | #### safety constraints specifically for stopped or parked vehicles @@ -1174,6 +1176,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.parked.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | | `safety_check.parked.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.parked.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.parked.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | ##### safety constraints to cancel lane change path @@ -1186,6 +1189,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | | `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 2.5 | | `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.6 | +| `safety_check.cancel.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | ##### safety constraints used during lane change path is computed when ego is stuck @@ -1198,6 +1202,7 @@ The following parameters are used to configure terminal lane change path feature | `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | | `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | | `safety_check.stuck.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | +| `safety_check.stuck.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | (\*1) the value must be negative. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 15eb23daecf95..71fac1246598f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -58,6 +58,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" parked: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 @@ -66,6 +67,7 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" cancel: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 @@ -74,6 +76,7 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 2.5 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" stuck: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 @@ -82,6 +85,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" # lane expansion for object filtering lane_expansion: @@ -123,7 +127,8 @@ intersection: true turns: true prediction_time_resolution: 0.5 - yaw_diff_threshold: 3.1416 + th_incoming_object_yaw: 2.3562 # [rad] + yaw_diff_threshold: 3.1416 # [rad] check_current_lanes: false check_other_lanes: false use_all_predicted_paths: false diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index 60386f535fc64..358f60f3193d5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -94,6 +94,7 @@ struct CollisionCheckParameters bool check_current_lane{true}; bool check_other_lanes{true}; bool use_all_predicted_paths{false}; + double th_incoming_object_yaw{2.3562}; double th_yaw_diff{3.1416}; double prediction_time_resolution{0.5}; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 450497fb2ca29..589338d7b3ace 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -152,6 +152,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("collision_check.prediction_time_resolution")); p.safety.collision_check.th_yaw_diff = getOrDeclareParameter(*node, parameter("collision_check.yaw_diff_threshold")); + p.safety.collision_check.th_incoming_object_yaw = + getOrDeclareParameter(*node, parameter("collision_check.th_incoming_object_yaw")); // rss check auto set_rss_params = [&](auto & params, const std::string & prefix) { @@ -169,6 +171,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s *node, parameter(prefix + ".rear_vehicle_safety_time_margin")); params.lateral_distance_max_threshold = getOrDeclareParameter(*node, parameter(prefix + ".lateral_distance_max_threshold")); + params.extended_polygon_policy = + getOrDeclareParameter(*node, parameter(prefix + ".extended_polygon_policy")); }; set_rss_params(p.safety.rss_params, "safety_check.execution"); set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked"); @@ -454,6 +458,19 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorsafety.collision_check.prediction_time_resolution); updateParam( parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff); + + auto th_incoming_object_yaw = p->safety.collision_check.th_incoming_object_yaw; + updateParam(parameters, ns + "th_incoming_object_yaw", th_incoming_object_yaw); + if (th_incoming_object_yaw >= M_PI_2) { + p->safety.collision_check.th_incoming_object_yaw = th_incoming_object_yaw; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, + "The value of th_incoming_object_yaw (%.3f rad) is less than the minimum possible value " + "(%.3f " + "rad).", + th_incoming_object_yaw, M_PI_2); + } } { @@ -483,7 +500,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "stop_time", p->th_stop_time); } - auto update_rss_params = [¶meters](const std::string & prefix, auto & params) { + auto update_rss_params = [¶meters, this](const std::string & prefix, auto & params) { using autoware::universe_utils::updateParam; updateParam( parameters, prefix + "longitudinal_distance_min_threshold", @@ -502,6 +519,19 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( parameters, prefix + "lateral_distance_max_threshold", params.lateral_distance_max_threshold); + + auto extended_polygon_policy = params.extended_polygon_policy; + updateParam( + parameters, prefix + "extended_polygon_policy", extended_polygon_policy); + if (extended_polygon_policy == "rectangle" || extended_polygon_policy == "along_path") { + params.extended_polygon_policy = extended_polygon_policy; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Policy %s not supported or there's typo. Make sure you choose either 'rectangle' or " + "'along_path'", + extended_polygon_policy.c_str()); + } }; update_rss_params("lane_change.safety_check.execution.", p->safety.rss_params); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 45fcd6fa99086..611a0b60a4366 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -1051,7 +1051,9 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const const auto is_same_direction = [&](const PredictedObject & object) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - return !utils::path_safety_checker::isTargetObjectOncoming(current_pose, object_pose); + return !utils::path_safety_checker::isTargetObjectOncoming( + current_pose, object_pose, + common_data_ptr_->lc_param_ptr->safety.collision_check.th_incoming_object_yaw); }; // Perception noise could make stationary objects seem opposite the ego vehicle; check the @@ -1792,7 +1794,6 @@ bool NormalLaneChange::is_colliding( constexpr auto is_safe{true}; auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); - constexpr auto collision_check_yaw_diff_threshold{M_PI}; constexpr auto hysteresis_factor{1.0}; const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->safety.collision_check.use_all_predicted_paths); @@ -1817,7 +1818,8 @@ bool NormalLaneChange::is_colliding( const auto collided_polygons = utils::path_safety_checker::get_collided_polygons( lane_change_path.path, ego_predicted_path, obj, predicted_obj_path, bpp_param.vehicle_info, selected_rss_param, hysteresis_factor, safety_check_max_vel, - collision_check_yaw_diff_threshold, current_debug_data.second); + common_data_ptr_->lc_param_ptr->safety.collision_check.th_yaw_diff, + current_debug_data.second); if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index 69c5c41405932..ed09e0c0a33d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -40,10 +40,12 @@ GeometricPullOut::GeometricPullOut( : PullOutPlannerBase{node, parameters, time_keeper}, parallel_parking_parameters_{parameters.parallel_parking_parameters} { + auto lane_departure_checker_params = autoware::lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; lane_departure_checker_ = std::make_shared( - autoware::lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, - vehicle_info_, time_keeper_); + lane_departure_checker_params, vehicle_info_); planner_.setParameters(parallel_parking_parameters_); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 8f4de1d3636de..16a3d4ffd842a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -44,10 +44,12 @@ ShiftPullOut::ShiftPullOut( std::shared_ptr time_keeper) : PullOutPlannerBase{node, parameters, time_keeper} { + autoware::lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; lane_departure_checker_ = std::make_shared( - autoware::lane_departure_checker::Param{parameters.lane_departure_check_expansion_margin}, - vehicle_info_, time_keeper_); + lane_departure_checker_params, vehicle_info_, time_keeper_); } std::optional ShiftPullOut::plan( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 59f6f20937f41..7e6a320f3ddad 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -46,7 +46,6 @@ DetectionAreaModule::DetectionAreaModule( planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA); } bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) @@ -180,9 +179,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); } return true; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 2d5da0f070ed9..5b8aca1c8ec9d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -44,8 +44,6 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using autoware::motion_utils::VelocityFactorInterface; - IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index f32210bc502bf..0e646e92035cf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -38,7 +38,6 @@ NoDrivableLaneModule::NoDrivableLaneModule( debug_data_(), state_(State::INIT) { - velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE); } bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path) @@ -166,8 +165,10 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = op_stop_pose.value(); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -214,8 +215,10 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId * // Get stop point and stop factor { const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0)); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto & virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -252,8 +255,10 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = ego_pos_on_path.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index bcb8b365f6205..c788a54073ed6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -50,7 +50,6 @@ NoStoppingAreaModule::NoStoppingAreaModule( planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::NO_STOPPING_AREA); state_machine_.setState(StateMachine::State::GO); state_machine_.setMarginTime(planner_param_.state_clear_time); } @@ -144,9 +143,11 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_point->second, stop_point->second, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); } } else if (state_machine_.getState() == StateMachine::State::GO) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 1163b00e08ccb..fa259b2bea9aa 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -70,8 +70,6 @@ OcclusionSpotModule::OcclusionSpotModule( : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), param_(planner_param) { - velocity_factor_.init(PlanningBehavior::UNKNOWN); - if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; //! occupancy grid limitation( 100 * 100 ) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index ad1ae1d47664a..5bff3ca343e72 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -28,8 +27,6 @@ #include #include -#include -#include #include #include #include @@ -51,8 +48,6 @@ namespace autoware::behavior_velocity_planner { -using autoware::motion_utils::PlanningBehavior; -using autoware::motion_utils::VelocityFactor; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::universe_utils::DebugPublisher; @@ -97,8 +92,6 @@ class SceneModuleInterface planner_data_ = planner_data; } - void resetVelocityFactor() { velocity_factor_.reset(); } - VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } std::vector getObjectsOfInterestData() const { return objects_of_interest_; } void clearObjectsOfInterestData() { objects_of_interest_.clear(); } @@ -107,7 +100,6 @@ class SceneModuleInterface rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - autoware::motion_utils::VelocityFactorInterface velocity_factor_; // TODO(soblin): remove this std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; std::shared_ptr planning_factor_interface_; @@ -143,8 +135,6 @@ class SceneModuleManagerInterface } pub_virtual_wall_ = node.create_publisher( std::string("~/virtual_wall/") + module_name, 5); - pub_velocity_factor_ = node.create_publisher( - std::string("/planning/velocity_factors/") + module_name, 1); planning_factor_interface_ = std::make_shared(&node, module_name); @@ -180,21 +170,12 @@ class SceneModuleManagerInterface StopWatch stop_watch; stop_watch.tic("Total"); visualization_msgs::msg::MarkerArray debug_marker_array; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); for (const auto & scene_module : scene_modules_) { - scene_module->resetVelocityFactor(); scene_module->setPlannerData(planner_data_); scene_module->modifyPathVelocity(path); // The velocity factor must be called after modifyPathVelocity. - // TODO(soblin): remove this - const auto velocity_factor = scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } for (const auto & marker : scene_module->createDebugMarkerArray().markers) { debug_marker_array.markers.push_back(marker); @@ -203,7 +184,6 @@ class SceneModuleManagerInterface virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); } - pub_velocity_factor_->publish(velocity_factor_array); planning_factor_interface_->publish(); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { @@ -274,8 +254,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; rclcpp::Publisher::SharedPtr pub_debug_path_; - rclcpp::Publisher::SharedPtr - pub_velocity_factor_; std::shared_ptr processing_time_publisher_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 71a5ea3b9e807..e2ddbc75c7b57 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -38,7 +38,6 @@ StopLineModule::StopLineModule( state_(State::APPROACH), debug_data_() { - velocity_factor_.init(PlanningBehavior::STOP_SIGN); } bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) @@ -62,7 +61,12 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) path->points = trajectory->restore(); - updateVelocityFactor(&velocity_factor_, state_, *stop_point - ego_s); + // TODO(soblin): PlanningFactorInterface use trajectory class + planning_factor_interface_->add( + path->points, trajectory->compute(*stop_point).point.pose, + planner_data_->current_odometry->pose, planner_data_->current_odometry->pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline"); updateStateAndStoppedTime( &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); @@ -152,24 +156,6 @@ void StopLineModule::updateStateAndStoppedTime( } } -void StopLineModule::updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point) -{ - switch (state) { - case State::APPROACH: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::APPROACHING); - break; - } - case State::STOPPED: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::STOPPED); - break; - } - case State::START: - break; - } -} - void StopLineModule::updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index aac830b0e9f24..7d430463c18b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -18,7 +18,6 @@ #include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" -#include "autoware/motion_utils/factor/velocity_factor_interface.hpp" #include "autoware/trajectory/path_point_with_lane_id.hpp" #include @@ -98,10 +97,6 @@ class StopLineModule : public SceneModuleInterface State * state, std::optional * stopped_time, const rclcpp::Time & now, const double & distance_to_stop_point, const bool & is_vehicle_stopped) const; - static void updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point); - void updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 9a402f31af0e4..1d1ee7fc996b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -53,24 +53,13 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat autoware_perception_msgs::msg::TrafficLightGroup tl_state; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { std::shared_ptr traffic_light_scene_module( std::dynamic_pointer_cast(scene_module)); - traffic_light_scene_module->resetVelocityFactor(); traffic_light_scene_module->setPlannerData(planner_data_); traffic_light_scene_module->modifyPathVelocity(path); - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = traffic_light_scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - if ( traffic_light_scene_module->getFirstRefStopPathPointIndex() < nearest_ref_stop_path_point_index_) { @@ -88,7 +77,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat virtual_wall_marker_creator_.add_virtual_walls( traffic_light_scene_module->createVirtualWalls()); } - pub_velocity_factor_->publish(velocity_factor_array); pub_debug_->publish(debug_marker_array); pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); pub_tl_state_->publish(tl_state); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index b051d5ff7eb76..becbaf5ee05f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -54,7 +54,6 @@ TrafficLightModule::TrafficLightModule( debug_data_(), is_prev_state_stop_(false) { - velocity_factor_.init(PlanningBehavior::TRAFFIC_SIGNAL); planner_param_ = planner_param; } @@ -293,9 +292,11 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( size_t insert_index = insert_target_point_idx; planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index); - velocity_factor_.set( + planning_factor_interface_->add( modified_path.points, planner_data_->current_odometry->pose, - target_point_with_lane_id.point.pose, VelocityFactor::UNKNOWN); + target_point_with_lane_id.point.pose, target_point_with_lane_id.point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); return modified_path; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 9ba7b0bccf1b9..a5779cfc0712a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -48,8 +48,6 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( lane_(lane), planner_param_(planner_param) { - velocity_factor_.init(PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT); - // Get map data const auto instrument = reg_elem_.getVirtualTrafficLight(); const auto instrument_bottom_line = toAutowarePoints(instrument); @@ -420,9 +418,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } // Set StopReason - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN, - command_.type); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_stop_line = @@ -453,8 +452,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( } // Set StopReason - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_end_line = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 9e7a1192d9869..0661267b3d361 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -113,10 +113,6 @@ class VirtualTrafficLightModule : public SceneModuleInterface void updateInfrastructureCommand(); - void setVelocityFactor( - const geometry_msgs::msg::Pose & stop_pose, - autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor); - std::optional getPathIndexOfFirstEndLine(); bool isBeforeStartLine(const size_t end_line_idx);