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/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/perception/radar_object_clustering/CMakeLists.txt b/perception/radar_object_clustering/CMakeLists.txt new file mode 100644 index 0000000000000..acb544bfe4f6a --- /dev/null +++ b/perception/radar_object_clustering/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.5) +project(radar_object_clustering) + +# Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(radar_object_clustering_node_component SHARED + src/radar_object_clustering_node/radar_object_clustering_node.cpp +) + +rclcpp_components_register_node(radar_object_clustering_node_component + PLUGIN "radar_object_clustering::RadarObjectClusteringNode" + EXECUTABLE radar_object_clustering_node +) + +# Tests +if(BUILD_TESTING) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +# Package +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/radar_object_clustering/README.md b/perception/radar_object_clustering/README.md new file mode 100644 index 0000000000000..6463dd7157dea --- /dev/null +++ b/perception/radar_object_clustering/README.md @@ -0,0 +1,78 @@ +# radar_object_clustering + +This package contains a radar object clustering for [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) input. + +This package can make clustered objects from radar DetectedObjects, the objects which is converted from RadarTracks by [radar_tracks_msgs_converter](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/radar_tracks_msgs_converter) and is processed by noise filter. +In other word, this package can combine multiple radar detections from one object into one and adjust class and size. + +![radar_clustering](docs/radar_clustering.drawio.svg) + +## Algorithm + +### Background + +In radars with object output, there are cases that multiple detection results are obtained from one object, especially for large vehicles such as trucks and trailers. +Its multiple detection results cause separation of objects in tracking module. +Therefore, by this package the multiple detection results are clustered into one object in advance. + +### Detail Algorithm + +- Sort by distance from `base_link` + +At first, to prevent changing the result from depending on the order of objects in DetectedObjects, input objects are sorted by distance from `base_link`. +In addition, to apply matching in closeness order considering occlusion, objects are sorted in order of short distance in advance. + +- Clustering + +If two radar objects are near, and yaw angle direction and velocity between two radar objects is similar (the degree of these is defined by parameters), then these are clustered. +Note that radar characteristic affect parameters for this matching. +For example, if resolution of range distance or angle is low and accuracy of velocity is high, then `distance_threshold` parameter should be bigger and should set matching that strongly looks at velocity similarity. + +![clustering](docs/clustering.drawio.svg) + +After grouping for all radar objects, if multiple radar objects are grouping, the kinematics of the new clustered object is calculated from average of that and label and shape of the new clustered object is calculated from top confidence in radar objects. + +- Fixed label correction + +When the label information from radar outputs lack accuracy, `is_fixed_label` parameter is recommended to set `true`. +If the parameter is true, the label of a clustered object is overwritten by the label set by `fixed_label` parameter. +If this package use for faraway dynamic object detection with radar, the parameter is recommended to set to `VEHICLE`. + +- Fixed size correction + +When the size information from radar outputs lack accuracy, `is_fixed_size` parameter is recommended to set `true`. +If the parameter is true, the size of a clustered object is overwritten by the label set by `size_x`, `size_y`, and `size_z` parameters. +If this package use for faraway dynamic object detection with radar, the parameter is recommended to set to +`size_x`, `size_y`, `size_z`, as average of vehicle size. +Note that to use for [multi_objects_tracker](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/multi_object_tracker), the size parameters need to exceed `min_area_matrix` parameters of it. + +### Limitation + +For now, size estimation for clustered object is not implemented. +So `is_fixed_size` parameter is recommended to set `true`, and size parameters is recommended to set to value near to average size of vehicles. + +## Input + +| Name | Type | Description | +| ----------------- | ----------------------------------------------------- | -------------- | +| `~/input/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Radar objects. | + +## Output + +| Name | Type | Description | +| ------------------ | ----------------------------------------------------- | -------------- | +| `~/output/objects` | autoware_auto_perception_msgs/msg/DetectedObjects.msg | Output objects | + +## Parameters + +| Name | Type | Description | Default value | +| :------------------- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `angle_threshold` | double | Angle threshold to judge whether radar detections come from one object. [rad] | 0.174 | +| `distance_threshold` | double | Distance threshold to judge whether radar detections come from one object. [m] | 4.0 | +| `velocity_threshold` | double | Velocity threshold to judge whether radar detections come from one object. [m/s] | 2.0 | +| `is_fixed_label` | bool | If this parameter is true, the label of a clustered object is overwritten by the label set by `fixed_label` parameter. | false | +| `fixed_label` | string | If `is_fixed_label` is true, the label of a clustered object is overwritten by this parameter. | "UNKNOWN" | +| `is_fixed_size` | bool | If this parameter is true, the size of a clustered object is overwritten by the label set by `size_x`, `size_y`, and `size_z` parameters. | false | +| `size_x` | double | If `is_fixed_size` is true, the x-axis size of a clustered object is overwritten by this parameter. [m] | 4.0 | +| `size_y` | double | If `is_fixed_size` is true, the y-axis size of a clustered object is overwritten by this parameter. [m] | 1.5 | +| `size_z` | double | If `is_fixed_size` is true, the z-axis size of a clustered object is overwritten by this parameter. [m] | 1.5 | diff --git a/perception/radar_object_clustering/docs/clustering.drawio.svg b/perception/radar_object_clustering/docs/clustering.drawio.svg new file mode 100644 index 0000000000000..64563a5f28ecd --- /dev/null +++ b/perception/radar_object_clustering/docs/clustering.drawio.svg @@ -0,0 +1,171 @@ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + distance_threshold +
+ +
+
+
+ + distance_t... + + + + + + + + + + + + + + + + +
+
+
+ + angle diffrence +
+ +
+
+
+ + angle diffr... + + + + + + + + + + + +
+
+
+ + velocity diffrence +
+ +
+
+
+ + velocity di... + + + + + + +
+
+
+ radar object +
+
+
+ + radar object + + + + + + +
+
+
+ clustered object +
+
+
+ + clustered object + + + + + + +
+
+
+ Text +
+
+
+ + Text + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/radar_object_clustering/docs/radar_clustering.drawio.svg b/perception/radar_object_clustering/docs/radar_clustering.drawio.svg new file mode 100644 index 0000000000000..4b512c52fdd38 --- /dev/null +++ b/perception/radar_object_clustering/docs/radar_clustering.drawio.svg @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp b/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp new file mode 100644 index 0000000000000..0e82c5388d297 --- /dev/null +++ b/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp @@ -0,0 +1,78 @@ +// 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 RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ +#define RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" + +#include +#include +#include +#include + +namespace radar_object_clustering +{ +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; + +class RadarObjectClusteringNode : public rclcpp::Node +{ +public: + explicit RadarObjectClusteringNode(const rclcpp::NodeOptions & node_options); + + struct NodeParam + { + double angle_threshold{}; + double distance_threshold{}; + double velocity_threshold{}; + bool is_fixed_label{}; + std::string fixed_label{}; + bool is_fixed_size{}; + double size_x{}; + double size_y{}; + double size_z{}; + }; + +private: + // Subscriber + rclcpp::Subscription::SharedPtr sub_objects_{}; + + // Callback + void onObjects(const DetectedObjects::ConstSharedPtr msg); + + // Data Buffer + DetectedObjects::ConstSharedPtr objects_data_{}; + + // Publisher + rclcpp::Publisher::SharedPtr pub_objects_{}; + + // Parameter Server + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onSetParam( + const std::vector & params); + + // Parameter + NodeParam node_param_{}; + + // Core + bool isSameObject(const DetectedObject & object_1, const DetectedObject & object_2); +}; + +} // namespace radar_object_clustering + +#endif // RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ diff --git a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml new file mode 100644 index 0000000000000..e216bec45798a --- /dev/null +++ b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/radar_object_clustering/package.xml b/perception/radar_object_clustering/package.xml new file mode 100644 index 0000000000000..388c034ff8001 --- /dev/null +++ b/perception/radar_object_clustering/package.xml @@ -0,0 +1,32 @@ + + + + radar_object_clustering + 0.1.0 + radar_object_clustering + Sathshi Tanaka + Shunsuke Miura + Yoshi Ri + + Apache License 2.0 + Sathshi Tanaka + + ament_cmake_auto + + autoware_auto_perception_msgs + geometry_msgs + perception_utils + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tier4_autoware_utils + + autoware_cmake + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp new file mode 100644 index 0000000000000..82b729f405647 --- /dev/null +++ b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp @@ -0,0 +1,233 @@ +// 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 "radar_object_clustering/radar_object_clustering_node.hpp" + +#include "perception_utils/perception_utils.hpp" + +#include + +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +namespace +{ +template +bool update_param( + const std::vector & params, const std::string & name, T & value) +{ + const auto itr = std::find_if( + params.cbegin(), params.cend(), + [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); + + // Not found + if (itr == params.cend()) { + return false; + } + + value = itr->template get_value(); + return true; +} + +double get_distance(const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + const auto & position = object.kinematics.pose_with_covariance.pose.position; + return std::hypot(position.x, position.y); +} + +} // namespace + +namespace radar_object_clustering +{ +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; + +RadarObjectClusteringNode::RadarObjectClusteringNode(const rclcpp::NodeOptions & node_options) +: Node("radar_object_clustering", node_options) +{ + // Parameter Server + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RadarObjectClusteringNode::onSetParam, this, std::placeholders::_1)); + + // Node Parameter + node_param_.angle_threshold = declare_parameter("angle_threshold", 0.174); + node_param_.distance_threshold = declare_parameter("distance_threshold", 4.0); + node_param_.velocity_threshold = declare_parameter("velocity_threshold", 2.0); + node_param_.is_fixed_label = declare_parameter("is_fixed_label", false); + node_param_.fixed_label = declare_parameter("fixed_label", "UNKNOWN"); + node_param_.is_fixed_size = declare_parameter("is_fixed_size", false); + node_param_.size_x = declare_parameter("size_x", 4.0); + node_param_.size_y = declare_parameter("size_y", 1.5); + node_param_.size_z = declare_parameter("size_z", 1.5); + + // Subscriber + sub_objects_ = create_subscription( + "~/input/objects", rclcpp::QoS{1}, + std::bind(&RadarObjectClusteringNode::onObjects, this, std::placeholders::_1)); + + // Publisher + pub_objects_ = create_publisher("~/output/objects", 1); +} + +void RadarObjectClusteringNode::onObjects(const DetectedObjects::ConstSharedPtr objects_data_) +{ + DetectedObjects output_objects; + output_objects.header = objects_data_->header; + std::vector objects = objects_data_->objects; + + std::vector used_flags(objects.size(), false); + + auto func = [](DetectedObject const & lhs, DetectedObject const & rhs) { + return get_distance(lhs) < get_distance(rhs); + }; + std::sort(objects.begin(), objects.end(), func); + + for (size_t i = 0; i < objects.size(); i++) { + if (used_flags.at(i) == true) { + continue; + } + + std::vector clustered_objects; + used_flags.at(i) = true; + clustered_objects.emplace_back(objects.at(i)); + + for (size_t j = i; j < objects.size(); ++j) { + if (used_flags.at(j) == false && isSameObject(objects.at(i), objects.at(j))) { + used_flags.at(j) = true; + clustered_objects.emplace_back(objects.at(j)); + } + } + + // clustering + DetectedObject clustered_output_object; + if (clustered_objects.size() == 1) { + clustered_output_object = clustered_objects.at(0); + } else { + auto func_max_confidence = [](const DetectedObject & a, const DetectedObject & b) { + return a.existence_probability < b.existence_probability; + }; + auto iter = std::max_element( + std::begin(clustered_objects), std::end(clustered_objects), func_max_confidence); + + // class label + clustered_output_object.existence_probability = iter->existence_probability; + clustered_output_object.classification = iter->classification; + + // kinematics + clustered_output_object.kinematics = iter->kinematics; + + auto & pose = clustered_output_object.kinematics.pose_with_covariance.pose; + auto func_sum_x = [](const double & a, const DetectedObject & b) { + return a + b.kinematics.pose_with_covariance.pose.position.x; + }; + pose.position.x = + std::accumulate( + std::begin(clustered_objects), std::end(clustered_objects), 0.0, func_sum_x) / + clustered_objects.size(); + auto func_sum_y = [](const double & a, const DetectedObject & b) { + return a + b.kinematics.pose_with_covariance.pose.position.y; + }; + pose.position.y = + std::accumulate( + std::begin(clustered_objects), std::end(clustered_objects), 0.0, func_sum_y) / + clustered_objects.size(); + pose.position.z = iter->kinematics.pose_with_covariance.pose.position.z; + + // Shape + clustered_output_object.shape = iter->shape; + } + + // Fixed label correction + if (node_param_.is_fixed_label) { + clustered_output_object.classification.at(0).label = + perception_utils::toLabel(node_param_.fixed_label); + } + + // Fixed size correction + if (node_param_.is_fixed_size) { + clustered_output_object.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + clustered_output_object.shape.dimensions.x = node_param_.size_x; + clustered_output_object.shape.dimensions.y = node_param_.size_y; + clustered_output_object.shape.dimensions.z = node_param_.size_z; + } + output_objects.objects.emplace_back(clustered_output_object); + } + pub_objects_->publish(output_objects); +} + +bool RadarObjectClusteringNode::isSameObject( + const DetectedObject & object_1, const DetectedObject & object_2) +{ + const double angle_diff = std::abs(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object_1.kinematics.pose_with_covariance.pose.orientation) - + tf2::getYaw(object_2.kinematics.pose_with_covariance.pose.orientation))); + const double velocity_diff = std::abs( + object_1.kinematics.twist_with_covariance.twist.linear.x - + object_2.kinematics.twist_with_covariance.twist.linear.x); + const double distance = tier4_autoware_utils::calcDistance2d( + object_1.kinematics.pose_with_covariance.pose.position, + object_2.kinematics.pose_with_covariance.pose.position); + + if ( + distance < node_param_.distance_threshold && angle_diff < node_param_.angle_threshold && + velocity_diff < node_param_.velocity_threshold) { + return true; + } else { + return false; + } +} + +rcl_interfaces::msg::SetParametersResult RadarObjectClusteringNode::onSetParam( + const std::vector & params) +{ + rcl_interfaces::msg::SetParametersResult result; + + try { + // Node Parameter + { + auto & p = node_param_; + + // Update params + update_param(params, "angle_threshold", p.angle_threshold); + update_param(params, "distance_threshold", p.distance_threshold); + update_param(params, "velocity_threshold", p.velocity_threshold); + update_param(params, "is_fixed_label", p.is_fixed_label); + update_param(params, "fixed_label", p.fixed_label); + update_param(params, "is_fixed_size", p.is_fixed_size); + update_param(params, "size_x", p.size_x); + update_param(params, "size_y", p.size_y); + update_param(params, "size_z", p.size_z); + } + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + return result; + } + + result.successful = true; + result.reason = "success"; + return result; +} +} // namespace radar_object_clustering + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(radar_object_clustering::RadarObjectClusteringNode) 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/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/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..803ede068e72b 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; @@ -96,17 +100,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 +170,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 +199,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/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index c9d4d2c191a4c..94d5bef5a31e9 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -70,6 +70,9 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da if (!is_any_module_running && is_out_of_route) { BehaviorModuleOutput output = utils::createGoalAroundPath(data); generateCombinedDrivableArea(output, data); + RCLCPP_WARN_THROTTLE( + logger_, clock_, 5000, + "Ego is out of route, no module is running. Skip running scene modules."); return output; } @@ -151,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); @@ -163,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/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 045484a7c8cd6..ca2ffec30146d 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 @@ -237,7 +237,10 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() // change turn signal when the vehicle reaches at the end of the path for waiting lane change out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); + path_reference_ = getPreviousModuleOutput().reference_path; + if (!module_type_->isValidPath()) { + removeRTCStatus(); path_candidate_ = std::make_shared(); return out; } @@ -245,7 +248,6 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; updateRTCStatus( candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); is_abort_path_approved_ = false; 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 eed66650520e9..4a6aa50381818 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 @@ -77,6 +77,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) debug_valid_path_ = valid_paths; if (valid_paths.empty()) { + safe_path.reference_lanelets = current_lanes; return {false, false}; } @@ -100,16 +101,7 @@ bool NormalLaneChange::isLaneChangeRequired() const 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(); + return !target_lanes.empty(); } LaneChangePath NormalLaneChange::getLaneChangePath() const @@ -566,9 +558,9 @@ bool NormalLaneChange::getLaneChangePaths( 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); @@ -730,9 +722,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; } @@ -744,8 +736,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); @@ -774,14 +766,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/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..ad4f2a9b3adcd 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -343,19 +343,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 +360,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 +370,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 +401,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 +917,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 +1088,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 +1102,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 +1233,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_velocity_planner_common/README.md b/planning/behavior_velocity_planner_common/README.md new file mode 100644 index 0000000000000..2abbb83575af5 --- /dev/null +++ b/planning/behavior_velocity_planner_common/README.md @@ -0,0 +1,3 @@ +# Behavior Velocity Planner Common + +This package provides common functions as a library, which are used in the `behavior_velocity_planner` node and modules. diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index ffc216e863500..d23c3072762d4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -24,11 +24,8 @@ #include #ifdef ROS_DISTRO_GALACTIC -#include #include #else -#include - #include #endif diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index d9f4240c25bbc..806392fbb10cc 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -18,27 +18,19 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_perception_msgs autoware_auto_mapping_msgs autoware_auto_planning_msgs autoware_auto_tf2 - autoware_perception_msgs - 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 @@ -46,7 +38,6 @@ rtc_interface sensor_msgs tf2 - tf2_eigen tf2_geometry_msgs tf2_ros tier4_api_msgs 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/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/mrm_emergency_stop_operator/READEME.md b/system/mrm_emergency_stop_operator/README.md similarity index 100% rename from system/mrm_emergency_stop_operator/READEME.md rename to system/mrm_emergency_stop_operator/README.md diff --git a/system/velodyne_monitor/Readme.md b/system/velodyne_monitor/README.md similarity index 100% rename from system/velodyne_monitor/Readme.md rename to system/velodyne_monitor/README.md