diff --git a/common/autoware_ad_api_specs/README.md b/common/autoware_ad_api_specs/README.md new file mode 100644 index 0000000000000..6683bcdb3a51d --- /dev/null +++ b/common/autoware_ad_api_specs/README.md @@ -0,0 +1,3 @@ +# autoware_adapi_specs + +This package is a specification of Autoware AD API. diff --git a/common/autoware_auto_common/package.xml b/common/autoware_auto_common/package.xml index 59e2907e91501..10524876f749d 100644 --- a/common/autoware_auto_common/package.xml +++ b/common/autoware_auto_common/package.xml @@ -5,6 +5,9 @@ 1.0.0 Miscellaneous helper functions Apex.AI, Inc. + Tomoya Kimura + Shumpei Wakabayashi + Satoshi Ota Apache License 2.0 ament_cmake_auto diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml index c7b620a9a737e..b84e2d77befda 100644 --- a/common/autoware_auto_tf2/package.xml +++ b/common/autoware_auto_tf2/package.xml @@ -5,6 +5,9 @@ 1.0.0 Transform related utilities for different msg types Jit Ray Chowdhury + Tomoya Kimura + Shumpei Wakabayashi + Satoshi Ota Apache License 2.0 ament_cmake diff --git a/common/autoware_testing/package.xml b/common/autoware_testing/package.xml index 769942b437884..8ceb64431870c 100644 --- a/common/autoware_testing/package.xml +++ b/common/autoware_testing/package.xml @@ -5,6 +5,9 @@ 0.1.0 Tools for handling standard tests based on ros_testing Adam Dabrowski + Tomoya Kimura + Shumpei Wakabayashi + Satoshi Ota Apache 2.0 ament_cmake_auto diff --git a/common/component_interface_specs/README.md b/common/component_interface_specs/README.md new file mode 100644 index 0000000000000..b9fcd83a29dc0 --- /dev/null +++ b/common/component_interface_specs/README.md @@ -0,0 +1,3 @@ +# component_interface_specs + +This package is a specification of component interfaces. diff --git a/common/component_interface_tools/README.md b/common/component_interface_tools/README.md new file mode 100644 index 0000000000000..630483acc1d9e --- /dev/null +++ b/common/component_interface_tools/README.md @@ -0,0 +1,7 @@ +# component_interface_tools + +This package provides the following tools for component interface. + +## service_log_checker + +Monitor the service log of component_interface_utils and display if the response status is an error. diff --git a/common/fake_test_node/package.xml b/common/fake_test_node/package.xml index 78d53831670d8..b3332a57a7a4d 100644 --- a/common/fake_test_node/package.xml +++ b/common/fake_test_node/package.xml @@ -5,6 +5,9 @@ 1.0.0 A fake node that we can use in the integration-like cpp tests. Apex.AI, Inc. + Tomoya Kimura + Shumpei Wakabayashi + Satoshi Ota Apache 2.0 ament_cmake_auto diff --git a/common/tier4_api_utils/README.md b/common/tier4_api_utils/README.md new file mode 100644 index 0000000000000..3af88bc32d619 --- /dev/null +++ b/common/tier4_api_utils/README.md @@ -0,0 +1,4 @@ +# tier4_api_utils + +This is an old implementation of a class that logs when calling a service. +Please use [component_interface_utils](../component_interface_utils/README.md) instead. diff --git a/common/time_utils/package.xml b/common/time_utils/package.xml index 00832c1fd190a..c4ad8f098a0e0 100644 --- a/common/time_utils/package.xml +++ b/common/time_utils/package.xml @@ -5,6 +5,8 @@ 1.0.0 Simple conversion methods to/from std::chrono to simplify algorithm development Christopher Ho + Tomoya Kimura + Shumpei Wakabayashi Apache License 2.0 ament_cmake_auto diff --git a/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg b/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg index 57e26b075506c..1b8ecff2c59fe 100644 --- a/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg +++ b/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg @@ -1,8 +1,9 @@ builtin_interfaces/Time stamp # states -string previous_state -string current_state +string status +bool in_autoware_control +bool in_transition # flags for engage permission bool is_all_ok diff --git a/control/operation_mode_transition_manager/src/node.cpp b/control/operation_mode_transition_manager/src/node.cpp index c54f9c1b76f43..9c268102d7b67 100644 --- a/control/operation_mode_transition_manager/src/node.cpp +++ b/control/operation_mode_transition_manager/src/node.cpp @@ -270,10 +270,19 @@ void OperationModeTransitionManager::publishData() pub_operation_mode_->publish(state); } + const auto status_str = [&]() { + if (!current_control) return "DISENGAGE (autoware mode = " + toString(current_mode_) + ")"; + if (transition_) + return toString(current_mode_) + " (in transition from " + toString(transition_->previous) + + ")"; + return toString(current_mode_); + }(); + ModeChangeBase::DebugInfo debug_info = modes_.at(OperationMode::AUTONOMOUS)->getDebugInfo(); debug_info.stamp = time; - debug_info.current_state = toString(current_mode_); - debug_info.previous_state = transition_ ? toString(transition_->previous) : "NONE"; + debug_info.status = status_str; + debug_info.in_autoware_control = current_control; + debug_info.in_transition = transition_ ? true : false; pub_debug_info_->publish(debug_info); } diff --git a/control/pure_pursuit/README.md b/control/pure_pursuit/README.md new file mode 100644 index 0000000000000..48e2a13ef664d --- /dev/null +++ b/control/pure_pursuit/README.md @@ -0,0 +1,19 @@ +# Pure Pursuit Controller + +The Pure Pursuit Controller module calculates the steering angle for tracking a desired trajectory using the pure pursuit algorithm. This is used as a lateral controller plugin in the `trajectory_follower_node`. + +## Inputs + +Set the following from the [controller_node](../trajectory_follower_node/README.md) + +- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current ego pose and velocity information + +## Outputs + +Return LateralOutput which contains the following to the controller node + +- `autoware_auto_control_msgs/AckermannLateralCommand`: target steering angle +- LateralSyncData + - steer angle convergence +- `autoware_auto_planning_msgs/Trajectory`: predicted path for ego vehicle diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp index d260376e55aa2..a6b9938e404f6 100644 --- a/control/shift_decider/include/shift_decider/shift_decider.hpp +++ b/control/shift_decider/include/shift_decider/shift_decider.hpp @@ -50,6 +50,7 @@ class ShiftDecider : public rclcpp::Node autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_; autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_; autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_; + uint8_t prev_shift_command = autoware_auto_vehicle_msgs::msg::GearCommand::PARK; bool park_on_goal_; }; diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp index 6f24578bf8d8e..0e47cc7378f27 100644 --- a/control/shift_decider/src/shift_decider.cpp +++ b/control/shift_decider/src/shift_decider.cpp @@ -83,7 +83,7 @@ void ShiftDecider::updateCurrentShiftCmd() } else if (control_cmd_->longitudinal.speed < -vel_threshold) { shift_cmd_.command = GearCommand::REVERSE; } else { - shift_cmd_.command = current_gear_ptr_->report; + shift_cmd_.command = prev_shift_command; } } else { if ( @@ -95,6 +95,7 @@ void ShiftDecider::updateCurrentShiftCmd() shift_cmd_.command = current_gear_ptr_->report; } } + prev_shift_command = shift_cmd_.command; } void ShiftDecider::initTimer(double period_s) diff --git a/evaluator/kinematic_evaluator/package.xml b/evaluator/kinematic_evaluator/package.xml index a83538cdb3196..7129221f6c27b 100644 --- a/evaluator/kinematic_evaluator/package.xml +++ b/evaluator/kinematic_evaluator/package.xml @@ -5,6 +5,12 @@ kinematic evaluator ROS 2 node Dominik Jargot + Tomoya Kimura + Shumpei Wakabayashi + Takamasa Horibe + Takayuki Murooka + Fumiya Watanabe + Satoshi Ota Apache License 2.0 diff --git a/evaluator/localization_evaluator/package.xml b/evaluator/localization_evaluator/package.xml index 30b2bf0980fa0..075bc83a1f985 100644 --- a/evaluator/localization_evaluator/package.xml +++ b/evaluator/localization_evaluator/package.xml @@ -5,6 +5,7 @@ localization evaluator ROS 2 node Dominik Jargot + Koji Minoda Apache License 2.0 diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 6150abdafe7d6..fc1fac04b9415 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -114,7 +114,6 @@ def launch_setup(context, *args, **kwargs): "lane_change.use_all_predicted_path": LaunchConfiguration( "use_experimental_lane_change_function" ), - "bt_tree_config_path": LaunchConfiguration("behavior_path_planner_tree_param_path"), }, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 35b8da526e98f..4ff862c7852c6 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -20,7 +20,6 @@ - diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index f73520e8d7c40..3fd9b0ed9968b 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -20,6 +20,7 @@ enable_yield_maneuver_during_shifting: false disable_path_update: false use_hatched_road_markings: false + use_intersection_areas: false # for debug publish_debug_marker: false diff --git a/planning/behavior_path_planner/config/behavior_path_planner_tree.xml b/planning/behavior_path_planner/config/behavior_path_planner_tree.xml deleted file mode 100644 index d2b2a6a79bf0c..0000000000000 --- a/planning/behavior_path_planner/config/behavior_path_planner_tree.xml +++ /dev/null @@ -1,74 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - desc - - - - - desc - - - - desc - - - - desc - - - - desc - - - - - - - - desc - - - - desc - - - - - diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 639f495a74598..5b252cef92714 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -92,6 +92,7 @@ struct DrivableAreaInfo std::vector drivable_lanes{}; std::vector obstacles{}; // obstacles to extract from the drivable area bool enable_expanding_hatched_road_markings{false}; + bool enable_expanding_intersection_areas{false}; // temporary only for pull over's freespace planning double drivable_margin{0.0}; 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 e20c2e8ae9091..841902857ac7b 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 @@ -196,8 +196,8 @@ class LaneChangeBase virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; virtual PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length) const = 0; + const lanelet::ConstLanelets & current_lanes, const double backward_path_length, + const double prepare_length) const = 0; virtual bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 8b3dea03a8c8b..083c8ba9e7c0b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -96,8 +96,8 @@ class NormalLaneChange : public LaneChangeBase int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length) const override; + const lanelet::ConstLanelets & current_lanes, const double backward_path_length, + const double prepare_length) const override; bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 313162d6473cc..205b3c6d4db46 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -149,6 +149,12 @@ class StartPlannerModule : public SceneModuleInterface bool isStopped(); bool hasFinishedCurrentPath(); + // check if the goal is located behind the ego in the same route segment. + bool IsGoalBehindOfEgoInSameRouteSegment() const; + + // generate BehaviorPathOutput with stopping path. + BehaviorModuleOutput generateStopOutput() const; + void setDebugData() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 63166b0cbf7df..4130a5a207d13 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -108,6 +108,9 @@ struct AvoidanceParameters // use hatched road markings for avoidance bool use_hatched_road_markings{false}; + // use intersection area for avoidance + bool use_intersection_areas{false}; + // constrains bool use_constraints_for_decel{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 811587ea1c276..f63a0105d1efa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -124,8 +124,9 @@ void filterTargetObjects( double extendToRoadShoulderDistanceWithPolygon( const std::shared_ptr & rh, const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, - const geometry_msgs::msg::Point & overhang_pos, - const lanelet::BasicPoint3d & overhang_basic_pose); + const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos, + const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings, + const bool use_intersection_areas); void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); 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 106263cc66ebd..79bb4b68dde98 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 @@ -27,6 +27,40 @@ namespace behavior_path_planner { +struct PoseWithPolygon +{ + Pose pose; + Polygon2d poly; + + PoseWithPolygon(const Pose & pose, const Polygon2d & poly) : pose(pose), poly(poly) {} +}; + +struct PoseWithPolygonStamped : public PoseWithPolygon +{ + double time; + + PoseWithPolygonStamped(const double time, const Pose & pose, const Polygon2d & poly) + : PoseWithPolygon(pose, poly), time(time) + { + } +}; + +struct PredictedPathWithPolygon +{ + float confidence; + std::vector path; +}; + +struct ExtendedPredictedObject +{ + unique_identifier_msgs::msg::UUID uuid; + geometry_msgs::msg::PoseWithCovariance initial_pose; + geometry_msgs::msg::TwistWithCovariance initial_twist; + geometry_msgs::msg::AccelWithCovariance initial_acceleration; + autoware_auto_perception_msgs::msg::Shape shape; + std::vector predicted_paths; +}; + struct LaneChangeCancelParameters { bool enable_on_prepare_phase{true}; @@ -108,6 +142,13 @@ struct LaneChangeTargetObjectIndices std::vector other_lane{}; }; +struct LaneChangeTargetObjects +{ + std::vector current_lane{}; + std::vector target_lane{}; + std::vector other_lane{}; +}; + enum class LaneChangeModuleType { NORMAL = 0, EXTERNAL_REQUEST, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index a724410192b59..4381cd706b34c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_planner/utils/safety_check.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -42,6 +43,9 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::ExtendedPredictedObject; +using behavior_path_planner::PoseWithPolygonStamped; +using behavior_path_planner::PredictedPathWithPolygon; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -82,6 +86,10 @@ lanelet::ConstLanelets getTargetPreferredLanes( const lanelet::ConstLanelets & target_lanes, const Direction & direction, const LaneChangeModuleType & type); +lanelet::ConstLanelets getTargetNeighborLanes( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const LaneChangeModuleType & type); + bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets); @@ -96,17 +104,14 @@ std::optional constructCandidatePath( const double terminal_lane_changing_velocity, const LaneChangePhaseInfo lane_change_time); PathSafetyStatus isLaneChangePathSafe( - const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects, - const LaneChangeTargetObjectIndices & dynamic_object_indices, const Pose & current_pose, - const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameter, + const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, + const Pose & current_pose, const Twist & current_twist, + const BehaviorPathPlannerParameters & common_parameter, const behavior_path_planner::LaneChangeParameters & lane_change_parameter, const double front_decel, const double rear_decel, std::unordered_map & debug_data, const double prepare_acc, const double lane_changing_acc); -bool isObjectIndexIncluded( - const size_t & index, const std::vector & dynamic_objects_indices); - bool hasEnoughLength( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, @@ -169,16 +174,26 @@ PredictedPath convertToPredictedPath( const size_t nearest_seg_idx, const double duration, const double resolution, const double prepare_time, const double prepare_acc, const double lane_changing_acc); +bool isParkedObject( + const PathWithLaneId & path, const RouteHandler & route_handler, + const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width, + const double object_shiftable_ratio_threshold, + const double static_object_velocity_threshold = 1.0); + +bool isParkedObject( + const lanelet::ConstLanelet & closest_lanelet, const lanelet::BasicLineString2d & boundary, + const ExtendedPredictedObject & object, const double buffer_to_bound, + const double ratio_threshold); + bool passParkedObject( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const PathWithLaneId & current_lane_path, const PredictedObjects & objects, - const std::vector & target_lane_obj_indices, const double minimum_lane_change_length, - const bool is_goal_in_route, const double object_check_min_road_shoulder_width, - const double object_shiftable_ratio_threshold); + const PathWithLaneId & current_lane_path, const std::vector & objects, + const double minimum_lane_change_length, const bool is_goal_in_route, + const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); boost::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const PredictedObjects & objects, const std::vector & obj_indices, + const std::vector & objects, const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); std::optional createPolygon( @@ -188,6 +203,17 @@ LaneChangeTargetObjectIndices filterObject( const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes, const Pose & current_pose, const RouteHandler & route_handler, - const LaneChangeParameters & lane_change_parameter); + const LaneChangeParameters & lane_change_parameters); + +ExtendedPredictedObject transform( + const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters); + +LaneChangeTargetObjects getTargetObjects( + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes, + const Pose & current_pose, const RouteHandler & route_handler, + const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp index 8f86af7b39ca1..94dea8730f51d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp @@ -48,13 +48,9 @@ using geometry_msgs::msg::Twist; using marker_utils::CollisionCheckDebug; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +using vehicle_info_util::VehicleInfo; namespace bg = boost::geometry; -struct ProjectedDistancePoint -{ - Point2d projected_point; - double distance{0.0}; -}; bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, @@ -76,47 +72,28 @@ double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, const BehaviorPathPlannerParameters & params); +boost::optional getEgoInterpolatedPoseWithPolygon( + const PredictedPath & pred_path, const double current_time, const VehicleInfo & ego_info); + /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. * @param planned_path The predicted path of the ego vehicle. - * @param interpolated_ego A vector of pairs of ego vehicle's pose and its polygon at each moment in - * the future. + * @param predicted_ego_path Ego vehicle's predicted path * @param ego_current_velocity Current velocity of the ego vehicle. - * @param check_duration The vector of times in the future at which safety check is - * performed.(relative time in sec from the current time) * @param target_object The predicted object to check collision with. * @param target_object_path The predicted path of the target object. * @param common_parameters The common parameters used in behavior path planner. * @param front_object_deceleration The deceleration of the object in the front.(used in RSS) * @param rear_object_deceleration The deceleration of the object in the rear.(used in RSS) * @param debug The debug information for collision checking. - * @param prepare_duration The duration to prepare before shifting lane. - * @param velocity_threshold_for_prepare_duration The threshold for the target velocity to - * ignore during preparation phase. * @return true if distance is safe. */ -bool isSafeInLaneletCollisionCheck( - const PathWithLaneId & planned_path, - const std::vector> & predicted_ego_poses, - const double ego_current_velocity, const std::vector & check_duration, - const PredictedObject & target_object, const PredictedPath & target_object_path, +bool checkCollision( + const PathWithLaneId & planned_path, const PredictedPath & predicted_ego_path, + const double ego_current_velocity, const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug, - const double prepare_duration = 0.0, const double velocity_threshold_for_prepare_duration = 0.0); - -/** - * @brief Iterate the points in the ego and target's predicted path and - * perform safety check for each of the iterated points. - * @return true if distance is safe. - */ -bool isSafeInFreeSpaceCollisionCheck( - const PathWithLaneId & path, - const std::vector> & interpolated_ego, - const Twist & ego_current_twist, const std::vector & check_duration, - const double prepare_duration, const PredictedObject & target_object, - const BehaviorPathPlannerParameters & common_parameters, - const double prepare_phase_ignore_target_velocity_thresh, const double front_object_deceleration, const double rear_object_deceleration, CollisionCheckDebug & debug); } // namespace behavior_path_planner::utils::safety_check diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 783b637baeb0f..bdf4b0da6a110 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -226,7 +226,8 @@ std::vector generateDrivableLanesWithShoulderLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); std::vector calcBound( const std::shared_ptr route_handler, - const std::vector & drivable_lanes, const bool enable_expanding_polygon, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool is_left); boost::optional getOverlappedLaneletId(const std::vector & lanes); @@ -235,8 +236,9 @@ std::vector cutOverlappedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, - const bool enable_expanding_polygon, const double vehicle_length, - const std::shared_ptr planner_data, const bool is_driving_forward = true); + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const double vehicle_length, const std::shared_ptr planner_data, + const bool is_driving_forward = true); void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, @@ -380,11 +382,8 @@ lanelet::ConstLanelets calcLaneAroundPose( const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); -std::vector getPredictedPathFromObj( - const PredictedObject & obj, const bool & is_use_all_predicted_path); - -boost::optional> getEgoExpectedPoseAndConvertToPolygon( - const PredictedPath & pred_path, const double current_time, const VehicleInfo & ego_info); +std::vector getPredictedPathFromObj( + const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path); bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); @@ -412,15 +411,6 @@ void makeBoundLongitudinallyMonotonic(PathWithLaneId & path, const bool is_bound std::optional getPolygonByPoint( const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, const std::string & polygon_name); - -bool isParkedObject( - const PathWithLaneId & path, const RouteHandler & route_handler, const PredictedObject & object, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold, - const double static_object_velocity_threshold = 1.0); - -bool isParkedObject( - const lanelet::ConstLanelet & closest_lanelet, const lanelet::BasicLineString2d & boundary, - const PredictedObject & object, const double buffer_to_bound, const double ratio_threshold); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml index a7df8ae9bf428..a4d3cab3c1bf0 100644 --- a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml +++ b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml @@ -15,7 +15,6 @@ - diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index b09bba73d9128..94d5bef5a31e9 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -154,7 +154,7 @@ void PlannerManager::generateCombinedDrivableArea( } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - *output.path, di.drivable_lanes, false, data->parameters.vehicle_length, data); + *output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data); } else { const auto shorten_lanes = utils::cutOverlappedLanes(*output.path, di.drivable_lanes); @@ -166,7 +166,7 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( *output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, - data->parameters.vehicle_length, data); + di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data); } // extract obstacles from drivable area diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 58ef8f5fa5b3e..59552fd545ec5 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2365,6 +2365,9 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; + // expand intersection areas + current_drivable_area_info.enable_expanding_intersection_areas = + parameters_->use_intersection_areas; output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 9b7a8b68b969d..179bc90adafc9 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -69,6 +69,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( get_parameter(node, ns + "enable_yield_maneuver_during_shifting"); p.disable_path_update = get_parameter(node, ns + "disable_path_update"); p.use_hatched_road_markings = get_parameter(node, ns + "use_hatched_road_markings"); + p.use_intersection_areas = get_parameter(node, ns + "use_intersection_areas"); p.publish_debug_marker = get_parameter(node, ns + "publish_debug_marker"); p.print_debug_info = get_parameter(node, ns + "print_debug_info"); } 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 c744bd49f6012..d834b7f670e26 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 @@ -466,8 +466,7 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) } PathWithLaneId NormalLaneChange::getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, - [[maybe_unused]] const double arc_length_from_current, const double backward_path_length, + const lanelet::ConstLanelets & current_lanes, const double backward_path_length, const double prepare_length) const { if (current_lanes.empty()) { @@ -538,8 +537,6 @@ bool NormalLaneChange::getLaneChangePaths( const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), original_lanelets); - [[maybe_unused]] const auto arc_position_from_current = - lanelet::utils::getArcCoordinates(original_lanelets, getEgoPose()); const auto arc_position_from_target = lanelet::utils::getArcCoordinates(target_lanelets, getEgoPose()); @@ -548,19 +545,20 @@ bool NormalLaneChange::getLaneChangePaths( const auto sorted_lane_ids = utils::lane_change::getSortedLaneIds( route_handler, original_lanelets, target_lanelets, arc_position_from_target.distance); - const auto target_preferred_lanelets = utils::lane_change::getTargetPreferredLanes( - route_handler, original_lanelets, target_lanelets, direction, type_); - const auto target_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( - target_preferred_lanelets, 0, std::numeric_limits::max()); - const auto target_preferred_lane_poly_2d = - lanelet::utils::to2D(target_preferred_lane_poly).basicPolygon(); + const auto target_neighbor_lanelets = + utils::lane_change::getTargetNeighborLanes(route_handler, original_lanelets, type_); + + const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( + target_neighbor_lanelets, 0, std::numeric_limits::max()); + const auto target_neighbor_preferred_lane_poly_2d = + lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon(); 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(), backward_length); - const auto dynamic_object_indices = utils::lane_change::filterObject( + const auto target_objects = utils::lane_change::getTargetObjects( *dynamic_objects, original_lanelets, target_lanelets, - backward_target_lanes_for_object_filtering, current_pose, route_handler, + backward_target_lanes_for_object_filtering, current_pose, route_handler, common_parameter, *lane_change_parameters_); candidate_paths->reserve(longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num); @@ -584,8 +582,8 @@ bool NormalLaneChange::getLaneChangePaths( break; } - auto prepare_segment = getPrepareSegment( - original_lanelets, arc_position_from_current.length, backward_path_length, prepare_length); + auto prepare_segment = + getPrepareSegment(original_lanelets, backward_path_length, prepare_length); if (prepare_segment.points.empty()) { RCLCPP_DEBUG(logger_, "prepare segment is empty!!"); @@ -664,11 +662,11 @@ bool NormalLaneChange::getLaneChangePaths( continue; } - const lanelet::BasicPoint2d lc_terminal_point( - target_segment.points.front().point.pose.position.x, - target_segment.points.front().point.pose.position.y); - if (!boost::geometry::covered_by(lc_terminal_point, target_preferred_lane_poly_2d)) { - // lane change terminal point is not inside of the target preferred lanes + const lanelet::BasicPoint2d lc_start_point( + prepare_segment.points.back().point.pose.position.x, + prepare_segment.points.back().point.pose.position.y); + if (!boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d)) { + // lane changing points are not inside of the target preferred lanes or its neighbors continue; } @@ -722,9 +720,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto current_lane_path = route_handler.getCenterLinePath( original_lanelets, 0.0, std::numeric_limits::max()); const bool pass_parked_object = utils::lane_change::passParkedObject( - route_handler, *candidate_path, current_lane_path, *dynamic_objects, - dynamic_object_indices.target_lane, lane_change_buffer, is_goal_in_route, - object_check_min_road_shoulder_width, object_shiftable_ratio_threshold); + route_handler, *candidate_path, current_lane_path, target_objects.target_lane, + lane_change_buffer, is_goal_in_route, object_check_min_road_shoulder_width, + object_shiftable_ratio_threshold); if (pass_parked_object) { return false; } @@ -736,8 +734,8 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = utils::lane_change::isLaneChangePathSafe( - *candidate_path, dynamic_objects, dynamic_object_indices, getEgoPose(), getEgoTwist(), - common_parameter, *lane_change_parameters_, common_parameter.expected_front_deceleration, + *candidate_path, target_objects, getEgoPose(), getEgoTwist(), common_parameter, + *lane_change_parameters_, common_parameter.expected_front_deceleration, common_parameter.expected_rear_deceleration, object_debug_, longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing); @@ -766,14 +764,14 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets( route_handler, target_lanes, current_pose, lane_change_parameters.backward_lane_length); - const auto dynamic_object_indices = utils::lane_change::filterObject( + const auto target_objects = utils::lane_change::getTargetObjects( *dynamic_objects, current_lanes, target_lanes, backward_target_lanes_for_object_filtering, - current_pose, route_handler, *lane_change_parameters_); + current_pose, route_handler, common_parameters, *lane_change_parameters_); CollisionCheckDebugMap debug_data; const auto safety_status = utils::lane_change::isLaneChangePathSafe( - path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters, - *lane_change_parameters_, common_parameters.expected_front_deceleration_for_abort, + path, target_objects, current_pose, current_twist, common_parameters, *lane_change_parameters_, + common_parameters.expected_front_deceleration_for_abort, common_parameters.expected_rear_deceleration_for_abort, debug_data, status_.lane_change_path.longitudinal_acceleration.prepare, status_.lane_change_path.longitudinal_acceleration.lane_changing); diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 2441f62ef6cf8..dba81909d21fa 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -153,6 +153,12 @@ ModuleStatus StartPlannerModule::updateState() BehaviorModuleOutput StartPlannerModule::plan() { + if (IsGoalBehindOfEgoInSameRouteSegment()) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); + return generateStopOutput(); + } + if (isWaitingApproval()) { clearWaitingApproval(); resetPathCandidate(); @@ -294,6 +300,12 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() updatePullOutStatus(); waitApproval(); + if (IsGoalBehindOfEgoInSameRouteSegment()) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); + return generateStopOutput(); + } + BehaviorModuleOutput output; if (!status_.is_safe) { RCLCPP_WARN_THROTTLE( @@ -832,6 +844,46 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return turn_signal; } +bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const +{ + const auto & rh = planner_data_->route_handler; + + // Check if the goal and ego are in the same route segment. If not, this is out of scope of this + // function. Return false. + lanelet::ConstLanelet ego_lanelet; + rh->getClosestLaneletWithinRoute(getEgoPose(), &ego_lanelet); + const auto is_ego_in_goal_route_section = rh->isInGoalRouteSection(ego_lanelet); + + if (!is_ego_in_goal_route_section) { + return false; + } + + // If the goal and ego are in the same route segment, check the goal and ego pose relation. + // Return true when the goal is located behind of ego. + const auto ego_lane_path = rh->getCenterLinePath( + lanelet::ConstLanelets{ego_lanelet}, 0.0, std::numeric_limits::max()); + const auto dist_ego_to_goal = motion_utils::calcSignedArcLength( + ego_lane_path.points, getEgoPosition(), rh->getGoalPose().position); + + const bool is_goal_behind_of_ego = (dist_ego_to_goal < 0.0); + return is_goal_behind_of_ego; +} + +// NOTE: this must be called after updatePullOutStatus(). This must be fixed. +BehaviorModuleOutput StartPlannerModule::generateStopOutput() const +{ + BehaviorModuleOutput output; + output.path = std::make_shared(generateStopPath()); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = status_.lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + + output.reference_path = getPreviousModuleOutput().reference_path; + return output; +} + void StartPlannerModule::setDebugData() const { using marker_utils::createPathMarkerArray; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 8ffd4b52e74af..bd5912ddfe425 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -908,11 +908,11 @@ void filterTargetObjects( } debug.bounds.push_back(target_line); - // update to_road_shoulder_distance with expandable polygons - if (parameters->use_hatched_road_markings) { + { o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, o.to_road_shoulder_distance, o.overhang_pose.position, - overhang_basic_pose); + rh, target_line, o.to_road_shoulder_distance, overhang_lanelet, o.overhang_pose.position, + overhang_basic_pose, parameters->use_hatched_road_markings, + parameters->use_intersection_areas); } } @@ -1137,29 +1137,43 @@ void filterTargetObjects( double extendToRoadShoulderDistanceWithPolygon( const std::shared_ptr & rh, const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, - const geometry_msgs::msg::Point & overhang_pos, const lanelet::BasicPoint3d & overhang_basic_pose) + const lanelet::ConstLanelet & overhang_lanelet, const geometry_msgs::msg::Point & overhang_pos, + const lanelet::BasicPoint3d & overhang_basic_pose, const bool use_hatched_road_markings, + const bool use_intersection_areas) { // get expandable polygons for avoidance (e.g. hatched road markings) std::vector expandable_polygons; - for (const auto & point : target_line) { - const auto new_polygon_candidate = utils::getPolygonByPoint(rh, point, "hatched_road_markings"); - if (!new_polygon_candidate) { - continue; - } - bool is_new_polygon{true}; - for (const auto & polygon : expandable_polygons) { - if (polygon.id() == new_polygon_candidate->id()) { - is_new_polygon = false; - break; + const auto exist_polygon = [&](const auto & candidate_polygon) { + return std::any_of( + expandable_polygons.begin(), expandable_polygons.end(), + [&](const auto & polygon) { return polygon.id() == candidate_polygon.id(); }); + }; + + if (use_hatched_road_markings) { + for (const auto & point : target_line) { + const auto new_polygon_candidate = + utils::getPolygonByPoint(rh, point, "hatched_road_markings"); + + if (!!new_polygon_candidate && !exist_polygon(*new_polygon_candidate)) { + expandable_polygons.push_back(*new_polygon_candidate); } } + } - if (is_new_polygon) { - expandable_polygons.push_back(*new_polygon_candidate); + if (use_intersection_areas) { + const std::string area_id_str = overhang_lanelet.attributeOr("intersection_area", "else"); + + if (area_id_str != "else") { + expandable_polygons.push_back( + rh->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id_str.c_str()))); } } + if (expandable_polygons.empty()) { + return to_road_shoulder_distance; + } + // calculate point laterally offset from overhang position to calculate intersection with // polygon Point lat_offset_overhang_pos; @@ -1170,7 +1184,7 @@ double extendToRoadShoulderDistanceWithPolygon( const auto closest_target_line_point = lanelet::geometry::fromArcCoordinates(target_line, arc_coordinates); - const double ratio = 10.0 / to_road_shoulder_distance; + const double ratio = 100.0 / to_road_shoulder_distance; lat_offset_overhang_pos.x = closest_target_line_point.x() + (closest_target_line_point.x() - overhang_pos.x) * ratio; lat_offset_overhang_pos.y = @@ -1197,12 +1211,8 @@ double extendToRoadShoulderDistanceWithPolygon( } std::sort(intersect_dist_vec.begin(), intersect_dist_vec.end()); - if (1 < intersect_dist_vec.size()) { - if (std::abs(updated_to_road_shoulder_distance - intersect_dist_vec.at(0)) < 1e-3) { - updated_to_road_shoulder_distance = - std::max(updated_to_road_shoulder_distance, intersect_dist_vec.at(1)); - } - } + updated_to_road_shoulder_distance = + std::max(updated_to_road_shoulder_distance, intersect_dist_vec.back()); } return updated_to_road_shoulder_distance; } diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 44b4275bfa2f0..fcb2a6fd5afe7 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -182,6 +182,27 @@ lanelet::ConstLanelets getTargetPreferredLanes( return target_preferred_lanes; } +lanelet::ConstLanelets getTargetNeighborLanes( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType & type) +{ + lanelet::ConstLanelets neighbor_lanes; + + for (const auto & current_lane : current_lanes) { + if (route_handler.getNumLaneToPreferredLane(current_lane) != 0) { + if (type == LaneChangeModuleType::NORMAL) { + neighbor_lanes.push_back(current_lane); + } + } else { + if (type != LaneChangeModuleType::NORMAL) { + neighbor_lanes.push_back(current_lane); + } + } + } + + return neighbor_lanes; +} + bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets) @@ -343,19 +364,15 @@ bool hasEnoughLength( } PathSafetyStatus isLaneChangePathSafe( - const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects, - const LaneChangeTargetObjectIndices & dynamic_objects_indices, const Pose & current_pose, - const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameter, + const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, + const Pose & current_pose, const Twist & current_twist, + const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & lane_change_parameter, const double front_decel, const double rear_decel, std::unordered_map & debug_data, const double prepare_acc, const double lane_changing_acc) { PathSafetyStatus path_safety_status; - if (dynamic_objects == nullptr) { - return path_safety_status; - } - const auto & path = lane_change_path.path; if (path.points.empty()) { @@ -364,9 +381,6 @@ PathSafetyStatus isLaneChangePathSafe( } const double time_resolution = lane_change_parameter.prediction_time_resolution; - const auto check_at_prepare_phase = lane_change_parameter.enable_prepare_segment_collision_check; - - const double check_start_time = check_at_prepare_phase ? 0.0 : lane_change_path.duration.prepare; const double check_end_time = lane_change_path.duration.sum(); const double & prepare_duration = common_parameter.lane_change_prepare_duration; @@ -377,23 +391,23 @@ PathSafetyStatus isLaneChangePathSafe( const auto ego_predicted_path = convertToPredictedPath( path, current_twist, current_pose, current_seg_idx, check_end_time, time_resolution, prepare_duration, prepare_acc, lane_changing_acc); - const auto & vehicle_info = common_parameter.vehicle_info; - auto in_lane_object_indices = dynamic_objects_indices.target_lane; - in_lane_object_indices.insert( - in_lane_object_indices.end(), dynamic_objects_indices.current_lane.begin(), - dynamic_objects_indices.current_lane.end()); + auto collision_check_objects = target_objects.target_lane; + collision_check_objects.insert( + collision_check_objects.end(), target_objects.current_lane.begin(), + target_objects.current_lane.end()); - RCLCPP_DEBUG( - rclcpp::get_logger("lane_change"), "number of object -> total: %lu, in lane: %lu, others: %lu", - dynamic_objects->objects.size(), in_lane_object_indices.size(), - dynamic_objects_indices.other_lane.size()); + if (lane_change_parameter.use_predicted_path_outside_lanelet) { + collision_check_objects.insert( + collision_check_objects.end(), target_objects.other_lane.begin(), + target_objects.other_lane.end()); + } - const auto assignDebugData = [](const PredictedObject & obj) { + const auto assignDebugData = [](const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; - debug.current_pose = obj.kinematics.initial_pose_with_covariance.pose; - debug.current_twist = obj.kinematics.initial_twist_with_covariance.twist; - return std::make_pair(tier4_autoware_utils::toHexString(obj.object_id), debug); + debug.current_pose = obj.initial_pose.pose; + debug.current_twist = obj.initial_twist.twist; + return std::make_pair(tier4_autoware_utils::toHexString(obj.uuid), debug); }; const auto updateDebugInfo = @@ -408,84 +422,29 @@ PathSafetyStatus isLaneChangePathSafe( } }; - const auto reserve_size = - static_cast((check_end_time - check_start_time) / time_resolution); - std::vector check_durations{}; - std::vector> interpolated_ego{}; - check_durations.reserve(reserve_size); - interpolated_ego.reserve(reserve_size); - - for (double t = check_start_time; t < check_end_time; t += time_resolution) { - tier4_autoware_utils::Polygon2d ego_polygon; - const auto result = - utils::getEgoExpectedPoseAndConvertToPolygon(ego_predicted_path, t, vehicle_info); - if (!result) { - continue; - } - check_durations.push_back(t); - interpolated_ego.emplace_back(result->first, result->second); - } - - for (const auto & i : in_lane_object_indices) { - const auto & obj = dynamic_objects->objects.at(i); + for (const auto & obj : collision_check_objects) { auto current_debug_data = assignDebugData(obj); + current_debug_data.second.ego_predicted_path.push_back(ego_predicted_path); const auto obj_predicted_paths = utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { - if (!utils::safety_check::isSafeInLaneletCollisionCheck( - path, interpolated_ego, current_twist.linear.x, check_durations, obj, obj_path, - common_parameter, front_decel, rear_decel, current_debug_data.second, - lane_change_path.duration.prepare, - lane_change_parameter.prepare_segment_ignore_object_velocity_thresh)) { + if (!utils::safety_check::checkCollision( + path, ego_predicted_path, current_twist.linear.x, obj, obj_path, common_parameter, + front_decel, rear_decel, current_debug_data.second)) { path_safety_status.is_safe = false; updateDebugInfo(current_debug_data, path_safety_status.is_safe); - if (isObjectIndexIncluded(i, dynamic_objects_indices.target_lane)) { - const auto & obj_pose = obj.kinematics.initial_pose_with_covariance.pose; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= - !utils::safety_check::isTargetObjectFront( - path, current_pose, common_parameter.vehicle_info, obj_polygon); - } + const auto & obj_pose = obj.initial_pose.pose; + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); + path_safety_status.is_object_coming_from_rear |= !utils::safety_check::isTargetObjectFront( + path, current_pose, common_parameter.vehicle_info, obj_polygon); } } updateDebugInfo(current_debug_data, path_safety_status.is_safe); } - if (!lane_change_parameter.use_predicted_path_outside_lanelet) { - return path_safety_status; - } - - for (const auto & i : dynamic_objects_indices.other_lane) { - const auto & obj = dynamic_objects->objects.at(i); - auto current_debug_data = assignDebugData(obj); - current_debug_data.second.ego_predicted_path.push_back(ego_predicted_path); - - const auto predicted_paths = - utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path); - - if (!utils::safety_check::isSafeInFreeSpaceCollisionCheck( - path, interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, - obj, common_parameter, - lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel, - rear_decel, current_debug_data.second)) { - path_safety_status.is_safe = false; - updateDebugInfo(current_debug_data, path_safety_status.is_safe); - const auto & obj_pose = obj.kinematics.initial_pose_with_covariance.pose; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); - path_safety_status.is_object_coming_from_rear |= !utils::safety_check::isTargetObjectFront( - path, current_pose, common_parameter.vehicle_info, obj_polygon); - } - updateDebugInfo(current_debug_data, path_safety_status.is_safe); - } return path_safety_status; } -bool isObjectIndexIncluded( - const size_t & index, const std::vector & dynamic_objects_indices) -{ - return std::count(dynamic_objects_indices.begin(), dynamic_objects_indices.end(), index) != 0; -} - PathWithLaneId getTargetSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, const double forward_path_length, const Pose & lane_changing_start_pose, @@ -979,28 +938,148 @@ PredictedPath convertToPredictedPath( return predicted_path; } +bool isParkedObject( + const PathWithLaneId & path, const RouteHandler & route_handler, + const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width, + const double object_shiftable_ratio_threshold, const double static_object_velocity_threshold) +{ + // ============================================ <- most_left_lanelet.leftBound() + // y road shoulder + // ^ ------------------------------------------ + // | x + + // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() + // + // -------------------------------------------- + // +: object position + // o: nearest point on centerline + + using lanelet::geometry::distance2d; + using lanelet::geometry::toArcCoordinates; + + if (object.initial_twist.twist.linear.x > static_object_velocity_threshold) { + return false; + } + + const auto & object_pose = object.initial_pose.pose; + const auto object_closest_index = + motion_utils::findNearestIndex(path.points, object_pose.position); + const auto object_closest_pose = path.points.at(object_closest_index).point.pose; + + lanelet::ConstLanelet closest_lanelet; + if (!route_handler.getClosestLaneletWithinRoute(object_closest_pose, &closest_lanelet)) { + return false; + } + + const double lat_dist = motion_utils::calcLateralOffset(path.points, object_pose.position); + lanelet::BasicLineString2d bound; + double center_to_bound_buffer = 0.0; + if (lat_dist > 0.0) { + // left side vehicle + const auto most_left_road_lanelet = route_handler.getMostLeftLanelet(closest_lanelet); + const auto most_left_lanelet_candidates = + route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); + lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; + const lanelet::Attribute sub_type = + most_left_lanelet.attribute(lanelet::AttributeName::Subtype); + + for (const auto & ll : most_left_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_left_lanelet = ll; + } + } + bound = most_left_lanelet.leftBound2d().basicLineString(); + if (sub_type.value() != "road_shoulder") { + center_to_bound_buffer = object_check_min_road_shoulder_width; + } + } else { + // right side vehicle + const auto most_right_road_lanelet = route_handler.getMostRightLanelet(closest_lanelet); + const auto most_right_lanelet_candidates = + route_handler.getLaneletMapPtr()->laneletLayer.findUsages( + most_right_road_lanelet.rightBound()); + + lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; + const lanelet::Attribute sub_type = + most_right_lanelet.attribute(lanelet::AttributeName::Subtype); + + for (const auto & ll : most_right_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_right_lanelet = ll; + } + } + bound = most_right_lanelet.rightBound2d().basicLineString(); + if (sub_type.value() != "road_shoulder") { + center_to_bound_buffer = object_check_min_road_shoulder_width; + } + } + + return isParkedObject( + closest_lanelet, bound, object, center_to_bound_buffer, object_shiftable_ratio_threshold); +} + +bool isParkedObject( + const lanelet::ConstLanelet & closest_lanelet, const lanelet::BasicLineString2d & boundary, + const ExtendedPredictedObject & object, const double buffer_to_bound, + const double ratio_threshold) +{ + using lanelet::geometry::distance2d; + + const auto & obj_pose = object.initial_pose.pose; + const auto & obj_shape = object.shape; + const auto obj_poly = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape); + const auto obj_point = obj_pose.position; + + double max_dist_to_bound = std::numeric_limits::lowest(); + double min_dist_to_bound = std::numeric_limits::max(); + for (const auto & edge : obj_poly.outer()) { + const auto ll_edge = lanelet::Point2d(lanelet::InvalId, edge.x(), edge.y()); + const auto dist = distance2d(boundary, ll_edge); + max_dist_to_bound = std::max(dist, max_dist_to_bound); + min_dist_to_bound = std::min(dist, min_dist_to_bound); + } + const double obj_width = std::max(max_dist_to_bound - min_dist_to_bound, 0.0); + + // distance from centerline to the boundary line with object width + const auto centerline_pose = lanelet::utils::getClosestCenterPose(closest_lanelet, obj_point); + const lanelet::BasicPoint3d centerline_point( + centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); + const double dist_bound_to_centerline = + std::abs(distance2d(boundary, centerline_point)) - 0.5 * obj_width + buffer_to_bound; + + // distance from object point to centerline + const auto centerline = closest_lanelet.centerline(); + const auto ll_obj_point = lanelet::Point2d(lanelet::InvalId, obj_point.x, obj_point.y); + const double dist_obj_to_centerline = std::abs(distance2d(centerline, ll_obj_point)); + + const double ratio = dist_obj_to_centerline / std::max(dist_bound_to_centerline, 1e-6); + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + return clamped_ratio > ratio_threshold; +} + bool passParkedObject( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const PathWithLaneId & current_lane_path, const PredictedObjects & objects, - const std::vector & target_lane_obj_indices, const double minimum_lane_change_length, - const bool is_goal_in_route, const double object_check_min_road_shoulder_width, - const double object_shiftable_ratio_threshold) + const PathWithLaneId & current_lane_path, const std::vector & objects, + const double minimum_lane_change_length, const bool is_goal_in_route, + const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold) { const auto & path = lane_change_path.path; - if (target_lane_obj_indices.empty() || path.points.empty() || current_lane_path.points.empty()) { + if (objects.empty() || path.points.empty() || current_lane_path.points.empty()) { return false; } const auto leading_obj_idx = getLeadingStaticObjectIdx( - route_handler, lane_change_path, objects, target_lane_obj_indices, - object_check_min_road_shoulder_width, object_shiftable_ratio_threshold); + route_handler, lane_change_path, objects, object_check_min_road_shoulder_width, + object_shiftable_ratio_threshold); if (!leading_obj_idx) { return false; } - const auto & leading_obj = objects.objects.at(*leading_obj_idx); - const auto & leading_obj_poly = tier4_autoware_utils::toPolygon2d(leading_obj); + const auto & leading_obj = objects.at(*leading_obj_idx); + const auto leading_obj_poly = + tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { return false; } @@ -1030,12 +1109,12 @@ bool passParkedObject( boost::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, - const PredictedObjects & objects, const std::vector & obj_indices, + const std::vector & objects, const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold) { const auto & path = lane_change_path.path; - if (path.points.empty() || obj_indices.empty()) { + if (path.points.empty() || objects.empty()) { return {}; } @@ -1044,13 +1123,13 @@ boost::optional getLeadingStaticObjectIdx( double dist_lc_start_to_leading_obj = 0.0; boost::optional leading_obj_idx = boost::none; - for (const auto & obj_idx : obj_indices) { - const auto & obj = objects.objects.at(obj_idx); - const auto & obj_pose = obj.kinematics.initial_pose_with_covariance.pose; + for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { + const auto & obj = objects.at(obj_idx); + const auto & obj_pose = obj.initial_pose.pose; // ignore non-static object // TODO(shimizu): parametrize threshold - if (obj.kinematics.initial_twist_with_covariance.twist.linear.x > 1.0) { + if (obj.initial_twist.twist.linear.x > 1.0) { continue; } @@ -1175,4 +1254,88 @@ LaneChangeTargetObjectIndices filterObject( return filtered_obj_indices; } + +ExtendedPredictedObject transform( + const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters) +{ + ExtendedPredictedObject extended_object; + extended_object.uuid = object.object_id; + extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; + extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; + extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; + extended_object.shape = object.shape; + + const auto & time_resolution = lane_change_parameters.prediction_time_resolution; + const auto & check_at_prepare_phase = + lane_change_parameters.enable_prepare_segment_collision_check; + const auto & prepare_duration = common_parameters.lane_change_prepare_duration; + const auto & velocity_threshold = + lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; + const auto & obj_vel = extended_object.initial_twist.twist.linear.x; + const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; + + extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); + for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { + const auto & path = object.kinematics.predicted_paths.at(i); + const double end_time = + rclcpp::Duration(path.time_step).seconds() * static_cast(path.path.size() - 1); + extended_object.predicted_paths.at(i).confidence = path.confidence; + + // create path + for (double t = start_time; t < end_time + std::numeric_limits::epsilon(); + t += time_resolution) { + if (t < prepare_duration && obj_vel < velocity_threshold) { + continue; + } + const auto obj_pose = perception_utils::calcInterpolatedPose(path, t); + if (obj_pose) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); + extended_object.predicted_paths.at(i).path.emplace_back(t, *obj_pose, obj_polygon); + } + } + } + + return extended_object; +} + +LaneChangeTargetObjects getTargetObjects( + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes, + const Pose & current_pose, const RouteHandler & route_handler, + const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters) +{ + const auto target_obj_index = filterObject( + objects, current_lanes, target_lanes, target_backward_lanes, current_pose, route_handler, + lane_change_parameters); + + LaneChangeTargetObjects target_objects; + target_objects.current_lane.reserve(target_obj_index.current_lane.size()); + target_objects.target_lane.reserve(target_obj_index.target_lane.size()); + target_objects.other_lane.reserve(target_obj_index.other_lane.size()); + + // objects in current lane + for (const auto & obj_idx : target_obj_index.current_lane) { + const auto extended_object = + transform(objects.objects.at(obj_idx), common_parameters, lane_change_parameters); + target_objects.current_lane.push_back(extended_object); + } + + // objects in target lane + for (const auto & obj_idx : target_obj_index.target_lane) { + const auto extended_object = + transform(objects.objects.at(obj_idx), common_parameters, lane_change_parameters); + target_objects.target_lane.push_back(extended_object); + } + + // objects in other lane + for (const auto & obj_idx : target_obj_index.other_lane) { + const auto extended_object = + transform(objects.objects.at(obj_idx), common_parameters, lane_change_parameters); + target_objects.other_lane.push_back(extended_object); + } + + return target_objects; +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index 88a5a532f170c..d2d70b0a46dfc 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -108,6 +108,7 @@ PathWithLaneId resamplePathWithSpline( std::vector s_out = s_in; + // sampling from interval distance const auto start_s = std::max(target_section.first, 0.0); const auto end_s = std::min(target_section.second, s_vec.back()); for (double s = start_s; s < end_s; s += interval) { @@ -115,6 +116,9 @@ PathWithLaneId resamplePathWithSpline( s_out.push_back(s); } } + if (!find_almost_same_values(s_out, end_s)) { + s_out.push_back(end_s); + } // Insert Stop Point const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint(transformed_path); @@ -133,7 +137,8 @@ PathWithLaneId resamplePathWithSpline( } } - if (s_out.empty()) { + // spline resample required more than 2 points for yaw angle calculation + if (s_out.size() < 2) { return path; } diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp index ef450c3538f65..535c822dc32d5 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/safety_check.cpp @@ -163,127 +163,54 @@ double calcMinimumLongitudinalLength( return params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; } -bool isSafeInLaneletCollisionCheck( - const PathWithLaneId & planned_path, - const std::vector> & predicted_ego_poses, - const double ego_current_velocity, const std::vector & check_duration, - const PredictedObject & target_object, const PredictedPath & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug, const double prepare_duration, - const double velocity_threshold_for_prepare_duration) +boost::optional getEgoInterpolatedPoseWithPolygon( + const PredictedPath & pred_path, const double current_time, const VehicleInfo & ego_info) { - debug.lerped_path.reserve(check_duration.size()); - - const auto & ego_velocity = ego_current_velocity; - const auto & object_velocity = - target_object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto interpolated_pose = perception_utils::calcInterpolatedPose(pred_path, current_time); - for (size_t i = 0; i < check_duration.size(); ++i) { - const auto current_time = check_duration.at(i); - - // ignore low velocity object during prepare duration - const bool prepare_phase = current_time < prepare_duration; - const bool ignore_target_velocity_during_prepare_phase = - object_velocity < velocity_threshold_for_prepare_duration; - if (prepare_phase && ignore_target_velocity_during_prepare_phase) { - continue; - } + if (!interpolated_pose) { + return {}; + } - const auto obj_pose = perception_utils::calcInterpolatedPose(target_object_path, current_time); - if (!obj_pose) { - continue; - } + const auto & i = ego_info; + const auto & base_to_front = i.max_longitudinal_offset_m; + const auto & base_to_rear = i.rear_overhang_m; + const auto & width = i.vehicle_width_m; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, target_object.shape); - const auto & ego_pose = predicted_ego_poses.at(i).first; - const auto & ego_polygon = predicted_ego_poses.at(i).second; + const auto ego_polygon = + tier4_autoware_utils::toFootprint(*interpolated_pose, base_to_front, base_to_rear, width); - { - debug.lerped_path.push_back(ego_pose); - debug.expected_ego_pose = ego_pose; - debug.expected_obj_pose = *obj_pose; - debug.ego_polygon = ego_polygon; - debug.obj_polygon = obj_polygon; - } + return PoseWithPolygon{*interpolated_pose, ego_polygon}; +} - // check overlap - if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { - debug.failed_reason = "overlap_polygon"; - return false; - } +bool checkCollision( + const PathWithLaneId & planned_path, const PredictedPath & predicted_ego_path, + const double ego_current_velocity, const ExtendedPredictedObject & target_object, + const PredictedPathWithPolygon & target_object_path, + const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, + const double rear_object_deceleration, CollisionCheckDebug & debug) +{ + debug.lerped_path.reserve(target_object_path.path.size()); - // compute which one is at the front of the other - const bool is_object_front = - isTargetObjectFront(planned_path, ego_pose, common_parameters.vehicle_info, obj_polygon); - const auto & [front_object_velocity, rear_object_velocity] = - is_object_front ? std::make_pair(object_velocity, ego_velocity) - : std::make_pair(ego_velocity, object_velocity); + const auto & ego_velocity = ego_current_velocity; + const auto & object_velocity = target_object.initial_twist.twist.linear.x; - // compute rss dist - const auto rss_dist = calcRssDistance( - front_object_velocity, rear_object_velocity, front_object_deceleration, - rear_object_deceleration, common_parameters); + for (const auto & obj_pose_with_poly : target_object_path.path) { + const auto & current_time = obj_pose_with_poly.time; - // minimum longitudinal length - const auto min_lon_length = - calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, common_parameters); + // get object information at current time + const auto & obj_pose = obj_pose_with_poly.pose; + const auto & obj_polygon = obj_pose_with_poly.poly; - const auto & lon_offset = std::max(rss_dist, min_lon_length); + // get ego information at current time const auto & ego_vehicle_info = common_parameters.vehicle_info; - const auto & lat_margin = common_parameters.lateral_distance_max_threshold; - const auto & extended_ego_polygon = - is_object_front - ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) - : ego_polygon; - const auto & extended_obj_polygon = - is_object_front - ? obj_polygon - : createExtendedPolygon(*obj_pose, target_object.shape, lon_offset, lat_margin, debug); - - { - debug.rss_longitudinal = rss_dist; - debug.ego_to_obj_margin = min_lon_length; - debug.ego_polygon = extended_ego_polygon; - debug.obj_polygon = extended_obj_polygon; - debug.is_front = is_object_front; - } - - // check overlap with extended polygon - if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { - debug.failed_reason = "overlap_extended_polygon"; - return false; - } - } - - return true; -} - -bool isSafeInFreeSpaceCollisionCheck( - const PathWithLaneId & path, - const std::vector> & interpolated_ego, - const Twist & ego_current_twist, const std::vector & check_duration, - const double prepare_duration, const PredictedObject & target_object, - const BehaviorPathPlannerParameters & common_parameters, - const double prepare_phase_ignore_target_velocity_thresh, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug) -{ - const auto & obj_pose = target_object.kinematics.initial_pose_with_covariance.pose; - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(target_object); - const auto & object_velocity = - target_object.kinematics.initial_twist_with_covariance.twist.linear.x; - const auto & ego_velocity = ego_current_twist.linear.x; - - for (size_t i = 0; i < check_duration.size(); ++i) { - const auto current_time = check_duration.at(i); - - if ( - current_time < prepare_duration && - object_velocity < prepare_phase_ignore_target_velocity_thresh) { + const auto ego_pose_with_polygon = + getEgoInterpolatedPoseWithPolygon(predicted_ego_path, current_time, ego_vehicle_info); + if (!ego_pose_with_polygon) { continue; } - - const auto & ego_pose = interpolated_ego.at(i).first; - const auto & ego_polygon = interpolated_ego.at(i).second; + const auto & ego_pose = ego_pose_with_polygon->pose; + const auto & ego_polygon = ego_pose_with_polygon->poly; { debug.lerped_path.push_back(ego_pose); @@ -293,6 +220,7 @@ bool isSafeInFreeSpaceCollisionCheck( debug.obj_polygon = obj_polygon; } + // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.failed_reason = "overlap_polygon"; return false; @@ -300,10 +228,11 @@ bool isSafeInFreeSpaceCollisionCheck( // compute which one is at the front of the other const bool is_object_front = - isTargetObjectFront(path, ego_pose, common_parameters.vehicle_info, obj_polygon); + isTargetObjectFront(planned_path, ego_pose, common_parameters.vehicle_info, obj_polygon); const auto & [front_object_velocity, rear_object_velocity] = is_object_front ? std::make_pair(object_velocity, ego_velocity) : std::make_pair(ego_velocity, object_velocity); + // compute rss dist const auto rss_dist = calcRssDistance( front_object_velocity, rear_object_velocity, front_object_deceleration, @@ -314,7 +243,6 @@ bool isSafeInFreeSpaceCollisionCheck( calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, common_parameters); const auto & lon_offset = std::max(rss_dist, min_lon_length); - const auto & ego_vehicle_info = common_parameters.vehicle_info; const auto & lat_margin = common_parameters.lateral_distance_max_threshold; const auto & extended_ego_polygon = is_object_front @@ -333,11 +261,13 @@ bool isSafeInFreeSpaceCollisionCheck( debug.is_front = is_object_front; } + // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { debug.failed_reason = "overlap_extended_polygon"; return false; } } + return true; } } // namespace behavior_path_planner::utils::safety_check diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 9d21dbae99171..1612280c14872 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -249,6 +249,13 @@ std::vector ShiftPullOut::calcPullOutPaths( path_shifter.setLongitudinalAcceleration(longitudinal_acc); path_shifter.setLateralAccelerationLimit(lateral_acc); + const auto shift_line_idx = path_shifter.getShiftLines().front(); + if (!has_non_shifted_path && (shift_line_idx.end_idx - shift_line_idx.start_idx <= 1)) { + candidate_paths.push_back(non_shifted_path); + has_non_shifted_path = true; + continue; + } + // offset front side ShiftedPath shifted_path; const bool offset_back = false; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 209cfd181b0cc..34684a32d6edb 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1343,8 +1343,9 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, - const bool enable_expanding_polygon, const double vehicle_length, - const std::shared_ptr planner_data, const bool is_driving_forward) + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const double vehicle_length, const std::shared_ptr planner_data, + const bool is_driving_forward) { // extract data const auto transformed_lanes = utils::transformToLanelets(lanes); @@ -1379,8 +1380,12 @@ void generateDrivableArea( }; // Insert Position - auto left_bound = calcBound(route_handler, lanes, enable_expanding_polygon, true); - auto right_bound = calcBound(route_handler, lanes, enable_expanding_polygon, false); + auto left_bound = calcBound( + route_handler, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, true); + auto right_bound = calcBound( + route_handler, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, false); if (left_bound.empty() || right_bound.empty()) { auto clock{rclcpp::Clock{RCL_ROS_TIME}}; @@ -1515,7 +1520,7 @@ void generateDrivableArea( // make bound longitudinally monotonic // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic - if (enable_expanding_polygon) { + if (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas) { makeBoundLongitudinallyMonotonic(path, true); // for left bound makeBoundLongitudinallyMonotonic(path, false); // for right bound } @@ -1524,7 +1529,8 @@ void generateDrivableArea( // calculate bounds from drivable lanes and hatched road markings std::vector calcBound( const std::shared_ptr route_handler, - const std::vector & drivable_lanes, const bool enable_expanding_polygon, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool is_left) { // a function to convert drivable lanes to points without duplicated points @@ -1560,11 +1566,29 @@ std::vector calcBound( const auto mod = [&](const int a, const int b) { return (a + b) % b; // NOTE: consider negative value }; + const auto extract_bound_from_polygon = + [](const auto & polygon, const auto start_idx, const auto end_idx, const auto clockwise) { + std::vector ret; + for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { + ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[i])); + + if (i + 1 == polygon.size() && clockwise) { + i = 0; + continue; + } + + if (i == 0 && !clockwise) { + i = polygon.size() - 1; + } + } + + return ret; + }; // If no need to expand with polygons, return here. std::vector output_points; const auto bound_points = convert_to_points(drivable_lanes); - if (!enable_expanding_polygon) { + if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { for (const auto & point : bound_points) { output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); } @@ -1573,60 +1597,100 @@ std::vector calcBound( std::optional current_polygon{std::nullopt}; std::vector current_polygon_border_indices; - for (size_t bound_point_idx = 0; bound_point_idx < bound_points.size(); ++bound_point_idx) { - const auto & bound_point = bound_points.at(bound_point_idx); - const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); - - bool will_close_polygon{false}; - if (!current_polygon) { - if (!polygon) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(bound_point)); - } else { - // There is a new additional polygon to expand - current_polygon = polygon; - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); + for (const auto & drivable_lane : drivable_lanes) { + // extract target lane and bound. + const auto bound_lane = is_left ? drivable_lane.left_lane : drivable_lane.right_lane; + const auto bound = is_left ? bound_lane.leftBound3d() : bound_lane.rightBound3d(); + + if (bound.size() < 2) { + continue; + } + + // expand drivable area by intersection areas. + const std::string id = bound_lane.attributeOr("intersection_area", "else"); + const auto use_intersection_area = enable_expanding_intersection_areas && id != "else"; + if (use_intersection_area) { + const auto polygon = + route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + + const auto start_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { + return p.id() == bound.front().id(); + }); + + const auto end_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { + return p.id() == bound.back().id(); + }); + + if (start_itr == polygon.end() || end_itr == polygon.end()) { + continue; } - } else { - if (!polygon) { - will_close_polygon = true; - } else { - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); + + // extract line strings between start_idx and end_idx. + const size_t start_idx = std::distance(polygon.begin(), start_itr); + const size_t end_idx = std::distance(polygon.begin(), end_itr); + for (const auto & point : extract_bound_from_polygon(polygon, start_idx, end_idx, is_left)) { + output_points.push_back(point); } - } - if (bound_point_idx == bound_points.size() - 1 && current_polygon) { - // If drivable lanes ends earlier than polygon, close the polygon - will_close_polygon = true; + continue; } - if (will_close_polygon) { - // The current additional polygon ends to expand - if (current_polygon_border_indices.size() == 1) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[current_polygon_border_indices.front()])); + // expand drivable area by hatched road markings. + for (size_t bound_point_idx = 0; bound_point_idx < bound.size(); ++bound_point_idx) { + const auto & bound_point = bound[bound_point_idx]; + const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); + + bool will_close_polygon{false}; + if (!current_polygon) { + if (!polygon) { + output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(bound_point)); + } else { + // There is a new additional polygon to expand + current_polygon = polygon; + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } } else { - const size_t current_polygon_points_num = current_polygon->size(); - const bool is_polygon_opposite_direction = [&]() { - const size_t modulo_diff = mod( - static_cast(current_polygon_border_indices[1]) - - static_cast(current_polygon_border_indices[0]), - current_polygon_points_num); - return modulo_diff == 1; - }(); + if (!polygon) { + will_close_polygon = true; + } else { + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } + } - const int target_points_num = - current_polygon_points_num - current_polygon_border_indices.size() + 1; - for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { - const int target_poly_idx = current_polygon_border_indices.front() + - poly_idx * (is_polygon_opposite_direction ? -1 : 1); + if (bound_point_idx == bound_points.size() - 1 && current_polygon) { + // If drivable lanes ends earlier than polygon, close the polygon + will_close_polygon = true; + } + + if (will_close_polygon) { + // The current additional polygon ends to expand + if (current_polygon_border_indices.size() == 1) { output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)])); + (*current_polygon)[current_polygon_border_indices.front()])); + } else { + const size_t current_polygon_points_num = current_polygon->size(); + const bool is_polygon_opposite_direction = [&]() { + const size_t modulo_diff = mod( + static_cast(current_polygon_border_indices[1]) - + static_cast(current_polygon_border_indices[0]), + current_polygon_points_num); + return modulo_diff == 1; + }(); + + const int target_points_num = + current_polygon_points_num - current_polygon_border_indices.size() + 1; + for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { + const int target_poly_idx = current_polygon_border_indices.front() + + poly_idx * (is_polygon_opposite_direction ? -1 : 1); + output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( + (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)])); + } } + current_polygon = std::nullopt; + current_polygon_border_indices.clear(); } - current_polygon = std::nullopt; - current_polygon_border_indices.clear(); } } @@ -2523,7 +2587,8 @@ BehaviorModuleOutput getReferencePath( dp.drivable_area_types_to_skip); // for old architecture - generateDrivableArea(reference_path, expanded_lanes, false, p.vehicle_length, planner_data); + generateDrivableArea( + reference_path, expanded_lanes, false, false, p.vehicle_length, planner_data); BehaviorModuleOutput output; output.path = std::make_shared(reference_path); @@ -2695,44 +2760,19 @@ lanelet::ConstLanelets calcLaneAroundPose( return current_lanes; } -boost::optional> getEgoExpectedPoseAndConvertToPolygon( - const PredictedPath & pred_path, const double current_time, const VehicleInfo & ego_info) -{ - const auto interpolated_pose = perception_utils::calcInterpolatedPose(pred_path, current_time); - - if (!interpolated_pose) { - return {}; - } - - const auto & i = ego_info; - const auto & base_to_front = i.max_longitudinal_offset_m; - const auto & base_to_rear = i.rear_overhang_m; - const auto & width = i.vehicle_width_m; - - const auto ego_polygon = - tier4_autoware_utils::toFootprint(*interpolated_pose, base_to_front, base_to_rear, width); - - return std::make_pair(*interpolated_pose, ego_polygon); -} - -std::vector getPredictedPathFromObj( - const PredictedObject & obj, const bool & is_use_all_predicted_path) +std::vector getPredictedPathFromObj( + const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path) { if (!is_use_all_predicted_path) { const auto max_confidence_path = std::max_element( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), + obj.predicted_paths.begin(), obj.predicted_paths.end(), [](const auto & path1, const auto & path2) { return path1.confidence < path2.confidence; }); - if (max_confidence_path != obj.kinematics.predicted_paths.end()) { + if (max_confidence_path != obj.predicted_paths.end()) { return {*max_confidence_path}; } } - std::vector predicted_path_vec; - std::copy_if( - obj.kinematics.predicted_paths.cbegin(), obj.kinematics.predicted_paths.cend(), - std::back_inserter(predicted_path_vec), - [](const PredictedPath & path) { return !path.path.empty(); }); - return predicted_path_vec; + return obj.predicted_paths; } bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold) @@ -2942,6 +2982,11 @@ DrivableAreaInfo combineDrivableAreaInfo( drivable_area_info1.enable_expanding_hatched_road_markings || drivable_area_info2.enable_expanding_hatched_road_markings; + // enable expanding intersection areas + combined_drivable_area_info.enable_expanding_intersection_areas = + drivable_area_info1.enable_expanding_intersection_areas || + drivable_area_info2.enable_expanding_intersection_areas; + return combined_drivable_area_info; } @@ -3006,124 +3051,4 @@ void extractObstaclesFromDrivableArea( bound = drivable_area_processing::updateBoundary(bound, unique_polygons); } } - -bool isParkedObject( - const PathWithLaneId & path, const RouteHandler & route_handler, const PredictedObject & object, - const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold, - const double static_object_velocity_threshold) -{ - // ============================================ <- most_left_lanelet.leftBound() - // y road shoulder - // ^ ------------------------------------------ - // | x + - // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() - // - // -------------------------------------------- - // +: object position - // o: nearest point on centerline - - using lanelet::geometry::distance2d; - using lanelet::geometry::toArcCoordinates; - - if ( - object.kinematics.initial_twist_with_covariance.twist.linear.x > - static_object_velocity_threshold) { - return false; - } - - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto object_closest_index = - motion_utils::findNearestIndex(path.points, object_pose.position); - const auto object_closest_pose = path.points.at(object_closest_index).point.pose; - - lanelet::ConstLanelet closest_lanelet; - if (!route_handler.getClosestLaneletWithinRoute(object_closest_pose, &closest_lanelet)) { - return false; - } - - const double lat_dist = motion_utils::calcLateralOffset(path.points, object_pose.position); - lanelet::BasicLineString2d bound; - double center_to_bound_buffer = 0.0; - if (lat_dist > 0.0) { - // left side vehicle - const auto most_left_road_lanelet = route_handler.getMostLeftLanelet(closest_lanelet); - const auto most_left_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_left_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_left_lanelet = ll; - } - } - bound = most_left_lanelet.leftBound2d().basicLineString(); - if (sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } else { - // right side vehicle - const auto most_right_road_lanelet = route_handler.getMostRightLanelet(closest_lanelet); - const auto most_right_lanelet_candidates = - route_handler.getLaneletMapPtr()->laneletLayer.findUsages( - most_right_road_lanelet.rightBound()); - - lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute sub_type = - most_right_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_right_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_right_lanelet = ll; - } - } - bound = most_right_lanelet.rightBound2d().basicLineString(); - if (sub_type.value() != "road_shoulder") { - center_to_bound_buffer = object_check_min_road_shoulder_width; - } - } - - return isParkedObject( - closest_lanelet, bound, object, center_to_bound_buffer, object_shiftable_ratio_threshold); -} - -bool isParkedObject( - const lanelet::ConstLanelet & closest_lanelet, const lanelet::BasicLineString2d & boundary, - const PredictedObject & object, const double buffer_to_bound, const double ratio_threshold) -{ - using lanelet::geometry::distance2d; - - const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); - const auto obj_point = object.kinematics.initial_pose_with_covariance.pose.position; - - double max_dist_to_bound = std::numeric_limits::lowest(); - double min_dist_to_bound = std::numeric_limits::max(); - for (const auto & edge : obj_poly.outer()) { - const auto ll_edge = lanelet::Point2d(lanelet::InvalId, edge.x(), edge.y()); - const auto dist = distance2d(boundary, ll_edge); - max_dist_to_bound = std::max(dist, max_dist_to_bound); - min_dist_to_bound = std::min(dist, min_dist_to_bound); - } - const double obj_width = std::max(max_dist_to_bound - min_dist_to_bound, 0.0); - - // distance from centerline to the boundary line with object width - const auto centerline_pose = lanelet::utils::getClosestCenterPose(closest_lanelet, obj_point); - const lanelet::BasicPoint3d centerline_point( - centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); - const double dist_bound_to_centerline = - std::abs(distance2d(boundary, centerline_point)) - 0.5 * obj_width + buffer_to_bound; - - // distance from object point to centerline - const auto centerline = closest_lanelet.centerline(); - const auto ll_obj_point = lanelet::Point2d(lanelet::InvalId, obj_point.x, obj_point.y); - const double dist_obj_to_centerline = std::abs(distance2d(centerline, ll_obj_point)); - - const double ratio = dist_obj_to_centerline / std::max(dist_bound_to_centerline, 1e-6); - const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); - return clamped_ratio > ratio_threshold; -} - } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index 885590f92d420..ec2d01e44940a 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -48,9 +48,6 @@ std::shared_ptr generateNode() const auto behavior_path_planner_dir = ament_index_cpp::get_package_share_directory("behavior_path_planner"); - node_options.append_parameter_override( - "bt_tree_config_path", behavior_path_planner_dir + "/config/behavior_path_planner_tree.xml"); - test_utils::updateNodeOptions( node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", diff --git a/planning/behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_no_drivable_lane_module/package.xml index ad21ed5328e49..061cf8d367ba9 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/package.xml @@ -6,6 +6,12 @@ The behavior_velocity_no_drivable_lane_module package Ahmed Ebrahim + Tomoya Kimura + Shumpei Wakabayashi + Takamasa Horibe + Takayuki Murooka + Fumiya Watanabe + Satoshi Ota Apache License 2.0 diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 293eee96dffdf..6a7a6f698bfbc 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -22,23 +22,15 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 - cv_bridge diagnostic_msgs eigen geometry_msgs - grid_map_cv - grid_map_ros - grid_map_utils interpolation lanelet2_extension libboost-dev - libopencv-dev - magic_enum - message_filters motion_utils motion_velocity_smoother nav_msgs - nlohmann-json-dev pcl_conversions rclcpp rclcpp_components diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index eff2a653f39e2..15210b267befb 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -451,8 +451,8 @@ void MPTOptimizer::updateVehicleCircles() std::tie(vehicle_circle_radiuses_, vehicle_circle_longitudinal_offsets_) = calcVehicleCirclesByBicycleModel( vehicle_info_, p.vehicle_circles_bicycle_model_num, - p.vehicle_circles_bicycle_model_front_radius_ratio, - p.vehicle_circles_bicycle_model_rear_radius_ratio); + p.vehicle_circles_bicycle_model_rear_radius_ratio, + p.vehicle_circles_bicycle_model_front_radius_ratio); } else if (p.vehicle_circles_method == "fitting_uniform_circle") { std::tie(vehicle_circle_radiuses_, vehicle_circle_longitudinal_offsets_) = calcVehicleCirclesByFittingUniformCircle( diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 8b5c7524e7a25..6ad42e1f8b03c 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -442,7 +442,7 @@ class RouteHandler * @return true if a path without any no_drivable_lane found, false if this path is not found. */ bool findDrivableLanePath( - const lanelet::Lanelet & start_lanelet, const lanelet::Lanelet & goal_lanelet, + const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet, lanelet::routing::LaneletPath & drivable_lane_path) const; }; } // namespace route_handler diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index f3db1cadf02a1..f4e5ca0399d63 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1971,52 +1971,68 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints( const Pose & start_checkpoint, const Pose & goal_checkpoint, lanelet::ConstLanelets * path_lanelets) const { - lanelet::Lanelet start_lanelet; - if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, start_checkpoint, &start_lanelet)) { + lanelet::ConstLanelet start_lanelet; + lanelet::ConstLanelets start_lanelets; + if (!lanelet::utils::query::getCurrentLanelets( + road_lanelets_, start_checkpoint, &start_lanelets)) { return false; } - lanelet::Lanelet goal_lanelet; + lanelet::ConstLanelet goal_lanelet; if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal_checkpoint, &goal_lanelet)) { return false; } - // get all possible lanes that can be used to reach goal (including all possible lane change) - const lanelet::Optional optional_route = - routing_graph_ptr_->getRoute(start_lanelet, goal_lanelet, 0); - if (!optional_route) { - RCLCPP_ERROR_STREAM( - logger_, "Failed to find a proper path!" - << std::endl - << "start checkpoint: " << toString(start_checkpoint) << std::endl - << "goal checkpoint: " << toString(goal_checkpoint) << std::endl - << "start lane id: " << start_lanelet.id() << std::endl - << "goal lane id: " << goal_lanelet.id() << std::endl); - return false; - } + lanelet::Optional optional_route; + std::vector candidate_paths; + lanelet::routing::LaneletPath shortest_path; + bool is_route_found = false; - const lanelet::routing::LaneletPath shortest_path = optional_route->shortestPath(); - bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path); lanelet::routing::LaneletPath drivable_lane_path; bool drivable_lane_path_found = false; + double shortest_path_length2d = std::numeric_limits::max(); + + for (const auto & st_llt : start_lanelets) { + optional_route = routing_graph_ptr_->getRoute(st_llt, goal_lanelet, 0); + if (!optional_route) { + RCLCPP_ERROR_STREAM( + logger_, "Failed to find a proper path!" + << std::endl + << "start checkpoint: " << toString(start_checkpoint) << std::endl + << "goal checkpoint: " << toString(goal_checkpoint) << std::endl + << "start lane id: " << st_llt.id() << std::endl + << "goal lane id: " << goal_lanelet.id() << std::endl); + } else { + is_route_found = true; - if (shortest_path_has_no_drivable_lane) { - drivable_lane_path_found = - findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path); + if (optional_route->length2d() < shortest_path_length2d) { + shortest_path_length2d = optional_route->length2d(); + shortest_path = optional_route->shortestPath(); + start_lanelet = st_llt; + } + } } - lanelet::routing::LaneletPath path; - if (drivable_lane_path_found) { - path = drivable_lane_path; - } else { - path = shortest_path; - } + if (is_route_found) { + bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path); + if (shortest_path_has_no_drivable_lane) { + drivable_lane_path_found = + findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path); + } - path_lanelets->reserve(path.size()); - for (const auto & llt : path) { - path_lanelets->push_back(llt); + lanelet::routing::LaneletPath path; + if (drivable_lane_path_found) { + path = drivable_lane_path; + } else { + path = shortest_path; + } + + path_lanelets->reserve(path.size()); + for (const auto & llt : path) { + path_lanelets->push_back(llt); + } } - return true; + return is_route_found; } std::vector RouteHandler::createMapSegments( @@ -2072,7 +2088,7 @@ bool RouteHandler::hasNoDrivableLaneInPath(const lanelet::routing::LaneletPath & } bool RouteHandler::findDrivableLanePath( - const lanelet::Lanelet & start_lanelet, const lanelet::Lanelet & goal_lanelet, + const lanelet::ConstLanelet & start_lanelet, const lanelet::ConstLanelet & goal_lanelet, lanelet::routing::LaneletPath & drivable_lane_path) const { double drivable_lane_path_length2d = std::numeric_limits::max(); diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index 3fe544c9b561e..691ae7960968c 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -91,7 +91,7 @@ PathWithLaneId get_path_with_lane_id( constexpr double vehicle_length = 0.0; const auto drivable_lanes = behavior_path_planner::utils::generateDrivableLanes(lanelets); behavior_path_planner::utils::generateDrivableArea( - path_with_lane_id, drivable_lanes, false, vehicle_length, planner_data); + path_with_lane_id, drivable_lanes, false, false, vehicle_length, planner_data); return path_with_lane_id; } diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index b0c9f549650fd..6036e760fed47 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -359,7 +359,7 @@ void DummyPerceptionPublisherNode::objectCallback( ros_map2base_link = tf_buffer_.lookupTransform( "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); object.initial_state.pose_covariance.pose.position.z = - ros_map2base_link.transform.translation.z; + ros_map2base_link.transform.translation.z + 0.5 * object.shape.dimensions.z; } catch (tf2::TransformException & ex) { RCLCPP_WARN_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); return; @@ -406,7 +406,7 @@ void DummyPerceptionPublisherNode::objectCallback( ros_map2base_link = tf_buffer_.lookupTransform( "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); objects_.at(i).initial_state.pose_covariance.pose.position.z = - ros_map2base_link.transform.translation.z; + ros_map2base_link.transform.translation.z + 0.5 * objects_.at(i).shape.dimensions.z; } catch (tf2::TransformException & ex) { RCLCPP_WARN_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); return; diff --git a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md b/simulator/simple_planning_simulator/README.md similarity index 100% rename from simulator/simple_planning_simulator/design/simple_planning_simulator-design.md rename to simulator/simple_planning_simulator/README.md diff --git a/system/component_state_monitor/README.md b/system/component_state_monitor/README.md new file mode 100644 index 0000000000000..327c7b7651caf --- /dev/null +++ b/system/component_state_monitor/README.md @@ -0,0 +1,5 @@ +# component_state_monitor + +The component state monitor checks the state of each component using topic state monitor. +This is an implementation for backward compatibility with the AD service state monitor. +It will be replaced in the future using a diagnostics tree. diff --git a/tools/simulator_test/simulator_compatibility_test/package.xml b/tools/simulator_test/simulator_compatibility_test/package.xml index ffc679ef2964c..d0ed3e69b9c30 100644 --- a/tools/simulator_test/simulator_compatibility_test/package.xml +++ b/tools/simulator_test/simulator_compatibility_test/package.xml @@ -5,6 +5,8 @@ 0.0.0 TODO: Package description shpark + Tomoya Kimura + Shumpei Wakabayashi TODO: License declaration autoware_auto_control_msgs