diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp new file mode 100644 index 0000000000000..4d5acd544b41d --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp @@ -0,0 +1,33 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_AD_API_SPECS__VEHICLE_HPP_ +#define AUTOWARE_AD_API_SPECS__VEHICLE_HPP_ + +#include + +#include + +namespace autoware_ad_api::vehicle +{ + +struct Dimensions +{ + using Service = autoware_adapi_v1_msgs::srv::GetVehicleDimensions; + static constexpr char name[] = "/api/vehicle/dimensions"; +}; + +} // namespace autoware_ad_api::vehicle + +#endif // AUTOWARE_AD_API_SPECS__VEHICLE_HPP_ diff --git a/perception/probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml b/perception/probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml index afb776d9fd961..4e335e35740ab 100644 --- a/perception/probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml +++ b/perception/probabilistic_occupancy_grid_map/config/binary_bayes_filter_updater.param.yaml @@ -1,9 +1,8 @@ /**: ros__parameters: - binary_bayes_filter: - probability_matrix: - occupied_to_occupied: 0.95 - occupied_to_free: 0.05 - free_to_occupied: 0.2 - free_to_free: 0.8 - v_ratio: 10.0 + probability_matrix: + occupied_to_occupied: 0.95 + occupied_to_free: 0.05 + free_to_occupied: 0.2 + free_to_free: 0.8 + v_ratio: 10.0 diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index 26c7482943333..355cc4bd8b5bd 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -107,6 +107,7 @@ def launch_setup(context, *args, **kwargs): "input_obstacle_and_raw_pointcloud": LaunchConfiguration( "input_obstacle_and_raw_pointcloud" ), + "updater_type": LaunchConfiguration("updater_type"), }, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -164,10 +165,7 @@ def add_launch_arg(name: str, default_value=None): get_package_share_directory("probabilistic_occupancy_grid_map") + "/config/laserscan_based_occupancy_grid_map.param.yaml", ), - add_launch_arg( - "updater_type", - "binary_bayes_filter", - ), + add_launch_arg("updater_type", "binary_bayes_filter"), add_launch_arg( "updater_param_file", get_package_share_directory("probabilistic_occupancy_grid_map") diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index f6cd4200f6e4b..5b6e0d1c3c242 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -26,11 +26,6 @@ import yaml -def overwrite_config(param_dict, launch_config_name, node_params_name, context): - if LaunchConfiguration(launch_config_name).perform(context) != "": - param_dict[node_params_name] = LaunchConfiguration(launch_config_name).perform(context) - - def launch_setup(context, *args, **kwargs): # load parameter files param_file = LaunchConfiguration("param_file").perform(context) @@ -62,6 +57,7 @@ def launch_setup(context, *args, **kwargs): parameters=[ pointcloud_based_occupancy_grid_map_node_params, occupancy_grid_map_updater_params, + {"updater_type": LaunchConfiguration("updater_type")}, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), @@ -116,10 +112,7 @@ def add_launch_arg(name: str, default_value=None): get_package_share_directory("probabilistic_occupancy_grid_map") + "/config/pointcloud_based_occupancy_grid_map.param.yaml", ), - add_launch_arg( - "updater_type", - "binary_bayes_filter", - ), + add_launch_arg("updater_type", "binary_bayes_filter"), add_launch_arg( "updater_param_file", get_package_share_directory("probabilistic_occupancy_grid_map") diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp index 01763b00d2a87..16de17b1c2e61 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_binary_bayes_filter_updater.cpp @@ -30,14 +30,14 @@ OccupancyGridMapBBFUpdater::OccupancyGridMapBBFUpdater( void OccupancyGridMapBBFUpdater::initRosParam(rclcpp::Node & node) { probability_matrix_(Index::OCCUPIED, Index::OCCUPIED) = - node.declare_parameter("binary_bayes_filter.probability_matrix.occupied_to_occupied"); + node.declare_parameter("probability_matrix.occupied_to_occupied"); probability_matrix_(Index::FREE, Index::OCCUPIED) = - node.declare_parameter("binary_bayes_filter.probability_matrix.occupied_to_free"); + node.declare_parameter("probability_matrix.occupied_to_free"); probability_matrix_(Index::FREE, Index::FREE) = - node.declare_parameter("binary_bayes_filter.probability_matrix.free_to_free"); + node.declare_parameter("probability_matrix.free_to_free"); probability_matrix_(Index::OCCUPIED, Index::FREE) = - node.declare_parameter("binary_bayes_filter.probability_matrix.free_to_occupied"); - v_ratio_ = node.declare_parameter("binary_bayes_filter.v_ratio"); + node.declare_parameter("probability_matrix.free_to_occupied"); + v_ratio_ = node.declare_parameter("v_ratio"); } inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index de3802b070326..95ff106eb4c00 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -15,6 +15,10 @@ longitudinal_acceleration_sampling_num: 5 lateral_acceleration_sampling_num: 3 + # side walk parked vehicle + object_check_min_road_shoulder_width: 0.5 # [m] + object_shiftable_ratio_threshold: 0.6 + # turn signal min_length_for_turn_signal_activation: 10.0 # [m] length_ratio_for_turn_signal_deactivation: 0.8 # ratio (desired end position) diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index 42d3fad3e49bc..b023b0dff9602 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -588,13 +588,13 @@ namespace: `avoidance.` | resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | | detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | | detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | -| object_envelope_buffer | [m] | double | Envelope object polygon with buffer in order to prevent shift line chattering. | 0.3 | | enable_bound_clipping | [-] | bool | Enable clipping left and right bound of drivable area when obstacles are in the drivable area | false | | enable_avoidance_over_same_direction | [-] | bool | Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane. | true | | enable_avoidance_over_opposite_direction | [-] | bool | Extend avoidance trajectory to adjacent lanes that has opposite direction. `enable_avoidance_over_same_direction` must be `true` to take effects | true | | enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | | enable_safety_check | [-] | bool | Flag to enable safety check. | false | | enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | +| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | | publish_debug_marker | [-] | bool | Flag to publish debug marker (set `false` as default since it takes considerable cost). | false | | print_debug_info | [-] | bool | Flag to print debug info (set `false` as default since it takes considerable cost). | false | @@ -604,31 +604,52 @@ namespace: `avoidance.` namespace: `avoidance.target_object.` -| Name | Unit | Type | Description | Default value | -| :--------- | ---- | ---- | ------------------------------------------ | ------------- | -| car | [-] | bool | Allow avoidance for object type CAR | true | -| truck | [-] | bool | Allow avoidance for object type TRUCK | true | -| bus | [-] | bool | Allow avoidance for object type BUS | true | -| trailer | [-] | bool | Allow avoidance for object type TRAILER | true | -| unknown | [-] | bool | Allow avoidance for object type UNKNOWN | false | -| bicycle | [-] | bool | Allow avoidance for object type BICYCLE | false | -| motorcycle | [-] | bool | Allow avoidance for object type MOTORCYCLE | false | -| pedestrian | [-] | bool | Allow avoidance for object type PEDESTRIAN | false | +This module supports all object classes, and it can set following parameters independently. + +```yaml +car: + enable: true # [-] + moving_speed_threshold: 1.0 # [m/s] + moving_time_threshold: 1.0 # [s] + max_expand_ratio: 0.0 # [-] + envelope_buffer_margin: 0.3 # [m] + avoid_margin_lateral: 1.0 # [m] + safety_buffer_lateral: 0.7 # [m] + safety_buffer_longitudinal: 0.0 # [m] +``` + +| Name | Unit | Type | Description | Default value | +| :------------------------- | ----- | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | +| enable | [-] | bool | Allow avoidance for object type CAR | false | +| moving_speed_threshold | [m/s] | double | Objects with speed greater than this will be judged as moving ones. | 1.0 | +| moving_time_threshold | [s] | double | Objects keep moving longer duration than this will be excluded from avoidance target. | 1.0 | +| envelope_buffer_margin | [m] | double | Allow avoidance for object type TRAILER | 0.3 | +| avoid_margin_lateral | [m] | double | The lateral distance between ego and avoidance targets. | 1.0 | +| safety_buffer_lateral | [m] | double | Creates an additional lateral gap that will prevent the vehicle from getting to near to the obstacle. | 0.5 | +| safety_buffer_longitudinal | [m] | double | Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle. | 0.0 | + +Parameters for the logic to compensate perception noise of the far objects. + +| Name | Unit | Type | Description | Default value | +| :----------------------------------- | ---- | ------ | ------------------------------------------------------------------------------------------------------------ | ------------- | +| max_expand_ratio | [-] | double | This value will be applied `envelope_buffer_margin` according to the distance between the ego and object. | 0.0 | +| lower_distance_for_polygon_expansion | [-] | double | If the distance between the ego and object is less than this, the expand ratio will be zero. | 30.0 | +| upper_distance_for_polygon_expansion | [-] | double | If the distance between the ego and object is larger than this, the expand ratio will be `max_expand_ratio`. | 100.0 | namespace: `avoidance.target_filtering.` -| Name | Unit | Type | Description | Default value | -| :------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| threshold_speed_object_is_stopped | [m/s] | double | Vehicles with speed greater than this will be excluded from avoidance target. | 1.0 | -| threshold_time_object_is_moving | [s] | double | Forward distance to search the avoidance target. | 1.0 | -| threshold_distance_object_is_on_center | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 1.0 | -| object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 | -| object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | -| object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 | -| object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | -| object_check_min_road_shoulder_width | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.5 | -| object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | -| left_hand_traffic | [-] | bool | Flag to select left or right hand traffic. `TRUE: LEFT-HAND TRAFFIC / FALSE: RIGHT-HAND TRAFFIC` | true | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| threshold_distance_object_is_on_center | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 1.0 | +| object_ignore_distance_traffic_light | [m] | double | If the distance between traffic light and vehicle is less than this parameter, this module will ignore it. | 30.0 | +| object_ignore_distance_crosswalk_forward | [m] | double | If the front distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | +| object_ignore_distance_crosswalk_backward | [m] | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | +| object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 | +| object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | +| object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 | +| object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | +| object_check_min_road_shoulder_width | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.5 | +| object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | ### Safety check parameters @@ -646,14 +667,13 @@ namespace: `avoidance.safety_check.` namespace: `avoidance.avoidance.lateral.` -| Name | Unit | Type | Description | Default value | -| :------------------------------------ | :--- | :----- | :-------------------------------------------------------------------------------------------------------------------------- | :------------ | -| lateral_collision_margin | [m] | double | The lateral distance between ego and avoidance targets. | 1.0 | -| lateral_collision_safety_buffer | [m] | double | Creates an additional gap that will prevent the vehicle from getting to near to the obstacle | 0.7 | -| road_shoulder_safety_margin | [m] | double | Prevents the generated path to come too close to the road shoulders. | 0.0 | -| avoidance_execution_lateral_threshold | [m] | double | The lateral distance deviation threshold between the current path and suggested avoidance point to execute avoidance. (\*2) | 0.499 | -| max_right_shift_length | [m] | double | Maximum shift length for right direction | 5.0 | -| max_left_shift_length | [m] | double | Maximum shift length for left direction | 5.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------------------- | :------------ | +| road_shoulder_safety_margin | [m] | double | Prevents the generated path to come too close to the road shoulders. | 0.0 | +| lateral_execution_threshold | [m] | double | The lateral distance deviation threshold between the current path and suggested avoidance point to execute avoidance. (\*2) | 0.499 | +| lateral_small_shift_threshold | [m] | double | The shift lines whose lateral offset is less than this will be applied with other ones. | 0.501 | +| max_right_shift_length | [m] | double | Maximum shift length for right direction | 5.0 | +| max_left_shift_length | [m] | double | Maximum shift length for left direction | 5.0 | namespace: `avoidance.avoidance.longitudinal.` 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 94a4cb9462a2c..f8d80d8883b38 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 @@ -85,6 +85,8 @@ class LaneChangeBase virtual bool hasFinishedAbort() const = 0; + virtual bool isLaneChangeRequired() const = 0; + virtual bool isAbortState() const = 0; virtual bool isAbleToReturnCurrentLane() const = 0; @@ -197,7 +199,7 @@ class LaneChangeBase virtual bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, Direction direction, - LaneChangePaths * candidate_paths) const = 0; + LaneChangePaths * candidate_paths, const bool check_safety) const = 0; virtual std::vector getDrivableLanes() const = 0; 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 6572d4f8221b0..f7c6cdc07a200 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 @@ -85,6 +85,8 @@ class NormalLaneChange : public LaneChangeBase bool isAbortState() const override; + bool isLaneChangeRequired() const override; + protected: lanelet::ConstLanelets getCurrentLanes() const override; @@ -100,7 +102,7 @@ class NormalLaneChange : public LaneChangeBase bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, Direction direction, - LaneChangePaths * candidate_paths) const override; + LaneChangePaths * candidate_paths, const bool check_safety = true) const override; std::vector getDrivableLanes() const override; 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 85a77df4c1e0d..4def596f78746 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 @@ -34,6 +34,10 @@ struct LaneChangeParameters int longitudinal_acc_sampling_num{10}; int lateral_acc_sampling_num{10}; + // parked vehicle + double object_check_min_road_shoulder_width{0.5}; + double object_shiftable_ratio_threshold{0.6}; + // turn signal double min_length_for_turn_signal_activation{10.0}; double length_ratio_for_turn_signal_deactivation{0.8}; 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 5b16a67f5349f..9570ffec56576 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 @@ -174,5 +174,17 @@ PredictedPath convertToPredictedPath( const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose, 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 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); + +boost::optional getLeadingStaticObjectIdx( + const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const PredictedObjects & objects, const std::vector & obj_indices, + const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index f7336dad3f895..0384a702ff129 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -787,6 +787,12 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.lateral_acc_sampling_num = declare_parameter(parameter("lateral_acceleration_sampling_num")); + // parked vehicle detection + p.object_check_min_road_shoulder_width = + declare_parameter(parameter("object_check_min_road_shoulder_width")); + p.object_shiftable_ratio_threshold = + declare_parameter(parameter("object_shiftable_ratio_threshold")); + // turn signal p.min_length_for_turn_signal_activation = declare_parameter(parameter("min_length_for_turn_signal_activation")); diff --git a/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp index 8c9734924e14e..1ae1f8a03e0c0 100644 --- a/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp @@ -185,11 +185,12 @@ MarkerArray showPolygon( } const auto & color = colors.at(idx); const auto & ego_polygon = info.ego_polygon.outer(); + const auto poly_z = info.current_pose.position.z; // temporally ego_marker.id = ++id; ego_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); ego_marker.points.reserve(ego_polygon.size()); for (const auto & p : ego_polygon) { - ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0)); + ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); } marker_array.markers.push_back(ego_marker); @@ -198,7 +199,7 @@ MarkerArray showPolygon( obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); obj_marker.points.reserve(obj_polygon.size()); for (const auto & p : obj_polygon) { - obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0)); + obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); } marker_array.markers.push_back(obj_marker); ++idx; 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 8c0d6d4acd48a..91f1c23c3008d 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 @@ -3045,7 +3045,7 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con } // compute blinker start idx and end idx - const size_t blinker_start_idx = [&]() { + size_t blinker_start_idx = [&]() { for (size_t idx = start_idx; idx <= end_idx; ++idx) { const double current_shift_length = path.shift_length.at(idx); if (current_shift_length > 0.1) { @@ -3054,7 +3054,13 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con } return start_idx; }(); - const size_t blinker_end_idx = end_idx; + size_t blinker_end_idx = end_idx; + + // prevent invalid access for out-of-range + blinker_start_idx = + std::min(std::max(std::size_t(0), blinker_start_idx), path.path.points.size() - 1); + blinker_end_idx = + std::min(std::max(blinker_start_idx, blinker_end_idx), path.path.points.size() - 1); const auto blinker_start_pose = path.path.points.at(blinker_start_idx).point.pose; const auto blinker_end_pose = path.path.points.at(blinker_end_idx).point.pose; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 81df39cb8f789..26051c4d78f3e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -67,22 +67,12 @@ bool LaneChangeInterface::isExecutionRequested() const return true; } - LaneChangePath selected_path; - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - const auto [found_valid_path, found_safe_path] = module_type_->getSafePath(selected_path); - - return found_valid_path; + return module_type_->isLaneChangeRequired(); } bool LaneChangeInterface::isExecutionReady() const { - LaneChangePath selected_path; - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - const auto [found_valid_path, found_safe_path] = module_type_->getSafePath(selected_path); - - return found_safe_path; + return module_type_->isSafe(); } ModuleStatus LaneChangeInterface::updateState() 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 548d23642581e..b56e382cc52ce 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 @@ -89,6 +89,28 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) return {true, found_safe_path}; } +bool NormalLaneChange::isLaneChangeRequired() const +{ + const auto current_lanes = getCurrentLanes(); + + if (current_lanes.empty()) { + return false; + } + + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + + if (target_lanes.empty()) { + return false; + } + + // find candidate paths + LaneChangePaths valid_paths{}; + [[maybe_unused]] const auto found_safe_path = + getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths, false); + + return !valid_paths.empty(); +} + LaneChangePath NormalLaneChange::getLaneChangePath() const { return isAbortState() ? *abort_path_ : status_.lane_change_path; @@ -472,7 +494,7 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - Direction direction, LaneChangePaths * candidate_paths) const + Direction direction, LaneChangePaths * candidate_paths, const bool check_safety) const { object_debug_.clear(); if (original_lanelets.empty() || target_lanelets.empty()) { @@ -517,10 +539,10 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanelets.back()); - const auto shift_intervals = - route_handler.getLateralIntervalsToPreferredLane(target_lanelets.back()); - const double next_lane_change_buffer = - utils::calcMinimumLaneChangeLength(common_parameter, shift_intervals); + const double lane_change_buffer = utils::calcMinimumLaneChangeLength( + common_parameter, route_handler.getLateralIntervalsToPreferredLane(original_lanelets.back())); + const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( + common_parameter, route_handler.getLateralIntervalsToPreferredLane(target_lanelets.back())); const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), original_lanelets); @@ -703,9 +725,27 @@ bool NormalLaneChange::getLaneChangePaths( {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, getEgoPose(), common_parameter.forward_path_length, *lane_change_parameters_, lateral_buffer); + + const double object_check_min_road_shoulder_width = + lane_change_parameters_->object_check_min_road_shoulder_width; + const double object_shiftable_ratio_threshold = + lane_change_parameters_->object_shiftable_ratio_threshold; + 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); + if (pass_parked_object) { + return false; + } } candidate_paths->push_back(*candidate_path); + if (!check_safety) { + return false; + } + 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, 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 37ce67a2927f0..919541f5972a4 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1058,4 +1058,109 @@ PredictedPath convertToPredictedPath( return predicted_path; } + +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 auto & path = lane_change_path.path; + + if (target_lane_obj_indices.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); + 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); + if (leading_obj_poly.outer().empty()) { + return false; + } + + const auto & current_path_end = current_lane_path.points.back().point.pose.position; + double min_dist_to_end_of_current_lane = std::numeric_limits::max(); + for (const auto & point : leading_obj_poly.outer()) { + const auto obj_p = tier4_autoware_utils::createPoint(point.x(), point.y(), 0.0); + const double dist = + motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, current_path_end); + min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); + if (is_goal_in_route) { + const auto goal_pose = route_handler.getGoalPose(); + const double dist_to_goal = + motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, goal_pose.position); + min_dist_to_end_of_current_lane = std::min(min_dist_to_end_of_current_lane, dist_to_goal); + } + } + + // If there are still enough length after the target object, we delay the lane change + if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { + return true; + } + + return false; +} + +boost::optional getLeadingStaticObjectIdx( + const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const PredictedObjects & objects, const std::vector & obj_indices, + 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()) { + return {}; + } + + const auto & lane_change_start = lane_change_path.lane_changing_start; + const auto & path_end = path.points.back(); + + 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; + + // ignore non-static object + // TODO(shimizu): parametrize threshold + if (obj.kinematics.initial_twist_with_covariance.twist.linear.x > 1.0) { + continue; + } + + // ignore non-parked object + if (!isParkedObject( + path, route_handler, obj, object_check_min_road_shoulder_width, + object_shiftable_ratio_threshold)) { + continue; + } + + const double dist_back_to_obj = motion_utils::calcSignedArcLength( + path.points, path_end.point.pose.position, obj_pose.position); + if (dist_back_to_obj > 0.0) { + // object is not on the lane change path + continue; + } + + const double dist_lc_start_to_obj = + motion_utils::calcSignedArcLength(path.points, lane_change_start.position, obj_pose.position); + if (dist_lc_start_to_obj < 0.0) { + // object is on the lane changing path or behind it. It will be detected in safety check + continue; + } + + if (dist_lc_start_to_obj > dist_lc_start_to_leading_obj) { + dist_lc_start_to_leading_obj = dist_lc_start_to_obj; + leading_obj_idx = obj_idx; + } + } + + return leading_obj_idx; +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 7de19df1b3b4e..0c9b27a685583 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -391,7 +391,7 @@ void MissionPlanner::on_set_mrm_route( // check route safety // step1. if in mrm state, check with mrm route if (mrm_route_) { - if (checkRerouteSafety(*mrm_route_, new_route)) { + if (check_reroute_safety(*mrm_route_, new_route)) { // success to reroute change_mrm_route(new_route); res->status.success = true; @@ -413,7 +413,7 @@ void MissionPlanner::on_set_mrm_route( } // step2. if not in mrm state, check with normal route - if (checkRerouteSafety(*normal_route_, new_route)) { + if (check_reroute_safety(*normal_route_, new_route)) { // success to reroute change_mrm_route(new_route); res->status.success = true; @@ -455,7 +455,7 @@ void MissionPlanner::on_clear_mrm_route( } // check route safety - if (checkRerouteSafety(*mrm_route_, *normal_route_)) { + if (check_reroute_safety(*mrm_route_, *normal_route_)) { clear_mrm_route(); change_route(*normal_route_); change_state(RouteState::Message::SET); @@ -470,7 +470,7 @@ void MissionPlanner::on_clear_mrm_route( normal_route_->allow_modification); // check new route safety - if (new_route.segments.empty() || !checkRerouteSafety(*mrm_route_, new_route)) { + if (new_route.segments.empty() || !check_reroute_safety(*mrm_route_, new_route)) { // failed to create a new route RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 5000, "Reroute with normal goal failed."); change_mrm_route(*mrm_route_); @@ -503,9 +503,25 @@ void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPt RCLCPP_ERROR(get_logger(), "Normal route has not set yet."); return; } - if (mrm_route_) { - RCLCPP_ERROR_THROTTLE( - get_logger(), *get_clock(), 5000, "Cannot modify goal in the emergency state"); + + if (mrm_route_ && mrm_route_->uuid == msg->uuid) { + // set to changing state + change_state(RouteState::Message::CHANGING); + + const std::vector empty_waypoints; + auto new_route = + create_route(msg->header, empty_waypoints, msg->pose, mrm_route_->allow_modification); + // create_route generate new uuid, so set the original uuid again to keep that. + new_route.uuid = msg->uuid; + if (new_route.segments.empty()) { + change_mrm_route(*mrm_route_); + change_state(RouteState::Message::SET); + RCLCPP_ERROR(get_logger(), "The planned route is empty."); + return; + } + + change_mrm_route(new_route); + change_state(RouteState::Message::SET); return; } @@ -575,7 +591,7 @@ void MissionPlanner::on_change_route( } // check route safety - if (checkRerouteSafety(*normal_route_, new_route)) { + if (check_reroute_safety(*normal_route_, new_route)) { // success to reroute change_route(new_route); res->status.success = true; @@ -633,7 +649,7 @@ void MissionPlanner::on_change_route_points( } // check route safety - if (checkRerouteSafety(*normal_route_, new_route)) { + if (check_reroute_safety(*normal_route_, new_route)) { // success to reroute change_route(new_route); res->status.success = true; @@ -648,7 +664,7 @@ void MissionPlanner::on_change_route_points( } } -bool MissionPlanner::checkRerouteSafety( +bool MissionPlanner::check_reroute_safety( const LaneletRoute & original_route, const LaneletRoute & target_route) { if (original_route.segments.empty() || target_route.segments.empty() || !map_ptr_ || !odometry_) { diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index d3ca6be7eb63f..4d09f72f81899 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -135,7 +135,7 @@ class MissionPlanner : public rclcpp::Node double reroute_time_threshold_{10.0}; double minimum_reroute_length_{30.0}; - bool checkRerouteSafety(const LaneletRoute & original_route, const LaneletRoute & target_route); + bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route); std::shared_ptr normal_route_{nullptr}; std::shared_ptr mrm_route_{nullptr}; diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index a09cb1222d2f9..28eec2e8eefc0 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/operation_mode.cpp src/planning.cpp src/routing.cpp + src/vehicle_info.cpp src/utils/route_conversion.cpp src/compatibility/autoware_state.cpp ) @@ -25,6 +26,7 @@ rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::OperationModeNode" "default_ad_api::PlanningNode" "default_ad_api::RoutingNode" + "default_ad_api::VehicleInfoNode" ) if(BUILD_TESTING) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index 1cc57d5436cd0..c9a3abf9f8e5d 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): create_api_node("operation_mode", "OperationModeNode"), create_api_node("planning", "PlanningNode"), create_api_node("routing", "RoutingNode"), + create_api_node("vehicle_info", "VehicleInfoNode"), ] container = ComposableNodeContainer( namespace="default_ad_api", diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index d99037fdc42d0..3e11cbb2952ed 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -28,6 +28,7 @@ std_srvs tier4_control_msgs tier4_system_msgs + vehicle_info_util python3-flask diff --git a/system/default_ad_api/src/vehicle_info.cpp b/system/default_ad_api/src/vehicle_info.cpp new file mode 100644 index 0000000000000..cff7ac5cd5eae --- /dev/null +++ b/system/default_ad_api/src/vehicle_info.cpp @@ -0,0 +1,71 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "vehicle_info.hpp" + +#include + +namespace +{ + +auto create_point(double x, double y) +{ + geometry_msgs::msg::Point32 point; + point.x = x; + point.y = y; + point.z = 0.0; + return point; +} + +} // namespace + +namespace default_ad_api +{ + +VehicleInfoNode::VehicleInfoNode(const rclcpp::NodeOptions & options) +: Node("vehicle_info", options) +{ + const auto on_vehicle_dimensions = [this](auto, auto res) { + res->status.success = true; + res->dimensions = dimensions_; + }; + + const auto vehicle = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + dimensions_.wheel_radius = vehicle.wheel_radius_m; + dimensions_.wheel_width = vehicle.wheel_width_m; + dimensions_.wheel_base = vehicle.wheel_base_m; + dimensions_.wheel_tread = vehicle.wheel_tread_m; + dimensions_.front_overhang = vehicle.front_overhang_m; + dimensions_.rear_overhang = vehicle.rear_overhang_m; + dimensions_.left_overhang = vehicle.left_overhang_m; + dimensions_.right_overhang = vehicle.right_overhang_m; + dimensions_.height = vehicle.vehicle_height_m; + + const auto l = (vehicle.wheel_tread_m / 2.0) + vehicle.left_overhang_m; + const auto r = (vehicle.wheel_tread_m / 2.0) + vehicle.right_overhang_m; + const auto b = vehicle.rear_overhang_m; + const auto f = vehicle.front_overhang_m + vehicle.wheel_base_m; + dimensions_.footprint.points.push_back(create_point(+f, +r)); + dimensions_.footprint.points.push_back(create_point(+f, -l)); + dimensions_.footprint.points.push_back(create_point(-b, -l)); + dimensions_.footprint.points.push_back(create_point(-b, +r)); + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_srv(srv_dimensions_, on_vehicle_dimensions); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::VehicleInfoNode) diff --git a/system/default_ad_api/src/vehicle_info.hpp b/system/default_ad_api/src/vehicle_info.hpp new file mode 100644 index 0000000000000..c7171ddf52485 --- /dev/null +++ b/system/default_ad_api/src/vehicle_info.hpp @@ -0,0 +1,39 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef VEHICLE_INFO_HPP_ +#define VEHICLE_INFO_HPP_ + +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class VehicleInfoNode : public rclcpp::Node +{ +public: + explicit VehicleInfoNode(const rclcpp::NodeOptions & options); + +private: + Srv srv_dimensions_; + autoware_adapi_v1_msgs::msg::VehicleDimensions dimensions_; +}; + +} // namespace default_ad_api + +#endif // VEHICLE_INFO_HPP_