From eb134333639523cc1c74b23667998fbe5e93e212 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 5 Sep 2023 10:34:11 +0900 Subject: [PATCH 01/93] perf(crosswalk): decrease calcSignedArcLength() process (#4879) Signed-off-by: satoshi-ota --- .../src/scene_crosswalk.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index cc1cc9f1ea950..4df404a3ccb27 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -34,6 +34,7 @@ using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; +using motion_utils::calcSignedArcLengthPartialSum; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; using motion_utils::resamplePath; @@ -731,15 +732,15 @@ Polygon2d CrosswalkModule::getAttentionArea( { const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_polygon = createVehiclePolygon(planner_data_->vehicle_info_); + const auto backward_path_length = + calcSignedArcLength(sparse_resample_path.points, size_t(0), ego_pos); + const auto length_sum = calcSignedArcLengthPartialSum( + sparse_resample_path.points, size_t(0), sparse_resample_path.points.size()); Polygon2d attention_area; for (size_t j = 0; j < sparse_resample_path.points.size() - 1; ++j) { - const auto & p_ego_front = sparse_resample_path.points.at(j).point.pose; - const auto & p_ego_back = sparse_resample_path.points.at(j + 1).point.pose; - const auto front_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_front.position); - const auto back_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_back.position); + const auto front_length = length_sum.at(j) - backward_path_length; + const auto back_length = length_sum.at(j + 1) - backward_path_length; if (back_length < crosswalk_attention_range.first) { continue; From e2cabf55d97dfa5881a54b51584382185f309978 Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Tue, 5 Sep 2023 21:44:15 +0900 Subject: [PATCH 02/93] Revert "fix(crosswalk): set large minus value in rtc distance if the ego already passed crosswalk (#4869)" This reverts commit fa8da4c5f161c55ea9fbd2e248cbcdbab3d80f97. --- .../src/scene_crosswalk.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 4df404a3ccb27..f70a03f5ae7af 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -226,12 +226,9 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Calculate stop point with margin const auto p_stop_line = getStopPointWithMargin(*path, path_intersects); - - std::optional default_stop_pose = std::nullopt; - if (p_stop_line.has_value()) { - default_stop_pose = toStdOptional( - calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); - } + // TODO(murooka) add a guard of p_stop_line + const auto default_stop_pose = toStdOptional( + calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); // Resample path sparsely for less computation cost constexpr double resample_interval = 4.0; @@ -996,8 +993,6 @@ void CrosswalkModule::setDistanceToStop( const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, *stop_pos); setDistance(dist_ego2stop); - } else { - setDistance(std::numeric_limits::lowest()); } } From d992f4d49b0e20330e179a13acc8c9649660a318 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 6 Sep 2023 01:55:16 +0900 Subject: [PATCH 03/93] fix(crosswalk): fix build (#801) Signed-off-by: kosuke55 --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index f70a03f5ae7af..2b1928a9d7ef0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -747,6 +747,8 @@ Polygon2d CrosswalkModule::getAttentionArea( break; } + const auto & p_ego_front = sparse_resample_path.points.at(j).point.pose; + const auto & p_ego_back = sparse_resample_path.points.at(j + 1).point.pose; const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, ego_polygon); debug_data_.ego_polygons.push_back(toGeometryPointVector(ego_one_step_polygon, ego_pos.z)); From 93013dd0d9b26864d155c6773ad374ab0cbf1948 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 25 Aug 2023 14:21:31 +0900 Subject: [PATCH 04/93] fix(crosswalk): guard invalid sharp angle stop point (#4750) fix(crosswalk): guard invalid stop point Signed-off-by: satoshi-ota --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 2b1928a9d7ef0..c402cdf33d12d 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1001,6 +1001,9 @@ void CrosswalkModule::setDistanceToStop( void CrosswalkModule::planGo( PathWithLaneId & ego_path, const std::optional & stop_factor) const { + if (!stop_factor.has_value()) { + return; + } // Plan slow down const auto target_velocity = calcTargetVelocity(stop_factor->stop_pose.position, ego_path); insertDecelPointWithDebugInfo( From bdccfb9fcca89e21f46cce912db4276b09e54f29 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 4 Sep 2023 20:27:38 +0900 Subject: [PATCH 05/93] fix(crosswalk): set large minus value in rtc distance if the ego already passed crosswalk (#4869) Signed-off-by: satoshi-ota --- .../src/scene_crosswalk.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index c402cdf33d12d..e3fb12effe1a1 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -226,9 +226,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Calculate stop point with margin const auto p_stop_line = getStopPointWithMargin(*path, path_intersects); - // TODO(murooka) add a guard of p_stop_line - const auto default_stop_pose = toStdOptional( - calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); + + std::optional default_stop_pose = std::nullopt; + if (p_stop_line.has_value()) { + default_stop_pose = toStdOptional( + calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); + } // Resample path sparsely for less computation cost constexpr double resample_interval = 4.0; @@ -995,6 +998,8 @@ void CrosswalkModule::setDistanceToStop( const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, *stop_pos); setDistance(dist_ego2stop); + } else { + setDistance(std::numeric_limits::lowest()); } } From 16956bfe0559098f9a02fd4999721b4ee7e3993d Mon Sep 17 00:00:00 2001 From: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Date: Mon, 11 Sep 2023 07:36:38 +0900 Subject: [PATCH 06/93] Revert "perf(voxel_grid_downsample_filter): performance tuning (#3679)" This reverts commit edcad42338a56cd2c7eaf0803f0861c56493608c. --- .../pointcloud_preprocessor/CMakeLists.txt | 23 --- .../crop_box_filter_nodelet.hpp | 1 - .../faster_voxel_grid_downsample_filter.hpp | 93 --------- .../voxel_grid_downsample_filter_nodelet.hpp | 15 +- .../pointcloud_preprocessor/filter.hpp | 14 +- .../ring_outlier_filter_nodelet.hpp | 1 - .../transform_info.hpp | 43 ----- .../faster_voxel_grid_downsample_filter.cpp | 178 ------------------ .../voxel_grid_downsample_filter_nodelet.cpp | 25 +-- .../pointcloud_preprocessor/src/filter.cpp | 3 +- 10 files changed, 21 insertions(+), 375 deletions(-) delete mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp delete mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp delete mode 100644 sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index c5f15ec4e49d5..b14d3a181e587 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -41,21 +41,6 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base pcl_ros ) -add_library(faster_voxel_grid_downsample_filter SHARED - src/downsample_filter/faster_voxel_grid_downsample_filter.cpp -) - -target_include_directories(faster_voxel_grid_downsample_filter PUBLIC - "$" - "$" -) - -ament_target_dependencies(faster_voxel_grid_downsample_filter - pcl_conversions - rclcpp - sensor_msgs -) - ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/utility/utilities.cpp src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -82,7 +67,6 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED target_link_libraries(pointcloud_preprocessor_filter pointcloud_preprocessor_filter_base - faster_voxel_grid_downsample_filter ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} @@ -195,13 +179,6 @@ install( RUNTIME DESTINATION bin ) -install( - TARGETS faster_voxel_grid_downsample_filter EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - install( DIRECTORY include/ DESTINATION include/${PROJECT_NAME} diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp index 1406679ce0dba..3b392401de49b 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp @@ -54,7 +54,6 @@ #define POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/transform_info.hpp" #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp deleted file mode 100644 index d92b511d40c69..0000000000000 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// 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. - -#pragma once - -#include "pointcloud_preprocessor/transform_info.hpp" - -#include -#include -#include - -#include -#include - -namespace pointcloud_preprocessor -{ - -class FasterVoxelGridDownsampleFilter -{ - using PointCloud2 = sensor_msgs::msg::PointCloud2; - using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; - -public: - FasterVoxelGridDownsampleFilter(); - void set_voxel_size(float voxel_size_x, float voxel_size_y, float voxel_size_z); - void set_field_offsets(const PointCloud2ConstPtr & input); - void filter( - const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, - const rclcpp::Logger & logger); - -private: - struct Centroid - { - float x; - float y; - float z; - uint32_t point_count_; - - Centroid() : x(0), y(0), z(0) {} - Centroid(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { this->point_count_ = 1; } - - void add_point(float _x, float _y, float _z) - { - this->x += _x; - this->y += _y; - this->z += _z; - this->point_count_++; - } - - Eigen::Vector4f calc_centroid() const - { - Eigen::Vector4f centroid( - (this->x / this->point_count_), (this->y / this->point_count_), - (this->z / this->point_count_), 1.0f); - return centroid; - } - }; - - Eigen::Vector3f inverse_voxel_size_; - std::vector xyz_fields_; - int x_offset_; - int y_offset_; - int z_offset_; - int intensity_offset_; - bool offset_initialized_; - - Eigen::Vector3f get_point_from_global_offset( - const PointCloud2ConstPtr & input, size_t global_offset); - - bool get_min_max_voxel( - const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel); - - std::unordered_map calc_centroids_each_voxel( - const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel, - const Eigen::Vector3i & min_voxel); - - void copy_centroids_to_output( - std::unordered_map & voxel_centroid_map, PointCloud2 & output, - const TransformInfo & transform_info); -}; - -} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp index ae285b4fa9239..7ebf80c2da958 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2020 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. @@ -52,7 +52,6 @@ #define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/transform_info.hpp" #include #include @@ -67,16 +66,10 @@ class VoxelGridDownsampleFilterComponent : public pointcloud_preprocessor::Filte void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; - // TODO(atsushi421): Temporary Implementation: Remove this interface when all the filter nodes - // conform to new API - virtual void faster_filter( - const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, - const TransformInfo & transform_info); - private: - float voxel_size_x_; - float voxel_size_y_; - float voxel_size_z_; + double voxel_size_x_; + double voxel_size_y_; + double voxel_size_z_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp index 47c0f2b403faa..f995be741272c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -52,8 +52,6 @@ #ifndef POINTCLOUD_PREPROCESSOR__FILTER_HPP_ #define POINTCLOUD_PREPROCESSOR__FILTER_HPP_ -#include "pointcloud_preprocessor/transform_info.hpp" - #include #include #include @@ -136,6 +134,18 @@ class Filter : public rclcpp::Node const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); protected: + struct TransformInfo + { + TransformInfo() + { + eigen_transform = Eigen::Matrix4f::Identity(4, 4); + need_transform = false; + } + + Eigen::Matrix4f eigen_transform; + bool need_transform; + }; + /** \brief The input PointCloud2 subscriber. */ rclcpp::Subscription::SharedPtr sub_input_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index ab7d4e0012304..a906eb913d863 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -17,7 +17,6 @@ #include "autoware_point_types/types.hpp" #include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/transform_info.hpp" #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp deleted file mode 100644 index a4806555695b5..0000000000000 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// 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. - -#pragma once - -#include - -namespace pointcloud_preprocessor -{ - -/** - * This holds the coordinate transformation information of the point cloud. - * Usage example: - * \code - * if (transform_info.need_transform) { - * point = transform_info.eigen_transform * point; - * } - * \endcode - */ -struct TransformInfo -{ - TransformInfo() - { - eigen_transform = Eigen::Matrix4f::Identity(4, 4); - need_transform = false; - } - - Eigen::Matrix4f eigen_transform; - bool need_transform; -}; - -} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp deleted file mode 100644 index 7c302eec20431..0000000000000 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ /dev/null @@ -1,178 +0,0 @@ -// 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 "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" - -namespace pointcloud_preprocessor -{ - -FasterVoxelGridDownsampleFilter::FasterVoxelGridDownsampleFilter() -{ - pcl::for_each_type::type>( - pcl::detail::FieldAdder(xyz_fields_)); - offset_initialized_ = false; -} - -void FasterVoxelGridDownsampleFilter::set_voxel_size( - float voxel_size_x, float voxel_size_y, float voxel_size_z) -{ - inverse_voxel_size_ = - Eigen::Array3f::Ones() / Eigen::Array3f(voxel_size_x, voxel_size_y, voxel_size_z); -} - -void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPtr & input) -{ - x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; - y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; - z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; - intensity_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset; - offset_initialized_ = true; -} - -void FasterVoxelGridDownsampleFilter::filter( - const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, - const rclcpp::Logger & logger) -{ - // Check if the field offset has been set - if (!offset_initialized_) { - set_field_offsets(input); - } - - // Compute the minimum and maximum voxel coordinates - Eigen::Vector3i min_voxel, max_voxel; - if (!get_min_max_voxel(input, min_voxel, max_voxel)) { - RCLCPP_ERROR( - logger, - "Voxel size is too small for the input dataset. " - "Integer indices would overflow."); - output = *input; - return; - } - - // Storage for mapping voxel coordinates to centroids - auto voxel_centroid_map = calc_centroids_each_voxel(input, max_voxel, min_voxel); - - // Initialize the output - output.row_step = voxel_centroid_map.size() * input->point_step; - output.data.resize(output.row_step); - output.width = voxel_centroid_map.size(); - pcl_conversions::fromPCL(xyz_fields_, output.fields); - output.is_dense = true; // we filter out invalid points - output.height = input->height; - output.is_bigendian = input->is_bigendian; - output.point_step = input->point_step; - output.header = input->header; - - // Copy the centroids to the output - copy_centroids_to_output(voxel_centroid_map, output, transform_info); -} - -Eigen::Vector3f FasterVoxelGridDownsampleFilter::get_point_from_global_offset( - const PointCloud2ConstPtr & input, size_t global_offset) -{ - Eigen::Vector3f point( - *reinterpret_cast(&input->data[global_offset + x_offset_]), - *reinterpret_cast(&input->data[global_offset + y_offset_]), - *reinterpret_cast(&input->data[global_offset + z_offset_])); - return point; -} - -bool FasterVoxelGridDownsampleFilter::get_min_max_voxel( - const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel) -{ - // Compute the minimum and maximum point coordinates - Eigen::Vector3f min_point, max_point; - min_point.setConstant(FLT_MAX); - max_point.setConstant(-FLT_MAX); - for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); - global_offset += input->point_step) { - Eigen::Vector3f point = get_point_from_global_offset(input, global_offset); - if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) { - min_point = min_point.cwiseMin(point); - max_point = max_point.cwiseMax(point); - } - } - - // Check that the voxel size is not too small, given the size of the data - if ( - ((static_cast((max_point[0] - min_point[0]) * inverse_voxel_size_[0]) + 1) * - (static_cast((max_point[1] - min_point[1]) * inverse_voxel_size_[1]) + 1) * - (static_cast((max_point[2] - min_point[2]) * inverse_voxel_size_[2]) + 1)) > - static_cast(std::numeric_limits::max())) { - return false; - } - - // Compute the minimum and maximum voxel coordinates - min_voxel[0] = static_cast(std::floor(min_point[0] * inverse_voxel_size_[0])); - min_voxel[1] = static_cast(std::floor(min_point[1] * inverse_voxel_size_[1])); - min_voxel[2] = static_cast(std::floor(min_point[2] * inverse_voxel_size_[2])); - max_voxel[0] = static_cast(std::floor(max_point[0] * inverse_voxel_size_[0])); - max_voxel[1] = static_cast(std::floor(max_point[1] * inverse_voxel_size_[1])); - max_voxel[2] = static_cast(std::floor(max_point[2] * inverse_voxel_size_[2])); - - return true; -} - -std::unordered_map -FasterVoxelGridDownsampleFilter::calc_centroids_each_voxel( - const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel, - const Eigen::Vector3i & min_voxel) -{ - std::unordered_map voxel_centroid_map; - // Compute the number of divisions needed along all axis - Eigen::Vector3i div_b = max_voxel - min_voxel + Eigen::Vector3i::Ones(); - // Set up the division multiplier - Eigen::Vector3i div_b_mul(1, div_b[0], div_b[0] * div_b[1]); - - for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); - global_offset += input->point_step) { - Eigen::Vector3f point = get_point_from_global_offset(input, global_offset); - if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) { - // Calculate the voxel index to which the point belongs - int ijk0 = static_cast(std::floor(point[0] * inverse_voxel_size_[0]) - min_voxel[0]); - int ijk1 = static_cast(std::floor(point[1] * inverse_voxel_size_[1]) - min_voxel[1]); - int ijk2 = static_cast(std::floor(point[2] * inverse_voxel_size_[2]) - min_voxel[2]); - uint32_t voxel_id = ijk0 * div_b_mul[0] + ijk1 * div_b_mul[1] + ijk2 * div_b_mul[2]; - - // Add the point to the corresponding centroid - if (voxel_centroid_map.find(voxel_id) == voxel_centroid_map.end()) { - voxel_centroid_map[voxel_id] = Centroid(point[0], point[1], point[2]); - } else { - voxel_centroid_map[voxel_id].add_point(point[0], point[1], point[2]); - } - } - } - - return voxel_centroid_map; -} - -void FasterVoxelGridDownsampleFilter::copy_centroids_to_output( - std::unordered_map & voxel_centroid_map, PointCloud2 & output, - const TransformInfo & transform_info) -{ - size_t output_data_size = 0; - for (const auto & pair : voxel_centroid_map) { - Eigen::Vector4f centroid = pair.second.calc_centroid(); - if (transform_info.need_transform) { - centroid = transform_info.eigen_transform * centroid; - } - *reinterpret_cast(&output.data[output_data_size + x_offset_]) = centroid[0]; - *reinterpret_cast(&output.data[output_data_size + y_offset_]) = centroid[1]; - *reinterpret_cast(&output.data[output_data_size + z_offset_]) = centroid[2]; - *reinterpret_cast(&output.data[output_data_size + intensity_offset_]) = centroid[3]; - output_data_size += output.point_step; - } -} - -} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp index f11f37397a142..5b22986a1b6b8 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2020 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. @@ -50,8 +50,6 @@ #include "pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp" -#include "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" - #include #include #include @@ -66,9 +64,9 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent( { // set initial parameters { - voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); - voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); - voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); + voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); + voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); + voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); } using std::placeholders::_1; @@ -76,8 +74,6 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent( std::bind(&VoxelGridDownsampleFilterComponent::paramCallback, this, _1)); } -// TODO(atsushi421): Temporary Implementation: Delete this function definition when all the filter -// nodes conform to new API. void VoxelGridDownsampleFilterComponent::filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { @@ -99,19 +95,6 @@ void VoxelGridDownsampleFilterComponent::filter( output.header = input->header; } -// TODO(atsushi421): Temporary Implementation: Rename this function to `filter()` when all the -// filter nodes conform to new API. Then delete the old `filter()` defined above. -void VoxelGridDownsampleFilterComponent::faster_filter( - const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output, const TransformInfo & transform_info) -{ - std::scoped_lock lock(mutex_); - FasterVoxelGridDownsampleFilter faster_voxel_filter; - faster_voxel_filter.set_voxel_size(voxel_size_x_, voxel_size_y_, voxel_size_z_); - faster_voxel_filter.set_field_offsets(input); - faster_voxel_filter.filter(input, output, transform_info, this->get_logger()); -} - rcl_interfaces::msg::SetParametersResult VoxelGridDownsampleFilterComponent::paramCallback( const std::vector & p) { diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index d5843be395adf..f0b716c73b5f1 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -124,8 +124,7 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) // TODO(sykwer): Change the corresponding node to subscribe to `faster_input_indices_callback` // each time a child class supports the faster version. // When all the child classes support the faster version, this workaround is deleted. - std::set supported_nodes = { - "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter"}; + std::set supported_nodes = {"CropBoxFilter", "RingOutlierFilter"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback; From 021d45aef6e1cdc6c6285faa973d5e1fd0d714af Mon Sep 17 00:00:00 2001 From: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Date: Mon, 11 Sep 2023 07:36:58 +0900 Subject: [PATCH 07/93] Revert "fix(euclidean_cluster): add disuse downsample in clustering pipeline (#4385)" This reverts commit 559f8737a89ca1b20eda705ecaf9d3cd1851a7b9. --- ...ra_lidar_fusion_based_detection.launch.xml | 2 - ...ar_radar_fusion_based_detection.launch.xml | 2 - .../lidar_based_detection.launch.xml | 2 - .../launch/perception.launch.xml | 2 - ...xel_grid_based_euclidean_cluster.launch.py | 96 +++---------------- ...el_grid_based_euclidean_cluster.launch.xml | 2 - 6 files changed, 13 insertions(+), 93 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 12961c1470eb9..aef7220680d67 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -30,7 +30,6 @@ - @@ -74,7 +73,6 @@ - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 3a24ec2fdf500..23f3bd0b532a0 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -4,7 +4,6 @@ - @@ -60,7 +59,6 @@ - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 6d67df5fa7a1f..9ab48a678240d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -5,7 +5,6 @@ - @@ -34,7 +33,6 @@ - diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 204fb561b10ea..58ee60b9409e0 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -58,7 +58,6 @@ - - diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 82ce5605b67cd..ba6e9e72107c8 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -16,10 +16,9 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.substitutions import AndSubstitution +from launch.conditions import UnlessCondition from launch.substitutions import AnonName from launch.substitutions import LaunchConfiguration -from launch.substitutions import NotSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -165,7 +164,7 @@ def load_composable_node_param(param_path): ) # set euclidean cluster as a component - use_downsample_euclidean_cluster_component = ComposableNode( + euclidean_cluster_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", @@ -177,108 +176,40 @@ def load_composable_node_param(param_path): parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_disuse_downsample_euclidean_component = ComposableNode( - package=pkg, - namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", - name="euclidean_cluster", - remappings=[ - ("input", "map_filter/pointcloud"), - ("output", LaunchConfiguration("output_clusters")), - ], - parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], - ) - disuse_map_disuse_downsample_euclidean_component = ComposableNode( - package=pkg, - namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", - name="euclidean_cluster", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", LaunchConfiguration("output_clusters")), - ], - parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], - ) - container = ComposableNodeContainer( name="euclidean_cluster_container", package="rclcpp_components", namespace=ns, executable="component_container", - composable_node_descriptions=[], + composable_node_descriptions=[ + voxel_grid_filter_component, + outlier_filter_component, + downsample_concat_component, + euclidean_cluster_component, + ], output="screen", ) - use_map_use_downsample_loader = LoadComposableNodes( + use_map_loader = LoadComposableNodes( composable_node_descriptions=[ compare_map_filter_component, use_map_short_range_crop_box_filter_component, use_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, ], target_container=container, - condition=IfCondition( - AndSubstitution( - LaunchConfiguration("use_pointcloud_map"), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), + condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), ) - disuse_map_use_downsample_loader = LoadComposableNodes( + disuse_map_loader = LoadComposableNodes( composable_node_descriptions=[ disuse_map_short_range_crop_box_filter_component, disuse_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, ], target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), ) - use_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - compare_map_filter_component, - use_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - (LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), - ) - - disuse_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), - ) - return [ - container, - use_map_use_downsample_loader, - disuse_map_use_downsample_loader, - use_map_disuse_downsample_loader, - disuse_map_disuse_downsample_loader, - ] + return [container, use_map_loader, disuse_map_loader] def generate_launch_description(): @@ -291,7 +222,6 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg("use_downsample_pointcloud", "false"), add_launch_arg( "voxel_grid_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 2833fed81c28e..6ae70b2612836 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -4,7 +4,6 @@ - @@ -15,7 +14,6 @@ - From 09a92f5f4463fcc4db18897931471f5aaa53bd9f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 23 Aug 2023 23:00:00 +0900 Subject: [PATCH 08/93] refactor(safety_check): use safety check common param struct (#4719) Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 8 +++++ .../config/behavior_path_planner.param.yaml | 13 -------- .../config/lane_change/lane_change.param.yaml | 12 ++++++++ .../behavior_path_planner/parameters.hpp | 14 --------- .../scene_module/lane_change/normal.hpp | 2 +- .../utils/avoidance/avoidance_module_data.hpp | 4 +++ .../lane_change/lane_change_module_data.hpp | 4 +++ .../path_safety_checker_parameters.hpp | 2 ++ .../path_safety_checker/safety_check.hpp | 7 ++--- .../src/behavior_path_planner_node.cpp | 17 ----------- .../avoidance/avoidance_module.cpp | 4 +-- .../src/scene_module/avoidance/manager.cpp | 21 ++++++++++++- .../src/scene_module/lane_change/manager.cpp | 30 +++++++++++++++++++ .../src/scene_module/lane_change/normal.cpp | 11 +++---- .../path_safety_checker/safety_check.cpp | 28 ++++++++--------- .../test/test_safety_check.cpp | 8 +++-- 16 files changed, 108 insertions(+), 77 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 8d413f54817fc..0c8797cb39c59 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -146,6 +146,14 @@ safety_check_backward_distance: 100.0 # [m] safety_check_hysteresis_factor: 2.0 # [-] safety_check_ego_offset: 1.0 # [m] + # rss parameters + expected_front_deceleration: -1.0 # [m/ss] + expected_rear_deceleration: -1.0 # [m/ss] + rear_vehicle_reaction_time: 2.0 # [s] + rear_vehicle_safety_time_margin: 1.0 # [s] + lateral_distance_max_threshold: 2.0 # [m] + longitudinal_distance_min_threshold: 3.0 # [m] + longitudinal_velocity_delta_time: 0.8 # [s] # For avoidance maneuver avoidance: diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 02dc13ffd84da..95c60612bfdb7 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -25,19 +25,6 @@ visualize_maximum_drivable_area: true - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 # [s] - - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lane_following: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 8fabe8daa85b2..544471d9f8a91 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -27,6 +27,18 @@ min_longitudinal_acc: -1.0 max_longitudinal_acc: 1.0 + # safety check + safety_check: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + expected_front_deceleration_for_abort: -1.0 + expected_rear_deceleration_for_abort: -2.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 1fdf4e020d71f..0cad130ffffdb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -142,20 +142,6 @@ struct BehaviorPathPlannerParameters // maximum drivable area visualization bool visualize_maximum_drivable_area; - - // collision check - double lateral_distance_max_threshold; - double longitudinal_distance_min_threshold; - double longitudinal_velocity_delta_time; - - double expected_front_deceleration; // brake parameter under normal lane change - double expected_rear_deceleration; // brake parameter under normal lane change - - double expected_front_deceleration_for_abort; // hard brake parameter for abort - double expected_rear_deceleration_for_abort; // hard brake parameter for abort - - double rear_vehicle_reaction_time; - double rear_vehicle_safety_time_margin; }; #endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 0b63d8e40ae81..00c60cbfa8a6d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -140,7 +140,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const double front_decel, const double rear_decel, + const utils::path_safety_checker::RSSparams & rss_params, std::unordered_map & debug_data) const; rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); 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 ac6afde3ff47c..b51fbba92b148 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 @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include @@ -302,6 +303,9 @@ struct AvoidanceParameters // parameters depend on object class std::unordered_map object_parameters; + // rss parameters + utils::path_safety_checker::RSSparams rss_params; + // clip left and right bounds for objects bool enable_bound_clipping{false}; 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 721c5b00f6e33..0556a780467c0 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 @@ -76,6 +76,10 @@ struct LaneChangeParameters bool check_motorcycle{true}; // check object motorbike bool check_pedestrian{true}; // check object pedestrian + // safety check + utils::path_safety_checker::RSSparams rss_params; + utils::path_safety_checker::RSSparams rss_params_for_abort; + // abort LaneChangeCancelParameters cancel; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 519f589f561ee..c4a6a86861e6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -123,6 +123,8 @@ struct RSSparams double longitudinal_distance_min_threshold{ 0.0}; ///< Minimum threshold for longitudinal distance. double longitudinal_velocity_delta_time{0.0}; ///< Delta time for longitudinal velocity. + double front_vehicle_deceleration; ///< brake parameter + double rear_vehicle_deceleration; ///< brake parameter }; /** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 8606f2cd2397d..6d9df7677ead5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -65,8 +65,7 @@ Polygon2d createExtendedPolygon( double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, - const double front_object_deceleration, const double rear_object_deceleration, - const BehaviorPathPlannerParameters & params); + const RSSparams & rss_params); double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, @@ -98,8 +97,8 @@ bool checkCollision( const std::vector & predicted_ego_path, 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 BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + CollisionCheckDebug & debug); /** * @brief Check collision between ego path footprints with extra longitudinal stopping margin and diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 29f1ee762abdf..1302b579d2875 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -391,23 +391,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold"); - p.longitudinal_distance_min_threshold = - declare_parameter("longitudinal_distance_min_threshold"); - p.longitudinal_velocity_delta_time = - declare_parameter("longitudinal_velocity_delta_time"); - - p.expected_front_deceleration = declare_parameter("expected_front_deceleration"); - p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration"); - - p.expected_front_deceleration_for_abort = - declare_parameter("expected_front_deceleration_for_abort"); - p.expected_rear_deceleration_for_abort = - declare_parameter("expected_rear_deceleration_for_abort"); - - p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time"); - p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin"); - if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( get_logger(), "Lane change buffer must be more than 1 meter. Modifying the buffer."); 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 66808be897ae0..24a286cc1809d 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 @@ -1924,8 +1924,8 @@ bool AvoidanceModule::isSafePath( for (const auto & obj_path : obj_predicted_paths) { CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( - shifted_path.path, ego_predicted_path, object, obj_path, p, - p.expected_front_deceleration, p.expected_rear_deceleration, collision)) { + shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, + collision)) { return false; } } 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 a039c0388b6a4..638b47e46fc66 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -136,7 +136,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); } - // safety check + // safety check general params { std::string ns = "avoidance.safety_check."; p.enable_safety_check = get_parameter(node, ns + "enable"); @@ -155,6 +155,25 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.safety_check_ego_offset = get_parameter(node, ns + "safety_check_ego_offset"); } + // safety check rss params + { + std::string ns = "avoidance.safety_check."; + p.rss_params.longitudinal_distance_min_threshold = + get_parameter(node, ns + "longitudinal_distance_min_threshold"); + p.rss_params.longitudinal_velocity_delta_time = + get_parameter(node, ns + "longitudinal_velocity_delta_time"); + p.rss_params.front_vehicle_deceleration = + get_parameter(node, ns + "expected_front_deceleration"); + p.rss_params.rear_vehicle_deceleration = + get_parameter(node, ns + "expected_rear_deceleration"); + p.rss_params.rear_vehicle_reaction_time = + get_parameter(node, ns + "rear_vehicle_reaction_time"); + p.rss_params.rear_vehicle_safety_time_margin = + get_parameter(node, ns + "rear_vehicle_safety_time_margin"); + p.rss_params.lateral_distance_max_threshold = + get_parameter(node, ns + "lateral_distance_max_threshold"); + } + // avoidance maneuver (lateral) { std::string ns = "avoidance.avoidance.lateral."; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 18c669a207095..fff078ce7ef3d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -85,6 +85,36 @@ LaneChangeModuleManager::LaneChangeModuleManager( get_parameter(node, parameter("check_objects_on_other_lanes")); p.use_all_predicted_path = get_parameter(node, parameter("use_all_predicted_path")); + p.rss_params.longitudinal_distance_min_threshold = + get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_velocity_delta_time = + get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); + p.rss_params.front_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_rear_deceleration")); + p.rss_params.rear_vehicle_reaction_time = + get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + p.rss_params.rear_vehicle_safety_time_margin = + get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + p.rss_params.lateral_distance_max_threshold = + get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); + + p.rss_params_for_abort.longitudinal_distance_min_threshold = + get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params_for_abort.longitudinal_velocity_delta_time = + get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); + p.rss_params_for_abort.front_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_front_deceleration_for_abort")); + p.rss_params_for_abort.rear_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_rear_deceleration_for_abort")); + p.rss_params_for_abort.rear_vehicle_reaction_time = + get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + p.rss_params_for_abort.rear_vehicle_safety_time_margin = + get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = + get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); + // target object { std::string ns = "lane_change.target_object."; 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 f4ef15ad50cd6..2ab306cebd7d0 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 @@ -937,8 +937,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, common_parameters.expected_front_deceleration, - common_parameters.expected_rear_deceleration, object_debug_); + *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); if (is_safe) { return true; @@ -951,7 +950,6 @@ bool NormalLaneChange::getLaneChangePaths( PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { - const auto & common_parameters = getCommonParam(); const auto & path = status_.lane_change_path; const auto & current_lanes = status_.current_lanes; const auto & target_lanes = status_.target_lanes; @@ -960,8 +958,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; const auto safety_status = isLaneChangePathSafe( - path, target_objects, common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, debug_data); + path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); { // only for debug purpose object_debug_.clear(); @@ -1218,7 +1215,7 @@ bool NormalLaneChange::getAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const double front_decel, const double rear_decel, + const utils::path_safety_checker::RSSparams & rss_params, std::unordered_map & debug_data) const { PathSafetyStatus path_safety_status; @@ -1280,7 +1277,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, front_decel, rear_decel, + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, current_debug_data.second)) { path_safety_status.is_safe = false; updateDebugInfo(current_debug_data, path_safety_status.is_safe); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 34ba607938385..c6a68e108e7e1 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -135,8 +135,7 @@ Polygon2d createExtendedPolygon( double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, - const double front_object_deceleration, const double rear_object_deceleration, - const BehaviorPathPlannerParameters & params) + const RSSparams & rss_params) { const auto stoppingDistance = [](const auto vehicle_velocity, const auto vehicle_accel) { // compensate if user accidentally set the deceleration to some positive value @@ -145,23 +144,23 @@ double calcRssDistance( }; const double & reaction_time = - params.rear_vehicle_reaction_time + params.rear_vehicle_safety_time_margin; + rss_params.rear_vehicle_reaction_time + rss_params.rear_vehicle_safety_time_margin; const double front_object_stop_length = - stoppingDistance(front_object_velocity, front_object_deceleration); + stoppingDistance(front_object_velocity, rss_params.front_vehicle_deceleration); const double rear_object_stop_length = rear_object_velocity * reaction_time + - stoppingDistance(rear_object_velocity, rear_object_deceleration); + stoppingDistance(rear_object_velocity, rss_params.rear_vehicle_deceleration); return rear_object_stop_length - front_object_stop_length; } double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, - const BehaviorPathPlannerParameters & params) + const RSSparams & rss_params) { - const double & lon_threshold = params.longitudinal_distance_min_threshold; + const double & lon_threshold = rss_params.longitudinal_distance_min_threshold; const auto max_vel = std::max(front_object_velocity, rear_object_velocity); - return params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; + return rss_params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; } boost::optional calcInterpolatedPoseWithVelocity( @@ -219,8 +218,8 @@ bool checkCollision( const std::vector & predicted_ego_path, 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 BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + CollisionCheckDebug & debug) { debug.lerped_path.reserve(target_object_path.path.size()); @@ -267,16 +266,15 @@ bool checkCollision( : std::make_pair(ego_velocity, object_velocity); // compute rss dist - const auto rss_dist = calcRssDistance( - front_object_velocity, rear_object_velocity, front_object_deceleration, - rear_object_deceleration, common_parameters); + const auto rss_dist = + calcRssDistance(front_object_velocity, rear_object_velocity, rss_parameters); // minimum longitudinal length const auto min_lon_length = - calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, common_parameters); + calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, rss_parameters); const auto & lon_offset = std::max(rss_dist, min_lon_length); - const auto & lat_margin = common_parameters.lateral_distance_max_threshold; + const auto & lat_margin = rss_parameters.lateral_distance_max_threshold; const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index e24f3274179fe..e1b74636f5ed1 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -178,18 +178,20 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance) { using behavior_path_planner::utils::path_safety_checker::calcRssDistance; + using behavior_path_planner::utils::path_safety_checker::RSSparams; { const double front_vel = 5.0; const double front_decel = -2.0; const double rear_vel = 10.0; const double rear_decel = -1.0; - BehaviorPathPlannerParameters params; + RSSparams params; params.rear_vehicle_reaction_time = 1.0; params.rear_vehicle_safety_time_margin = 1.0; params.longitudinal_distance_min_threshold = 3.0; + params.rear_vehicle_deceleration = rear_decel; + params.front_vehicle_deceleration = front_decel; - EXPECT_NEAR( - calcRssDistance(front_vel, rear_vel, front_decel, rear_decel, params), 63.75, epsilon); + EXPECT_NEAR(calcRssDistance(front_vel, rear_vel, params), 63.75, epsilon); } } From 37577fad153c92d9137a45398d2de3840fbf0315 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 29 Aug 2023 08:14:29 +0900 Subject: [PATCH 09/93] feat(avoidance): flexible avoidance safety check param (#4754) Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 3 ++- .../utils/avoidance/avoidance_module_data.hpp | 3 ++- .../utils/avoidance/utils.hpp | 2 +- .../scene_module/avoidance/avoidance_module.cpp | 16 ++++++++++++++-- .../src/scene_module/avoidance/manager.cpp | 11 ++++++++--- .../src/utils/avoidance/utils.cpp | 8 +++++--- 6 files changed, 32 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 0c8797cb39c59..968cd1d0110fe 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -142,7 +142,8 @@ # collision check parameters check_all_predicted_path: false # [-] time_resolution: 0.5 # [s] - time_horizon: 10.0 # [s] + time_horizon_for_front_object: 10.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] safety_check_hysteresis_factor: 2.0 # [-] safety_check_ego_offset: 1.0 # [m] 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 b51fbba92b148..c57f94f27e430 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 @@ -193,7 +193,8 @@ struct AvoidanceParameters // parameters for collision check. bool check_all_predicted_path{false}; - double safety_check_time_horizon{0.0}; + double time_horizon_for_front_object{0.0}; + double time_horizon_for_rear_object{0.0}; double safety_check_time_resolution{0.0}; // find adjacent lane vehicles 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 420ace9458220..ede0e5bfb728c 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 @@ -88,7 +88,7 @@ std::vector generateObstaclePolygonsForDrivableArea( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters); + const bool is_object_front, const std::shared_ptr & parameters); double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); 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 24a286cc1809d..bb62516ddbfab 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 @@ -1891,8 +1891,10 @@ bool AvoidanceModule::isSafePath( return true; // if safety check is disabled, it always return safe. } - const auto ego_predicted_path = - utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, parameters_); + const auto ego_predicted_path_for_front_object = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, true, parameters_); + const auto ego_predicted_path_for_rear_object = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, false, parameters_); const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { @@ -1919,8 +1921,18 @@ bool AvoidanceModule::isSafePath( avoidance_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { + const auto obj_polygon = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); + + const auto is_object_front = utils::path_safety_checker::isTargetObjectFront( + shifted_path.path, getEgoPose(), p.vehicle_info, obj_polygon); + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); + + const auto & ego_predicted_path = + is_object_front ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object; + for (const auto & obj_path : obj_predicted_paths) { CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( 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 638b47e46fc66..257ccf52f2a4e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -143,10 +143,15 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.check_current_lane = get_parameter(node, ns + "check_current_lane"); p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); p.check_other_side_lane = get_parameter(node, ns + "check_other_side_lane"); - p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); + p.check_unavoidable_object = + get_parameter(node, ns + "check_unavoidable_object"); p.check_other_object = get_parameter(node, ns + "check_other_object"); - p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); - p.safety_check_time_horizon = get_parameter(node, ns + "time_horizon"); + p.check_all_predicted_path = + get_parameter(node, ns + "check_all_predicted_path"); + p.time_horizon_for_front_object = + get_parameter(node, ns + "time_horizon_for_front_object"); + p.time_horizon_for_rear_object = + get_parameter(node, ns + "time_horizon_for_rear_object"); p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); p.safety_check_backward_distance = get_parameter(node, ns + "safety_check_backward_distance"); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 2e6d8f982b8a2..4b109a63ac1ce 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1310,7 +1310,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) + const bool is_object_front, const std::shared_ptr & parameters) { if (path.points.empty()) { return {}; @@ -1319,7 +1319,8 @@ std::vector convertToPredictedPath( const auto & acceleration = parameters->max_acceleration; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); - const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_horizon = is_object_front ? parameters->time_horizon_for_front_object + : parameters->time_horizon_for_rear_object; const auto & time_resolution = parameters->safety_check_time_resolution; const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); @@ -1350,7 +1351,8 @@ ExtendedPredictedObject transform( extended_object.shape = object.shape; const auto & obj_velocity = extended_object.initial_twist.twist.linear.x; - const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_horizon = + std::max(parameters->time_horizon_for_front_object, parameters->time_horizon_for_rear_object); const auto & time_resolution = parameters->safety_check_time_resolution; extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); From 3beb77cc94eedf50bb36660dcbef90b558fe792e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 29 Aug 2023 10:05:42 +0900 Subject: [PATCH 10/93] perf(safety_check): use light weight object position check logic (#4762) perf(safety_check): use light weight object position check Signed-off-by: satoshi-ota --- .../path_safety_checker/safety_check.hpp | 3 +++ .../avoidance/avoidance_module.cpp | 4 ++-- .../path_safety_checker/safety_check.cpp | 24 ++++++++++++++++--- 3 files changed, 26 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 6d9df7677ead5..4a50065014f59 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -52,6 +52,9 @@ using vehicle_info_util::VehicleInfo; namespace bg = boost::geometry; +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info); bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); 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 bb62516ddbfab..9122c244411dc 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 @@ -1924,8 +1924,8 @@ bool AvoidanceModule::isSafePath( const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); - const auto is_object_front = utils::path_safety_checker::isTargetObjectFront( - shifted_path.path, getEgoPose(), p.vehicle_info, obj_polygon); + const auto is_object_front = + utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index c6a68e108e7e1..582c4922c4b0a 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -30,6 +30,25 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double base_to_front = vehicle_info.max_longitudinal_offset_m; + const auto ego_offset_pose = + tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); + + // check all edges in the polygon + for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { + return true; + } + } + + return false; +} + bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) @@ -214,7 +233,7 @@ boost::optional getInterpolatedPoseWithVeloci } bool checkCollision( - const PathWithLaneId & planned_path, + [[maybe_unused]] const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, @@ -259,8 +278,7 @@ bool checkCollision( } // compute which one is at the front of the other - const bool is_object_front = - isTargetObjectFront(planned_path, ego_pose, ego_vehicle_info, obj_polygon); + const bool is_object_front = isTargetObjectFront(ego_pose, obj_polygon, ego_vehicle_info); 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); From 71277da29b07ca22e7f1e4fc566aba3105aeef69 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Sun, 27 Aug 2023 21:50:20 +0900 Subject: [PATCH 11/93] feat(safety_check): add hysteresis factor in safety check logic (#4735) * feat(safety_check): add hysteresis factor Signed-off-by: satoshi-ota * feat(lane_change): add hysteresis factor Signed-off-by: satoshi-ota * feat(avoidance): add hysteresis factor Signed-off-by: satoshi-ota * feat(avoidance): add time series hysteresis Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 3 ++- .../scene_module/avoidance/avoidance_module.hpp | 4 ++++ .../utils/avoidance/avoidance_module_data.hpp | 3 ++- .../utils/path_safety_checker/safety_check.hpp | 2 +- .../src/scene_module/avoidance/avoidance_module.cpp | 11 +++++++++-- .../src/scene_module/avoidance/manager.cpp | 5 +++-- .../src/scene_module/lane_change/manager.cpp | 4 ++-- .../src/scene_module/lane_change/normal.cpp | 2 +- .../src/utils/avoidance/utils.cpp | 2 +- .../src/utils/path_safety_checker/safety_check.cpp | 6 +++--- 10 files changed, 28 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 968cd1d0110fe..53db7ee81c40e 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -145,8 +145,9 @@ time_horizon_for_front_object: 10.0 # [s] time_horizon_for_rear_object: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] - safety_check_hysteresis_factor: 2.0 # [-] safety_check_ego_offset: 1.0 # [m] + hysteresis_factor_expand_rate: 2.0 # [-] + hysteresis_factor_safe_count: 10 # [-] # rss parameters expected_front_deceleration: -1.0 # [m/ss] expected_rear_deceleration: -1.0 # [m/ss] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 01c82e62c1cdc..cda680b9dec7c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -555,6 +555,8 @@ class AvoidanceModule : public SceneModuleInterface bool arrived_path_end_{false}; + bool safe_{true}; + std::shared_ptr parameters_; helper::avoidance::AvoidanceHelper helper_; @@ -575,6 +577,8 @@ class AvoidanceModule : public SceneModuleInterface ObjectDataArray registered_objects_; + mutable size_t safe_count_{0}; + mutable ObjectDataArray ego_stopped_objects_; mutable ObjectDataArray stopped_objects_; 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 c57f94f27e430..27cd89e7756cc 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 @@ -201,7 +201,8 @@ struct AvoidanceParameters double safety_check_backward_distance; // transit hysteresis (unsafe to safe) - double safety_check_hysteresis_factor; + size_t hysteresis_factor_safe_count; + double hysteresis_factor_expand_rate; // don't output new candidate path if the offset between ego and path is larger than this. double safety_check_ego_offset; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 4a50065014f59..a0d4282c5f232 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -101,7 +101,7 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - CollisionCheckDebug & debug); + const double hysteresis_factor, CollisionCheckDebug & debug); /** * @brief Check collision between ego path footprints with extra longitudinal stopping margin and 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 9122c244411dc..1630ce7465c6e 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 @@ -1917,6 +1917,8 @@ bool AvoidanceModule::isSafePath( return true; } + const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; + const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( avoidance_data_, planner_data_, parameters_, is_right_shift.value()); @@ -1937,13 +1939,16 @@ bool AvoidanceModule::isSafePath( CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, - collision)) { + hysteresis_factor, collision)) { + safe_count_ = 0; return false; } } } - return true; + safe_count_++; + + return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const @@ -2589,6 +2594,8 @@ void AvoidanceModule::updateData() fillShiftLine(avoidance_data_, debug_data_); fillEgoStatus(avoidance_data_, debug_data_); fillDebugData(avoidance_data_, debug_data_); + + safe_ = avoidance_data_.safe; } void AvoidanceModule::processOnEntry() 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 257ccf52f2a4e..bec3ba0afd025 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -155,9 +155,10 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); p.safety_check_backward_distance = get_parameter(node, ns + "safety_check_backward_distance"); - p.safety_check_hysteresis_factor = - get_parameter(node, ns + "safety_check_hysteresis_factor"); p.safety_check_ego_offset = get_parameter(node, ns + "safety_check_ego_offset"); + p.hysteresis_factor_expand_rate = + get_parameter(node, ns + "hysteresis_factor_expand_rate"); + p.hysteresis_factor_safe_count = get_parameter(node, ns + "hysteresis_factor_safe_count"); } // safety check rss params diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index fff078ce7ef3d..7692cd624bffd 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -294,8 +294,8 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // safety check { std::string ns = "avoidance.safety_check."; - p.safety_check_hysteresis_factor = - get_parameter(node, ns + "safety_check_hysteresis_factor"); + p.hysteresis_factor_expand_rate = + get_parameter(node, ns + "hysteresis_factor_expand_rate"); } avoidance_parameters_ = std::make_shared(p); 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 2ab306cebd7d0..eee17d79af648 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 @@ -1277,7 +1277,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, current_debug_data.second)) { path_safety_status.is_safe = false; updateDebugInfo(current_debug_data, path_safety_status.is_safe); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 4b109a63ac1ce..10766b7aec416 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -645,7 +645,7 @@ void fillAvoidanceNecessity( } // TRUE -> ? (check with hysteresis factor) - object_data.avoid_required = check_necessity(parameters->safety_check_hysteresis_factor); + object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate); } void fillObjectStoppableJudge( diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 582c4922c4b0a..6dd3b627e67f4 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -238,7 +238,7 @@ bool checkCollision( const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, - CollisionCheckDebug & debug) + double hysteresis_factor, CollisionCheckDebug & debug) { debug.lerped_path.reserve(target_object_path.path.size()); @@ -291,8 +291,8 @@ bool checkCollision( const auto min_lon_length = calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, rss_parameters); - const auto & lon_offset = std::max(rss_dist, min_lon_length); - const auto & lat_margin = rss_parameters.lateral_distance_max_threshold; + const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor; + const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor; const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) From 03c987c3868097bfae5eae0b24c36534327b1e8d Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Mon, 11 Sep 2023 15:24:21 +0900 Subject: [PATCH 12/93] feat(avoidance): improve canceling judgement Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 8 ++++++-- .../src/scene_module/avoidance/manager.cpp | 6 ++---- 2 files changed, 8 insertions(+), 6 deletions(-) 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 1630ce7465c6e..40fc6ef7b53a6 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 @@ -1929,11 +1929,15 @@ bool AvoidanceModule::isSafePath( const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); + const auto is_object_incoming = + std::abs(calcYawDeviation(getEgoPose(), object.initial_pose.pose)) > M_PI_2; + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); - const auto & ego_predicted_path = - is_object_front ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object; + const auto & ego_predicted_path = is_object_front && !is_object_incoming + ? ego_predicted_path_for_front_object + : ego_predicted_path_for_rear_object; for (const auto & obj_path : obj_predicted_paths) { CollisionCheckDebug collision{}; 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 bec3ba0afd025..a9e8982a9a48a 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -143,11 +143,9 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.check_current_lane = get_parameter(node, ns + "check_current_lane"); p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); p.check_other_side_lane = get_parameter(node, ns + "check_other_side_lane"); - p.check_unavoidable_object = - get_parameter(node, ns + "check_unavoidable_object"); + p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); p.check_other_object = get_parameter(node, ns + "check_other_object"); - p.check_all_predicted_path = - get_parameter(node, ns + "check_all_predicted_path"); + p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); p.time_horizon_for_front_object = get_parameter(node, ns + "time_horizon_for_front_object"); p.time_horizon_for_rear_object = From 5715d9c3d1e512d03ed3026f1782aafc2994b9e5 Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Tue, 12 Sep 2023 17:18:39 +0900 Subject: [PATCH 13/93] fix: update parameter value (#4955) Signed-off-by: ktro2828 --- .../roi_cluster_fusion/node.hpp | 8 ++++---- .../launch/roi_cluster_fusion.launch.xml | 3 ++- .../src/roi_cluster_fusion/node.cpp | 20 +++++++++---------- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 38f9e71eea02d..70a7866e79b87 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -43,13 +43,13 @@ class RoiClusterFusionNode bool use_iou_{false}; bool use_cluster_semantic_type_{false}; bool only_allow_inside_cluster_{false}; - float roi_scale_factor_{1.1f}; - float iou_threshold_{0.0f}; - float unknown_iou_threshold_{0.0f}; + double roi_scale_factor_{1.1}; + double iou_threshold_{0.0}; + double unknown_iou_threshold_{0.0}; const float min_roi_existence_prob_ = 0.1; // keep small value to lessen affect on merger object stage bool remove_unknown_; - float trust_distance_; + double trust_distance_; bool filter_by_distance(const DetectedObjectWithFeature & obj); bool out_of_scope(const DetectedObjectWithFeature & obj); diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 302b567942e84..b2b9bae80f071 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -49,9 +49,10 @@ + - + diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 9180b18adeed8..77f6a5c506d92 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -36,16 +36,16 @@ namespace image_projection_based_fusion RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_cluster_fusion", options) { - use_iou_x_ = declare_parameter("use_iou_x", true); - use_iou_y_ = declare_parameter("use_iou_y", false); - use_iou_ = declare_parameter("use_iou", false); - use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); - only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster_", true); - roi_scale_factor_ = declare_parameter("roi_scale_factor", 1.1); - iou_threshold_ = declare_parameter("iou_threshold", 0.1); - unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold", 0.1); - remove_unknown_ = declare_parameter("remove_unknown", false); - trust_distance_ = declare_parameter("trust_distance", 100.0); + use_iou_x_ = declare_parameter("use_iou_x"); + use_iou_y_ = declare_parameter("use_iou_y"); + use_iou_ = declare_parameter("use_iou"); + use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type"); + only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster"); + roi_scale_factor_ = declare_parameter("roi_scale_factor"); + iou_threshold_ = declare_parameter("iou_threshold"); + unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold"); + remove_unknown_ = declare_parameter("remove_unknown"); + trust_distance_ = declare_parameter("trust_distance"); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) From d5134ecb8ae64f5843aac8bed03f38ba35b1948a Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 30 Aug 2023 17:43:43 +0900 Subject: [PATCH 14/93] feat(intersection): strict definition of stuck vehicle detection area (#4782) Signed-off-by: Mamoru Sobue --- .../README.md | 5 +- .../config/intersection.param.yaml | 1 - .../docs/stuck-vehicle.drawio.svg | 443 +++++++++++------- .../src/manager.cpp | 4 - .../src/scene_intersection.cpp | 34 +- .../src/scene_intersection.hpp | 6 +- .../src/util.cpp | 130 +++-- .../src/util.hpp | 14 +- .../src/util_type.hpp | 3 + 9 files changed, 377 insertions(+), 263 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 3ab642abe1e8f..e087572a1143f 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -62,7 +62,7 @@ Objects that satisfy all of the following conditions are considered as target ob #### Stuck Vehicle Detection -If there is any object in a certain distance (`stuck_vehicle_ignore_dist` and `stuck_vehicle_detect_dist`) before and after the exit of the intersection lane and the object velocity is less than a threshold (=`stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path +If there is any object on the path in inside the intersection and at the exit of the intersection (up to `stuck_vehicle_detect_dist`) lane and its velocity is less than a threshold (`stuck_vehicle.stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg) @@ -107,8 +107,7 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav | `common.stop_overshoot_margin` | double | [m] margin for the overshoot from stopline | | `common.path_interpolation_ds` | double | [m] path interpolation interval | | `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection | -| `stuck_vehicle.stuck_vehicle_ignore_dist` | double | [m] length behind the exit of intersection for stuck vehicle detection | -| `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threhsold for stuck vehicle detection | +| `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection | | `collision_detection.state_transit_margin_time` | double | [m] time margin to change state | | `collision_detection.min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection | | `collision_detection.collision_start_margin_time` | double | [s] time margin for the beginning of collision with upcoming vehicle | diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index ce49163006d99..9f6a7d15ba47a 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -15,7 +15,6 @@ stuck_vehicle: use_stuck_stopline: true # stopline generated before the first conflicting area stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h # enable_front_car_decel_prediction: false # By default this feature is disabled # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning diff --git a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg index d2a8286122e54..8802dc786b617 100644 --- a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg @@ -5,28 +5,28 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="2831px" - height="1073px" - viewBox="-0.5 -0.5 2831 1073" - content="<mxfile host="Electron" modified="2023-06-07T05:26:53.508Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="B7vxgoBz0X2z8hjEAQoV" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="2842px" + height="1201px" + viewBox="-0.5 -0.5 2842 1201" + content="<mxfile host="Electron" modified="2023-08-28T08:00:31.600Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="CbgId97oWJwBUVWni98L" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > - - - - - - - - - - - - - - + + + + + + + + + + + + + - + - + - + - - - - - + + + + + - + - - - + + + - - - + + + - - - - + + + + + + + - - - - + + + + - - - + + + - - + + - + - - - - - + + + + + - +
@@ -259,22 +262,22 @@
- stuck vehicle detection area is generated on the path around the exit o... + stuck vehicle detection area is generated on the path around the exit o... - - - - - - - - - - - - + + + + + + + + + + + - + - + - + - - - - - + + + + + - + - +
@@ -366,14 +369,14 @@
- %3CmxGraphModel%3E%3Croot%3E%3... + %3CmxGraphModel%3E%3Croot%3E%3... - - - + + + - - - + + + - - - - + + + + - - - - + + + + - - - + + + - - + + - + - - + + - - + + @@ -510,7 +513,7 @@
@@ -521,18 +524,18 @@
- stuck vehicle detection area is generated from the path, so in this... + stuck vehicle detection area is generated from the path, so in this... - - - - - + + + + + -
+
@@ -542,76 +545,158 @@
- stop_line_margin + stop_line_margin - + - - + +
`
 - ` + ` + + + + +
+
+
+ + + stuck_vehicle_detect_dist + + +
+
+
+ + stuck_vehicle_detect_dist + + + + + +
+
+
+ + + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bfont-size%3A%2035px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3Bw%2F%20traffic%20light%2C%20right%26lt%3B%2Fb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bb%26gt%3B(Left-hand%20traffic)%26lt%3B%2Fb%26gt%3B%26lt%3B%2Ffont%26gt%3B%26lt%3Bspan%20style%3D%26quot%3Bfont-size%3A%2030px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3B%2Fb%26gt%3B%26lt%3B%2Fspan%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dleft%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22-17325%22%20y%3D%22-7209%22%20width%3D%22350%22%20height%3D%22140%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + + straight +
+
+ (Right-hand traffic) + + + +
+ + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%... + + + + + +
+
+
+ + + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bfont-size%3A%2035px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3Bw%2F%20traffic%20light%2C%20right%26lt%3B%2Fb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bb%26gt%3B(Left-hand%20traffic)%26lt%3B%2Fb%26gt%3B%26lt%3B%2Ffont%26gt%3B%26lt%3Bspan%20style%3D%26quot%3Bfont-size%3A%2030px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3B%2Fb%26gt%3B%26lt%3B%2Fspan%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dleft%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22-17325%22%20y%3D%22-7209%22%20width%3D%22350%22%20height%3D%22140%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + + left turn +
+
+ (Right-hand traffic) + + + +
+ + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%... + + diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 75c282d117e97..77ee106a893b6 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -40,7 +40,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) { const std::string ns(getModuleName()); auto & ip = intersection_param_; - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.common.attention_area_margin = node.declare_parameter(ns + ".common.attention_area_margin"); ip.common.attention_area_length = @@ -65,9 +64,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); - ip.stuck_vehicle.stuck_vehicle_ignore_dist = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") + - vehicle_info.max_longitudinal_offset_m; ip.stuck_vehicle.stuck_vehicle_vel_thr = node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_vel_thr"); /* diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 153f73a7b850e..9498935858f6a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -756,22 +756,18 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( [closest_idx, stuck_stop_line_idx, default_stop_line_idx, occlusion_peeking_stop_line_idx, pass_judge_line_idx] = intersection_stop_lines; + const auto & conflicting_area = intersection_lanelets_.value().conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, *path, associative_ids_, closest_idx, - planner_data_->vehicle_info_.vehicle_width_m); + lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area.value(), + conflicting_area, closest_idx, planner_data_->vehicle_info_.vehicle_width_m); if (!path_lanelets_opt.has_value()) { RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); return IntersectionModule::Indecisive{}; } const auto path_lanelets = path_lanelets_opt.value(); - const auto ego_lane_with_next_lane = - path_lanelets.next.has_value() - ? std::vector< - lanelet::ConstLanelet>{path_lanelets.ego_or_entry2exit, path_lanelets.next.value()} - : std::vector{path_lanelets.ego_or_entry2exit}; - const bool stuck_detected = - checkStuckVehicle(planner_data_, ego_lane_with_next_lane, *path, intersection_stop_lines); + const bool stuck_detected = checkStuckVehicle( + planner_data_, path_lanelets, interpolated_path_info, intersection_stop_lines); if (stuck_detected) { const double dist_stopline = motion_utils::calcSignedArcLength( @@ -942,9 +938,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, - const lanelet::ConstLanelets & ego_lane_with_next_lane, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, + const util::InterpolatedPathInfo & interpolated_path_info, const util::IntersectionStopLines & intersection_stop_lines) { const auto & objects_ptr = planner_data->predicted_objects; @@ -953,20 +948,16 @@ bool IntersectionModule::checkStuckVehicle( const auto stuck_line_idx = intersection_stop_lines.stuck_stop_line; // considering lane change in the intersection, these lanelets are generated from the path - const auto ego_lane = ego_lane_with_next_lane.front(); - debug_data_.ego_lane = ego_lane.polygon3d(); + const auto & path = interpolated_path_info.path; const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( - input_path, ego_lane_with_next_lane, closest_idx, - planner_param_.stuck_vehicle.stuck_vehicle_detect_dist, - planner_param_.stuck_vehicle.stuck_vehicle_ignore_dist, - planner_data->vehicle_info_.vehicle_length_m); + path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); const double dist_stuck_stopline = motion_utils::calcSignedArcLength( - input_path.points, input_path.points.at(stuck_line_idx).point.pose.position, - input_path.points.at(closest_idx).point.pose.position); + path.points, path.points.at(stuck_line_idx).point.pose.position, + path.points.at(closest_idx).point.pose.position); const bool is_over_stuck_stopline = - util::isOverTargetIndex(input_path, closest_idx, current_pose, stuck_line_idx) && + util::isOverTargetIndex(path, closest_idx, current_pose, stuck_line_idx) && (dist_stuck_stopline > planner_param_.common.stop_overshoot_margin); bool is_stuck = false; @@ -1054,6 +1045,7 @@ bool IntersectionModule::checkCollision( const auto closest_arc_coords = getArcCoordinates( concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); const auto & ego_lane = path_lanelets.ego_or_entry2exit; + debug_data_.ego_lane = ego_lane.polygon3d(); const auto ego_poly = ego_lane.polygon2d().basicPolygon(); // check collision between predicted_path and ego_area diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 9d1fab1339d0e..76b76214297ce 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -65,8 +65,6 @@ class IntersectionModule : public SceneModuleInterface { bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check - double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle - //! check double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped /* double @@ -246,8 +244,8 @@ class IntersectionModule : public SceneModuleInterface bool checkStuckVehicle( const std::shared_ptr & planner_data, - const lanelet::ConstLanelets & ego_lane_with_next_lane, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const util::PathLanelets & path_lanelets, + const util::InterpolatedPathInfo & interpolated_path_info, const util::IntersectionStopLines & intersection_stop_lines); autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index fe3ebeb1d95b8..b51e7f45d8fc7 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -324,14 +324,25 @@ std::optional generateIntersectionStopLines( std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon) + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, + const bool search_forward) { const auto polygon_2d = lanelet::utils::to2D(polygon); - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - auto p = path.points.at(i).point.pose.position; - const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional(i); + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + const auto & p = path.points.at(i).point.pose.position; + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional(i); + } + } + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + const auto & p = path.points.at(i).point.pose.position; + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional(i); + } } } return std::nullopt; @@ -341,21 +352,39 @@ static std::optional> getFirstPointInsidePolygons( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, - const std::vector & polygons) + const std::vector & polygons, const bool search_forward = true) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - auto p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } if (is_in_lanelet) { - return std::make_optional>( - i, polygon); + break; } } - if (is_in_lanelet) { - break; + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + bool is_in_lanelet = false; + const auto & p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } } } return std::nullopt; @@ -915,35 +944,29 @@ bool checkStuckVehicleInIntersection( } Polygon2d generateStuckVehicleDetectAreaPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double stuck_vehicle_detect_dist, const double stuck_vehicle_ignore_dist, - const double vehicle_length_m) + const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; using lanelet::utils::getPolygonFromArcLength; using lanelet::utils::to2D; - const double extra_dist = stuck_vehicle_detect_dist + vehicle_length_m; - const double ignore_dist = stuck_vehicle_ignore_dist + vehicle_length_m; - - const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front()); - - const auto closest_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - - const double start_arc_length = intersection_exit_length - ignore_dist > closest_arc_coords.length - ? intersection_exit_length - ignore_dist - : closest_arc_coords.length; - - const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist; + Polygon2d polygon{}; + if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { + return polygon; + } + double target_polygon_length = + getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); + lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; + if (path_lanelets.next) { + targets.push_back(path_lanelets.next.value()); + const double next_arc_length = + std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); + target_polygon_length += next_arc_length; + } const auto target_polygon = - to2D(getPolygonFromArcLength(ego_lane_with_next_lane, start_arc_length, end_arc_length)) - .basicPolygon(); - - Polygon2d polygon{}; + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); if (target_polygon.empty()) { return polygon; @@ -1126,13 +1149,17 @@ static lanelet::ConstLanelets getPrevLanelets( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const size_t closest_idx, const double width) + const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, const size_t closest_idx, + const double width) { - const auto assigned_lane_interval_opt = findLaneIdsInterval(path, associative_ids); + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; if (!assigned_lane_interval_opt) { return std::nullopt; } + const auto assigned_lane_interval = assigned_lane_interval_opt.value(); + const auto & path = interpolated_path_info.path; PathLanelets path_lanelets; // prev @@ -1140,7 +1167,7 @@ std::optional generatePathLanelets( path_lanelets.all = path_lanelets.prev; // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval_opt.value(); + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; if (closest_idx > assigned_lane_start) { path_lanelets.all.push_back( planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width)); @@ -1158,12 +1185,27 @@ std::optional generatePathLanelets( const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); if (next_lane_interval_opt) { const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = - planning_utils::generatePathLanelet(path, next_start + 1, next_end, width); + path_lanelets.next = planning_utils::generatePathLanelet(path, next_start, next_end, width); path_lanelets.all.push_back(path_lanelets.next.value()); } } + const auto first_inside_conflicting_idx_opt = + getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + const auto last_inside_conflicting_idx_opt = + getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { + const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); + const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; + lanelet::ConstLanelet conflicting_interval = planning_utils::generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = planning_utils::generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } return path_lanelets; } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 5041336c58558..c0f5ea7095f31 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -83,7 +83,8 @@ std::optional generateIntersectionStopLines( std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon); + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, + const bool search_forward = true); /** * @brief check if ego is over the target_idx. If the index is same, compare the exact pose @@ -131,10 +132,7 @@ bool checkStuckVehicleInIntersection( DebugData * debug_data); Polygon2d generateStuckVehicleDetectAreaPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double stuck_vehicle_detect_dist, const double stuck_vehicle_ignore_dist, - const double vehicle_length_m); + const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); bool checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, @@ -157,8 +155,10 @@ double calcDistanceUntilIntersectionLanelet( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const size_t closest_idx, const double width); + const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, const size_t closest_idx, + const double width); } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 4d2cf9695b2db..9f7f5a449e8b0 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -142,6 +142,9 @@ struct PathLanelets std::optional next = std::nullopt; // this is nullopt is the goal is inside intersection lanelet::ConstLanelets all; + lanelet::ConstLanelets + conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with + // conflicting lanelets plus the next lane part of the path }; } // namespace behavior_velocity_planner::util From 89b9894d94b8534d337207877567ad3f4d927b92 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 6 Sep 2023 12:36:20 +0900 Subject: [PATCH 15/93] feat(intersection): do not generate stuck vehicle detect area before attention area if attention area exists (#4872) --- .../src/scene_intersection.cpp | 16 ++++++++++++---- .../src/util.cpp | 12 +++++++++--- .../src/util.hpp | 4 +++- 3 files changed, 24 insertions(+), 8 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 9498935858f6a..b21f4f5bbbbfa 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -326,9 +326,16 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "StuckStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); + const auto closest_idx = decision_result.stop_lines.closest_idx; if (!rtc_default_approved) { // use default_rtc uuid for stuck vehicle detection - const auto stop_line_idx = decision_result.stop_line_idx; + auto stop_line_idx = decision_result.stop_line_idx; + if ( + !decision_result.is_detection_area_empty && + motion_utils::calcSignedArcLength(path->points, stop_line_idx, closest_idx) > + planner_param.common.stop_overshoot_margin) { + stop_line_idx = decision_result.stop_lines.default_stop_line; + } planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -338,7 +345,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -354,7 +361,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(occlusion_stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(closest_idx).point.pose, path->points.at(occlusion_stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -759,7 +766,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto & conflicting_area = intersection_lanelets_.value().conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area.value(), - conflicting_area, closest_idx, planner_data_->vehicle_info_.vehicle_width_m); + conflicting_area, first_attention_area, intersection_lanelets_.value().attention_area(), + closest_idx, planner_data_->vehicle_info_.vehicle_width_m); if (!path_lanelets_opt.has_value()) { RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); return IntersectionModule::Indecisive{}; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index b51e7f45d8fc7..72695d36fe243 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1151,7 +1151,9 @@ std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, const size_t closest_idx, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx, const double width) { const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; @@ -1191,9 +1193,13 @@ std::optional generatePathLanelets( } const auto first_inside_conflicting_idx_opt = - getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + first_attention_area.has_value() + ? getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) + : getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); const auto last_inside_conflicting_idx_opt = - getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + first_attention_area.has_value() + ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) + : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index c0f5ea7095f31..e7c77ed08451a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -157,7 +157,9 @@ std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, const size_t closest_idx, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx, const double width); } // namespace util From c3d91eb6719d943812313258fb45bec70de965f5 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 30 Aug 2023 15:38:54 +0900 Subject: [PATCH 16/93] feat(intersection): suppress intersection occlusion chattering (#4788) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 1 + .../src/manager.cpp | 2 + .../src/scene_intersection.cpp | 76 +++++++++++-------- .../src/scene_intersection.hpp | 2 + 4 files changed, 51 insertions(+), 30 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 9f6a7d15ba47a..095073202c8fa 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -47,6 +47,7 @@ denoise_kernel: 1.0 # [m] possible_object_bbox: [1.0, 2.0] # [m x m] ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h + stop_release_margin_time: 1.0 # [s] enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval intersection: true diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 77ee106a893b6..a039b74036954 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -113,6 +113,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter>(ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = node.declare_parameter(ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + ip.occlusion.stop_release_margin_time = + node.declare_parameter(ns + ".occlusion.stop_release_margin_time"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index b21f4f5bbbbfa..878e2c64089c0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -82,10 +82,15 @@ IntersectionModule::IntersectionModule( turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); collision_state_machine_.setMarginTime( planner_param_.collision_detection.state_transit_margin_time); - before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); - before_creep_state_machine_.setState(StateMachine::State::STOP); - stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); - stuck_private_area_timeout_.setState(StateMachine::State::STOP); + { + before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); + before_creep_state_machine_.setState(StateMachine::State::STOP); + } + { + occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time); + occlusion_stop_state_machine_.setState(StateMachine::State::GO); + } + decision_state_pub_ = node_.create_publisher("~/debug/intersection/decision_state", 1); } @@ -646,6 +651,35 @@ void reactRTCApproval( return; } +static std::string formatDecisionResult(const IntersectionModule::DecisionResult & decision_result) +{ + if (std::holds_alternative(decision_result)) { + return "Indecisive"; + } + if (std::holds_alternative(decision_result)) { + return "Safe"; + } + if (std::holds_alternative(decision_result)) { + return "StuckStop"; + } + if (std::holds_alternative(decision_result)) { + return "NonOccludedCollisionStop"; + } + if (std::holds_alternative(decision_result)) { + return "FirstWaitBeforeOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "PeekingTowardOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "OccludedCollisionStop"; + } + if (std::holds_alternative(decision_result)) { + return "TrafficLightArrowSolidOn"; + } + return ""; +} + bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { debug_data_ = util::DebugData(); @@ -657,31 +691,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); - std::string decision_type = "intersection" + std::to_string(module_id_) + " : "; - if (std::get_if(&decision_result)) { - decision_type += "Indecisive"; - } - if (std::get_if(&decision_result)) { - decision_type += "StuckStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "NonOccludedCollisionStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "FirstWaitBeforeOcclusion"; - } - if (std::get_if(&decision_result)) { - decision_type += "PeekingTowardOcclusion"; - } - if (std::get_if(&decision_result)) { - decision_type += "OccludedCollisionStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "Safe"; - } - if (std::get_if(&decision_result)) { - decision_type += "TrafficLightArrowSolidOn"; - } + const std::string decision_type = + "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); std_msgs::msg::String decision_result_msg; decision_result_msg.data = decision_type; decision_state_pub_->publish(decision_result_msg); @@ -897,10 +908,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( first_attention_area.value(), interpolated_path_info, occlusion_attention_divisions_.value(), parked_attention_objects, occlusion_dist_thr) : true; + occlusion_stop_state_machine_.setStateWithMarginTime( + is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, + logger_.get_child("occlusion_stop"), *clock_); // check safety const bool ext_occlusion_requested = (is_occlusion_cleared && !occlusion_activated_); - if (!is_occlusion_cleared || ext_occlusion_requested) { + if ( + occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || + ext_occlusion_requested) { const double dist_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(closest_idx).point.pose.position, path->points.at(default_stop_line_idx).point.pose.position); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 76b76214297ce..fc35da07c78df 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -107,6 +107,7 @@ class IntersectionModule : public SceneModuleInterface double denoise_kernel; std::vector possible_object_bbox; double ignore_parked_vehicle_speed_threshold; + double stop_release_margin_time; } occlusion; }; @@ -221,6 +222,7 @@ class IntersectionModule : public SceneModuleInterface // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop + StateMachine occlusion_stop_state_machine_; // NOTE: uuid_ is base member // for stuck vehicle detection From 6dacbccfdefc4e9c17aa0f7334c066b34d4503dd Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 12 Sep 2023 18:40:54 +0900 Subject: [PATCH 17/93] fix(intersection): stuck stop line generation when ego is over fist conflicting area (#4903) Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 350 +++++++++--------- .../src/scene_intersection.hpp | 69 ++-- .../src/util.cpp | 108 ++++-- .../src/util_type.hpp | 26 +- 4 files changed, 301 insertions(+), 252 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 878e2c64089c0..f34c828db0f13 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -120,7 +120,7 @@ template void prepareRTCByDecisionResult( const T & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance, bool * occlusion_first_stop_required) + double * occlusion_distance) { static_assert("Unsupported type passed to prepareRTCByDecisionResult"); return; @@ -131,8 +131,7 @@ void prepareRTCByDecisionResult( [[maybe_unused]] const IntersectionModule::Indecisive & result, [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, - [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { return; } @@ -141,17 +140,16 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::StuckStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "StuckStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto stop_line_idx = result.stuck_stop_line_idx; *default_safety = false; *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); *occlusion_safety = true; - if (!result.is_detection_area_empty) { - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + if (result.occlusion_stop_line_idx) { + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx.value(); *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); } @@ -162,15 +160,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "NonOccludedCollisionStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); - const auto occlusion_stop_line = result.stop_lines.occlusion_peeking_stop_line; + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + const auto occlusion_stop_line = result.occlusion_stop_line_idx; *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line); @@ -181,11 +179,10 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::FirstWaitBeforeOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FirstWaitBeforeOcclusion"); - const auto closest_idx = result.stop_lines.closest_idx; + const auto closest_idx = result.closest_idx; const auto first_stop_line_idx = result.first_stop_line_idx; const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = false; @@ -194,7 +191,6 @@ void prepareRTCByDecisionResult( *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); - *occlusion_first_stop_required = true; return; } @@ -202,18 +198,18 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::PeekingTowardOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "PeekingTowardOcclusion"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; - *occlusion_safety = result.is_actually_occlusion_cleared; - *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); - const auto default_stop_line_idx = result.stop_lines.default_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + *occlusion_safety = result.is_actually_occlusion_cleared; + *occlusion_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); return; } @@ -221,15 +217,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::OccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedCollisionStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -240,16 +236,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::Safe & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto default_stop_line_idx = result.stop_lines.default_stop_line; - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -260,16 +255,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::TrafficLightArrowSolidOn & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "TrafficLightArrowSolidOn"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto default_stop_line_idx = result.stop_lines.default_stop_line; - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = !result.collision_detected; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -286,11 +280,13 @@ void IntersectionModule::prepareRTCStatus( VisitorSwitch{[&](const auto & decision) { prepareRTCByDecisionResult( decision, path, &default_safety, &default_distance, &occlusion_safety_, - &occlusion_stop_distance_, &occlusion_first_stop_required_); + &occlusion_stop_distance_); }}, decision_result); setSafe(default_safety); setDistance(default_distance); + occlusion_first_stop_required_ = + std::holds_alternative(decision_result); } template @@ -331,16 +327,10 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "StuckStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); - const auto closest_idx = decision_result.stop_lines.closest_idx; + const auto closest_idx = decision_result.closest_idx; if (!rtc_default_approved) { // use default_rtc uuid for stuck vehicle detection - auto stop_line_idx = decision_result.stop_line_idx; - if ( - !decision_result.is_detection_area_empty && - motion_utils::calcSignedArcLength(path->points, stop_line_idx, closest_idx) > - planner_param.common.stop_overshoot_margin) { - stop_line_idx = decision_result.stop_lines.default_stop_line; - } + const auto stop_line_idx = decision_result.stuck_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -355,9 +345,9 @@ void reactRTCApprovalByDecisionResult( } } if ( - !rtc_occlusion_approved && !decision_result.is_detection_area_empty && + !rtc_occlusion_approved && decision_result.occlusion_stop_line_idx && planner_param.occlusion.enable) { - const auto occlusion_stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto occlusion_stop_line_idx = decision_result.occlusion_stop_line_idx.value(); planning_utils::setVelocityFromIndex(occlusion_stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stop_line_idx, baselink2front, *path); @@ -386,7 +376,7 @@ void reactRTCApprovalByDecisionResult( "NonOccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -395,12 +385,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -409,7 +399,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -438,14 +428,14 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { if (planner_param.occlusion.enable_creeping) { const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; - const size_t closest_idx = decision_result.stop_lines.closest_idx; + const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { planning_utils::setVelocityFromIndex( i, planner_param.occlusion.occlusion_creep_velocity, path); @@ -460,7 +450,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -482,10 +472,9 @@ void reactRTCApprovalByDecisionResult( // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const size_t occlusion_peeking_stop_line = - decision_result.stop_lines.occlusion_peeking_stop_line; + const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; if (planner_param.occlusion.enable_creeping) { - const size_t closest_idx = decision_result.stop_lines.closest_idx; + const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { planning_utils::setVelocityFromIndex( i, planner_param.occlusion.occlusion_creep_velocity, path); @@ -499,12 +488,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(occlusion_peeking_stop_line).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(occlusion_peeking_stop_line).point.pose, VelocityFactor::INTERSECTION); } } - if (!rtc_default_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + if (!rtc_default_approved) { + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -513,7 +502,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -533,7 +522,7 @@ void reactRTCApprovalByDecisionResult( "OccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -542,7 +531,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -556,7 +545,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -575,7 +564,7 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "Safe, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -584,12 +573,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -598,7 +587,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -618,7 +607,7 @@ void reactRTCApprovalByDecisionResult( "TrafficLightArrowSolidOn, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -627,7 +616,21 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->occlusion_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -690,6 +693,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + prev_decision_result_ = decision_result; const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); @@ -726,13 +730,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { RCLCPP_DEBUG(logger_, "splineInterpolate failed"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } @@ -751,33 +753,33 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( intersection_lanelets_.value().update(tl_arrow_solid_on, interpolated_path_info); const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); - const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); - if (conflicting_lanelets.empty() || !first_conflicting_area) { - is_peeking_ = false; + const auto & first_conflicting_area_opt = intersection_lanelets_.value().first_conflicting_area(); + if (conflicting_lanelets.empty() || !first_conflicting_area_opt) { + RCLCPP_DEBUG(logger_, "conflicting area is empty"); return IntersectionModule::Indecisive{}; } + const auto first_conflicting_area = first_conflicting_area_opt.value(); - const auto & first_attention_area = intersection_lanelets_.value().first_attention_area(); + const auto & first_attention_area_opt = intersection_lanelets_.value().first_attention_area(); const auto & dummy_first_attention_area = - first_attention_area ? first_attention_area.value() : first_conflicting_area.value(); + first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( - first_conflicting_area.value(), dummy_first_attention_area, planner_data_, - interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, - planner_param_.common.stop_line_margin, planner_param_.occlusion.peeking_offset, path); + first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, + planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, + planner_param_.occlusion.peeking_offset, path); if (!intersection_stop_lines_opt) { RCLCPP_DEBUG(logger_, "failed to generate intersection_stop_lines"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); const auto - [closest_idx, stuck_stop_line_idx, default_stop_line_idx, occlusion_peeking_stop_line_idx, - pass_judge_line_idx] = intersection_stop_lines; + [closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt, + occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = intersection_stop_lines; const auto & conflicting_area = intersection_lanelets_.value().conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area.value(), - conflicting_area, first_attention_area, intersection_lanelets_.value().attention_area(), + lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, + conflicting_area, first_attention_area_opt, intersection_lanelets_.value().attention_area(), closest_idx, planner_data_->vehicle_info_.vehicle_width_m); if (!path_lanelets_opt.has_value()) { RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); @@ -785,10 +787,10 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const auto path_lanelets = path_lanelets_opt.value(); - const bool stuck_detected = checkStuckVehicle( - planner_data_, path_lanelets, interpolated_path_info, intersection_stop_lines); + const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); - if (stuck_detected) { + if (stuck_detected && stuck_stop_line_idx_opt) { + auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); const double dist_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(closest_idx).point.pose.position, path->points.at(stuck_stop_line_idx).point.pose.position); @@ -802,24 +804,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool timeout = (is_private_area_ && stuck_private_area_timeout_.getState() == StateMachine::State::GO); if (!timeout) { - is_peeking_ = false; + if ( + default_stop_line_idx_opt && + motion_utils::calcSignedArcLength(path->points, stuck_stop_line_idx, closest_idx) > + planner_param_.common.stop_overshoot_margin) { + stuck_stop_line_idx = default_stop_line_idx_opt.value(); + } return IntersectionModule::StuckStop{ - stuck_stop_line_idx, !first_attention_area.has_value(), intersection_stop_lines}; + closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt}; } } - if (!first_attention_area) { + if (!first_attention_area_opt) { RCLCPP_DEBUG(logger_, "attention area is empty"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } + const auto first_attention_area = first_attention_area_opt.value(); - if (default_stop_line_idx == 0) { - RCLCPP_DEBUG(logger_, "stop line index is 0"); - is_peeking_ = false; + if (!default_stop_line_idx_opt) { + RCLCPP_DEBUG(logger_, "default stop line is null"); return IntersectionModule::Indecisive{}; } + const auto default_stop_line_idx = default_stop_line_idx_opt.value(); + // TODO(Mamoru Sobue): this part needs more formal handling const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); @@ -829,24 +837,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); const double vel = std::fabs(planner_data_->current_velocity->twist.linear.x); const bool keep_detection = (vel < planner_param_.collision_detection.keep_detection_vel_thr); + const bool was_safe = std::holds_alternative(prev_decision_result_); // if ego is over the pass judge line and not stopped - if (is_peeking_) { - // do nothing - RCLCPP_DEBUG(logger_, "peeking now"); - } else if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { + if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { RCLCPP_DEBUG( logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection"); // do nothing } else if ( - (is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || is_permanent_go_) { + (was_safe && is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || + is_permanent_go_) { // is_go_out_: previous RTC approval // activated_: current RTC approval is_permanent_go_ = true; RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } + if (!occlusion_peeking_stop_line_idx_opt) { + RCLCPP_DEBUG(logger_, "occlusion stop line is null"); + return IntersectionModule::Indecisive{}; + } + const auto collision_stop_line_idx = + is_over_default_stop_line ? closest_idx : default_stop_line_idx; + const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); + const auto & attention_lanelets = intersection_lanelets_.value().attention(); const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent(); const auto & occlusion_attention_lanelets = intersection_lanelets_.value().occlusion_attention(); @@ -880,8 +894,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getState() == StateMachine::State::STOP; if (tl_arrow_solid_on) { - is_peeking_ = false; - return TrafficLightArrowSolidOn{has_collision, intersection_stop_lines}; + return TrafficLightArrowSolidOn{ + has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } // check occlusion on detection lane @@ -890,6 +904,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( occlusion_attention_lanelets, routing_graph_ptr, planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); } + const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / @@ -905,15 +920,17 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, - first_attention_area.value(), interpolated_path_info, - occlusion_attention_divisions_.value(), parked_attention_objects, occlusion_dist_thr) + first_attention_area, interpolated_path_info, occlusion_attention_divisions, + parked_attention_objects, occlusion_dist_thr) : true; occlusion_stop_state_machine_.setStateWithMarginTime( is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, logger_.get_child("occlusion_stop"), *clock_); + const bool is_occlusion_cleared_with_margin = + (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // check safety - const bool ext_occlusion_requested = (is_occlusion_cleared && !occlusion_activated_); + const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); if ( occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || ext_occlusion_requested) { @@ -928,15 +945,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( before_creep_state_machine_.setState(StateMachine::State::GO); } if (before_creep_state_machine_.getState() == StateMachine::State::GO) { - if (has_collision) { - is_peeking_ = true; + if (has_collision_with_margin) { return IntersectionModule::OccludedCollisionStop{ - default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, - intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, + occlusion_stop_line_idx}; } else { - is_peeking_ = true; return IntersectionModule::PeekingTowardOcclusion{ - occlusion_peeking_stop_line_idx, is_occlusion_cleared, intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, + occlusion_stop_line_idx}; } } else { if (is_stopped && approached_stop_line) { @@ -944,53 +960,31 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( before_creep_state_machine_.setStateWithMarginTime( StateMachine::State::GO, logger_.get_child("occlusion state_machine"), *clock_); } - is_peeking_ = true; return IntersectionModule::FirstWaitBeforeOcclusion{ - default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, - intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, + occlusion_stop_line_idx}; } } else if (has_collision_with_margin) { - const bool is_over_default_stopLine = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); - const auto stop_line_idx = is_over_default_stopLine ? closest_idx : default_stop_line_idx; - is_peeking_ = false; - return IntersectionModule::NonOccludedCollisionStop{stop_line_idx, intersection_stop_lines}; + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } - is_peeking_ = false; - return IntersectionModule::Safe{intersection_stop_lines}; + return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, - const util::InterpolatedPathInfo & interpolated_path_info, - const util::IntersectionStopLines & intersection_stop_lines) + const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) { const auto & objects_ptr = planner_data->predicted_objects; - const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose; - const auto closest_idx = intersection_stop_lines.closest_idx; - const auto stuck_line_idx = intersection_stop_lines.stuck_stop_line; // considering lane change in the intersection, these lanelets are generated from the path - const auto & path = interpolated_path_info.path; const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - const double dist_stuck_stopline = motion_utils::calcSignedArcLength( - path.points, path.points.at(stuck_line_idx).point.pose.position, - path.points.at(closest_idx).point.pose.position); - const bool is_over_stuck_stopline = - util::isOverTargetIndex(path, closest_idx, current_pose, stuck_line_idx) && - (dist_stuck_stopline > planner_param_.common.stop_overshoot_margin); - - bool is_stuck = false; - if (!is_over_stuck_stopline) { - is_stuck = util::checkStuckVehicleInIntersection( - objects_ptr, stuck_vehicle_detect_area, planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, - &debug_data_); - } - return is_stuck; + return util::checkStuckVehicleInIntersection( + objects_ptr, stuck_vehicle_detect_area, planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, + &debug_data_); } autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( @@ -1521,54 +1515,54 @@ bool IntersectionModule::isOcclusionCleared( } /* -bool IntersectionModule::checkFrontVehicleDeceleration( + bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, const Polygon2d & stuck_vehicle_detect_area, const autoware_auto_perception_msgs::msg::PredictedObject & object, const double assumed_front_car_decel) -{ + { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; // consider vehicle in ego-lane && in front of ego const auto lon_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; const double object_decel = - planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive + planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive const double stopping_distance = lon_vel * lon_vel / (2 * object_decel); std::vector center_points; for (auto && p : ego_lane_with_next_lane[0].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); + center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); for (auto && p : ego_lane_with_next_lane[1].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); + center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); const double lat_offset = - std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); + std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); // get the nearest centerpoint to object std::vector dist_obj_center_points; for (const auto & p : center_points) - dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, -p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), - std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); + dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, + p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), + std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); // find two center_points whose distances from `closest_centerpoint` cross stopping_distance double acc_dist_prev = 0.0, acc_dist = 0.0; auto p1 = center_points[obj_closest_centerpoint_idx]; auto p2 = center_points[obj_closest_centerpoint_idx]; for (unsigned i = obj_closest_centerpoint_idx; i < center_points.size() - 1; ++i) { - p1 = center_points[i]; - p2 = center_points[i + 1]; - acc_dist_prev = acc_dist; - const auto arc_position_p1 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); - const auto arc_position_p2 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); - const double delta = arc_position_p2.length - arc_position_p1.length; - acc_dist += delta; - if (acc_dist > stopping_distance) { - break; - } + p1 = center_points[i]; + p2 = center_points[i + 1]; + acc_dist_prev = acc_dist; + const auto arc_position_p1 = + lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); + const auto arc_position_p2 = + lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); + const double delta = arc_position_p2.length - arc_position_p1.length; + acc_dist += delta; + if (acc_dist > stopping_distance) { + break; + } } // if stopping_distance >= center_points, stopping_point is center_points[end] const double ratio = (acc_dist <= stopping_distance) - ? 0.0 - : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); + ? 0.0 + : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); // linear interpolation geometry_msgs::msg::Point stopping_point; stopping_point.x = (p1.x * ratio + p2.x) / (1 + ratio); @@ -1582,18 +1576,18 @@ p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_poin autoware_auto_perception_msgs::msg::PredictedObject predicted_object = object; predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); + tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object); const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area); debug_data_.predicted_obj_pose.position = stopping_point; debug_data_.predicted_obj_pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); + tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); if (is_in_stuck_area) { - return true; + return true; } return false; -} + } */ } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index fc35da07c78df..8ce77ea24f727 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -111,62 +111,55 @@ class IntersectionModule : public SceneModuleInterface } occlusion; }; - /* - enum OcclusionState { - NONE, - BEFORE_FIRST_STOP_LINE, - WAIT_FIRST_STOP_LINE, - CREEP_SECOND_STOP_LINE, - COLLISION_DETECTED, - }; - */ - using Indecisive = std::monostate; struct StuckStop { - size_t stop_line_idx; - // NOTE: this is optional because stuck vehicle detection is possible - // even if the detection area is empty. - // Still this may be required for RTC's default stop line - bool is_detection_area_empty; - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t stuck_stop_line_idx{0}; + std::optional occlusion_stop_line_idx{std::nullopt}; }; struct NonOccludedCollisionStop { - size_t stop_line_idx; - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct FirstWaitBeforeOcclusion { - size_t first_stop_line_idx; - size_t occlusion_stop_line_idx; - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t first_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct PeekingTowardOcclusion { - size_t stop_line_idx; // NOTE: if intersection_occlusion is disapproved externally through RTC, // it indicates "is_forcefully_occluded" - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct OccludedCollisionStop { - size_t stop_line_idx; - size_t occlusion_stop_line_idx; - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct Safe { // NOTE: if RTC is disapproved status, default stop lines are still needed. - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct TrafficLightArrowSolidOn { - bool collision_detected; - util::IntersectionStopLines stop_lines; + bool collision_detected{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; using DecisionResult = std::variant< Indecisive, // internal process error, or over the pass judge line @@ -207,14 +200,15 @@ class IntersectionModule : public SceneModuleInterface const int64_t lane_id_; const std::set associative_ids_; std::string turn_direction_; + bool is_go_out_ = false; bool is_permanent_go_ = false; - bool is_peeking_ = false; + DecisionResult prev_decision_result_; + // Parameter PlannerParam planner_param_; + std::optional intersection_lanelets_; - // for an intersection lane, its associative lanes are those that share same parent lanelet and - // have same turn_direction // for occlusion detection const bool enable_occlusion_detection_; @@ -235,7 +229,6 @@ class IntersectionModule : public SceneModuleInterface double occlusion_stop_distance_; bool occlusion_activated_ = true; // for first stop in two-phase stop - const UUID occlusion_first_stop_uuid_; bool occlusion_first_stop_required_ = false; void initializeRTCStatus(); @@ -246,9 +239,7 @@ class IntersectionModule : public SceneModuleInterface bool checkStuckVehicle( const std::shared_ptr & planner_data, - const util::PathLanelets & path_lanelets, - const util::InterpolatedPathInfo & interpolated_path_info, - const util::IntersectionStopLines & intersection_stop_lines); + const util::PathLanelets & path_lanelets); autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 72695d36fe243..17f5570341164 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -227,15 +227,20 @@ std::optional generateIntersectionStopLines( const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); // (1) default stop line position on interpolated path - int stop_idx_ip_int = 0; + bool default_stop_line_valid = true; + int stop_idx_ip_int = -1; if (const auto map_stop_idx_ip = getStopLineIndexFromMap(interpolated_path_info, planner_data, 10.0); map_stop_idx_ip) { stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } else { + } + if (stop_idx_ip_int < 0) { stop_idx_ip_int = static_cast(first_inside_detection_ip) - stop_line_margin_idx_dist - base2front_idx_dist; } + if (stop_idx_ip_int < 0) { + default_stop_line_valid = false; + } const auto default_stop_line_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; // (2) ego front stop line position on interpolated path @@ -251,13 +256,25 @@ std::optional generateIntersectionStopLines( const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon(); int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); - for (size_t i = default_stop_line_ip; i <= std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, area_2d)) { - occlusion_peeking_line_ip_int = i; - break; + bool occlusion_peeking_line_valid = true; + { + // NOTE: if footprints[0] is already inside the detection area, invalid + const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects(path_footprint0, area_2d)) { + occlusion_peeking_line_valid = false; + } + } + if (occlusion_peeking_line_valid) { + for (size_t i = default_stop_line_ip + 1; i <= std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, area_2d)) { + occlusion_peeking_line_ip_int = i; + break; + } } } occlusion_peeking_line_ip_int += std::ceil(peeking_offset / ds); @@ -279,26 +296,43 @@ std::optional generateIntersectionStopLines( // (5) stuck vehicle stop line int stuck_stop_line_ip_int = 0; + bool stuck_stop_line_valid = true; if (use_stuck_stopline) { + // NOTE: when ego vehicle is approaching detection area and already passed + // first_conflicting_area, this could be null. const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_conflicting_area); if (!stuck_stop_line_idx_ip_opt) { - return std::nullopt; + stuck_stop_line_valid = false; + stuck_stop_line_ip_int = 0; + } else { + stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); } - stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); } else { stuck_stop_line_ip_int = std::get<0>(lane_interval_ip); } - const auto stuck_stop_line_ip = static_cast( - std::max(0, stuck_stop_line_ip_int - stop_line_margin_idx_dist - base2front_idx_dist)); + stuck_stop_line_ip_int -= (stop_line_margin_idx_dist + base2front_idx_dist); + if (stuck_stop_line_ip_int < 0) { + stuck_stop_line_valid = false; + } + const auto stuck_stop_line_ip = static_cast(std::max(0, stuck_stop_line_ip_int)); - IntersectionStopLines intersection_stop_lines; + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stop_line{0}; + size_t default_stop_line{0}; + size_t occlusion_peeking_stop_line{0}; + size_t pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stop_lines_temp; std::list> stop_lines = { - {&closest_idx_ip, &intersection_stop_lines.closest_idx}, - {&stuck_stop_line_ip, &intersection_stop_lines.stuck_stop_line}, - {&default_stop_line_ip, &intersection_stop_lines.default_stop_line}, - {&occlusion_peeking_line_ip, &intersection_stop_lines.occlusion_peeking_stop_line}, - {&pass_judge_line_ip, &intersection_stop_lines.pass_judge_line}, + {&closest_idx_ip, &intersection_stop_lines_temp.closest_idx}, + {&stuck_stop_line_ip, &intersection_stop_lines_temp.stuck_stop_line}, + {&default_stop_line_ip, &intersection_stop_lines_temp.default_stop_line}, + {&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line}, + {&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line}, }; stop_lines.sort( [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); @@ -311,14 +345,31 @@ std::optional generateIntersectionStopLines( *stop_idx = insert_idx.value(); } if ( - intersection_stop_lines.occlusion_peeking_stop_line < - intersection_stop_lines.default_stop_line) { - intersection_stop_lines.occlusion_peeking_stop_line = intersection_stop_lines.default_stop_line; + intersection_stop_lines_temp.occlusion_peeking_stop_line < + intersection_stop_lines_temp.default_stop_line) { + intersection_stop_lines_temp.occlusion_peeking_stop_line = + intersection_stop_lines_temp.default_stop_line; } if ( - intersection_stop_lines.occlusion_peeking_stop_line > intersection_stop_lines.pass_judge_line) { - intersection_stop_lines.pass_judge_line = intersection_stop_lines.occlusion_peeking_stop_line; + intersection_stop_lines_temp.occlusion_peeking_stop_line > + intersection_stop_lines_temp.pass_judge_line) { + intersection_stop_lines_temp.pass_judge_line = + intersection_stop_lines_temp.occlusion_peeking_stop_line; + } + + IntersectionStopLines intersection_stop_lines; + intersection_stop_lines.closest_idx = intersection_stop_lines_temp.closest_idx; + if (stuck_stop_line_valid) { + intersection_stop_lines.stuck_stop_line = intersection_stop_lines_temp.stuck_stop_line; } + if (default_stop_line_valid) { + intersection_stop_lines.default_stop_line = intersection_stop_lines_temp.default_stop_line; + } + if (occlusion_peeking_line_valid) { + intersection_stop_lines.occlusion_peeking_stop_line = + intersection_stop_lines_temp.occlusion_peeking_stop_line; + } + intersection_stop_lines.pass_judge_line = intersection_stop_lines_temp.pass_judge_line; return intersection_stop_lines; } @@ -327,8 +378,13 @@ std::optional getFirstPointInsidePolygon( const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward) { + // NOTE: if first point is already inside the polygon, returns nullopt const auto polygon_2d = lanelet::utils::to2D(polygon); if (search_forward) { + const auto & p0 = path.points.at(lane_interval.first).point.pose.position; + if (bg::within(to_bg2d(p0), polygon_2d)) { + return std::nullopt; + } for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { const auto & p = path.points.at(i).point.pose.position; const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); @@ -337,6 +393,10 @@ std::optional getFirstPointInsidePolygon( } } } else { + const auto & p0 = path.points.at(lane_interval.second).point.pose.position; + if (bg::within(to_bg2d(p0), polygon_2d)) { + return std::nullopt; + } for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { const auto & p = path.points.at(i).point.pose.position; const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 9f7f5a449e8b0..a4a0ed88217f7 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -55,10 +55,10 @@ struct DebugData struct InterpolatedPathInfo { autoware_auto_planning_msgs::msg::PathWithLaneId path; - double ds; - int lane_id; - std::set associative_lane_ids; - std::optional> lane_id_interval; + double ds{0.0}; + int lane_id{0}; + std::set associative_lane_ids{}; + std::optional> lane_id_interval{std::nullopt}; }; struct IntersectionLanelets @@ -117,19 +117,23 @@ struct IntersectionLanelets struct DescritizedLane { - int lane_id; + int lane_id{0}; // discrete fine lines from left to right - std::vector divisions; + std::vector divisions{}; }; struct IntersectionStopLines { // NOTE: for baselink - size_t closest_idx; - size_t stuck_stop_line; - size_t default_stop_line; - size_t occlusion_peeking_stop_line; - size_t pass_judge_line; + size_t closest_idx{0}; + // NOTE: null if path does not conflict with first_conflicting_area + std::optional stuck_stop_line{std::nullopt}; + // NOTE: null if path is over map stop_line OR its value is calculated negative area + std::optional default_stop_line{std::nullopt}; + // NOTE: null if footprints do not change from outside to inside of detection area + std::optional occlusion_peeking_stop_line{std::nullopt}; + // if the value is calculated negative, its value is 0 + size_t pass_judge_line{0}; }; struct PathLanelets From 4c2772c7c9d5c6954b046e7970c81615266398e1 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Wed, 13 Sep 2023 10:51:17 +0900 Subject: [PATCH 18/93] fix(tensorrt_yolox): update yolox-s model (#4949) * fix(tensorrt_yolox): update yolox-s model Signed-off-by: badai-nguyen * fix: cspell check Signed-off-by: badai-nguyen * fix: typo Signed-off-by: badai-nguyen * fix: lint_cmake check Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- perception/tensorrt_yolox/CMakeLists.txt | 17 ++++++++++------- .../launch/yolox_s_plus_opt.launch.xml | 3 ++- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index 01ade9bac43d6..80a7f0964b4bc 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -30,7 +30,7 @@ set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") if(NOT EXISTS "${DATA_PATH}") execute_process(COMMAND mkdir -p ${DATA_PATH}) endif() -function(download FILE_NAME FILE_HASH) +function(download SUB_DIR FILE_NAME FILE_HASH) message(STATUS "Checking and downloading ${FILE_NAME}") set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) set(STATUS_CODE 0) @@ -44,14 +44,14 @@ function(download FILE_NAME FILE_HASH) else() message(STATUS "diff ${FILE_NAME}") message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/${SUB_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) endif() else() message(STATUS "not found ${FILE_NAME}") message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/${SUB_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) endif() @@ -62,10 +62,13 @@ function(download FILE_NAME FILE_HASH) endif() endfunction() -download(yolox-tiny.onnx 97028baf73ce55e115599c9c60651b08) -download(yolox-sPlus-opt.onnx bf3b0155351f90fcdca2626acbfd3bcf) -download(yolox-sPlus-opt.EntropyV2-calibration.table c6e6f1999d5724a017516a956096701f) -download(label.txt 9ceadca8b72b6169ee6aabb861fe3e1e) +set(V1_PATH models/object_detection_yolox_s/v1) +download(models yolox-tiny.onnx 97028baf73ce55e115599c9c60651b08) +download(models yolox-sPlus-opt.onnx bf3b0155351f90fcdca2626acbfd3bcf) +download(models yolox-sPlus-opt.EntropyV2-calibration.table c6e6f1999d5724a017516a956096701f) +download(${V1_PATH} yolox-sPlus-T4-960x960-pseudo-finetune.onnx 37165a7e67bdaf6acff93925c628f2b2) +download(${V1_PATH} yolox-sPlus-T4-960x960-pseudo-finetune.EntropyV2-calibration.table d266ea9846b10e4dab53572152c6fd8f) +download(models label.txt 9ceadca8b72b6169ee6aabb861fe3e1e) ########## # tensorrt_yolox diff --git a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index 43f59e944f889..cb89f5829c65d 100644 --- a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -1,8 +1,9 @@ + - + From 7c15e169a321e7556efe71f9f0a2b0c8fd741242 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Tue, 29 Aug 2023 21:09:17 +0900 Subject: [PATCH 19/93] fix(tier4_percetion_launch): fix order of Camera-Lidar-Radar fusion pipeline (#4779) * fix(tier4_percetion_launch): fix order of Camera-Lidar-Radar fusion pipeline Signed-off-by: scepter914 * fix clustering update Signed-off-by: scepter914 * fix from Camera-LidAR fusion Signed-off-by: scepter914 * refactor Signed-off-by: scepter914 * refactor Signed-off-by: scepter914 * fix merge Signed-off-by: scepter914 * Update launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> * style(pre-commit): autofix --------- Signed-off-by: scepter914 Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Signed-off-by: tomoya.kimura --- ...ra_lidar_fusion_based_detection.launch.xml | 28 +- ...ar_radar_fusion_based_detection.launch.xml | 354 +++++++++++++++--- 2 files changed, 329 insertions(+), 53 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index aef7220680d67..941de9c7df11e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -1,8 +1,25 @@ + + + + - + + + + + + + + + + + + + + @@ -28,15 +45,6 @@ - - - - - - - - - + + + + - + + + + + + - - + + + - + + + + + + + + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - + @@ -78,10 +77,279 @@ - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From d0af89b4bfed0185e71d50bb979dc8c7fb87e8e1 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 12 Sep 2023 21:14:24 +0900 Subject: [PATCH 20/93] feat(image_projection_based_fusion): add roi based clustering for small unknown object detection (#4681) * feat: add roi_pointcloud_fusion node Signed-off-by: badai-nguyen fix: postprocess Signed-off-by: badai-nguyen fix: launch file Signed-off-by: badai-nguyen chores: refactor Signed-off-by: badai-nguyen fix: closest cluster Signed-off-by: badai-nguyen * chores: refactor Signed-off-by: badai-nguyen * docs: add readme Signed-off-by: badai-nguyen * fix: add missed parameter declare Signed-off-by: badai-nguyen * fix: add center transform Signed-off-by: badai-nguyen * fix: typos in launch Signed-off-by: badai-nguyen * docs: update docs Signed-off-by: badai-nguyen * fix: change roi pointcloud fusion output to clusters Signed-off-by: badai-nguyen * fix: add cluster debug roi pointcloud fusion Signed-off-by: badai-nguyen * fix: use IoU_x in roi cluster fusion Signed-off-by: badai-nguyen * feat: add cluster merger package Signed-off-by: badai-nguyen * fix: camera lidar launch Signed-off-by: badai-nguyen * style(pre-commit): autofix * fix: cluster merger Signed-off-by: badai-nguyen * fix: roi cluster fusion unknown object fix Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * docs: add readme cluster_merger Signed-off-by: badai-nguyen * docs: update roi pointcloud fusion readme Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * fix: multiple definition bug Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen * docs: update docs Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen * chore: pre-commit Signed-off-by: badai-nguyen * fix: update camera_lidar_radar mode launch Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../object_recognition_utils/transform.hpp | 64 ++++++ common/object_recognition_utils/package.xml | 6 + ...ra_lidar_fusion_based_detection.launch.xml | 59 +++++- ...ar_radar_fusion_based_detection.launch.xml | 44 +++- launch/tier4_perception_launch/package.xml | 1 + perception/cluster_merger/CMakeLists.txt | 27 +++ perception/cluster_merger/README.md | 72 +++++++ .../include/cluster_merger/node.hpp | 73 +++++++ .../launch/cluster_merger.launch.xml | 14 ++ perception/cluster_merger/package.xml | 28 +++ .../src/cluster_merger/node.cpp | 73 +++++++ .../CMakeLists.txt | 6 + .../image_projection_based_fusion/README.md | 1 + .../docs/images/roi_pointcloud_fusion.png | Bin 0 -> 118768 bytes .../docs/roi-pointcloud-fusion.md | 93 +++++++++ .../fusion_node.hpp | 5 + .../roi_pointcloud_fusion/node.hpp | 53 +++++ .../utils/utils.hpp | 36 ++++ .../launch/roi_pointcloud_fusion.launch.xml | 77 +++++++ .../image_projection_based_fusion/package.xml | 1 + .../src/fusion_node.cpp | 1 + .../src/roi_cluster_fusion/node.cpp | 8 +- .../src/roi_pointcloud_fusion/node.cpp | 165 +++++++++++++++ .../src/utils/utils.cpp | 194 +++++++++++++++++- 24 files changed, 1094 insertions(+), 7 deletions(-) create mode 100644 perception/cluster_merger/CMakeLists.txt create mode 100644 perception/cluster_merger/README.md create mode 100644 perception/cluster_merger/include/cluster_merger/node.hpp create mode 100644 perception/cluster_merger/launch/cluster_merger.launch.xml create mode 100644 perception/cluster_merger/package.xml create mode 100644 perception/cluster_merger/src/cluster_merger/node.cpp create mode 100644 perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png create mode 100644 perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md create mode 100644 perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp create mode 100644 perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml create mode 100644 perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp diff --git a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp index 4446c87427e88..31892853a855f 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp @@ -15,15 +15,22 @@ #ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ #define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#include + #include +#include +#include #include #include #include #ifdef ROS_DISTRO_GALACTIC +#include #include #else +#include + #include #endif @@ -45,6 +52,23 @@ namespace detail return boost::none; } } + +[[maybe_unused]] inline boost::optional getTransformMatrix( + const std::string & in_target_frame, const std_msgs::msg::Header & in_cloud_header, + const tf2_ros::Buffer & tf_buffer) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + in_target_frame, in_cloud_header.frame_id, in_cloud_header.stamp, + rclcpp::Duration::from_seconds(1.0)); + Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + return mat; + } catch (tf2::TransformException & e) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("detail::getTransformMatrix"), e.what()); + return boost::none; + } +} } // namespace detail namespace object_recognition_utils @@ -79,6 +103,46 @@ bool transformObjects( } return true; } +template +bool transformObjectsWithFeature( + const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + T & output_msg) +{ + output_msg = input_msg; + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + const auto ros_target2objects_world = detail::getTransform( + tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + const auto tf_matrix = detail::getTransformMatrix(target_frame_id, input_msg.header, tf_buffer); + if (!tf_matrix) { + RCLCPP_WARN( + rclcpp::get_logger("object_recognition_utils:"), "failed to get transformed matrix"); + return false; + } + for (auto & feature_object : output_msg.feature_objects) { + // transform object + tf2::fromMsg( + feature_object.object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, feature_object.object.kinematics.pose_with_covariance.pose); + + // transform cluster + sensor_msgs::msg::PointCloud2 transformed_cluster; + pcl_ros::transformPointCloud(*tf_matrix, feature_object.feature.cluster, transformed_cluster); + transformed_cluster.header.frame_id = target_frame_id; + feature_object.feature.cluster = transformed_cluster; + } + output_msg.header.frame_id = target_frame_id; + return true; + } + return true; +} } // namespace object_recognition_utils #endif // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 95925e846a55c..2f2472515ebad 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -17,7 +17,13 @@ geometry_msgs interpolation libboost-dev + pcl_conversions + pcl_ros rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen tier4_autoware_utils ament_cmake_ros diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 941de9c7df11e..34c2fd4cdc27c 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -79,11 +79,53 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -95,6 +137,8 @@ + + @@ -287,11 +331,22 @@ + + + + + + + + + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 8d4a285094651..7d1353433da85 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -142,12 +142,54 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index c1723c1fa07e8..336588891d9b2 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto autoware_cmake + cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover diff --git a/perception/cluster_merger/CMakeLists.txt b/perception/cluster_merger/CMakeLists.txt new file mode 100644 index 0000000000000..49506f4b439fb --- /dev/null +++ b/perception/cluster_merger/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(cluster_merger) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(cluster_merger_node_component SHARED + src/cluster_merger/node.cpp +) + +rclcpp_components_register_node(cluster_merger_node_component + PLUGIN "cluster_merger::ClusterMergerNode" + EXECUTABLE cluster_merger_node) + + +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() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/cluster_merger/README.md b/perception/cluster_merger/README.md new file mode 100644 index 0000000000000..b46f7401b70ec --- /dev/null +++ b/perception/cluster_merger/README.md @@ -0,0 +1,72 @@ +# cluster merger + +## Purpose + +cluster_merger is a package for merging pointcloud clusters as detected objects with feature type. + +## Inner-working / Algorithms + +The clusters of merged topics are simply concatenated from clusters of input topics. + +## Input / Output + +### Input + +| Name | Type | Description | +| ---------------- | -------------------------------------------------------- | ------------------- | +| `input/cluster0` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | +| `input/cluster1` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | + +### Output + +| Name | Type | Description | +| ----------------- | ----------------------------------------------------- | --------------- | +| `output/clusters` | `autoware_auto_perception_msgs::msg::DetectedObjects` | merged clusters | + +## Parameters + +| Name | Type | Description | Default value | +| :---------------- | :----- | :----------------------------------- | :------------ | +| `output_frame_id` | string | The header frame_id of output topic. | base_link | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/cluster_merger/include/cluster_merger/node.hpp b/perception/cluster_merger/include/cluster_merger/node.hpp new file mode 100644 index 0000000000000..8da5999f00384 --- /dev/null +++ b/perception/cluster_merger/include/cluster_merger/node.hpp @@ -0,0 +1,73 @@ +// 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 CLUSTER_MERGER__NODE_HPP_ +#define CLUSTER_MERGER__NODE_HPP_ + +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace cluster_merger +{ +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; + +class ClusterMergerNode : public rclcpp::Node +{ +public: + explicit ClusterMergerNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscriber + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr sub_objects_{}; + message_filters::Subscriber objects0_sub_; + message_filters::Subscriber objects1_sub_; + typedef message_filters::sync_policies::ApproximateTime< + DetectedObjectsWithFeature, DetectedObjectsWithFeature> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + Sync sync_; + + std::string output_frame_id_; + + std::vector::SharedPtr> sub_objects_array{}; + std::shared_ptr transform_listener_; + + void objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg); + // Publisher + rclcpp::Publisher::SharedPtr pub_objects_; +}; + +} // namespace cluster_merger + +#endif // CLUSTER_MERGER__NODE_HPP_ diff --git a/perception/cluster_merger/launch/cluster_merger.launch.xml b/perception/cluster_merger/launch/cluster_merger.launch.xml new file mode 100644 index 0000000000000..1bbd0ebd91e12 --- /dev/null +++ b/perception/cluster_merger/launch/cluster_merger.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/perception/cluster_merger/package.xml b/perception/cluster_merger/package.xml new file mode 100644 index 0000000000000..14826ad07e098 --- /dev/null +++ b/perception/cluster_merger/package.xml @@ -0,0 +1,28 @@ + + + + cluster_merger + 0.1.0 + The ROS 2 cluster merger package + Yukihiro Saito + Shunsuke Miura + autoware + Apache License 2.0 + + ament_cmake_auto + + geometry_msgs + message_filters + object_recognition_utils + rclcpp + rclcpp_components + tier4_autoware_utils + tier4_perception_msgs + autoware_cmake + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/cluster_merger/src/cluster_merger/node.cpp b/perception/cluster_merger/src/cluster_merger/node.cpp new file mode 100644 index 0000000000000..48bf953027510 --- /dev/null +++ b/perception/cluster_merger/src/cluster_merger/node.cpp @@ -0,0 +1,73 @@ +// 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 "cluster_merger/node.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" + +#include +#include +#include +namespace cluster_merger +{ + +ClusterMergerNode::ClusterMergerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("cluster_merger_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_), + objects0_sub_(this, "input/cluster0", rclcpp::QoS{1}.get_rmw_qos_profile()), + objects1_sub_(this, "input/cluster1", rclcpp::QoS{1}.get_rmw_qos_profile()), + sync_(SyncPolicy(10), objects0_sub_, objects1_sub_) +{ + output_frame_id_ = declare_parameter("output_frame_id"); + using std::placeholders::_1; + using std::placeholders::_2; + sync_.registerCallback(std::bind(&ClusterMergerNode::objectsCallback, this, _1, _2)); + + // Publisher + pub_objects_ = create_publisher("~/output/clusters", rclcpp::QoS{1}); +} + +void ClusterMergerNode::objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg) +{ + if (pub_objects_->get_subscription_count() < 1) { + return; + } + // TODO(badai-nguyen): transform input topics to desired frame before concatenating + /* transform to the same with cluster0 frame id*/ + DetectedObjectsWithFeature transformed_objects0; + DetectedObjectsWithFeature transformed_objects1; + if ( + !object_recognition_utils::transformObjectsWithFeature( + *input_objects0_msg, output_frame_id_, tf_buffer_, transformed_objects0) || + !object_recognition_utils::transformObjectsWithFeature( + *input_objects1_msg, output_frame_id_, tf_buffer_, transformed_objects1)) { + return; + } + + DetectedObjectsWithFeature output_objects; + output_objects.header = input_objects0_msg->header; + // add check frame id and transform if they are different + output_objects.feature_objects = transformed_objects0.feature_objects; + output_objects.feature_objects.insert( + output_objects.feature_objects.end(), transformed_objects1.feature_objects.begin(), + transformed_objects1.feature_objects.end()); + pub_objects_->publish(output_objects); +} +} // namespace cluster_merger + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(cluster_merger::ClusterMergerNode) diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 906ad71d21732..ce7c3b5ea12a9 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/utils.cpp src/roi_cluster_fusion/node.cpp src/roi_detected_object_fusion/node.cpp + src/roi_pointcloud_fusion/node.cpp ) target_link_libraries(${PROJECT_NAME} @@ -39,6 +40,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE roi_cluster_fusion_node ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" + EXECUTABLE roi_pointcloud_fusion_node +) + set(CUDA_VERBOSE OFF) # set flags for CUDA availability diff --git a/perception/image_projection_based_fusion/README.md b/perception/image_projection_based_fusion/README.md index 1bbee35ab44f8..207989d8a6f25 100644 --- a/perception/image_projection_based_fusion/README.md +++ b/perception/image_projection_based_fusion/README.md @@ -58,3 +58,4 @@ The rclcpp::TimerBase timer could not break a for loop, therefore even if time i | roi_cluster_fusion | Overwrite a classification label of clusters by that of ROIs from a 2D object detector. | [link](./docs/roi-cluster-fusion.md) | | roi_detected_object_fusion | Overwrite a classification label of detected objects by that of ROIs from a 2D object detector. | [link](./docs/roi-detected-object-fusion.md) | | pointpainting_fusion | Paint the point cloud with the ROIs from a 2D object detector and feed to a 3D object detector. | [link](./docs/pointpainting-fusion.md) | +| roi_pointcloud_fusion | Matching pointcloud with ROIs from a 2D object detector to detect unknown-labeled objects | [link](./docs/roi-pointcloud-fusion.md) | diff --git a/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png b/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png new file mode 100644 index 0000000000000000000000000000000000000000..c529de7c728fba996a08f3718aef2a7a6d705ccc GIT binary patch literal 118768 zcmY&cHYQ9p*|u$KvTfVed-{C!d;6oV^IW^+WMP`U472(Z|&ARr(J65_)0ARyp9ARwP6pdo=z(%c)Efj{8(f)a|*z>f#CK?v}S z0>Vho$h6y3OV6>F8bZ&qmyq|3o^db9kC9dC6BrsD2f=jAwk4tyAcJ}pzw-^M<<_z5I5X%uPuA<0q6DtS4o1l9!kOYPA->bg-2 z1*=u?(Lb$w@X|nB3gl#a!?o-ww8>&weaF&FATUt;EXG(+KZzBXE})iQB>D=aLm=qwH_ zVnifX4w2xwH3`0wam zLoT-)6WdP~_h+kx#bMtIdNyr9{zwEk9!>?;TP`cB6aq8Tj3eopS)rCkGRkJpj34@Y zy7p9TrVhNX(Sp*+LW0!}C-ah7|CYrz%XpH_uH(*G8TOeG738S?qM#@%8SIN7wCyUF z{T{q^+b#M)BrX-xrxOdp(%NRnV@8vS%o5yeWI@!DY6I1-`p^J}cccLvWOl~L(`&)v zVpG6kY?;~96dteZ?A3q0#US}}=JiXBHzPZB&x-wo6*muO!w;E5(7b)`)0kA#*rQu$ zC*alwUzgVIpC_#Lc=^XGMVxqHVksyABht!_CbM9%T3}8W$kH)=DkYTVd##m@LKRdj zp_B?TtrZQwl{NwKU&|>@Mg!*TPg!lPyOuaUig)j(M3c=4F86U63OyD8nB3v1|X9ZpEWfKby+p!u-~}2U|(E0;3?am?(9&K zbMo}Njp1{88xvO_^RucefLNB1z{M3dsS%Ts#QI}(yw^-!aPzq+_C;D+!@kZh;`R!BoJ`Nj`E>1^Fe;;gzU2>_(TV}K`tjoj4v!nE zv$OM9Du;s6-<^#ve?zxi*TV>E+nQy%lPhuZ+%nIBZi~Gwm0&bA3B ztI3%uJyGcnJG0Vw2vaZ_%hUiNQwW%>6MW}S)@j=b52xkfdJPB8Kv~hCsS%64$eLI6 z0TAljh!rkf+LZ698s231d?*1;?MYmIsrSqIdC~pvY=1;TA9uHSD=mlxMl~I|IQ6|A zX6pF|bc zl)V#Y6&dqq7>WrH9D3`!+*-yZ4^ zqebm>aMp2y`1<%-VCd087I;A6ImAX>eFK3H0{gBydcd2bw)M2CSmBCnd49EF(#?$b z)iAHJ(^XUB=-y|$0fHNgF&r%^^SXMCy#bnL!#BK|mq8 zo99sQoz8OpblreKy&>3v=_GGrd_TARmoMVaJie}U0l)1G5QdF|Hyo4ZM;BO93V7E; z%VlH*aVUr_>atSnt1PDYvFpSQtoZiPzL#Qm3CX?%Geb49!DrtKfgT`M81>zozM%5N z6kLUSba;Ru*qm84SqRIfKU!mBr}=WV1Wk<%3?7!I^;Sq3T*oj7g^YcSiBe#5aADs)>v}r#n+sV6F>2>WN5jL-4_@>%bhVB#awjp z->vg=>B9mm^UCmk<5>5+$MyB~4G0e2-rYUHc)mI8nb+}-m^AsbN)(7ZkAC~f>-AQ} zvC{_>qgbBW`xUt^3lU8YOspRbf1UNx2`zPi=P0PyzJaQ^c&~_0x>aMgB{FBa1efsV ztUg4s`tK@n&)ll(#_#4z1r|>A+k;y9yxQxot@Z5%%?E|oE&~0}X7`+9+iT8aa?WV% zQRnV0_O{P7T%$kkcbk@m)bkOoBy6R9d(x`Vix!U(WLe}ul}hRg>B=fG)_X3kg9>>$ z6%imE53*yn1)+an8S_bbzuwLtA?_y2|GsQrTI_jL(_DA|^Q15E$VpcJU4g5G_u0Jh zv83YM0guk%rT9CHoeJozjJZ6N_n&M^<7F9*TwW{NG=-X-RKFFGBOooO?g zBYUZ8O%m6m`uSzFdj=X#&&>RyV)b03;&X=*HJs^Tsy{KRU};ruGtf#Hc((OhA1gfz z+b{R6>boo{EEBtlG6@~j)+j6wqY&5U(Me>YoTcNJymB2qG!&ZSxpR>WZIW&1qdVxFmc_-h`UUpCvcjK%0Lt+_U9)$i z6Xes2)SH?Hw&{u&<5yXb0~j|e#PLV2V+>M}8iU~L>4h0HaypR*^oq~<=Ggtio6cZz zHl)#x!?Cnn?fsxau9ufVV`t{K}`41{cDRxNH%)DdyTjoe>8b*pneXRxeWe1j_7 z+><@nK9xGVuNQ!awypQJF{EC`l~Zg?dU)snF~)!Hg(JB=cElU7Ct#s_=J{BX{jXoY z^tXCLQ+Yj;2M5K0z^tX$c5z{|RB!1Xm-Dd$Enz5cH_S+t@h_%~`c7J}=;t8nDN;vC zU)esT!sVs@ko^AR78dKPO3l`@uzWS+57d2HeTu_!ScpszvrU5PskG~Fjx02_CQ44x zqU)?)E{FAK?*|~{l2QaT^N&waD~8<5mkILIW~hjUEq~>ms>dgTTMUYG0f&pFRAo<2 zQPs^TbS($T!{AH677|cMZ-R;i}btm+}O)76|P=&IU-n_4LH5vpCYzJAq1H19q_v!m1!Ta)t zO2RwTyH^J8Rf-@RI=DAqfq>xbH=sLAITn7aF2kZBI;K*tDQCXa}q+r zEx%t|Y!(X_t%kr2*D5!YEcjkjr_J0PGtaUTVd(T)Y(f&7-EF|@S&Bi|pPvPCaDyph z1n>4xvqCrs8%7nn7lh|LUY&>Ch8JZoYfDaEj!!NEq(IiA#TR(6)u;Q^@zepxTXs$B zUxeQ=1Wk{go^L9KAIVwKASzdw!T=i<2oya@S`A6woTUfZafvn0U-DP@<3OSA?L~vl zw1q#IVUHiO)VjgDIe*I_99ZNP+ZwQ6tS!L=^W7;CC1u#njRPbSo^*rNYB3ivLBNAp`4RKb@Z~dx zzDr*mwE1Hvj#Jv;_V|qujPhBa^1RlKynAQt&!eN0kP3bsHb5H-GGoAF7e7%11KBix3PJel}tDo51dZG{A zR*?1J*uJW5_2lN|gvU`n{)i2^ZAh2tP0Ceq;wuQ@UVdF{#mS@XM!vunby>lIq8CJ} z=TXI%!pFqK;jn>>&!?NrwQ$?$e9{5B$Ov^wS5dM6LQx~wsAoLNZ!>wFDs@awSs zgl_MKTjjPkQ^?~p)mhFM>e-HncW2E`mTZ^KF8;lPW(ttu%?|jmjKL)W-BlAhK zB`QpgM~V)dxYPMEvBHVSTg^_Z#62<2FIz|Sx8S!?8?^K1nc1aj#Bq4NyC7`IdX|Tp zNO_s$fw?pTl1g$5)wEff`z4om0}bidx9h+h7Wxa;+aFA0q z*&eplS98wJUD@!Ky_^};57k)k90QB&4G>nT7L%<3F>i|2PdF0(`RQ5eRm)&qLO%Jr zPOQv`xt?Ep%4mKpVq=;kT!2Qag#zP@*mPWH-FUr`8mqqY6YTHhQ0T#B6wq4 zGURm0N+`!NX!gkCr-Gcx_Qu-_+})34H)Rj4SA#Q#{h6T}MGNAS!IypOAfRrBMEjb7 zXg%KMo1>bcwK^H{cW`ucx9xt7h?6rJs6QF?`{9$>thHD5UVb#x{;^*H=RPouMgX!{qw z!ND1{zBt*wP}FukL@NVX(L>GSPW|D_Gu*zLYTa?b)q{TfJGGBrOoEtG$M{#cX^{P& zcVCUXM@G00X=C_+zEQXReB%&D!SJPEXy_oM|zA>&v!O#U~LVf3&qpyxmX_0HQeQb<@s&Te+N^5e`!;eluFu{w8Me4C_(D##_SOZiO5>1`67XrJa94Jr z#PjkKw;vvHZ=IayQl!L+{%z0rPXcDf2q?1f9ji1Z4)j+^Swhl^yl=U5rxBDQfod*Fj%@fC2+BfhBnTGdGxtyI7x%je(H=rAHZ}v$`qe(d2m3r{+#t4~G4Z3Ho*FSh` zC{TB>TsNBN)elp-DDGWcnVnmVjNfi-Ws@7=zYTU^z4_PwJXEdDia7cW6Dt@(m182t zTrnQHmk>LP8~Q*NPp30hs#XIa3ZMXNlc`YfeHIsRl!{5mq*!~4p0w|2b71(gAb}%_ zU(Kf5TVJ)9r%AtWE%F<09S_#CSjg6F=OgHz%$vJ93q!ZBLEfZ8s;^Ba!2GzRMzmJn zWT{1X{*2oZRuom2m8DHUL=pM$fEuZv`AQV4TFa2^wO=j^6ysZAlPvgsE!)dkP&k1U zFU}6cT$)_(M`xIQ#WE#{oRvHXJZN6BGP=j(tacOI%LIPT%B09>o7YhlX%#zYQLNYQ zHndcH2m{Xsgc3z#f#e7>Lav6k)F^(bQQ{5=JudGZbkj{$_R=a=>ab#PrLCxSL9#l( zc6JLqCc*C3w;pVNMffdgB{o=bC_vR0_^yq0mD!d)8X=f(qj30!W|HOCIJ|y(YI4$W znpVfe{W|VFaNpU0_YxN`zS0aXFMD)p%J#cqzV}+RB4&2-XobnYLbC6Api9_mlo^?U zFWwo?T(wm#B3m55k8BU$#w7LjWo>&0$SSEuwadBB4bWjySYs6mFYZJ24bo~87KgM- z(zO=lHV@Xox90k)Sca7I$C|>vFNIY!(dBH7)3VG*zUJD~NGJ{c1ir|EPnDO~*30|l z63V*$k#snLuE64Ak_V#LyCen&4Rf}^a zM)d3U4=l?S3wNa(lId7yW@Tl+l1shR{RD}4rY3-b#?2TEQnRFNj7+Pol|~ItZ{|hE zc8j+bgC?NdJ~1{mFd+8w@|wjRLns&>pzh1l}pg96@q6@u7V{!Y=A3r(u*w!z zg#Uf}hxw54hBMLE33p$%Y1R?aR*;gJf_7`HYG)}Snsc9MPW{PvRmJTc3U^xiOMw; z?!$xXT=qtly^lfb1dQJt+ZCVu;SLc z7BpGoMb%OGY*h?;-YF;el4hBj79(#%3ZWJ3@>83y1+OA*c|B!p&KhqgWfbN_zWcB^ zn6Eu~?3izFf_BS1zD@fmd7QR)z&4~sjBe{ywtvDg#=HhK4NpuDmJdJN;4$75T-&8P z1cZ^1F(fREkcB1g?e!VJJv08bq{d^T(MqX2bIkq%owhzAWoO0UG)t*HWDbUyh6K{+ z?TKyYcTDs*Sx)f}%o@Pcz7y!J8rjvM3*y01u*s~fwBQTi*|5__dSa;FAD3dYl-ulhwbaQ!{Z!5O#LX7W0vv{QmZ;?{ag9(@X)TofYMeP)kLpqQM_*GcpkuL!{R zdhnHqCA|a?b4I;xaESmI0xN8`vqAby?cl23GrYf9Wf}{~TbdeuJ-VA??{N8LO8NW% zc=3_{Vu|1o_BjP5TcIbjQR$Qy8Fv-nS-g(+e}-CdjMl+o`86`bXS|DofOl|PUZsyH zH`HrnZ)I4$TT>OTrCi@$Toqm9pDILgV$bg{KW{O$#aREPT|!tyq{{2n&Cky-BqStL zxlH|^7^2C-=D-NgpTjiqvX;)=(a=l;)Ple)wm6jvGk5m3^N(yRbbI5Nq9H`E179S= z;lXH|vx3zVBVycxy1S+CEYyYEL~c{mFUytg2pS-H++@{YcRdjCxW$1|$JEpm9-BEN zoyT3-S$sZl-TOTa*r(}YrII5=5$GQfRuBInEVdX;Z!x)7Z5s{kB`I`DV}f}wRvg?~ zi%s0JMa_lJ)lh&sUs}7yci*6L-thUX<^190dfap>7e*mpI=Xkvl^qB1?tFdR7Yr)w z{q}f1%4)S)$=L+R?+O3Ikn}<{O~OkJC!<<4*wS<0QyYqD7hzH4E_Q7ki^Wnt1!i@H zSIqu;p&3HcR#4Q|*2V)MR*SXPq~E`hyL);j&FW@x=UBF*_}(@Es1=9Z#%E?m5fPUY z+($G=KuL+y`HVW0p1_L!zhsVPK>F{swnCJefQLkuotqvlbT10f;~J(AkHP&6?VX?o=X34s>}+=1 zE%2To1R`SM+-9L-C)Sh*FepTuiwyyJ1qBY*D`F7!viR=q?kzVfD=TKRnUE}@;D1;S zs-0wsprVB;lEpz=pZ@9a*?C2)fkeCg9iwPBdXzrUI%#FA`*<3CVAn+*nLPH^;qS=< z8ogGRj2RV#=C7_2hz{mAWc!04rURtC)cB&3nCdrY&B*n2OcXTN4a@>^=mS}<_otr{ zL*3eW4o%u>&F9WO56z^PxY9a-duEInShzsTM8IN6lqB-mBVKkuT|6n;qpcrC==X)8 zQEN5-0S@Ek)-SrLT#48t#G<9V++2FgC1wzywm}pdB6Q9}(KCL>TN{XDUzXaIuE$<46i8{+cz* zQ>oDAf83dG3PQw{yxtqF5LFcnDLF~T!pawjYEOqzuq;$i(J^wy8aN-udY6aiXh1SN ztHm=6GJpeapo;nrJw`ZDX``MZ#PHWG!%r1*=S7p)40B9bpzv-{RQ3R<&RPKq5OsG&jSIfD^k!Rf3OzICr&W1I;fuwE^`ZYg|MgOQc$>In(LIXdT zJ9zzkHB9%o{{AL*JXaP9I4uExNDRJ1(%BYgYu?wJ=`E2fP(>w?g6YXdTYcb?mT(9H zXG4)hXCJKvw; zwj}U+K23CZzaJqUCf@;@g@S@26oJh`4*c+Xu`=9^FpVqfI{*hV3w91(LG@7(Wdz`e zX#7>vHRP}68;CAhQUD?c@FTwpZ(^$bv??YSmF{i^OxeHF8H+RRuYa&H4PbYz(@7dK zUD6UZkx)`niNarN02al#@;Q~I&w+Drfjx9W*+?DSiGZ2eXODN(<>#C-t*7IskK^qh zmWB&QXtKUAXj$feI8_FhCE(?f@^XOR6yJlP#wXu5HWdPyIBJy448NPkV>s;Gx`vli zQi0XbYG7eId5Fr3g(b-ZyzP_L{3)hSUVF4V)QI6UjB%S^hIaqD#op%q9jMJxRZs!7 zQ8LSsSZx_`kjy!J@47|qxb+Ltf3yHi9W`a&-i;m}9P~V-)ssGtHkG9JoP-%4bSt*I z=ka&}Q@iaXk2mg4!WV6Pk|EO4iJ;>+UyivBqLC)j_uPGId6go(um)JAOrUUu+%~Sd zxLBw=(Vl^__cV4N7n7o+2tb~%$?$p#1xjeUn}Z+y08JcP2`4TqYdlw~=5e=ZTx-3a z-mvBdM^{n(>jEC1H#J`-MOh&L*qS`p7(F*XPA7h()UoBEwKNiv(95svdrt&8s3*i( zGucUMTwfOVLDJ>~pXD;bG+3E?8UjOWkV;p%Wtgw)rV2x=Mq<5vI-4-|e)M2?d%U#l z3JROjDL&$0i0OKpqZG5T#$1OmNaRA;+8g`e#6p`fC`4?(dL7h-xqGTxp4(0`he`LY zZYnzt#yRa8uiZP3X;oW&q&(KmgA?9hq>+JpvWnpEx$TH2cz)S)|WCIqdV-0?f-8p&H{Z%e$M9--_7$1nV{!*@Kc?7bs* zQBYSmP|tFjw`ST*aHJn8s^SOvbG<*lU*2-|6L8Dm;NXA?VZJnuO6BS=RfP?(1==o~ z5FbbeHnT}*tSqmZ=Dsf!-alnEW*+jESy9X?u?%jDO#>7vy3y5fBy(reg3U{QkBI}M zpWut_z*u=>S~=McEEkGXHi)P_24`(~kx)wRnneoOke?l~LCWxdZS9{Y)NI5;P-{2E;;)}Y<^`hpYcO(I z$mM7eAK%k%laySqa4yk$#3YWKwM_=F1o?SdU!LDPXMv1zj&8I>OpbWjvrb;xN*}7m zb_;&h`JSj{#9Xr$C%k+;Yd6!5kKD13+;OJe>5rifULZQ;%Jqg4VQcV7j0wq>nJFpi zi4{Ecb=6J9yWr>kaTA=l63C4MF3Na%z-({%;?S9O_YdznG`6zIAsh){H`a3V@)%sN z4BDU1xFF#%JAcu&57IQO$gk>Y#eQULe<&1Gs|{dQn;o~y8rHJ4TAccJySlm@4kmo# zX*8~Gre#(1bAg*sBpIW(gx&@I2tt0coFvXG_viCKf18{X#)!|ww^oOX=P26~dv+8< z4QCvvP8=M`Wl9%HODm2rirA&?Jo__trk@Uc9-60nxr_fW3Yz+eo^7dR-W)pXlisMW z*jzd-O;Gf9dlvZ}tZDPcVm86wIlPJ9eU9Y93TW{jU9ohms~vX$Y3;lqg{r7koivltgx&yjO9d~XvNc{2e|JFnHf zteeP;@zk^RhH5;d35Yt=$nW4nMBTL}^_r#tF*_D?b(4PHiZ;CEjM{GcogCK!sCh!n z$DVGFlh|zu!?KaFuq4jc+Df=`&F6qzBZ<)v*45Ru6*$%Kc-)D}EW`Ie{)=evaIray zoQ0g-tT?m-&1MOb#x=K}_6G%LxhilqlJ~)4sdJ8-{?Ykct+~P$_g>B2ASCN@{-*OV zdOaTaguRLh{aL3-d|*={q;J=X_K_8HM9O3(WG+W$;S3T!;JD|5{_ya%nWV8x^u52KxuXVO;ef@l6ydkO8ruO8NwPq0OC*fq?;s!!95g-z#wd z-}}YyHbRt0H0P31ge;&DU2#&9z|BvHbK`ah_Iuu`fpdQMe*8Xo06=5 zXjX=YROCLiQ9p6?wofT@^OHGH#ZDt>!0uC#0Ojq+N}ta=m<$Gy zo1IS8R|&LYVPRoIA|v;JPYaYvl%YpQaeu&84z%)KglR@3A zSS(7xHp0%;u;mbSH}|dIJAH70gQB-@Ao}<3!T11n);9|wTZ7A0E-+Uj^0=ngR!Z*o z6CM{=Y+n?g^sEfSOVb6{BbAxuxxx)qrAua;yGJL1`nDB$(4-`%k7Br-{zC~9NdZZ1 zNv(M>jmrO#1Jzw76a=eT%XfWNpo;J2x~cCGamr|0F|f1;ME z=fpU>J;VH~l@JqYFZYtG0Zq_*JG%7ruGs=X^-|B&G~#fFGAufkrH^rTzWB@A)Agj) z(qtlMzQo5KC1L$8A=fhuHg+-D=HUf&hZ1$B#?JdaS~|VOrw%4~;r$jFpi2}J`^r!2 zVP%Ku;%sVa+jAc95J{F(B`U(+v<#Eo&^@Hb=I^#s9t~wB)P8J!XSHhXddrB>;@*QLCD1*!BOdmBp>u={ zqk6Z1-Rog3sQxlgH2m<=bP~sgH~jIeZPDT{_-u6F)4b^AX8P4Moop#~XvZR0Syy++ z_%P`byl76z_@qLCuepJrG7_h;b)1S#$g8{Ll+&nN!b;B=NsZds-wZUgrH2PSI_@LP z%O;-i-NS^dF-_jGda$S)o4X+hkFHn@)_`uz(HvyeZA;G3jfL$84v&t zP>Ula744GS{ShTx)C9ebnSW#SK)MJ7#>S+6$W2TWGS|Aap*1YwLc^58kdwn6n1dtF z9Byf)<)=erxQ)$w=h9_X%tqbahwH!YmN(M%%1C>{%*e@QfZPrrMT)MJI>-bw9@mYz z9DZYSN67J~S^13054Zm8YAnW^N%pXaL;C**00H4~_fbenB#0z%U=PBIG`MEdGtmx%;|jjdP3 zjzLaKE@@zgMCJVCyqN|*&l7GPjG@c-Wpw*(^kAY44%Y+B#c7w~k7gj(Q`BBp$y=MI zY{F%<0~^SLlj%aHJ#=ExqU5qS6IQqMP%k z^~cOo%Tlyw>1DJ-ut+l4eMSh09Jy@$H!+ho-XqbgZp9P zTuCx(7jQ>{!m%&yuW+^MOs8D&9*c9jgyg;WugR9=on_8Wo7&fX%QI#z7gHmk79Lg% zMNPj`qPstfB~YzNz}gZ~By#3G)Gd?gHR96Lsjeq%B;JD^yKto)<(jc zT2EV=6zNz@K6`?%_Xg3eEO0)Mqe8AHk66;)?XMzY&@+uhF?75`J3|TC3-NhALRre! z>C#WcL0OCbuFFcYKTtd201k9SqXFmo;c!H!Db8PGo4w_%Eu{e98bldx>lhpyZ+~FJ z-=wj}Ryc?22g+p85A?siLZSjW?+l`oD+{e*;FceC$wer?^F&3UpABQHKo z>rJCnb-iDOKunyTMNc0!Qc4Z~R6VbV##Ik^;9bA|>44dJzj>vF{oUSBlnfWaAg}p!cc@IxChw0)f z%);ot_$&m-XMT0-e@$fYK((LGjZrYm7tLvbV!Z5m42QSl?Ix#O8LI7cciEq(H+o-4 zzwIGZt1d9m8bI&TPwLpAX>JxBKf`0ND{VIw&PD}A$_F~oh|FRc{Ncw?1 z84-F5y^j{d)Eq~ivfvfoPoVxUtBZVe0WD(I)HR{vC01~YZ(8gP?9~rO>g*M*4e^TZ z5=~>ne~EJTF1l{;Fcu9TGM1YisQ`G=q)r{kd|=?=TLo2`y>NLwfE~cRJ6Y`Uyk8Yl zRmDtCPoKq&|A2B4hM(S;uw_xubrc+ffomu=eaKsDd!N?O+RtR2jIBAD}1mJP=&ITMBsw zM;6@R-{13$&?>dM$-aP1Zpy_dguF#OE(G;Wx;&z+8s-nWiR z_;|2RFjICOjPd(z5jj1*DLOuCOs{Vcua>cE9V@id%lk{kICe%jJFxhWus3I_dSS3u1}22c13g1VeGkSL9-3a3PxAq;7f=tJh?8y$4b(1d>6NYo zefkr)j}$9*Iyej!8D-Xe030#W>z-I+3#xOD)%B~4s(lS#M`r&P-`)h{l4V$%&aurH zV{4necQps&@{LUYfD`bP-DxA`P9(ZUH9TIO2 zh_xHqV9X6qpRvCjnW^XvHzVwrb6%Z{B)C4DBXX!U=K=J6P`Ez`S6m4r(!tIO3)j@|9^;}1Z2khGqK-LIFY~fb&3vS)yo1W z?2lSZ!44*GonXV{#aGgW`cSv#%L!HLE;^@oE9OhRG1GWZ<=dmS+2sR12_3h`%U-@acM9}*uR*H5I^`}CpcC+-GvMNwXvfW}ahejjYO)aN#)FO9e}HXD3- zktqT-@a)EH9e_!tpQ_ifm(_Z>(U7Y57B9lCZ+RA=vHLPb6Oyk~%N$o+Os(1YyRng- z)8U}=H-S(gP~yxod-*TanPUK_!oa}bVvs26VnILZpD^_GH_#)~n6v_#JepY!!U`(m zGnR=3eU$JAI1dJXOQ6i&V0m+Y`RpF(BVgeu+K@ZYFQ&{m>~_SPhKpkjPCa5Up3jo2a#NC5gm$hzHY)k&g>MMTEWM^_Ja1x^AxtUszVf2Y zl{!+ZN9B6!;U}JnxyAv51*E+;K7WanOJt@Oj0ZyMr!(L6blyRL%I5HRpqJ&jMO|%i zejlh>3%RFH|8yaMn&zm5>4fS!&RfJ|dc;o`R!gBU;=^A3BWHPNPd zb+UYe zX{a!1ol!o*8L+P}mjn3Rm%S+5Hxoi1+~gJ{nK{r4l}h^?e)~8k0Vs9|w-*?&5C4+& zSWTM^vAg{NgfMY9nvO6GCrts(|M1ghD<(mChBC0QmN?R5bO4Z+``ZXgh>9=ykK0Om z#gaImYI_@X3IfE%U6D(aHiD5ynKp18Rb1NaF6(Z3=E1(_<=Q-?R(JD#33(+9I(IVA zs!f|RV?o%OG%lzi=S}X4*xa~Mt>>ck!aFX!5Xhu!U{bhmChnH3T0Zr7*bHvHUvp=+ zT1jME_fYtv2jEtM9}QatYZ&BTm97|xh`^MT7+T`qA53x`g_M*423r!5ROtu&$J`4= z-2~n4vm;4=0fgahJig4E*=LRGOr`PkT1v;2ABo-0!wvCjzuW(bs9fP<5xSrJ#v3An z(1G)8|)6gLRG;knx;3ki8~|^q!EXa6jMj(S5#C1jqNV zcE+11I`9=Fsc=Ec=uSnDc41FE%bAOBVT&mE>`xY#u8{NQCMnO@MmHxx537KxK(HJXfOH1)M9|2rOs9XL=c>+IWoM?H)us;|ND>FaK7**Zec}AydZU>vIefgI{^XWO-;i?OHPcWBr z?92?4{!G_d*<|Zmps!QiDEMs~SjijP+6#Fq@1s7}n<{<7*V`9Q!oX3fwXGU3x20BO z(dFUSHySRJ*tj?mX=!-A_ZKFBPlLy#_i?*D`q2IW{c#8h-xr^~@RvqcpboI=2gk_t zl zmoDM>J;!iCQ`@-nwr!zKNMjpqO(=)z9WyY9^ea=oL%%p{tQN%Gr1^sUuDh&)2lzJp zqn;HjqYMs@)jp_Eb(8^mCNp?X^fa2u?dfuSd-O451KlC#*QZrJ-CY&P^o$44nsl`u zYLGO%jxqg*@s~sHgD@HQjiJAhwnP^%INeR$4sk+kog7s#L)3a6=8y_F)m)^XFj-6h^-=4ziGw)o|diK<^ zAN86T9o?)^&zyQ{jj2KV`<${HG6+fJ&u& z2atY{a&zP8d(zF&`OJ3ebb@6bep0N>xksR7WsNr+j@M&M!elanzhAMJbqpp3(xn7= z1vD-WCkO~0JV-D=sa?Ii&`cDF6V7~(ls(I#&^4>c#nA*C_C2a9-8UJLi-PZbC!huY z^i{?%qmvyKF^@DRJ}EdqGTNvetpZAU#i)pV>~_CP_%6>l$0hNyWm^4ALRk$~?4C3x z`C}Yb2dgr6yqQAP^5gcsNU7@n3Xkm2C@Ix&WN-m!h5gd&f6DrQcNxCO_5!j&@g;qt zfgWl8uT%{OX*B@>jnrava`o?91^57%{o?m&(&`Yz)MDNZC3zwz#O(n7KkdDZh!h6K2G#o|EV+9EaIuem z3S<>5Nl8f`S=P>u!PDL8a86XbbkqztjOLEOb_6vENgz;3dOe@Ax!s)%0Vu$P+3>k2 zZ27iImVV6556J<(V8Su z1R+;zb3$w0jUHS9EzBE5)Mw3Q^pZ&Gc5nS}mLqt~*0|CMR$9;qu$}VjgFJ0Sk9+)! zO632Ly^G-wS4drS)eC=Pu7Af+ zGljly+~z@`E-g+;(z3E0GY0{MI7vzXH^LU){W2Ty%(C4MbNQk*oZ%%TM8!gKseUmn z%g125B^-&*H}-J3JxttfdyVb$0F8#K1wf-=uPmV&ii#7FejIgs z2=rFcJJ2oCtWmZyKPRZS_r0Ffu*!ZG9`aOL@2!6~psv-%GoQFPq#cEHZS!)WI{nzgeVT$%5F#}5^yrG=K6CN*H;VyX+BC{ zwe#bSJ9^WdAunb|B6c=3!fW6$u}KfY)6j-K-BDnJZSahOr@@U*!M z3|=?&jUuF_ECjAsbcR@qwHH!?9HR_VX-S_>4y{KAmRP3f#QPiuR|lI`jFd8e>^P zLPC?7!sw-Fvw(#K)$E)c4%@9>7N%VQeJ4esJP;t%F6nANtSiTR^JYZxY7AMga|=k};=G<(@~XP@N897- zc>JUbFs+&3nXP%1U{aavApc=aVGUMO!MT9-M~0tdt%n4kFET54r+FVj zSrMKKy1vC%wi076{*!%|@|ywE52QE^99+o1_IFyOmhHMN2o`Bwdx-Ubl-Py05{Vrg z2UJ6)vJOlGb0L}CsSy>R??_Nz4sG9|MY)*+%16z;6n^KnNq8_^O5OFJ7`{AQ0(4s5 zmf*tYZ|LkgS}?PEb?Twv;RbInj~|(Rll{IVfi(1w93?G=lWMOenr7LpzDci0F}G&E z=TG;Xyb|LJ?SM2~MaP2VA>H-Gcu&{L5=A-~@vSkY=jb3V7h}=c1MB6l#S>?}kn%@7F%g=|+kk{kRHzKUrtt}?E?tzHR6vV6RhDs+TPLP3w{>ss+ zld5TKo$a}_{q>~rm4>~mvzJhZDWz7Ms@x6d_|h?u%+PVUAf&b#(5#`2%N6rV zx`CB;10D!|iT}seTSry7y-}keA|-;-Eh^p8tr!SMN~d%;(jig;0@Bh-OLsRC(kaqi z(hXAIvpx5B$2ab{|DEF)ynFBUuJy#6^Oc;wB%fZx!4=%5? z3mHBceeIKAav_e2`%YLE%Kys9&EoclzIRx>EjlYH=-OJ8x?dkUP_oYj8O2pBI3JT2siZp;G&}t9G-Q%) zFmNelpM9Ct1r@ni!wNk?sqtg^|0uLPHa0v$=h2^U3-^%@2N1*9Qps+ zubWPDF9o@AZ}7&cT^5|PAbtIj_@2N2>gao2o6xY+iDBVjm#sMRj-nW|7d|!}-yWZl zcpt6hGxVCx+0aZ~Co#dQ681MSl0l4iK4fR<>FGAd7cMu13TBHKp5z*8Xj3vUFqD}L zE_Bg#t7p0l#*lSC`qS{(_~RDKwfp1^IoHeTVhb#9j%Xp5TkL=PLmomraAaU+Ht1fQ zTq|7+WlG_@Q8D>++;$YTIuz$k0S`qL%4uX{mRUR$#|Rv)#1QM-w{HkG2EfR2eHhZo z;L9Lh`LpG@jGRx&R!=5}^x`dxzL=7JAycJ)S$No}{HvkyI#c)2JX2JsI?{3SRdt3= zHc{E9R~g8`%orBsQ64jAdE|Mj#VcB7bvv1jf$I}5l=Fx-bz+v(KF%s1HxZTC^Z9tV z?!928{F@seP_syrkM0{35gBPaYG#^OedRm!q+_#qX=%wkSl7}!rK`GVvDXm)=*I+R za;+=jSV(rZoLl))0x@vc&+~y?em`ik)VB70txH|86whLo&oR`YD4+G(llS18v~&q5 zm|xT4f%FyBRF7@uo|Hd?@h)szNsc`B^PDcl;%IVlZ;7k9x+kvD}KhPeh*}Kr2Hx&z7QIXct(t)^vukAYYTRl zc*s=Z&CJ}~M-y~RW+OU*v{zy_qBciayF^EIf#+5Y-uAV#Klq^AH}fN4QpTmAkOpNV zMw8rCU_hZOxk|IN`5E(RSs53+%8wx-889RAiAt2x+In2=CA$2%qAAK{@Z%GU`UNL3 z31w!t-s8e)Ep^+Y=i!fiOa<>vv}w=R$VD`5Q>?29=|q(KS&DDz@LCP|$PjGOuFp{W z9*2H-Q^D`;KyiFyWb~TP*P*&sp1IVHnJ4)}_jYvksmtF(icYO({`-Ghm^ihhO(`V< zjE)QYsbnAVH!j#7HRqeSaIAW(UeVWV9wkq-&OD2yyPN7tqg3K@_2No^;^)sXnG|_S z6#r=3X7&zyG0i?q@*gujGfx?eC4}DZuSkD-*Ks7vz`aD<4jOa$wB?nReSm!MC@6Y` zPo`QYtsAikj<&b#M;H4D5-0O9Z{!#(cSS>ugk3t`bI>o^zw|RQ0~f|1%M%rDOU+!f z%>;9y9N*g8n7RHG66$~GjS?_aslbGDblX$!LmHof;n8{N+4!JCZ8~2#4X3C*II2C}_~Cu?F>7erzLEJX{}h2DFHihWz&y&)1>b11BN^lLKj``bt)>O>YA{}aSPT1enXO7ie2x;lpXR8->OfR4f?U2eW6Cj6cl9aQ$ClCDczzsF;HplnC!21 zym_g-@Q8xF_QNzQBFdV9ArIo{U!I>FOxgGE{niha{d1+%hbr>*uwnnRg1qcc9;NUS zR|+6#A&uQ5Ab4?ob$K@DgR?fg`UmqXJc3n*!JkCTXPlkicja6C`V7w(H_?e;0vl7@ zu2U89IhSd{-!4(@u>9;SsHo8D-y>^Is@xGelr_>Lx| zLh#LOiX<8iJ)_LrC7Q-i_0E&O0p&}5V-@=4>ZZTU?%fTYrhNA_^7LHea(CpZ_A#UO z8pZ3Z-R-h<8zQP5W~9SI&lOH>hl_<_G_=@0A;Bu44+EdK58Z^~IoGN;j&MjQZof4g zt-C72py+&$OtJp>^x?63K7OtLy*&rEDWv?#C))87II>^Uf46wi`3_V3_wqK@kOeqhFwolD4~{^R2800SNU z?M$7^lx;@_Lu;4GrXaaa0Tw;k`}VOJsbbqL<^}arT}i-$N{#zz>F5GL(ZK?YBqOOmow7>G)5*oC}8e{vlsP+$naGX;MsjQZ4Yz|y`wk5MVIeqW4 z#j7@K@=rwf?GD+W2j`9-_9|{~Ed-h;`<+_4u^W3^Zy>+ETD=wHHPLvuL!(vMxc^JX zx^0*vcx&mKumQhOzUk%5d*$=uSI4{gr|bFY+gknp*cb+LFAC9&_qN*UUusualx;1U zGO>j_28{Qa(I&}W;mw23uNOd} zgg*HJGx=PU?#>iO6qnrb2RrhpNQFECv5z&ZoScz>v4&+x#qNVK6}$b)>)Zb3ZoUlG zK6a#0NnZ`TOHwdJKguf$h*XVloIc4QBxqS!T#Vy3Cj@EOZ3=-X9Ff!a+tV4knYgLj zHj*fpdWXKk`y`X??yutc!#SPtrrNLL$2BVZ1$`~G+khQ>U@>HzmwZTa7W2GEO&K2t zrJ1FkF%eriX5}(d_rj7i#gIm?LRJ27rp#hJtf6P;Lo%zrh{4ep9I1|XCyxh8&dQ^C zQN8>d*K>icDudsQ$`Zz)$PBNjG?lX zZ+VR0%;|o8Ce`u9gKgJl1iQh6I_C+Pbb0eumF6_2*~COdv{+V37`xm0II8SU^==ZY zk!+9rNoIxg!e|Y71;z^({`~10`TnNxI=L-@Y;rg2Lqj{ayo3Y>ZDb&nR)aO~%n|C0 zj?j|nN>~lZqMJk0N5m7pie)EV*tRD)v_BeCIrF}}k&O|laSiAiI_JWoO;=M2)u=va zbY=S>sHjIu_SN|mZ>sv952ie}LPR77J`qavr1ZTfS@c|k>#rmvY_-|tosN)>V-BWt zayUN%s9W;~LcQT=j?&@Upbw<+g=MAcll6C_+taliajy}2QQ&4?hUUHIl!o(^l&En# z_a%{MN;iuA>mODng|ZHVz5{jBA$=5hVo@~w9N|S|uyy3$r$$Xa*ZCMk&*sfil1PW9 zXpV0&i?oAc+hicuWuA&qHYvZ7k!d&KvOC??rSv@2rF7qYTK@3+A2_UEUeo?6)R6^> z;fs*VW6+->s!sH*9L_d&jbBfBEM7IuNJ{l@$@Wk`P+XQ18$kvewZxvz`%P){*g9li z2XT#yKPMzeEw&pv#1fO@KO1w> zMHPaVGiu2s?bLBwFGlFO%-x$6>(Ye-=5A3iUzRw|QQEz8D}Gf6KYLir`;rpmwD}7L zseKzbk|H*@O^_5%0!GZMOK=@YCQ-gJ=l8zMYB+~RkUH;fFh159zq`x~d6 zmH*uzeAp=Y8kIw7;yAzosH|inL*iC}&Zmbh@veSr?jJ|q*TY;<)UAe$UvQOJxVZ_a zcq>KAk2c$GsGFJP;J4;Hm65p<+;HtjmqME17?qQj)sj?`_)}Rv?P__H@EPFH(gMJ6 zy-W)SAd1p$lViJjl@jq%U~G<7K!7ylWh)4@B_K_Wh60#3TF}QFi7&<{ZfCcjGIKM{ zJ9Xey)Aq5s z?&zO$Kc;kIWz;BK`bzHKD5G)D;roZc=odMBe^@3&JqnGz9R7&W z!p5tvB&V70j!_d^j&FI`PjVJ-$r)vTMpVfeo3o8nX@jpE9eIEhcBsGda0XF3?3{o* z6blH|`&5|)QcqvM8kCR3yjF=5 z;fi=fL;*hyS}b_%uP;wH9XD0~`_MPJk`g@k&iaSzo!@y-Z?mGD9{v`1pq`y1`LTux z&`hM>lyUL>d$KQ@rPHWsXflEn0=qh2W9rYh7*2s$=BP$I2oad9SNoD6YAPrww16&s zZM-xKK2ly&^EJ#en*YlCCY``jclCF}VaCdK})a64CdwNmJn=K!;Eo@#TE zZtF-Wbb4ZZFB}}Wn3VJ8erZ^PJ{XU<=hH=-u>tI?^Bue)qo~>lVZC?M&#TN z&t@k2AR+3A#=+4|%w_VKs~a5UwHm;lp8V0eP_{JO5F^#EFO==<>`r^}TfaRpc_lJVh(#FIy<-}i68);>+jG19@9eR-@1*VQ z99oF9{SNH7ir{m|3)3B<}z#IzfqW`^zM5JzQ1XG3xPT8yjG1{8*!@BX( zf6ye|cCr0kE{8iUE}8?#-BF z6W6({?M(hY+(TEdGa!KPf;*% z%wG%}j$*TS;tu zp1Dmtkf7m%edzk5Q#beW0xKcwo*DaY6P*{5O@RqP*e${BxV5U)Vq$Gl(hto8L6jb7 zF)L;(v*7_og|y$Za~kZ$MqW!uNK~1Za}|8>eB&89ZYc3sFi(9Q{n1vKH)7&4F_FA{ z5i#e>yVZDo$p%w>G@vk8MMNANh*CF`t{irWrdZ@((9)Wf{n1VOcS2D^92<(Q_c><+W#$$S68uq2RmuVBU;+Wo)^wA#F^um&ZwV&d9j$|BiM9I zPM@OV;tb{}ux&K8v|Rpt&srNR)Zxy+hftEyW#o^%Mv+l|D9nHR%8ae{S9_7`8zJ-A ztw2h8hKi!&Ncv6tXL%P_eFqtSMUzw-EzQp~RIA-7YH`~3XTG*0k)Iz~u3V*5FL=L; ziaC<8y80xIOcnk)=B1uG>E!HISeAZW6{!7)yrufbFYZ--i51%%;~KiNH0!o7PDRvq zN+?%yq>&nikL23!Sry$Ym8!bCLmy)ivg{S+_*H2YH>gMB^qEmWpa655MSEjgF;-+o`%#eaor z({g2KX2$4seLj2kC$|v7CLN4l#fri7%9L*olfM>^clwJHi8{WC{=HYSC&oH++Scc3 zpSCu}qp{mi8}aHp8(t4i8zRXza**qYKb2Y}Y_Ts4$;!CX)Rjz=o4vWf=wyi0@3l+5 zCA6u3;yiygRE)qRs_&sPCyW*+-(x~ke!ge^!NVo|0};+Oo3}oio?4?bqgKXzLv|~~ za`XW+hO+TjOsur$viR*Xf(0yPuXqx2MY5|{C%x|?0lX!){0O4M^JM};!u>WP zo!wCtoxwsKQOI`U;6^nXs&rJsH|1ppfsbHVU!L z3GT~J#0v99NMa+7K;#*$t|+9B9fXy-B-)cVzQ{FHXuTC()6sHpM$TNKk|O*b3JZf2 z{%1x;v?3xReHg4=>EEasig8MksSsJfLJy?$sqMjGVRVll-wSSQ0TnO_pUu~a@WdHh^6(`uFPs^-7#F4e1_bFK-$ znwZg`+|KA`4YofhG_BL2B;wsM-+AXlBbvV|cU68UDl|M67M$O;uYh;zvS(a(d?+VS zx!xB%n_#_}DK_%JsFWl>b+`5GfSk`Wkwv4zX7e{k=<4c$?e_SF_Ot?>l1d6wENbP^I?x{16=0hP*~W1dT*zD?cc^i?URQvB2c z2}#KoY5C619s}4&k7#Hhs@(CME(;uPy24s6wp3-ZgfO~XoAwm? z?LwbjJe@*A`@-u*2`aHS+$sO#nts&>s(K$g$rxFfL_^>E z4jRz(6oD5wL_{*@e-UgYGa(jCjhMlcbOgg)b63Q47ul9la%{(G(o8PR(qdhqgxO1V>0 znm~Y_9gXR%Pn0vUHG*Tb9hAePKN#AI2!&U7e@0Qc%yU1Se6z)wTle7>`&r~GZLG$T z8STj;-cl87n$sjTcLUAdnUR|n0?Kk{6;w?Ry^=aM`j6}+8`)9WUiP>LWC&&*JWGvU z;0z=e+4NIFqZBqeSTtiZs=K0=@{}?-@t5LMBR%tbY|!E`e1E zOZMT5<_tqA^>Q=RC!8;I-)qa7cWf4xR54&ilDX{qy!09G$3d#^ZGYnAX&~v3Zb0HQ)Ytc89tv|*b0JR^fkf+nFT+MZZo|9H_ytmp`O6R=7c$(tFi5U zbQ4m4cHNnKv^}K}mNGvg(6)O&n64K#u4wwb)mBq!Vp3`R=)b5IFWAt-Ols*gGi|*% z{W$qxjVF#KfU4P!D}+LjH3|Iec{J9EkLrmD zsi^uRXrvPDtSOKQM0VoGQ|c0xp1)v;(L;>2nw`M=E;Z^UwW{6Xfu?zPe;;TFcB=kjwU zY6jQ@sIp0+QTXFQM-S`|r#P<9{^tuou8Hw|$W3`=R{59p!`| zCni1p5m=Rjh*7ua3&(9Tp4VHiWb|wCPPa->$eMYGGDQU_z??hTWrgHwKeq9A3b59F zu}KL??zbvle!cy@q7xoc*eNDT{evJPj4I2XfUUb$6X|nA)Kb}baF?wxk<9jY#A-z# zRtr4~W(p_(FY@y8`VTKk8VOwz`iJDi5k1ZvxH_)@qlWCmFI(U3wmI=TQAm|-MPQfp zjY}(_%L_tH6A>Td-&k;w{FayLNiNOu4-7ycl6d^H6Nk;k0lI2~W0(3bB)?O%wdJ_J zI%x#Xdnr_Kdk)bPO!~1mIexuZbxmCNd;xVI3&GUp+vw*A`}=6d1d`0Hi$^J59z@Db zY0{&tXMn5Yl#ag+>^;tkeWC21qWnzaDTYyLoa5F6za_b?M}cpI<$5QKh;?W=O#@Do zpscF;u}$x3?&!kC2Ds2lWfb=I^gLo?!_m>vA?1k*n}c4PksRh*P_YOTfR{5bV2oRy|!(7%0v*HfpZW$U!?Si(#E8w&%2M363bQyg~M zF6B8P%&irU)@#Tl@-@T!D10*V{rmUd$7;I+dm zHV#`A3dwn>BOC6#l zWn^dv0!v$v>bOX!%GH4rO%e#rCRY*o9DT0~#NE|^WZS-brzALp4aJx7_Ea?xi4k#e zf!fyB#FsGDxm-S`om+ID$Lq=+a1egiLwd-sH_^~a4SwEJFEe@CpDdiaK?7xLlI`{s zKWs?YC(D2U!GRLndNpb^+u#A?Tu{|Z&iVANA3p0~l4XCte^@@oF#Exh$>t)5ZUsp? z311liFMb5zB}qde_);pNcYX0DJ?77$=Z)=rm1orPA=3zN8xrA~Zh-0=W; z4VlkzllWxXiAwbH@J+SXM;aQMyJTeD^Q{5n^=?k<#h^3XhiXQO18#7z8ffr1hi(dp z;9Z#Nw})Oc2Lc{GAtCTkZT+Ixuc10}*~qUn8P3GsM45D)!ymX8Rq%v=YbZ+ptp@s! z@&2~^f)e16#VG;LlV?0yl5iC$2I}!vi2pPLgAA3pVkj2NPp%Wa@e;$^B#)OS+8ekv z6%_8ll4ba@zXzfvB0RkLXCz&By%hjGNYHl$-y!FFUszaZ--1c~oT407m5@#Q^{ZFt zV6&J#^7yFcEzBFk3fjWG7VV!%>>U^ivc~1`%k0M}W@rDgk7cRw^hc0{$Akl9w z2P<{gSLfw5Cz+5Scpr1VhHqg!=3G@ZP(>P~3q8LvKmb#A-7GRxvYIJ(M41E5i(|@x z0l|lS4S81`)?2OA2pjYQi30C}00JCcW%?X6t7yS#W4w(9G z{(y?b=kb>d2@DKBk#PE3O*3kz>_d(`0Mr5}sqboIV01KkU_cIfv4z7ah^Yy2uW3Cm z_Qb`WJwrGKnGV;?u21U~BQRc=9+{t?|3o1e12ObFg%p6dDF`(7CeqKqKw?SB9LqYS z{22UoGUYUry``n*W(y;37~Wz^(o!Oua^1*xKO7VX5Dq9hYZIeM{V?Hnm`&mHx)K68 zx6Es>c`2U-Hsd3N&4F*vK9O<<-F^5Hk<;_E>*KSt=}CF5dZ1loqFMo%l!fl!2ZI{K zA;rXjmxLrR zHkJs~WQ+jEC<#Uk)6ml111*83COJ^Q#(@5ES^SpU0#`|Ds|fs7Ut9wtqX@Tc@+->G zla)T%T*LG6QurrH>eO5EHZ(NHPv_7Ono`*6|3LQCTX0M-zGj_LZ>p7f34LO5004Y;@9&2F zfAHoDxFDs~ge&69mrfXPA{KHkf(Db7ni@Is=MU(Kh+(<_+MgF1(jH2PkdA{0NbKN1 zX88Pt z2jczsW*b8x2G~Aa_b}Y(K=te(Tq_I)0}b3GAJlIIvzD!rC7G8I{^XXg_kAn|;vW?4)O9Auf>sSCp@gkFrYC z^P%uV7;lM*i8-&t8JkSklq2v~F}^r>Z1yS?)<1jt+ADQ^%%~=1({%g4zgb#Y8LF~f zG^5f{?4jAR!t7&Pg#8hTC>cgjy1~(HzQwQ1ax$jz>ZtH0D(ZKNpWhKx=TCzLz}7)o zk_--UITYZK`J6i({^%%i+~QV~j^8qlXDs(%Agxo_I3rMaRVzHKy$r($w?>Aw7bL7T6xC$x@P%4+f?wl2R4V ze_w@W#6ehsE#e)-_mjoaOCQQ7GjRZB=O4jiTTHkVR8>1c=&?RsTM664?DFpsEDKyt z^aU(XppR5U1Qc|FR~)ZDy7pP{EUIYUW*xm={Q50*E`K<>PX>=W*llOW#@4jyAFK}E z3N082C$kd6?ltHx?%ur%bz~03Pt&2HA@;b}n=|zZGxctqfbZ4SLvsclrWnnC-&({o zv4F7$vz8voG*t&rYmONCX|3~8INeAnJTx^ZBP*-)ZSxIqI_%t?Z*^Jw762ugB1qJK z7(AX|UTy=yifoFA#|VP95;H_Rl9Gv;dFi>UfDdU^ZcMXm+Wg$lNJxLc(dMY%vCU|| zY;0{!V{n@FsQd?9D1JluC3becLbwz`ySO$}UkfX;0{UTC5#Y2**xw={#&M>#>Q96a z?dBcW+45mpn#pUcYttG2-3e=z=3_GDmEcW~#tK6g;MX>ZYseH8dpN8>H3kdFj? zz2xWEF7H2qKsUv6oBzTkBy0!pXX8+&57=@rp?%9v{KtJz7Qu5Nw$+g;VQfWB zS{D~Q4+j6y+tS--cc@YbOFlhEkN%xUSaCtzn!|#TzN%__vY&O7k3uY>hxC*(wvUNF zwrQwvOrN3V&n*FA?7G`!e+H7Yzg<-+ng8tyJRh@hZ8$*tx_Y)_ck19E{aVclyuT17 zb{M50?2&ghL($uqJukFB z=F)R&4mDq2w?`|f%lytx*atx?-lXoX%&>>2t1-83r}9cDmSVS^$33Eg$dk zQsd!beev>#{4f4*LViQwQXL}Nl+eF-Ng?d;F|S1|6p2nY`FYmxJe0>7>AtpO>klh- z-9^xXgF1!n{b8J6z0#_8r9~{0cmM0)J50SvOiVQ;n{}M}TR~zaT!V#o-EXlzqR$qF zRFC*^GYle?e*F4u1T7ae*xzKug|nRU4Hm<1$k634cA7Rv`Jw~DMrg&NLuX?3{`_!qVctsZWT=2WZfRlg$^4KiAn@S2dlnNED` ziM;$>;FK>QRUd9m#(w;WJ~j1c<#rvA4Em4bUP(#m1IzS@oL^z<6=LV|{rj~^WvcYI z5M1E*H(5v~lNBsej$|A8sLOr0;G#DFR{n0S)6NPUHYDfOSu2!Q7(}< zZh9$^Tc+Q1TL*^CpHoTYHqR%jJ4}T~dnwOfFEp-gI2)@(L^pNqtZ=SczdSZfTOmj`wMe5DrBdKvb-73-q@|akz{)O7_?9T>6bh;6|3|$bk7Y7) z=f1BTaOIENc*vmQWn%F~Mwla|ViCP!V_-O$h%j^2j8;3mO3ia#YHi>f8HIp@oL~^{Q3S2Qjg-stH!F>z zv6V(4be;0|hj_?eH5R=@iux=)KQ1|ytA>6e8)`5d#<_0P7=^-7%D$aNbr z_nxkU)`rmpsa4PzLW**ai*f~aheN+eOXH^HsZ-a*Q z#w*6eHp32_AMtqi^R}}$Shl%L9~}=$MUgvRJ3ZL(DvlSsek&Awk6+&}@zYU{*OFlk z#YIfB^dV@21|D-D+lOBMrDb`xJG^1~N51*~J21?pL`XerOAMSp~z<}DZVbA;U|t~BIHSc z=9q8rGlykUl_T0}8%g>2rDuCs-0?w)r*j|)ts6CSQ8(8wI+%aRiO!piY?9@4`-WyG z%Q|1rKbTmpt={pwtu2r_==IWfLnE0ia=MK*JxVA1A-l)=sujn#)wkrX?vnM#hW$nQ zI{5dXCwA`#-H-ikly{}P$}Z4Cb@aq>j@LS|^^2TaAhsJ+-H7exaej?O^64JkR>}p9oC0XH_i^$ z$heT^LN~Zpu}94Khx14PMf;&wJdpYV3xEUf+*F%m3z}b79~o7qtT+m+s)a6!xX1}p zGwFW@)BM>Q&$HdcCmFCL=e$1R?>{-Jn|E{JIcR@__Ul2_=g3}?Dw$dKTK~VuLPwHb ztf9a2W(|_jucL^@h$3RI1vT__e+cvEg=$N<-lYeRqv&w}95Utp8+F)Ck!SAlU5e(o zh!BnmpNa0Ce%B*pgk9|AoQg&MQ!31HF_-@w=@oAlGVfJ=b}-nMe+&%#27N)@-#@uv z

Q`R}!D?4d@xbR=R32R1lOQ$>E#1vd%sNh zk`pxv*rcbB$oy{3)I=bo+#2~Q`qei(A3uq=IZ15&bEN;c#r%>8-kV$2hQsExSJujf zWQ-^Tst3h{kcK;-dwh0%6X!@*= zTy_}*8KYCPOy4hke{i-(&X4J663)juBAu{3# zqI<(oEaU!Bzq0Gv|710Hr^>!0{U+OsZ{Ddzxw|*c`%~yRIq|5dQZ{CuDTcFxmp&FC zVZIX*zPPc2uAxf@Nyp>yZFIjl+%{j$Oh$F4^7`WqoZ(Ul%bnS&>S>Ct@BhebZjNS6 zUl*>Eie=GW+O{>VoV#Hz!n0qzGETK&F~ikhh9>@R*g#NvqDbMh&iZez{i=NYHy%SKdmL z+pLrX%1$(S_aq7s%WewNT4@t#te+DQS_GOsG z)CTZk&S+}pD=Q{STg@mHX7QyYZi*ps@vsSzDY-h!heEF}a<+swQEuJ=!BB$^OBd8r zD*$(|ZjP5CPB&CT|0NLxKy5y{c3SZ0XMcU#Km8qlTPq?NIIzB3sddO!JgGGa}35ijQ)iOfi|DF~Y z14Ju|4YYh|gPqVIIzwwiEaF}@5uT$72m@^-2FURN)$50B_8(pliU6sPKA)Z1UIYby z`#^g(+Dm|LV@nOF>lXcy*j+*^)5cJQBcneySq_Cbx_9#KmB;RdE4Fc`?tBmW1aEwI zQNhd-g?dk2qjl3CzQW5bio$Av>nYn`Eb3yJcj!y(7`bDVf(dt9WzKI%Bx#_BUF-9B zSiiNC_PlbvI$tr{K5uM3{WJGt0q4vvtNzjIGUw-+%E<#51h~P{cuj5}9zD9#zp_ew zJ#U2)g-=&vuo3_fE@a5`g70d<7H>AyZk*SKfuV2yRU*l@>H4Rqisb-h!AS$f*D@#p zGR(&c%*1mP+S=Nn_fjs@hJUyG!fAYmnwt8bR8&4-G_ldw4|lXZ@k?hCBU?CgFx#=1 zmPe{7p6uN{C0E($!Hpfvb;1h#{9hs9+Sn-B;aGv_@3tl@)95i-)XUzEWXng_Sb?`8 zKr?nAFMxy4ZCN?}vLkei5Wh=7PVR|1O!kvL5(3AN-G{(@&5Y@t5VL^Yav?=Yrg~%>y#d@5#JNj z2qV9@aF2iSH+RW(S#0&Byy`)fIpwvx6;_()c=z{KEaE9gis_nD`@Goj8-hdI_Vuxr@#XZ4*U-&!qCzR(WJ9OZ%kvrN2 z2D2?dJwbm@pj`1ZKOonQ{-U+KJuiE3JWayx zd=oDv;nkP<_Jh-^*^Vsv#nZ#_arep&j@BMdUk*-@#o<`NsN}5wN3pMEwD(t>_Sd6F z_i9=0zJKZYZAw^IV5fm2#!)7Y;8s3e&tiGTLuV%sfnTZQ_ZIK1&nJG%8HEZGAw{C0 z;Q)OSI2Y!D6e&737Q~Qxpg(cgo{D35lZlIupYF&EYkTDTZ*8F?CVkOdc#H7qg@ZbM zk#U~6)RTgh?d?7YM7f8_upU1;bi88X+)gn`xponXg8ZSqBnDR2P{0vO;ku_#%5vWO z0(ML22wz#)ngo5g=gYA=fXZ0_I(4WG?Z`5Cur7gGu61;@yk`S(r6xjU3wYv(HofNV z?qK`XzC8%M`|xKmZb`Vj&xYB3F}!rzd|i}>dN5S&p6u;DsFG$+R;f21MwKyYJ^Lxs zJDlBNkg892;9_R#OPM*N-GvcldBXh zJcaum$fRM~l2>cyVdlba2Y&^y*uLNusUC5eQhrK_#IznY1iJ`#xI+_=u)WI0%ltlq zIbO>N-=78#*x16vsXWS*FGUb}llNcVq-9_TM5=Fid6hCdTkOEe!0^3~sC7i)6*)3X zCBQ|IX5!V50X@apuKou8ml&IY+}=*cst7 z+_zFY`@|hRYGw$CxW#yh9~2|Fckg;<-m2g}EiEZ2fs0V|0MGaJ^W(6Z;=8yg&^l&9 zK!+OA7jCNP^Uqg)Ja;!&J-7%aWyI{&)PxskCjPxt=x;%)MC6mdCp(3XQDk=WEvODP zCD#f0Cf|WOz-Z$3dQC_$Ub$#*&(!EM2*k-xL^GqRu0BC<4Nq61fsF$A6nwn%{!cpd z>3*;TsdZ!lt!h-J<8sgZomD5L+44AV?Esrapy*Y+!}Pcp~7_{5~ax3UGGtezG~}+ET2?m7FRlqYd7PacAo&cqsvtWn;p#J~xPxG-*z!Qd`@CDK%E-DHiS`t{QG~gZp z0&VC!K}Bd_4}etq=$c+sge#>?c*R`=yix-xHrntoOkXMrN+vCipU~YvnZTu_EP$mV zBqV$Z+#B-RPzDy{1Tf+QV_w(k<)D7y6LN%4EASPG!fr7yLr9U?T^&ez16YT_rx?cn zW#G@G-2AmP6&Tvs*J2Sc|rVg4Iz@6m?PiG7~S}1rWSwM6K zPrU=g4GjmAo3L&iJ(>d zg#1+*L%H-;*Otvy z&POttspAKZJ8z^)Y~&@WCA&xat%nqQiPoU#fC$g3S=EHaRwu_KnoZvi@d@_e0-!U9 z8(*M8`+2SwN?wVeTWBVmb3-Tn!a; zYF;_>m)Y`Z0IK7IucWi9>%#i_Synrd*vD>AYC9qHcW}GkJ=pxf+d)mz16&D+y+|O1 z&PA(ee}iuGU-?U2Kn)Q7pSz{wpHg}=9Y0~Zm~(Lrt;5cr`tJBeAa%eP1zAMg<#b*R zPC|Ltq^$z5CC?$n0^aX~hLeImHT*vID680)tBa~Smy|J&TS z%m3=1k7sN0>fb3+?_IoqG%jD##kExod{xWZ$oEJ@370xEQRl)5O|rAIGrPxM2Oy|G z3q%8p_#V*ntQ1d#_j%pfNx*hj~#cYIVFA{@9O`9@hToe9a9YU(O~IucD*_6 z@+4|`=6dg>=lz{3Y>BP~u)4gzLV0IfV>=@*?rpqJjA=!-rBV0KWs6aJnZ}Y;e*QN} zdPO}Mb{I6=(odx2AN`0%e$<4m_(3;mmI^&k@Z%Ko?3j)X+t}q(S@Kh7t+8HcPyX~w z;=@*!GBKey?oVEZX-{LAua`u$C)C>?qjbea@EBZN@Ck?(3kK;amMheAM&W-VH|L%t zw1{Oaj_AE8TPu!hYPy0e613OZ;XWVb!;Y7Px4(4#?H=gzA|cZ!+fe5=Ld|9p<`h!!gR?BSpL6-Xj+m8 zhv|8~!ZS|;u(UFls1S8Um5aK`-y#(GfVYL?aYFfU*9z;K`It=>uvg>L2>A_~qX(m2hz}v^L9IhaYJ& zY8F93(HxWOkf{Fsi^Wv!btljjCZjpgL$-*m0(W#YS@0YHhrkIlB~XWkhK5RZ)R+SS z+^knz1lIxGxN+mlV?)U#S1h1^KmL8u1$#a4Q9?)>51IA2g`{}3{?9w**Rg0BzXkml zf3rtI`K`JR*uP}W+mLw^Jz5@dxc>cg=$Del-cz_RS@L6Zd`xgoAl8<-1cIS7=g09MxDx1d*nO{SjcAP}#G*J>rT+S%;<7*oq+Rj2U;P3qOFCWFVIG6;gm z1%0Wcc+NfOLII3TOiV;jS;TyDGbf0-C&$Rg5M>v_sdyPFzO`)kpQF?qD+s*IUmD*M zhkG%Cb8~Zt;H(fhSarDdBs4}TaS(NfekXUhDRNBfb@^80a@tzdzn(oQf2Vgf`lOZ5 zVY`2k5shM6yl*)Phmf=2nMAH3U9h#7I=hD@a_FD!y7LWW?b<&t0?w}{Rx{7ND6Sn@ zWkni42%FNRn3JxP987iinlQ>wjoOj%STq4V(%|6+mo0|I#>NhXZ>s~@0hHs9_IAW% z59}h7q4dC^V#uo>YiSLk<$2#;7Sy-b$Uu>%z!gcUA|RH(I7j5;qb~v_*&Em0)=wPU`8#v)-b}wiu$n?E9^0=|Ac^ zM5U?S4?c`ZckowU(P$S(;p--A5#Bo<7G3x9`gyxZ%lebvk9`x?%Z8Z7dXo0vN^eZm z1M?h9R8QuwnV4Gq8*7rNu6_mhH;q$Z5sUowk-Q`s;Q`JG&J;rLinyHvQI?us3j4kq2@*6pe&NZX*#0 z`1g9HoM__c4VzWZj(HAqY#J2vTRbH3qd5t2eaCURlVy?EP{_O#E=#qzQTn;g7W;18 z8|RUX1-0N{$5M}TL!?A<6qg0RhzGZu-VJmM37$SQvhFH8Z7N5om%5?DUkAt%&UYGA`9M)4EU{nWpA^9`dzJjbH5U*qC>r<2rU{e|CH(m+- zYKaJYM^Q6;oMRtfp|85T@?|nqmo12H#YRG6dEklcDrIk1k_z`h+gL7jV(&vb-sDBP z_mX*@rCD@(y^Lp7wo;-3Q(mK-Hfe6W!WIJRd*0^WVA2JQd|*h(@_Yc*3eW^{0z+DY z&b#+vD}x~gJ5ZgJUYZs32pLj@IcgAmJG);9fS2ip{)|FbXnrQ|DlF z>vkE|ip_{HicMCodZP_i0-3e=--Vjy57{b9bS0lpe?5%r`MSEomIh`Vj7cypG$`%+%UO({{;qs>=vTiJJ zXUzC@I#@3y(W)fp?tAIR*_Fg5LL_k*7!j5*GZxuL@x+fXzrp}Ug5^o+!Y=t;ZRLdq zZhmqO&AM>ulX)P2KD2#-RX7jYL}$=L{Vp@b16N=p$V%lk=paLT zfEWgY4fSJqIMVB4kxJno_285aq6_%0*~nbewENFvBdXt$?0w5aFTe4Y?X~fi=#23w zo4mzjO-QixRG~)9DIH6ik&sJs*p za`TOvKVP%y!><$8gPl&ExH{|_?t9%sGO>1!*i2(G*x6T&EguBX3vn*tyq@U&8t(z8wJd9ug7bnkS!B!9PNxR8Wo9Cc;(as#<8Rb zMRNu?%&vrLw5wNRugRp(?+CLySc{*0A6z>>k6C7@&*Fie*9goeE>=aasX+_4+#S2P z1JNf>64BTB;E2qLUZ3GyU0oskJ>iBX&3}8>SMPssf_j^(?9<1OvT>K6iF%7{_ut~9 z<*RX-ZFl<;sgyVlKXB!3I3i|r_;h~a=;$dB5Z)jVboFZKi32{yI!}DrW?vc>%f;#N zy}uh*bsfbAdJPx-gY#*HV~hVb9*~UGEF~Y% z>7BAji+2W(h(a?{&6{ONAaDE+V#u}isaKT*&Q+Ao)1ET@35}bPZwy` zhH9QmZ+JjY@54lxmMvNsr>0y8G#9#GoU?8f$#M?>(&8w(Pt=}vDd6z zuUPeCaIpTs*QcO(90tu?20~PV%l!eWDd;Ss7EWS6pIqI`&{zDDbApud@T0)X4Etw= z_O%G!WEg`9n|9Y{7#^Lh=(tS|Gy7R%Fc3SDL7K zx1n0^$_*C`_N0iWXxF>)fJ_>r97FlJYJt3=~fxNCx0gwL^Ug zO>?s6U(;LIr0EXnl28iW2NevQ;>Ttq^iVPkm~crVX343|$QeK!BsM~PCz2L9GAa}v zPA6$BcXoPO1TqpJ@blr%OI4X+3>&##yB}PA&5r!9;SYb*BWex!58rGRM7J<*vC`y? zf>6WF&5`ZYE0ZB)8pW!wq@@j8d@)|fKf3-3<2+(hq5qcY|FHGm@mThM_%M}*l1fs9 zq#`?ttdv5LP*#ZSRrVgKJ1T_AN>pVZ7_c-3ialDCY@)HJ3px+J*=n!ul+l8a?Q9vnr0`Yx4i2WPauBRd1XzH6?$WVW|GYYuzu}e1B$-%13*&Uqnuzuo8%^;ocjfn7K`%yR zM0A^wC=d%e{#+~D7yr&nd#=sc9!P(Q_zfopSaQYL#E^FtyNRKKSRn@DVW*#h>G=1_ z(ENKcS%jSl{y&h!MjFFqz&%b{Ie^s>6Sg28eNSgQAk4zD1+N2ZnWyLGbisE1I{%C9 z0G9B$@5=sN1rTTa6wvhsT*t4dtbnfPiKv*!IIqx~t(0m36G%}>Gt6ccAa$WBw`Fgi(V&=^(& z+UV=Ual1}kH8L{Fea0>z5R5)4Ll>r2|NXZCy!^Tz74^PN?C`$$_h?VGBcawHQjWDH z(V*T8zhd7M8S*T^T{D@>zW!SBvGo7+RiVM$=S>)Q!q5{ub&ISa8u3eb&H78T!wGFB z-|d9$@>knHgAJSRCzI%YW0*fxKFPN<5a1lQs{(iSR-8*I%S(oPtt~khhI;-NeThrn@T-I5AlEDfElnXWH_Bg zuEHP?f^@3v{8TaTQc=|@;uFNcD25Bn7m6uP*H-ZOSb&1@f|*pDt?iL;4-V^Kgy1kd zi;Q&ZarBYApOnN-l(M$t?c_&bANcRhQ&^xg0mP19OicB__GNv>46}iej=kEkFHBFO z*w(NY;o{oqUGX1~ThMaRybNnr?TxAwglB}<+D~S}uoq#8>HLE<+i_(mYim;!lG@&y zw0R*-w}6gK)=JLwP5AF8x<~Qf{#>r2<|>29Dg3j3 zBn^Ihy6PvOO~CUC=G)bO{`{$^qR3?84!AxYCVLpFlT6Rdlr%I@n46n}EnQ{R@e6M9 zEi&9h)ViR^+caD^hlfWPyqFNWFaE>lzz#0VLGh{t*1`?tqzh&xCAMObAx z=ICPO7#`L%)T= z$tyRwON*cY829a*SQtN3?Br-P<|rj4Md%dFHBV8bFGdN+AARO;LNho zCrXm<*fF#;8p*9<*JGx2GE1!i;m~ww@f_2)?SGGGxu`yl9?WeZJ;k*nxUDuZ=Nq1h+zn0%DjxIRyOzbt!{g+;3O5zDv782|Y)h)QE; zRIRV0lT(IHg_o7HyXSarULIljw2OvD%F?pt>Niuv2Z2|U+H&x(u?)NwsPMPP$K7)t z{LNzn5h#f&;1n`cy_;~BVQ~GS5X;=72vA3&qQb%C!IOO?G?M=!wU-&Oi^}Yvn)j;-$^p3cce}7#G`N^#2b8Zk@ev8YG;n z)v2+L(S>akY{24mMZ-O%ka>*7%qgz1Y(KJh1u|)%DZav$3s#rWYpc zR(@I*t^G1ZMPO-TGlWT|9uD@f7mwvA&`5Dn8jsjTxXXS71^PV7uI zZ&^AKM`7%!(!|kJS>*Sh!^*s^YGz59{fSWB;4kTh$Ni6S>Po$!V5wqqvH8JrY(ne8 zml&lTPRmA9)?9iJ8c&`yghZq|mDI-H6X-G&&bledB z6Fy#^m4;3IzgqNlITAc3x&-|f8U#r{1{BO)UOqlV6$`o7i$ulx+cwLt@4J=Qo+Y>E zJDDknvzphhKYm;)oL{u4QOw3!=C$^zdU+~^Ir-CjnYfC_;|cLT=Y_PmwMrZL7Edf> zQ9kD@-!5iv@r@@Sm)miG`s-EwCYrWU9CD20TR?@#&@FUcjDss9dED!8FK=%E3CDPO zpJ6yk@&SS;sC3RlG4>Npvb4rySI57aq&o|8_G#Rt*4Ikx zy?2&x!@(ZigE|`r#}RZ7)6*8%d~&vIGsRb{hl@@oC%=oizQ76ms1hWr9H84be1YEa zK$nufYKgDFRtG!!U!KR>IZkQVX6-XnS(FwXHTaIEEc4av^^gLCgR)L)Lq3a3bmwT6AN;7i5E`e4Hv1JtHD8wC^M$Ly~ncLJXEiLsk0YKBC8fS)n z0)&1IjWrHWTOoS9MpQ8`_;1%1IL=MaJnak+|9Igl6ct3#C0$onht!@4i5N!IkDHAy zL<80z4?>mpcLbn6J5(bw{eEM?OeLMeX~CZ6RR&s$;-QgeAW~|krYdd@Y^Ny9R?pR4 zVxeobn9vdG@Op{zuZqJI5wu2sJNHfASl4P@rg56xJ~jt8Wnm9^2{8Eor*rTCrw>s z<4QU9P%@Of2$3077XA7yi9J|Cv6$F#K0n?u2cAZB$2gdvT!`{YLdwbh&z_0v=^ep< z6G`q1yww2O^X%UtusT(QYLqb00;P}7yjKKWwPr)2$1Z#6e=*^B@#+E{t{?ov|CWFO zzBV$!+*-%*!ZCq^zK&)ehw53B=EM|%Wb%b@=&+L04JaRC>(Xj-3M@BI`mI*~^ox3) zX1}3z-843&L;TL^H905YvkR{R>z(aRBt`M2eD=O9vs}cqGM?$Tzl-#{-i++U?QyA` ztb&F5*yxs<&?9PjQRXP)U3ms`dZ)Gs?WTb{JrUacJKc)zUiVf`W0`1rob2>qxUB~(STP)=rze=RsC-c3^(?I6kYExFQp}Z8 zG1TJS6#vR34_8<hS`bX(W3=)<5dO2+l@O~g5T|w|4}7ZAY!~vYjv`Yk5T>k(vyfd zy_xZB^XYG)jC|%9TW&9K4Ulf3*Il3GQ$OZXF~_I;q}Be!`trifFe~dcVfCeA)|1@6FV3pvb+ZF=DI96ZY5=~7zp-rn6ssttNdi~d7YXn$xc5$ zM-rA>goI>yIhef=-TmInO2UC$_f|meA|>P{&=^*ts{Tq5idqC{or282@HP1f+ni8a z6DALTs)LhAMlDbQY@?vi2T=yxMs#xJg!#kdi&jiN6)lY8A4-;u<-0`dc^HQoKl0hw zf4zWlm8wZI>siK^3p);Q=JVP7;O&j%i@B8;wRVaxB>yw-RF!$Box^TQn}^FQ6g#ZC zW%;D!Pt7q~Oz(Q3fJ$aj?F0Xhmvdj3b`;HqmJXZh?4_Z5dpk~q9 zvCjtUlJDzGf0h}gSqe7nl*dgjc*1(@VLsq{X10Z??Lh)xF`Gf zv)0i;X#KBevDbrZ8{que*;$i{GZ!!rLQOsJ{X6!~i zX9|8IHR@q`?%8qc_{b+)CR2OPg2{cLYnMJpmjN#%hd44GdrWqSrn%g(o z-&*!NY<|g{>J?!wce(tKe7IlJ43e0Ah{>9u-X_zXYy``b4f-(sDm8w7lmOj@-dM&rrQ^Zsp zZZ<@fP%u0X<<m3Xo( z7aMxzZc(RveLH;P5T%jD{ew{^P^?igFf`%;;1P2PpFy4`yVk~qVwwr<0cu5(4+vv~ zLjC5=Llkti+Zo&wrv5!%fwzEu&@KV!=g`PWlWdAQ4LvhDR%n>`2%O<^h?NwbR=h^% zVqHTC>GuWUI}xGx&v&l6{`!%q@s&-NS!YROP2iYcv-z#6jJI;R2h{tyUFrm!lT0-C z8`0%{*oA&7v$k^YQ6k-3xUdOX?4)V?W1v5EvxVf)GO(&|4hawcP2GdbBQ!YsIL%H* zLLjg84>d`d!@~l{;|eY$;F*)Ab`$^QyqK#LT=>TY(1mZM3j!J;t0S~-SWoxGcS`os zrT#lvdEyfLN$i9dliKw4Up(^?l(110@;Ufm&&h0;-3BaA??aU4sbXxCIZd_Ha8PoyyAh}q395X zg0V47OfFS=l5yyjvi0!=G-!eEjAz5K}kzFo%Mz=Aa>CKC@{}zvl)>gpTz?_1`ZOLnW z$%4{Ht!Zd1-u3mjx}19kpu8 zs`X3R`7XVy%wi+UZ9)-RKTg0Og34`+#fJd_BhtuZ)!I+XQ@>vjcddT2(aqOai&CGxV=Cx3BMuCe1$io9& zYhT;*Sn~*t?zh`0*eu=7r2S~oC;WZ@$XTgee=R+kd*My|?bEqqGghsdww03Yf^=6> zgkI;}sSydk=oCa@C?I?c@|^w}jfxvevHAotmNiP+#cyFD%iCW>UdPBQ4ybWESbU3_&V=Ia6&-?L$EyGm2J*~CIUlB zLUIlYQ<7eHYrI_R<>*Ak-J3s<7{xVPApEk`T%!)tnE#pA`}Og@9#OW}8fs&f#@oNsUL(F)j*_TRY(fnh(+1xiVblL$vK zltd3e3Is|>ln5|lC2-xOaf;9g=}SVkO5y(Le*gYP zaNgL2Q(DIWT9Y43>Rg!WlY@~Ys79YA>|9)2z#E0`hW3<6jEx9f4vzjoJT8b@ef_|M zs~605LkIO8vL#>vfw%CWCG39a1aP1Qe~fpeN>?@2)G(bArH{}LAQIS@3f;Pr`O{QO@4=`z@8TSa+#^+B5@85tQN z+Z`HGwEO;4ia{GKO-_D3`|~#=wRLsup`>>37bYPP1^)$4Yy&EE0QXT~7O@dB05I?3f#U=i z{Ft59`C|m2QfKX-tlT}7v6X!|Fq`}9g= zTu#o{cr_H6lz`wB^05-=UloSn%Hlk+5(xw;B3riw{qN{k_{vVu9Ez@x~=+qSO8gI&PB^8#U|~ zIPEV7uySy4u)miM)&WfE4FnP5YT$1+S8w&nL|nk{qyv?Hlq%~pnOhjgrXbn^$K zWRuk`ai9N|29Jc`oC`~bdnGpy9XYbe(9kfJIgko75AjV-F{6OUS#Fq< z3H0?32somz%1Nm55o?!#S%6tWctH$n{&z@1)e-(}aLgs#C+^%i1H74-I3svfM+WX% zd?Fn?fWCI+=b+y(IZuAiot`YLj6H7~~ z+qWCPZvHa(0#ee&B z-%C;-J*ri;^U2B}9od0OT#I{b(tTAYR^PxN=(f8XLII&`MpeD}{<*a`?($EtBnA2# z9Q&&{Yi0(|IsD$7{+nA&m_i#KKo{_bL%ct_`B9t~zk4cq@%oxD$|{&gN1|6p_i9q# z#o_P&tUA5x(&ftl_-XGFHhfq{=Bq*1N-K~zRtQA6rmjYN!oH6*&AjgnfJ5}Ps#8%r z>Mn_uzG621bnu2zN3!s3y1t=Oak}^cowSGsiMDe3VzZ*RAA^d^&FThp_9cKcX#DCl z`_ua;JT=O<`Em7(4ZSX$`Skt|cq3!=(>{KJQ@MCJ>`bYG=|29q-J`=Y@yY^>D&U4X z5<>#a;{)kUp>Jb!TRDN(q{MT*f%x708^5EGB(a(54HvW?!F4eCxVVq$TPAWNfD4x% z%j*o+E&rV}UgTq3&Bh4PIxOqnFXE6!ygN{ChGsh*^hef6PsT)xw=~@8OqV~9t`!trWUxtOr4ffsFdSy{w27Ucbv2RU72zESIt#XJwuv3D8Er+j`qj#xdjK4ng2|eJrfpi0B{vgJ3}@}y@c%Ll2#}*tA|v4cJXuc(SPWhA2RIjK zy3AD2W|_l=Bpv}6RU#i;83=m%ApcCU(}FUP^Cc(~Yh5QT?o)WCj%+f@8a6z8;|8s2 za=S2Bnb+LTG@H`>K@3J!gKpQ$3Yv>f8!&1QiqT9jWJu*+*xScFA$k7LaMJO|p;-)4 z#khuit7E;OWrQZa;tLWW$vs1FZrM!)3eT49=B1 zF>~IHD!AAicG5d-`{t2Q4iScg_;cDqL`R@+o_urd0?X&hHL+iPw+y;O?VxQYgf6h< z!nx80=LsfoZ!~r*_r=Dh(Dn?_qTmERdJVAfUwaT$xWvQN zKr&gmZnj$bFFqW#532D}J>GOT#3nz61~3zFEF019slu-bmG9p{Girqg(tSq~H@djG zkYKj~W?uZ*PPi?*i(^Zxu#i=p#CA;oVHQx)*?EA5CfI6_&Wtbr&BvuSnV*pfBEJev zDhm=QkIp5!jI|dsc^2tjUyoe;wDehJhppGp{o(Q4q0{buMK?Ut%K}NeB{RPizlx!s ztXmHW&M+vs`Pk|GtC$ZmOKoIx=IkM=pBt7qMy6Q!cTLYSElf$@Jhc$v>k zWWGYEm(uLI&}q4%;(aZ`yJzQ}T{2p$v4Y^V_Qo!%e$qkSAUYYtBj0A`1U~dMgu8vW zD~;I3s`y|SbEC*et%dE;td>w8W#~Tvx2dY6`4NnJjM_9sNZyq7+hvyJJ@83vFNaEghwp&+yjYp!ax z#vEZ8j8o=oyj16QZ~4~(r+Q_lM(lugy!w>$sgg@|odY_wKPF{Se%AR?ntZtH9u-Ai zpwLpekSz8zxtY(%#(1}~c&pJ4_Jz0^F*W+UN_h(-x-P%_qjx8nb z(eGHtb~yW>J86&nsUpf`<|FGFLCMGOAL#wnG_LNWRsV2sYPv3U1CAeN76aC?A2&@e zE>=M(^Gp1rMJ(AT7`>361&&8Be(v47ce=YI^qR-6S~zl)>G5J8MH5Q*5LA%4a};#^ zXn-R}z;yK=ej0eBj}$yf@R-sFuNbINe9;x}+OvmPMH=9{>28l%71%vcO%l4-feP=L zxw&gX?YEDrC%cqmiDhac${NELt(em77C#L6jLv&o%5dZ&3Jf(akIrGs`(^I z`Tz-x)u6lN>1iKOc7@i{)A6MIxUuW~K;tWBW~T!+O;;&QnQVT{4okW%I~6i;9te9E zSE3|*ucza4xT*b=NrG(@lf1nAtfQr}$hLT-nbw z|CZFJtudr8gWqR2EjO6cZiZH;>ykBN3cFEc3>0ia$VTV^}d)vMX*-7qqnEMz;g$h z>8}UMqt>sNbgwm-@znOHg9%3`V0J`IERq;FV`TSW#{nz32&FF68$lBrfumNUBdEg4 z2BL^3q!|w4kA#1qI3)TINE5bh+xE&**P}hGW8=T{%Tv5EyB!X2C_3TCbP;78PEEk~ zDzs9+`&h`2AQdF4UkDv^Jyi^DXMIeVs`(i^hu`)0*YvQM|KmrI%J z41NoA(RnvL`^hHk&gxi`Ys2d&%1N=9!0}F zxRRW`N2E{I)x0*X#S*h5Gj+)Xi>mnZ8_5q-30iVc3TcS4)Kz*u%KBbs!)))9Qoc{hSw9ES)kqG^^i`jip^Nk?-X`Cp>gPvuOW4+ZFu4Doy1he(8@(ER_!hEe zNBLcY^R8ciSsm#Q<>e{*FuNeoN02xO+D%;Op>HFFX)yqxFs0*l`8{o=|gig`Bun zyReZx?W9g z;`Sy{`#G<~$xsG*?M<5ZO}TxhUv`aGkB{3`?4gw5N~5)(ef&(te*To7ELumt?a#Df z(xuz@^TcS+E@fHOl9o$UHE#AyWvezrVZD_qb-~jTUcR1g>b2$4c86U@xd#(%{bq*g zqPP9hJ)SAK-17MH9q*ryhU?lbn^y%ji)Py|a%%}RR)jiQmtJV0*~FLN9buqn#b(CL zSiHd4Y%%;N7gAf@aHo^M_^fw)l7*7iet4wZx%?vVUEqNLMXB#1Uu&J1Ncy@s{ z6g#+$X;K`PTnK4|kR1<&*pOfo4AAao(s`45>{oP9DpRtO7D%o1cV7%55gJh z%a@xrdCu!`@12ih3tAr^?^hSU;HSB!C(HHqp7{G)ckxOArR^2yQYhqoI2`10d8uH? z!9S>Ho1;0|6z5lEyNSQ(r>5h$Zf`_EkV@*ojx5H~m38Mx{mNSIq~^C_)wRiV7NNz@ z<<@8~f9g`1HSXEEKmRf1ditX^m9<}?n>cjK)ShQo*Na7H*5$|EKW#KKArS1hmLBk> zox17wNapc=!fKU$)%GKqprD{WU4PYYSR2vra)d|-+LPatB(gg{`?uhwPcJMOfOF?S zFohE}uXaw@EjL`L{J&w;F{H`i`6}^8yg+WuvnW)ACtAeyA+}2j?E24d7b)_b<%#<_*1w9{n59^pY9d5Vg53`wc z$a?+xSxZE?@4U@1n(bLR&SS=o-ZUY@5i;d3A|x1uy4CS7LRo=l5iLA=TR3hUsasCT zK&wX(GkkqFOV^(J4W+)X8LKqkO^pSCfD4Jz6#t2&@4*@{qM$NLt*QUb-+g5A4g~7IaO&ugyJJ2nWG>U|?!W z4ra5Q>dI@k^P|4~ofRukz*O)N7|FB4b+89|cxUz3ae7Z&P23t_MnGuxK<950<9w{s zq$2u7#IWVC;|3@!Dyp9{kM$M1`8+30+!WfN{EkvpL*pO`DJiMQ!YADylRfJFHsX(* zhw(Sj2cvOu9C|8+eP_<+v?*#4iF<*{^q9rx*o8Ulz!h{gtEHJxBP!dw(0G~KDjA>Te1NL+=j|q{L#%u zgp6*l)=t9q5*R6}L^R5CrQ5kbhIZrzPxcLK%3>={>gNl^DZ$!of#iYGN~zn^Aj%48 zAdx|j1_C4i>bBtn!Qzi*|1Z8R7><>7_4Px5jxot)ga$lEEt2yuVo$vcIvF7n!Vvg9 zeDpEoJ^o@8Uz!OV63i1KTxJQxIMyiQn?aUZ^Xs;gsd1%opi(F&SF$ZXXuA$Lg@|31 zl^Z@_Ab~3MEC$V`WANI)jm{Ji2-Z*~bmj%;km%5Hg3Nl<5^OUDeYm`h3ZY!3RevGL`5vbz`D!i{RL6t%!&{mhN8Vgs{S%zYXSv5{2 zwyQ%v&up(;zI?fpBP}ZGu#nyN>T7bM1O^8O00OF4%*4OOcm$J~c#O>;-FmB*+>&nM z4u|B7fuy%9@E9S%P+XCX8u;F|n5`TCUFULOKVXRwq(FENwqhJDF$b>-!XudQ(FF4I z+I8ui?6ZKd%h@Jshp*S3kM5VZvZD4eVCL-KdVrt)mzmQGrRIczM$qeD-(pOIFY=d> zkxCpwU?eZ0hakQmD*?XYc;fwqoN*OmmV7KP^ehU^=Czcc(nYiD_e`?#Zn<5;;ggPy zN=VqcWy==A3cg}i`j4{0k_HrPM4j7R@0zpCiCclGxYsZqCKCoxke|1}ZWdwRsQt}a zJBzWg@zCd6IVy9-AM=yiE_B|444`Me>%6!3z>=K2+?hv_7meDp9|vCAve=EuMIHHH z7#ove!dxO_<6{!Molwzk8_@$vX5uaU%U(u)%&uM&GHdSxOF zv(%>Yb|jg3JXFpY>KfS}eEDT^i}q^;blIJIs=-Dk{Q#e5l+!YBO08YpgQ_zUV`IEO zgwEsdI+SGhff9p$Q`*jMfism((CP>RN@|>_OFvH==*4cp@!U%||-66fz^8{jcXc?Z@AL_WNn0 z@AjZj^xnJqFWDaWdVzsAksOJ*e*5#u9h=V=Mkvk7NbLK^1vu``%FWNe&%fpZ7VCie zWAOeiDYCRB;q(lPZcG_mtjgwg?%auBJsAKm2-bk6Do8pIQwueoB{Sba7IYlemxT!I3b_YGht+DmULSaAe zJdJg+H66yi)b<}VEXY*44OwNYk6PCB>_2Jy;hJ&Mi@?ujc8$h@qxQ8$;r??q^#|$; zHLh9-T)%KmqJm7@Vk0%#UTxLU#VtZ6p6ooV9dnU$MnTJW&eQ90O)cj=>al*l!LQNA zy#J0|pI?RnZ^#?6G(fB0+LEMDghNMwAzc?i8~E>fmI*cTe_2;oDBgoo2FEG+O(P}%XpeS)abyQ7gl_ewBvayE>}j!_;weHIfBG$O>4 zhZjJv!F9F_|C*Y&tfSYoI~+fC`C9~gKJysmpBwAb?9ZH+;-T3qHX`5Vt!-X!xb|>k zf=a3RIL)1%Q8n(jf2=!;B=wm&)maynj#^ylY%D@^O%ZnI6%nQNYx2#we3k6vGP ze|kp1OGn#0TW9(|mY;faG%-c{s*824XI!;K)5Uh?beL-;Vxda*0;w?Oc$j&$pSk&t z!2xsm=RjuMqgbH};~{kG_wV1PPZZBTQ&3e?Gsf+vV3e-8h$H6W0}O;m zIf|ziY=lLmFvyHcg;WV)m*1@WA|CkCbQz^F+zDhe6m7;ptJEmE>`HgzK(e`cQ$$@b zVCHy`e%u_3SOp&BmTl%0h&vE1rdE=4@62iaipAuy0J*9YQ35U5aa zMkyyHBK0jj*exz%g$j(=8-S@g#wOoBChCq~q;790b2j6mT^R51Bc`~hUr{NlCAU{z zGXM9G_Q3rMR%g)2^M^M&4A)X)nnciIz^%p5Q1%yX&;21&mA!TOj%&6Zi?S${P)E2q zwb!@z3HT3xk(fGrN+9xvb%M|nbNl#d?m*#5GkL}8^XX;O_5wK$I((+g1|MFH`ovB9 zCf^TPZtGFz$(+y5(z{;pRje19&P-E&w&%Tk*}ECvdCX-NCK|d*dQ3g~V#V%`y|FD$ z6th?Z{4wQ5C>T9a%|Ta?)jN&}Vmwc1@6bQOb|?bF)UG2{(bamq z3LPb+Ds&X5EwVDBQ=*yUx=uD{KLO*Ct8qc|CT=lKI>#!kYE`sx2Yp@gFAOv$>U)+r(Q14y;9YeACDT5X8#a0?H=IQ z(l}mQU;lRBd(?8i{{B=5s-TC}@@!)&ckP5Ltyh$Mv?RrWi`F!R)j#IT_Vm4kI(8GW#b3o84M%GFr#{yoy1muNQNf^Wpe)ek7*UzY^X43j& zdxY)*-pLH5kVW57ThG2*YYCP>u=P!jfl@bpsg%k z=Zsu$Smb6YS_{icamZ4>SAj8d$?t3Pdsaq&7?evv{f?K59YDUkt(=WZ{%faINv% zlhsZU{4F>N$y(j&&e@HbDMVk~g+KP-$7fvfmAZ~DagvrF9s@R}$vadAn6nnE0ZU2s_c?^^W~!P>o>wz z9L!e@G`YIUG>>8^nU+PVia(1W$!>xRR#^xm}IVwnE!3E|c#Bau!gJ2hf z$Kls7&G0uDk|2s5L6wP(Qy@VkfcBMZc-nFH2>LQ4No-CA<0MViN@Qn>mh59L)XA%#kQ@bMEKUBP83 z*P37I9`YC7JnPc;&BpDU)-Cm1kM1uoyRM|XEu%7zq#b&|c&y)o%OSAOrV*SBb-8=7 zn13fJb3%Fu+1yF<-hytctxODrP8Af>C+LMl_f>>v%;xNzBK8~Lg|lUW&bwElA5#v9 z$_aCw(4n8Oru&HkHc=&&!MtE*Ga`5PCEu!w`H`h6xyX?VR=n`M1iw)z`*xk0l$2(| zRV%5KLWp^|*$zX>g}Z+>R+<`6!7Eqi&qBgI=}hZQor2}VLl$cT5}?FSn|$25g@y*y z77LjZI~0FX?7xsDA@k|Yo1vBWhfD{TO6wb3er4ZOHYA(7rbFUmmMP~WbSklbtKr~a zO+}YeMf&tNgFCA47;~Dl&bUp{Ja#O*xO%C0#%<7CbT!t>{451awPit8ovVUxNOx$) z<{b=|Z%8c3DBt}rx{qhAVXKSguK>aGFM@s+=BlWqXrv`_&K&V0bz%U`AmCWO#eUa;l?~2+?56Q_Nea;-Mtas9}Xot0&l0Gsl zFffv>RE8E`(f(PUS-_h|WfqeDW_iK-&SfjZFWm=ruxY=Dam_aTdSh;j@(OQg@Vc>f zSR{*w@rgGpCTCc>$i6TzUTMyn(_Y|oNpR!Zw>rL&Co-hO$cKiZ?TZeUKo&*RMTl@~ zwr>8iVXt7$phA_$kMb;+MLFhR*KXr}#*6u+)<0P-=($tFN~bN4*MC@Qjfo3sU2dP9 zDB$jNisOrP9de9Z4>F?$xdv2ZjCeN_iEd@W+^wvsiPF$;^r0Gr&o6)GWof3eoH`YV z#&`FwT{n?juIlgaWiB=*>ei^1n!3943VR{9C~1`wlf8KHku14<2a7Vlaf`csuOrA? zLIKVXs@iHySs-q*X*)?!734 zpvmbMdD`+n$ZuDM=NER~cCZ~&{%&wZJHm4Fx$WMJsmjGvOj<5=G_5j|*GE-_pEs@3 z+#ee@U+SOOy+U$YabA2m;^~wVM^wja&Y{u0#qF;u&z{lB+~4*#cV+q<&-U$RZ>|;F z9-89#^=vUBNg>RW-zc6e(;}+WtFz>dZTs}1=sg>wsfwXk^+aA%p}Byu9qKNwWYn_G zkH5&QW?!DLTkl};J1D608>f)~d`3)WaawtKK(TsFfItL!6J;r8xD1%MrQ61kk0N~o znUFqFP}KUUl2d)SFimu`YD5Tfa=O*3;TXhNYc>*+TeY{t`x03KO$11j0Py0w z`Ti_buf`ylQ1sy=FXtLMgy99Az?$)cocm_IynZ^4T^+Z^H*pX!F3oR~nXGu%7$&2r z$p8vTdlj$=q?`izEOJh1)~Jbwx>1_U5KykUGz*MeUCJNrJ}oGy&9gs^yWcCeBJ5sG zfH>DFJNaA(rk1RVmh$w9JNf5-+M3! zj$$PT9x1Z?J+olCbo)DBMW)-xav1gmvMB+N#ZrUFceXLSM_CAqimeL9fJq(zF@cP! z5mOByqVwG6O5%fY=?E zTtm#mfE+);fD;6brYB72YemoNTNXOrkuqfYEG(QZD*W*C?ayDvhAz-cRpnoCzco6v zHRqYC{9UO`gO3^6@gdR@_M=@5Y6Y)S3Pq`sa%QhBFLc{bJ!QCZ<-+i&KM-()!|5MC z-n{ksGBGg`0v;o+&t{d5XAk$o>7z_7&*nIIr*Q}sL~Y%X>b5AL#2e`7KzQZi&CIBJ z`}jztcKC;fHvrWjOulMu*AYU2Ac0Me*;8pnBnCjUtV_)u$W_1O4qqtC( zzq?UB)9&_&>%svuv!mT&P~FeSL$Ko!#Y(VhAb|!s^A>`o?~bYC$Nqk*9l>4FOeM68 z{KoQ;SbB$Y8^<1(0cE-^2s<4+0y}H2#eY>4kN!2={;}lVmBuS!9IQtcY6GiY?+OYH zl>uN*Fk43&rdeKpTPgFc(u zF$jed0!m$>;NqV}^Vsm&aIAxuJs;Mi^^T4X36oP!gJrCnc8&~Fg+w;Vt5DCIvz3bu zU+sP)mRgOiXyDV6$u(^;7S*!+-jh%4PWj^=0cHA!Bo%Iqrj?R<*8`!_ci0aM16+2@ zE^&*AolLmeuV7RGnZCY5Q{>b=+RQ(~CUMLgCMPL*!ly{FDGkR4Y_=G{v_@H< ztG@015IykgNX;+bqjIhsoGOrNhX>2q+&R;^Z+qMK?>X?y%HP+}G57p(oK(hsLWhhd zkx-_?VjiPC9*~`&hi0frmD)Pv@CihHP&6nhDItqxj=#D-G}146m9-%7@8hZ#w*9u( zsW1ZF@(CO$%nsF3mQyuypTT?STJarM6!4PZsnprJS%M*5TGF`FkGJTUV}(xt+?ynl zx7Ic>{+U#q^fGI0pUZj~wg?L&c+mR9UCs;)LV>F)cNco{!I6;%YRQ#IxENTf6S%St z+E2uNfSZKVGeILpy!$1G%A6<9dJ*W$I^gXS$) z#Wih%LqY<6t4m8O$tKic=4>umxn-Bqx@OX{M~!rJ67|_-!QXAab`wv9fE-~r`?A>x zEy0UqOm{btfc-W3L!GXOJ*^?@;%9#l8ykC_u0QnhF-**n za(^x@T^clIy5R8^tUbg^+zG<|1he5zp{pKGp`$Ii`J`Wz^55|~qEYCC*n z=0ELgl(;%Y3 zb&wjS;PQLPgoTBB9R?}#?57CuM-o}ZwEYdCq8L7NZrRdWu7W37S-oBGH=Io5I% zPY{hE9DxG6rog2?4j>hpL~qGxCWz@VsST{_9Vh|S#NTdg5BA+m{>;1Kxb(fLDL%=c zN)&p4T!!lmBHt%G^KCpa)_bj(c$LjmGtZ{RX#>ZJ6T4|>XpjSd;NWQm*D~OHG5|`0 zdLCP-fO=mRd*yIc!5s9oytMf4*r$p;@$b!7WKnP6qDD2wO`}VcSLI} z+$g>yia9727GvW~Ek+mDxt6^i+OK}=j|?3(ROe949s8_jPGh(^VN!#KU8Z9$=6ccr=R=tmxtWjfUP{ zk5E^iy>n(FENs8awV83vU!H+S{Y)iZ!((I%+XI+!U-!O=1?K;N%Vwin{I6i`%utD^ z)jU4`@y&IAtk_ez@>Y|%%(*s**wafKe8N>6xB!|4aKZkAmk$n62a6K?M|fR-+2A@| z#Up9a=M4c&HyHo@U?W^wbPOM+Ml;hIvZzbtOV|nvcTZ({uE7K++4Vh~UgThPNm#t& zMIqz_UB?<>R#UI2G~?D|czrmjxPHB>vVOUt4;Nv`ef_~=!0tb1ymSN&-@PbS#Z=93 zbSPMgtUlrl{)jHm{)-^$L%R&t%=cR^CKp9cU(-2kclK#T>7hV!Ekl;tRL-^4M03N3 z$4|`oreaQh2=){xApW9$=v)ZzE{sTnu3g2~p~FxQB?~sP5leJ7Zr+iGTc?fdi37{A z&l|Os?w`QmF1v~e0O9?GVhS`M$TjuzA}e%Cj80~NCIM!0C0eVP_&(;ZsJ;NigyF|s z19YOOtUl}(I7r9I>Io*@d0`=TsYsqC4`kf7Dm9$0n>f3LFmG-JdD=uWKitGVqE;O z$B%a)*up4}qjmMd7_qAoYU6m+7N}+)nzX&{6l&R+Yc=dOzv&a!@mK;R-1i;{L61nn z3cnYZ-dS5RM0E4G5+4l%_;eFaO1;}8|_Gl0$R+O*X-^CuzcMDPj^qk2N{B7rx`-9J^G;i%W=5iMNq zE0U6%Q5(UX1&^*8v-#WfH6sWiupl{d;>1(;l0i z37O2s%|t0=K~O79p9||H+XIZJsik8{i6JT+m(P2yj<9eK+heA zf(bK$qRWfI)+0NBC17$lP1Ja8CW$zXI3o;=@M&4X8V#h$XuJzTR*Kf^%(L}g=vhn) zH#;JHq4iJhID}5+cU`2w6~uL;7Bf_Wiv&ig7u0kt&F9i75%b<@j_2;F#LG0pu61{2t^+eMGK* zj7d~;$_-&$ER=)#?>aEaKv9kn4M1T=poBzWoVi+lzpKz$00#{PV|lv*gBmok@a{mN zxZwl-Z&OB|KxwZX@2m_$BibxS_C(s-&c(d4XZhGukf1VLgPHyj|)OdM(fM-stP+@CQ2R})lPGn8Qd2{p88-;^MjAL>Zl9@XRV_f8mPo15_DnR1G{++;4-iG{=FdHKFU_gL@7g?(p3BnYrK*D7R z+p0$qx=|qKUtZWl*#z@w0P`(ie4&!m89%+q!NO9dMj~wUbr){t3lP$pp_`$Lr&^S( zc#^xU$~)gKuF_ngS`-;K>>~m0LtMs;%*GnsxS7$VcR_Cy2PhdvG@+V3|ezEkO&Q-AB^6&2BjupXMaW}Aebw^Z~6lT)n9 z3sYKi=QS*u>PH1di#a=*%XgEfni)uDpIH%R;5wF~>smi*GnYSKXe)Xw{qUHnlV+WA zz`mxuqgvCwQsm36U%oDM7W|B&q`SlLpkB$<2JU>@BovH3yd))7N5R7t7mN>+%?hKF zYB+scwhaD1T)lTZ*ZuxKURS#`NkvGaAz2}_Wfb9RkSKd)W$)1-Ar#3XRAD=@~SI(>0M31z{JywAI=Uxt(CN zpIWdlu{?G{ZeiAz>EW>(^?g^n^Fq`5_VE3txVjXfqp;KUk3?Ro{obt>!pds5=c&A8 z8YZT~9O}aq!@neq4NoNO`(dGRXUo3%@zJB%v$v1T)ffqlv~%1oU#gvSOYLV{m;CPh zmH;86^-}8G9t_6Usx3OADroISbEt+LN;sFCWS<5-J?q)d`8vca=eQTB5<~0%*shR@x?H(yk>q!S z`@4gZa!o#J_d7=)3U9`qco&rqF9ndk7}5Qd;?lzS4)~8&#YMRq#S6zid3H`RuFg(k zrWBO0%Jin_jTYBduRo#PqAYxME=prh%6(P#f9r00rA_l+*>$D-_$om^L1k&-=-#1= z7u$S>jKv~M*H4svxuDrUq-L)pB(ZQ&FUK4R>><)1p+>{5wvYL09CvS-zV zl$@GM1>5XL9agKEq2oGxnF_>}$`zb70f$M&E7xgEi}$^pc=UG?IwGZ}#)z7UP!K`F z!x4Pqgcdi`vZE6aa6i9dX!Ie4J`*^^H1Zv6T+A54Fdar{v(MeTcO`li%TVCH^BnW_ z^@Sbp`S5C#KtGy?HR8)&6n*mUqqK>uw#$iH_-Q;$SsnOFd$Q?ySIsqt$fc5?yjYXf zndP;WV{LqKa$U%t9}EjGw&%8c@6)Xxyz1B@h2Up9p0Iw#!uUoUr_APx%Be$qq&$pv160d=Of;gP`q&WKDhi6Kf?PmjekpKv5HM816TV+97w^!Efw*TK!J0cZagA`S#+`wG))LAed2!#}x0s}t`?i){q9C6UD zcj%PuTgMktdxXPm${ZDj`%-nB@`0M(^_L%X#XcHrFkqoHP8=1@-g!4K*-wRE+`<$}aJa&JEXRz_r zn$WP%W1Y#FsR=pfX_LbjW{mrIPwc-{I-4%5`~B#HvbZb`6{WoIKcbT}r+Nfy847|k z>5tp+4ZM_fy>Y@yQCl8rn!N36+)U{ao z%KCB9eV@3BDeZzwhvn$aop_yZO!jijC|2E(>QwXEU$r4LU&L*lz-h;vXs_o+rmH5( zGpz%bO@iC8?(Kj}q3KI)fs=8@2ms(KFxp~sz<+dcVcW&2PWUO`GsB%S^ zTIM}%*dY3T@)pxlNL;U@qMKjljWkx)9V8DZF+8_)8X7KC4 zg8^l;3T&?ce+MnAV9MCLln9F`&kU-_8T_ctXS2Gmj!dBt5Y%d}_N zDgfs&3vfV;m^%h+M=R3|hjEDTj(6m)6m;j-uR+GOgN`_dlAS&=Iodwew&-;iSj@ z#?*XWqLFRUrT9N3{P7-}K8PM_b;PyvPpnuxrScUqRj+;UI8U*NT z(;eFqJGm=@c<(N?WH?uRytb&ou%@c5rN8ZCtFfa(Cc}~3lBTAm>U8y6O<<8@%MlSO z3Tc)jN6N5%x-g_cr_c@QhyVYcPFE_-Z_6P^!PHz0;~XthIK7N_10@1=LlPcjU-eS_ zY(=x!FhvO*sJl~<7ps_Z3}zdlxLve(_r6a4(bosx5ybZ$0*z!%C{P8 b(9@yOF1 zpRjMg+3dF6s`u&sasH&-{a+^1JQ4(&_)iv=mTu~rC}+R4%pgi$a9UCxZV6MT)vh>1 zFY`~SPZWDI|4G;nZVH<`dPK78>%B|5PT|(n4D@#1Q6DRg)2VoKdc>=`3bM0vaynIb zT9|Tim*p&OpH{e;MD-@oNO9x$-D!j2%AYSj-lsb^nvgDsuZNziqa#1~PVPnc4;#>x zvk))e54*znH+6CAvzOI&ecwLaw!PrEd^ zt{PWju%IU0?8EJjr))Jk1*tz=)180yrX*~urcXgtl6i7GuRkN5o{xnl|Eoof!4whg zV~SQwgVCQ~#JU+Mr}}L1I&&}3BT7Rd&tP?IxQCR9*9nnJGY$J~V-_dm)y{)ch6=A5 zu7*Ha-rh)xjAQc40<{}33+$=P2kW=S$+H&P(fFOo|1DfJ+|%2e2+MTGr3EWk+O2}fJ?ib-vXb(V6R^p~ya?b-1_E>d z*ATrT@Ck@s4e4Zs4#xbp_IA3YT#&THFZrd1ZGO`8j*WIVRnV#w35Q~d(_&*Kvu69h z3x&jsc=Xp_DQ;@q==J7axRms4-u|0LE*z^$S$t6+}gg)2l6_; za5K|-&G}fJFSI#4#I=X=`owI$U-n{D!^we@4;t<-DIO`4RH>3^{+)KKA=>hA`-~;C z6T=a)LH90zk*K8s!tE2bJqejY3L0mOmi+#b1XkY#TML_*SV4ryNZ@}-SuFfbV1hF@ z5R@NCvE96_(^IFud$relwuqS zaTghN`sB>RbR%GGX=;Ubs>qXKM(Gb$V_kKAO3}X;&3ljLnG{idroK9E7c{l^%P$+1 zamh0_drYpG2PfV8F09Qx$snG}d}!yQ-L6$5(*NugoV?mAb6F{hOQ6>JVbyg9Z<{U( zYH=OY-|9kH3AP+#VoZ}Ibn5Q}?wg&-ZMt;g*{RCv_SakQ#65mfP}-2Y=QiI)Fh=CM&cy!u^(*hD1yH;A zuMVnk>_-Kxp{{f~@WZQe72_-ly8v>veq#yI$E-cZr~4b$l5js3*EE zbAdJ{9?taSR-tSOBpi>uloVgS*hQIBHA=SuQH3oC5LNgux?Z`pgrWe4GhrNq%>Zfu zQeIie%dYz!IYE^D7*2woOoPFeQfX!^XrCyGV7m^!j}hBlxsrAs?=YB(Vcwuy`*tT1 zK$9)@GZq>b)Lcu_{C7dG-8O**eqEZ&&lgj!h^_lXaBb;m$_5mz%(`tpwAwf54T zF#~%yr$saRkrAuhPluh4*a6nW&IN9>apOifaQ%3Cit{F%y$Lg!D_cTg1>kuriVsvs zH)aH|0}pvW+ck3}l;$K2T+skb5M#P4w^XR4q@|hp_@Xpj<~4&W9r%_R*;ZkL7>mb2srtu^+Me5jP+*~N$?N9S>#sA&5VSxA9jS$ zxKSDQM;?OiO7$d7-17XD2tVR9+%~UFu0@%l>Ap{N8&?<0T z?_%VQ`AjtaD>{zdeTyV02IQTf$oQ6`f4$v)QW}dc9Ou>z*?Vw!>FDXf@gjd&Hr~yS z*sO(#ttSkF{)VHcRO(tM?CDKTZk>G$GdfPtZn&zkZ)H9Mk{4dn zkA?aamrE=L1QhEZP>k+OV>Ua3UHz!E68tz|$qrsd)(qe%)e8WXD5dLTrCA4>7 z-abtp>8qdM_t250P+YtYKW_cD{knq+X%@4;ZVw(M3V1;8#jr1apcog41|-$%uw()D z(I;e$ANi#JxqmmeaY;9Of=Q&X&Nd45-LYd66Jdm$@e-y#e`yBd;U8)n_H8U z>Q2+KJo@A~J(Pg|Pc8?fUrMEakU)O^M9-INRt&E^Bz( zpooPX4q;mmxa#MTJjG`&xGYKiLP9SvgTR7G^)mOspg~FYE?^Q0=n#Q9kzpTd+C&tl zk|=mVlpqlY$ zz%ESeGIB~gAT)t-S&P7xjAjT4@W+5VLkG%UXea9SBj^obt?m6(R(1x{{h_A9yQ|mW z<8MaC=V_MjB2-TN&c_&xl=MX)`RlyUco8K=0EM%SpaHS|NU1^A6q&O?iXMs;1F#x| z_s7vr6-C9xA9SnLq0VsWZ@9i>%9f}}EmV3bGW^b#cALJCE9JYWsUKGFl~h(fu?o_> z_^(&5x*TC|STY!a5h8}Kc=OHp%0!<-$eW$QXukah2KP+ME+4B!8ruJz;ea8EPRFmg z8MPb0X&5@)erY$tkDQIGdivM3A*v-(-TA4OJD7nISp^}YV2Q_pmpI5%hVA+--9!V= z9Y^6rK{sa#CZ>3I&*$Y|C@${uO3R?OIczf&e*5cbhFKkP*yv$vF)5`QFrKdReEclc z+-BgI(^BcX4a$>eZ%IC4=3g_kzinKvPt0?(;}Kp#o|oZKm8)v?s@+(2YnDa1Q20zS zKfSTa(&*)kwcP=&UvvIGKKH|pV5a#9GbAEnQIa@z9S1IO2f|z-<2i{|-_45}%Rx-j z%h|Zl6l2$H!@-RK()DA5y~~ucOXej*2IeH{ieZYqzci&Y`Ng0>x4V*a*3i2xV){B_U4Cfy?|yNkpn}g+GxcTB^;Z<3C2ijiuKoWiJdVjR z{+xYNfwxdTMn8UL1y2#`B5X?KgeXbBVtc`2R*)QIJ~8)VNr}3ca&^@cB}skvQYB1#fLg+*6T@oOA@j~eTLh0C0@kkC=XOsQrV6oz>5RH5?}k)Q#{=kXZE- zm^9=xyfPNCSa^yFXO%NdT|ikQ#6&D=disc~8+Yzj!JHniAH#efTg?Mv9||I7)e5%h z$RxbD8$P**P^RG!>Oa$rDGpIa`}p{zKWPn_ALC0G@%WaZv9_*N4?e)7dD&Gy%cGH` z@XgpA*Sww>x-)Z$xZLzR9KsXw*2y&}o|)BehUwy)$i9CgBKL?4L*EtL5WGAm)@#8r#b}w4@%6QY6OoaTWL$?atR6CERzbr+Hh`%hCr{ZW=#_RJ-6C!utH{}Y=z0%yjKihh; zVRy2^>+WH(xw7L&9NQyu9|nFpCbI3F)91O(?BX0wy(fG3G4box0b7iOlyhD`+@tMS+}coE9~kwgS(`9QZT$ zwWVY_scD~58MyyPHNtK1d;5C26#EI4CA$kpLes6HP;DTX6;&C^pf{3c2lY^9V^=`_ z!;GssI(q-oDB)w#GyNUF2n&OIv<(2(Q9lblM7;rLt*rFK%@Byu*Yi)AesY+);z-+q z;whd(y4TFq6uNEP8mmL)WB2jsQDF#{WFnx(O4L}&1_#)qWTKVv$_m~3PFRTjy(!wTN z*8YbJFxIqX@kOQEnN7ECOD*C>DDumnO7%*iDgO?o~6 z28DgaK2CM#-ye?2#gNQ4+Q>{bls(RH#zLqevi-1^R>qWXuSB?m6HE(9FzN<4!Cd-O zt@FOf`B{tWgZpg)@h64gC+AoWXc<!*$BWk+vO>~D-J zdgX9z@Mr{u*Q;OMc{>L;8rj5IpS)QpmG_c~)j`-#%DTPEaEd(I?{jDFCcq#Ds$KUS<&V{PYP!uZZ2-`=t9 zlndrX#$B}Q^_8^>U9QzT&M*r;$cxn3v@SdBW1;=;-kV|H3fc=E?CHJJleNb~)_3L``{+PoY~kWu;VHjIpSTKFy{xk6$*icD zY@OamsnuJNQDvjoeAS?g!+6X|rH6s>uuXX2@R3+qhk~fn%pY&weAO{(61;I7MkiOt zW1scL&4tc@H3z#7Vpz8Q^!)oux+K0mxF;*5eF%d@RPf1iU3e48nJ{^Kj=dhF*#>Ng zSU3{%^3Uvf$Lr2Y!&Dck0f!DAypIq-NLFsK@6$#zdAnq-f{6(y@ghZtsGp=uaBy%$ zAtg1jR7Oh(){V9>wGE&ADbw4ZYhIYoQUdP7{BdBwpiJo3Ny|MteD+nLx!UlYx)ls1)z~$h)}lu5X*UVA6z-5; z5^-MgnBTA`W^2)%_KUrD5852w4S}lqtIljQR4C1dMWw&aK2pj(#1`_rgSEzmf^9h)1|oHv79ZP)!_5J z>yUkY-1MiDms~Rvk=%RP4Xg*L%ot0BmbVuPWZ2K%UG*s|mg84O2#<88qOORQ43AAYdZ}7Z8y(B5q zsxS~?Q|5;L18hcw$(Z$4oQ;#$&|r#tOv{SUkA&P=AWoZMvilKERxL%rux_#M<}3ql z@**Sz-5#unxHZe3%&{M!--st}Kk4a3+YpjI6Zgkktlg9DeENH*bt<(E_g>p`*tFC& zj$e4tP=7@{fl*>3#S1vi3e$YZWXkOx^ylK=y6@u-!LrzgiSH;TVha}3g~r$My|OZ4 zsC@rwfzy(Pi-tik{ok!(<>Q)ig@v9oYkR&qoz%F)+VR2k*T>lH|E^7`dzjU3C1qH7 zQN(WK{I+LxPSU=P5o5te%Zpyc{gdBryYYMu-M}qw%N~Zkc1t^siUb>6I`^A~CrH9^ zD)ZsY*4M02M^>9wy^Og3v~qSV;OSGfp+H_^g$0cTdB^a5e`-b(X1vi~fr<&%FmbN{ z!gWL7jXWa`M13DkBKEBohcp0{3jBF1U})pVdL1hq@-+gB;qJ46kugk4xyv)R@8aX* zBUK}gFE+|(PgVIn+PHs0z34j;ej|l0o-bOtSr@ik# zhg_`xaK+$TuZESkv)hru?? z2AQYY_uWhK9sj8%-M>|mCPlE3-#pJOj%8lE;c`)vaBvfXCBwZ=ZCD7&Jj%y0U(4}5 z^UEez?Zw@iu$)c5IDQ4tiVXTO?cYF65l)ivTB z$m0O4acRfxQrLZpL&Hj9c&ye=An~B{sRT{D>TNbmgG!FQ4+^3|&ql%`;WS1q`_(O# zXfd(Qfyg2_qQLm{ya>>Gcu}ZVWy-VG3DoaePdTTe8OqP*V?o_khn%!v&cDE|GQfT>Y z9jc*B56od@<;yo@t@GQnZ`%6ntqjw3s7KY+yVmeeQR?Z+0xB_RPIki`*f}`( znG7TlX{?m|66X0}_rfr)qr1Bh)R+eCNZM4n%a<>Uii$o74BV8j;VrD)`N#;tOnmb9 zkRRLy|ET-xwgB_XZ)6)d|DH?sxVJs%J;d z{k&|opZR38UOJCLbu-J|G$xq|%QgOWouA(_S{kRi4}7Bda_D*fd!>Igd=za&l(^th>icR)@QGtg7uOi|dLX zWbW7OQNL7Fr|fko=AQP9@`D`XLzHXQf1~t`4xM&d4Bqd-k{eTK_&R@pT~6 zX3q}9BCWjS8)M)t_#_F0$lwWk{C&dW@Q8$M0sIe%r!}B~H0w&t=z(X0Un&+S&y83H zO3~&IIS4v^fwB`!-c&a?54bXcBfEo^Rtm&l^brWqczq=rGJ>3?f4c6;DJgl#L=mwS zYhZV@yJPgN+&@Cxeo{X(O@+uw)Lnmre~IBKZ&Lv9b{`01tM4=$tJU-o^Hbtrk2+ba8O_s=Gs-FaE1 z9cNo!GYZZ9oXe)VanZ44kNteZj$aAhMXQlee%C5DgI^)k%)kG*lI`Sld$aZ`5#^~T zm&-k4be~zJ?U-pgID5;ZGa+_K(qN4nTf*&oLh7`|(RvqVN7f{>zf};fcpBuRD>8dw zy2iyZ8PY#QdBxNO)IZ_CNaX$k#f16}1R0GOY#>L2Fi znJ9XcCl>gudk{kY%kj}zI^tX zZ}7#<9QpC=$h`4SoOyGCdEZa_TW#|*Hobbp(PXLky`$F;mSVHl^Sz2OeolQMT9Ky@ z-LCuWv6yq$AU$Ydoh$|a$FHuv<7Zn{0!+MR^@RQH%d(WzXX?`Sl!?r)u33NQV-~|r zNdc*ss|^gY6=qc!I40@ccv!Qo7o8SA#q;Yo`rWGN<-104Q*iV4zRxeD%R=AK80^QN z{Qipe@EOI~mG?%xOiB@V02s@-XaI&uasiVCbjkT zrd>swpo{}k&NAmt1Y6UV<7lQ~z!Mf89vvUA4MC{T!mk?0tTQbQWAkDr+y$+Ld` zZXn<8@vijp<`NH;pG+pSQ^!W_>196lBo(QwH=wi8?4q3S8kV)OkRP3U_fP71l@8bQ z%Jwt88|EF9j`JVo{9$ZH_sWe?Xlcqddo0OfHg47@fxC9Tw`Soc$Ev!o>>WHJx0W^x zonAc5VCJGoac#afw9;L~d0u~u|E8Ys%A?Posj&R9=5*v%))+n=`N43uQd7aN^R#fA zKqh?TU`7-lAHTigKyZue^nB-bCsiQO&$nF^>UcLWFmOq+e>q6Lgp85h!%!`M#C@oc ztb5s`9#VLSu+8xBVL%9U12pPF(P~SIo}M1i%M-0em0`L-c-fG36tuLosB>y*YLe6H z(j^KUT||6==>kR(V`I8`7b(>CeYc<7ihx8aWtw}P`Ge25MTs4|80s{3cz)n|URSjQ zgJG?(#8OA0v$x~g{WgvcoUho-LgVMT_MD(r>lqSTa`c+MQE;Z(h2n(&gRAW9v(p*h ze@BkL+S_J4Xw|1#YL~S;iAi`iy*lgqqRQesno_ zEGuLDLS6~oaMtA~1umc03;Z7cm=tJ|s*{qE0=yL52w2P@&=pfJ&Hncl$B_6GB6Aq! zRxp#_GADfzvATOH*^wWV(Zv* zXAW9eSP%nUbfq42E=adI2{SVw@?P-T6j9xN&gCu#1A9VnUsHS!MPv?xh-r}V+UwW7 z#SecRDfyJKU0KAFj;7tj9i^;H z(O*|vCh!mw9H3DUY^-QwrfkeJJ^+bdQH~%_xe!1^6$DIdbkBE)`z$NN6ffVpMF-Uu zM3s17K=9f0zPx<;^d9aP%qBrZhiHkdc_@eNC>t9N%;K3*jKi{Lx;&uP?)cd_yL%H4 z4-b9}YNb^quncZ<^k|n-QY>Oi4+*?P)%x_g< z0V+nt* zeysjxCwPKm#8jT7^_+ialCdOh_;}&xdAKWhpjA}FHwzk`{XK|<0Xz>u0ua+E3LHwB z{WJEGuid&ZtWeiEeb2>Z(MFsqblk+*(Aw5^0vZ-B>#+{)1i$5E*~4I%WtjDWvU?xJ z`lfJ0Ck(;(?Poafbv1Q#y8HXff&N+leC>LCm#nsSwEl>OZ^JaGVOg9g^nE z5djDplz_KyAE_)(_=7@Mi~^+&PZ=^H-Rp1e2{<^V<<0Gm(9%bQ9&aW zCRp0)HR}O&0WIe-DH^_t+QwOjHyIZ8p`mv%SBH4Bc_>BZr4X~StQVnbj zPWgM7g{)y`X9s_X%-{hH$5FfPn5NvH9l&rkFHydiHgZ?0|haTULSK&m7KRu+%^J-9q<#gPL3Hk^Au*43rRsqgssg8GF0<2N;X z-rCRWPvkkpO8?#-J7jZL=0xD7z{HAguQ!^gfKb2;fk(mj?+oAoB_$VUjT8J)Q zg+Kajqk_3LCZq3cOqhZ6GX~nw8-_V4M6_L=&5VTnmPX3;V9~8%s5fAP{tZVQhW2#y zHa~XasG}wVhJ=QzH*Z$J6}lKm7b@f(+KkIphA1WIH|Q8a%aT}b59hVxCflsOe?V>} z1d!krX`>or@k$=%cbGki3oxbeSsZpK0zA$WH*`p#Q4Yf}jv@Wfn;qCP_JD!#)dCjY zEd~q&}!91>O zWaJA(cg3&^3d$D8fD#iAd`k}Wu6WG*4jKx=LhT%e$M9PDfq^>c8j}bGO=(`lq(A(_ zc7+=^_Cd)AG7FWTq!{XbCAMvh%|SSp5IFXa+YK)keo2Pt4mf zO`_T5V|&2LCE@C*M3ivO+s5ygaeSKs3oecO@eZa&WwtWBK=1u6_a=0)w8(Lss9aqWS zzhQ%`d>dfm?!R~is!1590ye|kEk*D6x#Yu8J^(BO0qa7wmddB8gpbcf;%FY!U0BDk z1j)lV>H2Z5i@s1*s6941|&*ea$l<3&j123u?I9S4^8M-knpPoF?{ z0;dm_yu}~_24|OmzOWJ84cuhAS>-u&zU;5rpr?j|M+-nE;jBO!jF6gId46$I;P(KO zeSCIdGu_Cx=ES{$n2v^_xh>W0cr{7juE1vL%$I%}1!kAzS=ak|C%QW+#Z)`~x#;Z1 ztA-IQm?7)HcYw!%!)&sVX*y3U(1x_U1vW6G6U{H2Q+Rx6d1j&VSs z4L^nDm|}i`#cJ8FVqpz;w#1{7C(8ifxUbUp0Ng!PxOIL!?ZN?148_1sHmedY1q-*5 z@~z8nsN*58B$9nsSJ$Q~a9fH>N^sm1VoR@yXE>LFKowU6d&vm$pnfB z>(QjfnpwYRDbKDJY6-bYa2hs;3;p_fK92sujc-QUnKM37V6`w$RLrkG@R7`g2vm8tloxLiFp~bd7V#Tg_-0u7 zz2!D|{0yXHkbr-~%E8f$xsLv?93&Em!z&N5YQQGNVOX(`i6;T;iMXraTEIZ_foVB)c3vawHU{ZY7&J%w%3ub)8K? zJBBzf$tJ&Xsu71>g0po}wiW{p_hj0V`fHiLTYEov!0j-b&TSkjh(e^~7(24Ox8&|( zyrcoL+&iT-FZ9Jppf|6l*sI$AN+Fu8K{T7L%qJyyS!6w8E7hmu%TFm$%q7hf%CwqV)+eKFl+Vxe|*4AGUhO$M1$+^d%Jv)#DIN~~mX4lxtDh&r) z%~P_QHgWZ#v=wzV!}JrgRTQg4_~2>85{!OML18ausVkzT%JbWWHQQCCiMK@I8Ep+xi6clpTg)zQD80O@d5nBfQ~S2#2Jn{9;K^q zhJ$q#reyd@84>4qg`I!0+}_cB!Ufd`J|qM_OfIFb2*}sA`6WhdmwgiGh2f1F4K7(f`9hV--#d zSbt`_FmZVf4Cwu`b#R6KuBg(XWt4#K98eGUFk}2WD10R@T(}TEf`jQCPL+UwGE~%f zB_%-pDyma2nV5{zmF__8O!A}20bWiIc(X)&1}00ZRB>Vg1x!u7auZ469}{tfoSq+B zgOZ5EbVGlQVs2O3_R$-u2K!)Nn2b8f{M7QvRy{(9+Zaz88nU5)uDH| z+H`yJJ4+Opbvt(!KoMj!H=IT*Oc8n%?8Cd?W?^R%$1K>pAHqcj)>zy0OB^@&-Z+n8&hc74_Cr#V zzKd>fJW3jzIv7Kgm6e4;B!Pd*X)~eeEB)jXAQJg|K*fo|9Q%YQn*o>{vLln{NaJO% zx!2Hf=$;JPW_EL_e(WV22Pt7`fzx4m4fv8W?2+YBI8);Z0LAp!!>$cSLeI=QpmCBI zFjV9LBzkTut#)fQYl>{_{wInIp0(shg)((%nV-=d0y-NYZTJ$Ah$Xm!wNAf;)fr@yH5;+C`|EKFvwX?oC!QD$E4+M6DP-jZ^-Du z{R*+gy}#u+dHKUI2--0mvV2G-v!X^L9R!5X_WHc3dLxX_>=eh4Np~nU^qbRun(K^m~ zV76Vu!-n|n#{~sp2PL7devm|BA-t%P9Pr^!FaHawP_qk8KO9PcYVoA853Ii10mK9d zdJ{BRe1Rm@g8TB1lE+c_lmfSeQNTYi3i3w(8gOXmb3hjc&adFD3PEK6m)2%<6wrRY zEgsy0MbN8>40|dUXwZn}0X+jR9^1CqVv;?HcS@p#BK(#0CUIB(e0zw5CxunEByu@z(_+AW@4U&!x~-@AHp0|={J=`(%zKodf1lW8 zw3!$OqK&n#8l0S%zzZRPwJ;ahfi0AvoTGTOIg2<-q0dKh>8`)-kG}8b5X(&yn~wqu zLT(P0d=!O-KEmWstS9kp;4HzxeHj!j-Yn%>t3IrTrMdmsa;c(gM(8uR35ewmj;=hX zB|GIj+(tMV5UGM7Yi^r~AN8Al(PIYL1mZZvN$?NlMDIjh>K7cWKePrb;sMIV4?7(t zbdMYd&Az}KMFFt*u&{q1)I=m%#Cg4^Qykcb8~*Nxeb~l8&#;+FQ1=jZHul3ZnZOHv z{=mK672|S8&CBbFO8{nXki6+t({|uAp=Hen%W?JN-KwM_l}pEZb{1rIs4OcA;#fjDOiQ7eT9it1KI1!rc*cv-VGKEzUyCJ&2hRb9`p934m9$`%G$wK zzHG)tod)(Df&a0ek#$rjY^RJkaU!~tlG1>D2K|PDX00uLIo|1h6b5(U!I64uAEDjh zdAoUf;*YNdtDHy%0n`8;j@(Fe9q-ej;)Ca$iK%JV|9WvYRMkWTfD;^3mDE5q0gtkL zm+xI_UqqgU9l?Z%H|B!@-Z$n0L}~>8Kj0jSN59Rs+l>mNs5dq>RVQJ57~{e4l1D!- zJ6i}9C|Tl=Gr{Hsrw>*hr3CK1a|-o1ohzY#BN8MGx6{+p6$IXBXQ3iSvK;V8i$vzaJAEd4EN5ZwrF77Rx*>DwFJndVorL>5={ za7!A%1r$QCMClkD)bD>1G(6QQj96KwQ>PRhA8x7Mc5xX=k7L0Df&fukS_>NmffZx% z4JAcIc-tAazmLJ03HL;r;tMP*9bkfZ6GT1-gWJZTL#_Bno|{N^9varq*vPz(RJWjciq7I>$fh42$}1=;fxpQ&k4bn5ai<670n&@cl*$AyS^LyWC; zA2jUVvj^UEWDrM6lsNe`#cxM2Q36~Nu)+haM2h67U|k|3=GmxmpqV2cpe38%Pt6;q z6Y`w`d{5CmV+Lh2)ucmohS+DRn6%?x+>oMZ2nhe&#+nrxP*MzfJm|yF=L4faZ45ah zueBkz7B0NzW<|_7Q040O=&C`?jIba;^i?cv*KgdojngC`U}HYeK1_!sBCZ57!4`W* zm^6{10hJ>YI{YLKRdS@HreaF9QB86At*h~(j=?5JuWQpAs6!5nu}nx{$<)Ac{uF{6 zQxv5Ek5y7%#~1^r7b6N?7>A~b>0=q9%{UK`3WFH{;Q&sWC!LRz&yB{JIs+$5Y(#K& z1Bv1O^KH>h&CO^)oNuqZcI^*leKNoaT;XH_{}K@t>)C@Au*!z7D|AJFwCSV_h+6=O zqw?C%|4%+*wvVv0-!biO0bHZO@SX_DVCIF3*cERb_uT&1%Wqu~sYJ-Kk(Vz4{yJzW zc7Y3v#$7#E7SZ!C61)~GEqsGSTQ+0%0xN{JGMG&%egQG~|Az;&iEqt*XrwT!=u-KsO%g~N9K?9_* zTLC-;GQwiX2aihon>G2^Da;4A#mRf5r10PbLxRc|4DCOoLB?F3n)GtI4&S(u110K# zXmbF|pziA+-SlnUfM9%dZ0va)Dq#*DkN%d0-^~dw-RpLUPNV3?1H*5X&X-NWa*V7^*BBY zW1o8%oC5%~ElrCiuys_;&XxHu&Nl4K0L@X1qX?a3ej4lr14BdFy6@qssr4Y=q99m> zHEyBcm)Hv|EYh3gvCECH!^N3~@(O^?pl}|h*}x?0>+5l!qbP3g5*{2FP(Z312%E%C2x{){Y3{NA zXM!B}VS6KBC#C(q&E7A@5J?eu7&f_~th!=pPBCm)R3W=Gh<^0rUD zS}MabmcX6|vPFRFO*@kwJw{(S$^crbvb&2f6_k{MHD6)N<2b?*B=o!XD)wbs`y_&< zp?=@Vz#xl70FqVZ%VTm^uABoHHYg0rO6}XHE z3F88nv^1-nJ}aPx@ZtpCglC4ibx;ZjDJ)3cIE0p^LD|j|5_O(|Tr6i&{&;!y0;>Sy zg@#f!KE5~BXJr|oO2O~Mor&Iy%VOY~@921O-lCY1j7(0cxHrQcpT)4={n*Ms6_%Tw zVNv-hvBjzFY;)V`Kem4vdAV?+b@rF|pVIF?>+2$J*ugIat;*jaCH_i$vWC5gurM7c zgn#F@(Ar5zl(Gz8zIv5hS>V;c_XmKvIF6O8k(bb{bjD9ZhQHVQY;SKbapOcWh1mk{ z;BV5EY|s23n*&`*F|L~*c3|dAA|sLecdGkvSEhhzmPL2o(o`CCoS_#fA-$w%er|!n z%*cv~xr{g9+=BW2zc+5U!GI9Ho6onUNiP>I$tHY8dllv?ZKp)( zJ{GjcU!DdZGjTC%^fLRc$NC8`J=^smX?v%V_Y$hmNpScuE5B_|+-V>|fa*Trw37%5 z-s(ZDWg;mj9su_5u)6GFFqxj-nq$ib4HSk42tRIY&c_?URg_<}<~WeL+DKsHUwY z0E=Ihio*r^J5$ELs)s9z7^M?VQaEg5Y_uz46=HkW;?|?KR5d@GP zsKTcq95kwh&pxAbdK%=U@8RbIe9lWeyz{jyJ=fezz~;aH+;GX7lQrCnI*p(!;q?S2 zgb^7yN0qqqMD6fcFQQJ%z>!Iy+0wXo@6wf~V^Q&f{SNA2IPJI}fGdEF8;P11!}H|eUO1WU5R3ceEct35PrhD*{niEk>0zt(gO*{>R{`~NbOSQ6;r=~wW z_tj{Pc}fa1D{Jg!{dxGRqwKQ%_0yzuj+#**^XSz`0N)CuAF`M~IxY;VEVt7eV{fe- zrI#Zjw2DgT`q0X-DfT%njc!@|HNrq+Khkn1uZPGe5zvkESfi&7Gb2J%`<};uVJw`+ zANc!U51-CGB;jnFVo#(W97lfC%cOS!S z7FZux6l88%?!#QthY74GRvQXGgjf)^R;wBDGeVya#y^h;uldh^+a0Fc)+d~HoKOF- znSvxj0yRVbi@HFsTxvIdNzpBP_*sKy#^E?`4a|2`G0yM=ikh{d3Bw^O*fWjqN4vx% zrKD&LUaQ976ie2@=V@wb?S!E?+Wk@M_L7qIXhoGBe%INKeikcSKA;W%t*EB_(Wi3T zFoY{L1QX_=x@Zacy*~*4eYGF|ft-p93*sC{-Y%xos?r=q<2OP|X}* zX2zsomgK9nS}Xx!MbUZj;wB=jyOFpSEaAYJLj<nGsuOzn52c-1f!C-BmOdsYPziqF1Uy4)gYI#JohkHQfZOD${a!b3Vks z_<+Xd=80CbD#9Abicf(@5t%Ll`@;7IY8F%AxE8AfeId5+DM3NX9Xm*04hWyc&X6oC z-A01GQmwPHnr7LW&*=(S_gmo?FTty7U#FuiH)DQ)79v#_$1$B4x*B3d$i}wyF3uK$ z>qFgIjy~~`o7?Ni$o0VR;PqUnpOC{+7#Vbi0Jqf-f808%T9Zo0BJJFk*jv}o-)|sm zY|P;;Jhj(&(lZ*7d*I2V#z98mCBTv+8D0+_5HSAWkt097-aIc*<)FWJ2{0zvBM>+S zfQJ%1&qE^lx@2&8qQC=RgW&%wjG7xKBi~hAnr)A$wmQtpD$vwuj0hKaSD;?dpFkrb z5QNSRZt%p^2fWo7KJwh6$=13{rufKZ=d~_UXbq8TYZKp29*m{sE0i%BIGzIM;d<=qI)rQWpVvU|bD}76u|Wa%_2$_U{k)@pWvE*o|XVPw>_m>+J_2ok9>6Nn3@7KdKLFX+$#+Gz-o_F)_!$LatWjI*y=j z+!k-&Zp2KU-f7+hdl^eh+ivU1>VcpYNnbjUOA<2@Lzcj23pH;pOs@~>Tk|63UDp-q zt$xhRZ_l4QUf*!(Q9b6poET4BfN>S>;hyVxvwq_lb6cNprl3HH{k4_RQeou=>=^Qn z9(vUHAY~t=+N%G@*IPzq*=AwGAQpn4f{28uNJw{wB8W%{(%qfXZP6v&pdj7dUD5*5 z-QCjl?#r2Z*LuGn-&)REub_YlFmov3u=UOVu<%yYQ>G_d zXSgS3DdNxIJt-*>Y1#YdCMIPH@)RyY&1iu3x@y1-*y6nocO?@*jmCWh#JqRq74C`KRWM+2@TEuf<%P z>1PoRcK{R*x5D75rpo^Vm>@GVRapD_4$|h$ZUQ9|fOz<+k)p%S@u#H^O74o5vG1Je z7H@#8++9ru4Q<`#xqtx7g5sIl_S_)IpaW=v20#1GVg%R)!+z@;`S*XTV)`(?PutL@n_N7ic8UztNTMt&lB z@JAtxV*qo2QQG(YJ#voFYW~wxcEXH%e7@y2rN=J=hk!7%Vb|E$n?c4Rry}}pNy$}C z&e^kmr^m|1#@hFz?hwC(C?Rm_(1Uvg;^qiQfR4Oc4i8#b3?L%xoMS z#Di^%n)8kj>?#J+ivN8hw}Io6`15DzdaS5(Ew6>lr|kc60b;XfDp&f-aZb$H``q@<0* zH!op#ionnS0VaXw4cOHK_<#)D?9BsY4jLIGLJS1>Kt3DqdomFQYDI2pU+=*|qKq^h zNug`1*;xv!^IsEG&jXV-OG@CyCl+}7_7<`ghb$lz(5A!yprXWaNqsIJo|A+vzvupzc1N>z5ABGD|DJn^^QKsiLBmsGZu3V+kmH4A*c|==v zZ0i__^Jk+oh0hT*iLeSvI^>FR3<&*GBq#ZGVt0P`4#4FAcV@sg0DRy;`0*cxA>254 z-Uyrg&TS;dQ3@B(0fbDB&uyEP{Hh`qC|^{W8E;DRpVXFw@7UVHH`w=kox8MxE0~#C znfLV*vE-_%SSWcBzywxNQ9-(G0H%x!Wn)ZLp`ru?u=A@=a9uyP0*bXhtZ2mJ)1D)* zBnoh#D4)}k(O=$DF4@#L&EfU0$j!+K=+_VCjIe!#ZO2ZYPX0SRq~B$$?N{e)she{k zoEe&sTVE~@XDFITd|3}r2?4A^DDI$#&9>D+Uc@{8P-VCPrEiO5)P|Y?qOJ+}a)zK0 z&MPQL#Iw%`!I>A6!Q3$7l;3Q07et-lIL=Bap@$qIv!?Fqr zZ=mP>tTd73g0v&{-v2tX$tQZvL_pHdJCx4P94?r=gB{dLbJJT^L580 z%p@g-cIQZII4&Ca;G5QA9gnen;{CHX_LUhaO+gS3#JeB}J}olrM_7wik6c1? zN-ePMjE$i*7||x(DWwMDJ|c0M2b=&F`pCTpnP@rIS(OsdSPyupl97=?YUZW~wJhw2 zAX@Grd=H4~pvY)oHNXn-1nSk8hZxk5kPh=Py8@#w$G$#hBWFx*|J&3KQs3E?rYkxD2#R@|@Ffn_Iv@8h|tx3d#5;4Td`;Bt0--0ZHL! zUr-J;KYs$u)==$V!Z?QHGysj~8C2l2&sUVj1@88IXEOP<~jN8N7*(kOwAwWW!P+Koej3g3;(Wh!=wt&p+i1w3S8w}Duk!hpQXqQxTJS& z?WOqr5_qeD4#!OeI#{ z3>CxP01QbXxlJB-69}fL7V^sDf6sYjsIX%BCE@mjzP>azmB$dHL$1Z5u#29+Cc2@s zo>g&}A>YmNPQRL>kP4suQB}8x*Wq$#dBE}9=i}B3q8E)xQ+j*(U71Ic=K(Lxg$idj zuvjBsjsqDD6AZH60LazDcWN?V2{01%$ro>9Ai!j-q9OhWS=rf{Cm(G#9I5w&sCj`X z#2Ss*=Xp8F0d0m81WQ||A+p9pR;ywvaeh!+8*`OQET4Vgny9p{NPNs@baY1|K43x2 z`Lh3!16^C+{OXp{1X#?OFjgg6T#pXpje~| zf^-nl$81M9ANXpD)0oxZFPf!$%(h28E0x9cMV~8v5FV^#YqfOfTA=NZ*Vv9RxU*B4 z`6lQ0p7ZjW-`?PiA$n_gP0NE*t7Wze?McThZnZemJYN2<6ba@M#+N6x)CRBa0JR2& zeol*NlseP#8#5^+Ne&gHH+SgO9@;_pT+9kKvGUKxAbJ9;~11)(MLltIs>#gcYtB9DQ zeHZG7LsFm1uHoNf>1m_AA*!oE*mP?SN()|ueYrg#vpc; zyI(IxjsquB)tErpU21-NkKh7+cqV?f}mA99l*8b=|5zhyid>R5mW82Nij!Ay( z#I1?FjKapmz=U}jlb6*|#3g5~vRB0u#QMqi_ehXc+N@fUbs8--@2PUwGw4lp zheQ4xo)-=|wDU+xYV5F9JBi7S`;nAlB2UJo(IS5_roa2~qlzOe6qnGXSc!y6*$(mH*y+9CaZ^{VIdCI5u%=01uFY`AbXPAM+MhbB&!uHIdDN0E0Ox1Yw~tm zV91@>9C;OK0SO5@MhXe!Vm#R7|8D&ZW!;3;`C?({Xn)mGh%rm}FvogrJgCS|42DVE zxAw`G1T~8TwFB#p&PerBS%IY%*fSxsDfAgPv2v!WK~8(Dk4QTa0WR+$5x@`So5!&G zjO}UH`nWhq*y!|5@l7VRu82BQ{zU)JKxJ>sVYi1DN3RQL|2jL`V+WqTaOi%-=Q=Bb z=js+2YsB15;N-vPPIg_5aMLHsq$Xj)&r>d3j<0-0#ClUQ(Xb!8^y@@Z7^>i&oA&ro zZAQFi;GAn%yynAGyuH73xqA~oVl%V@y7qYv>`1q+6}P0A)zBUy7~D4OOAsXuOw}0y zm6erqn`(uHSGR+U0e5{2bnsZwg!JKHNk7!mE~7Go2numzn)Q0$p82%e{%Rhy z;q40d%LEH!ErSEC_K+Nbn`Pv0Pde5=^z?kMRZ1SXqO}$*_up|;x4*G5d7^#su++UK z*F?AfL`ERqI@wa6_e+-ToX$0>)PSQKn2TB?R0Cbt?-PukV3B{fTgay#x*n-;*d}bk zmpeC-OzmLY>CUMU=d!%`A)xV`Uw47uZT5KP2}|Xyk|iyOt;u1AN(8fN*hqf?;ly*| zl^h^b$jV-YXpx`bn+|GEaAR28pHgNQMe1GUYX=8TsCPeqrsO+zIS!hQXIcLwEGQ{> zN_E0P=sMOCIrA!KotRCF((~!lw(hoU_QFP6&?WHg(8SZOnfKg7ExafcaP71dkU{Ju({;OOg4D$=!;NZC9^L1&|4d6rB6eT6lq69=g5sP&` z?hZDJc~VtwcGFyKfl5`GWcubxTU%Y+)*D4B*KZp!bB2E?BZFSisbh|$Z3#4Js|ybN zxgGVu)|5O1y;#X*#00d&k70PXv<+9~Hh=c~d3pv)IQ(D$NIYaQxC1l=SidCC4NyJ@ zBzLvi=N2<(IVZrRg>YxNDivbw(XwgF24_RD`=QQkWKeg%u(4{)vA0wnyN`*ft-W22 za%KS#O#W=O4h%{D9BW+U(c`xPx<>Xu0KwU>BuA{8n#X|z37Q=s6f{84uid1BCm5Da z3|4oOlh3M2St<{WrpvGt6g~Tab?Sl2vCK8=GJvN~E@7(PmLcxMK@=foTZ1k}eycx; z0X1~6^XUGFR8#D=$`US>T>I3Vmtz8>3MvSn6f)frb$Vwk=UxG^V)FBgeOS87vo-|6 z(3veax3q*n{eUvda;aq_wLMV{7_?**>=6k=raA-B`?yhJP?&WG<-(hqTMADi{KYEV z{>s{h6SZG_@#IVhQ>EAQ1YvtnkVAY!;?s2#)35-9g*iR|lV1y{^q0p<>ERf()X(^s zqOG|Dp&g>zAp~#f(~U>d5Vu;p;e_M_#|8`7#f+KnZ`LO#hubtOCDkv|Z+)X~*FI*` zVF+{~_LQ|!zdkwih0U~hSV6$yjI)G9BE1}i9EZGawC|umm8y9ls!I>43jk%k1zrt! zb!wg+?et-HgjEF9xj#EZ&`kKJ>@_M4!UKBD1p~&`cc5CeKKsgEk-Oldr zEW|}1nsJ5k*MSutu)y(vCxs|xQQ+f2#HH3~ks(;7K~vwjDoDYvCg9~rM$Zr2Sryl@ z8t7aHL&D>KSpOu@)J+F!K-dC*4!Qm4&&*CNLV=?i{Nss4b{=O(>5;k!n`4Ei)$MW)Tf`eG^MIAhAoAST0QMe_p zg<)0XHCO!(48OTFUQ|@3L1Tzug%j%>jt>CZ%~?eY2Y-N&5FDV&0`Fw%fN9z36D~kE z`vR^nQQ^i~A1CgHTV6zrD-|A7Uq5rEkiPb*Mf+E3Jx_%m@Gffww+hgT z1LWk!+syBoJl#Gu!`E*}FPr;n8gRvnG%KZh__xB3l9cs)oNM0Q0 z(q=KVFdmN#%JwX5opMXMJHKt7e&$uK}ZcJ5PlP!ciVKCBY)npwSkqB ztV zkOdw~lL3ns*f%*M@t*ZmCh&DZ?YS-;5U`ja#0Kf+#} zc+OGnPUdzQ+mBaMb)v;^Ta!^4Y#5O}ETCJ@$jEwA#0eictuunFdX{yOBQn+_9`o?l zqku#1FVGKthEyV3Tf2kQJvCA!So5imkJ$YDqQ&&PtJFhTq*RMv`+`=##$|i-ysFib z+b*K^(G-ti(R)+p0_^7`fvGgu#*|ysw#V!uVN_k2bNn7^E8>TLKeudZ#Pj(hKFie?vu*W+05+e1x4Dy82(fl(g7=q-hTz+ss18$mZTqHZVR|+O%>wUnub*#O{MZj9Ii!%b3B2LMya5s6Xij8gL!*^20yt3&sDrcf9 z$A<%6F0j`DYKC*=4gv~j0uV$S^fMrvSMMz?A~&75$xp-Gn4ff7 zmK$_SKl2{TAejM;=x>EE4ShvFe_8aB@aC{f#V`$R#m~vj1NgCw%yO#e}>mMxf}q9KBw7(bJ%w19dbHQc4DMR7Js@5;4#Q_GmGe zn$#1q0?)*;ii#_gEuVuiF7IoLpi{m5C4IlIz4m^Wjd<_ymuxiq60|t>zu0LbLyuQm zV84pE#&THBUIpY1NqU5|9BgG}<>aIRQCL=M01p}Z%^OhifO7-J}`a7>A zZPNCRj{JgxhW7U0f`ShwtZcXup&G`#Ooq-14bii&n*DC)qW z`da3KM4CW!A^-vWt~?OzD-ZV#5KqK_DF6{{NX|X%F$UYpJP#W8Y&bY|+Xd`b!zu_R z|HMMa1L0kKhzt=%pn1DLnH)gH8lcGoZ72^94;&37*{;gLDtT@K2XO*Qlv`MOu)S1I zYLK-yRWkclI$b8L>u>E@G`qUvh0lm4Y&?p39j^Q?G7K@EY#psA5Y7Bj&5Ke}65SD? zthPMxq*e!M4D85xc!<8#e}p-fzH0kQwd1}tSD!AdT@gNN>69yyiGt)4*^FAB+0Q4p z>Pb83%-`C5Yx10O@ZCET(NAsRS3*1*1eC?Dl|+6pdH(v<^ODe{dOuo=y@@4BqMYNQ z$3|*UY=I>RAa2U)Fs$eBSq#2?__3M}u|*kI6hLqX1V1Q|z-kW-uV=dW?~*JFc6ues!r0_S6?kmGxbe&I7^Jlht$>i(tvc_8Sq{3J9#KzTbkP0*4bs z(l8D&nT?lm^;l#mF?P0bKB`_~l%}T2yB=%)LHC77IoUO`lAm~E-}DNa3pxXMuZ2IG zX=)O|4vIiCp8Cl2Sko@M3R4OV?DqqhV-Csh41Hi?B*mAr7oDw>AH&>6x18pRa{9uJCXIlc37UqdF?(jpg^Z12GbJE z^#Cw1KsNw}uDRAfJ7B=Q0|GaoHA8Q|02Ur{Cg`&3L85^hN-YJH6cLFSwPW^*N=$0n zUkTBZFNs+CrYD7J`+KSyx?uiOi2m&}WLs*_QM% zuCo3=SUA}=M&xpgAFi;WhKB%0hn-R5+}`x}6v?EjdQfWtvxj-{<(oG*k-vgPayMKw z&?-*y&I3YGSn5HS|LI`kI6d$42-d++)rGa0|HLm)oa&e@nUhUH)(L?PAl?ev7uwW@dDmg;Dr9pB?(H_wJxbRQT|=!)9x~1 zdv?JM#~%f$^H4nDiUGwAvbh{2o|$$xPJH?kKP43SS4h4L>~O!Nrs@yuzzuX7Kqa7B67ubY^f3?Gfj2|RNCXcos! zMJxJ$o1s3goD0|Jg?ksdIB{nugU|0y~e#wP_*f617tIJd(*;#(|!a}_2ie(}E zH3;U690=fNz_6$$hl%q}NlA$nZT(kJ`+g%_9$ElTKn*|Mu8jYNjHYtKqa#`=1>Q9$ z~tyaT-A&;S`SSdajqib$&v;Ta}o z)8xgdDAwAjw|2xM`8xn!5_|bo$vm;Lv%B#iht)_&`W0;9dpy(7OSw#Vc@M4}*RWM4zBk`U+f@T`;}| zLytgM3ia(YssAgr+?{&kDk^#31ct@)0Ym}LWZ73irPAPdRCt{PRi8sA4{}Vne>|{= zVLj*77erSG6bsk09bvF?KsI&2j*)?SAVE_7x2)XmiV-z6?CwFBh*$(3)O=o#EL za=-)a!5A-ZrVpyP3>Epvh?Y&h@|k1ZxGJ)_Y8OvKVrB1t+d1^2fg8|LVSyZDm?h;XwhrS>O*)-)h9>+5RZty&3^r4A^Wy+tbq0l9GX~0MJNS z0EJobzvGc2e`ppO8cIn#mt)C71tFwQJM{|x-b40^MkAm_Kw1Z&44@&#j*vkw&vg$) zuP_Rh2YcWp55g_PLUv_1-}MleS3O%_;YNg@l99>pF@+FaHf;=!DV@8yg<)iI*+jpX zW1h3bSXNu8ekw00cZE9ubgap;OYK_tb@`;f&EMUwiM&n)b^{=c&I7h7s89?cr2>ln z{^F8=ArLubujPHYL0xKrT<+c!@u1{Ns_DL`A;pg@=%kF{DeWC53#)b}Z@d*Qr!K2t zSr{+ZakLIG4MNVzDBO*0;LXYjo@zi0e!>CVVGVxx4P;v(DPo{)CYOvy5I&!PuLVZx zrPgn6)X60`9I)|^8~>E(1CkJ@LmG5gP+;jnMU$hi7-?9iNhA=8}_NP?R z$72m~ooY2wj}>rI*sX-@S8gOpF@k zMkPHZ4R2)3pAQ|K?d^^@pv|d%WjHfp5YVctE%0h?kZ*{$lpL`iwU`k=0Znr!3>s?- zA5_SYEzLeEbCek9Fz*r(`9WX%Z-0Yh_n(XVzJ>bv@WQg$x7ctALRI4^gjOW*Cm1x- zXwa;Tm@EQuI2q8;x58Jx10xIY8n}-j8H`ktXXh$xex$et#k~W{{tkaGQzC3_G$Chv zg8H#!z&4b}b$N9uBGpW@J~maMi0^v8yHVVzT5Ho1-&|p-aBtRXT%PxNNBsPXyx+Pg zqpWIf$q=LCX6xDM7jJttj#8R^jLpdS;SqqAFdNr1rR~#`EFgaoMDK92GjvSH6DBIcvV3xVZ*wVu zv2kcIyhfkB15icqlwpEAENJg>c`GDic7-+q;mmL8!g>WGz@8xzKPU1qk4co5NS0?v?;(q5a2y3V)T#jyFzHs z3knJl=M=cJ0Wc(^$fOOjHhrjWJZGOaV8ZPb4shpaQSs~Bp=kL|xjdzbM~}B8i5gFD zj4^D)CD=O_Vapx&W}R9*GN+9cq?^fdsO4sRF=2JO_?lCvGbMHkFAb11?vQExhT)0-7t!y)`}tf^TOUNhxAvv z0lSoZ(JayOcEe03=C9Qfh+j~P;|aSRdp%G;_A1vdupB+)_b7kiu=8j#@`SRZXU*Zs zpIB3^7#cPr9lk zAt3=p7p$zb3=Lt6&JT3RL?{OEAs7GWZCsivW%nl{`qj}R&-J!C8A6ul=ll!hN>q_G zjS&ct?m;62ZXrkr0+2vM1FqHq6G{<`Fo3a@0tFSJww6^^QWLL*H_MHQ6j=Fwh1XKn z^revHHR#tsu$N?t5tTZ9wnN?1bvDDxcbh)tQuI1z?uis^+QN>fIC)KV!-ZlK^J3OfLGzypoh zdil37|@2%C>{Gm0&oeN$h@}U9J=24&c zsvR{9pS#f_qCr+&&;!936&o9i0Pm2Q1>+f=qWA1t<&~d>>IoDfVfG>v{%ZYAEhgZOmZ@P!KJHU0_zeeF^=Fbw6#$f*7W0@qO zHI!uGl7fwAEVbrL+GDDG@uMvX-bp;itrSi-r$SzNZT8&=_n&BGuxwf&(a0D*`8|3| zLPs~`HixFxn1}i=_BH&ScPMVE)5Cg33}4j7CH|&l|EwPT)Pm*TtMIZRC(J|wQ$wLc zbg3kAo9i;#!fC41mrw}AH-nYL@54akEIV)`3&b8^aiF2`*s;21vJ?M)vE-x;tS|&& zlV!NM`C9sePoLCk+^)|p@&H2dI+t@D@1IcGq+S670o;!WkMJ z09hcSL`Yv;U(d468VirWaNvxnG3zfo@X=Jlz%Uswx<~i-w(-PWI*GTwK4mhP%pqbX zCX9o4TuQ4sRe{+tWA4YQ8Ufs|RUX=x9{d=QZTYieZSZEnM1{#B{b=Bqs^^;el!!6a zmXefSVwn=wN8XM_=F8R&N%l-Fk&>^m%)LbPUoO-Fw^x%i#V&`mUfZ4p#eZpGpweLY z;_0ua5n;@}c$o=`Yy!|u?AySLv+^e$XL53q-6haT3J#MjDh-%A0>7OZXr<61ZAD)= zC+htxlN7+YHLpDYM~MB;@X3V1aDQ^JTAx&?r=e|{ZA}Dk&uoqzn5F@n9?`o(9wHE- zlvduLfWDX>lBEEHL<}5}8K^I4Q$eP9@mQ}fzm!e&be>>ZQ&UP7)3|@8Z)drY;h>=< zW^tEB?3hd;l681|dLpMyMzO5GHm`bhkB#8AM&h$W`rM9SYCXsEfo7tGg*Kvd7hZ4q zj@gx07?te;5w~a$FPUS{Q4{j#W6VeLo`>4Gp)aV`CF$SEb*=bHaYLm#?lmw{5Dh>3gTT~Fj(2Hx_Oq{V$L+q!7}%$0gui?Q$`25P0<#z%1{mm6#FCU^$RXnDFW0`| zJX%V6awL+R^an$nfE$Oy=H6UuK+}&$&PudUFvmAQk=_K-2?I#!G_T&r1C#+;Ai&|C z0Oc7jETCZv00*7Gf3fAk$k?1{o19QYNw|0LJ#3u>GqMVUb-sT-8hO)?Y6HD zk>VqQ8>&4OBsKF^UxI4=#p@4xXd_OdQ2}v9lKJ3gLYrP)^$85@88Dytxc>iH{eVB7 z58TOYY{iA7sb&9xy;9YpE{T{)T%=d!v@Q-;Cu#?~$W5cm9O*emRgI90_s@ z*v}#6pRn`=u6VNc4j|zL(3C@_a5vBrR(g|Qk!>0lbLx?V1&_z`zJ>4 zFZlSA!nY4*j3Dn41y>vR2!x!Aqcn+CjB4q#GIe!7F$35Ef8We3wzt!F6o&h%CS%F< zZhsUAT+WG=F{woeaTEryvv~9|Fcz;Ca`8u+cVnjLjYM}WBMR*0@re#p>Rl=hhguFAnguh3qFU*nSOijnt z6WY6W;fOgN5lM>gH9uLpvlj0BOFmzzrHVfx9}9)pVRmA~ncWh7OEONZt#a?-pp@v$ zNVaq3HgC<_KTOA84x^l7XiL|HtGl_AhIWqHDI))BFzvm%vTe;~d9Y1Sqj|Vt)^WY>?`~^7cE-^w8AU-Tk*GkJazM!+gl`k(S%FcBjpbk&uwE!e;ZA-;I>t zm8mQ%h}6pHEod>IwE-Om64D9+iFE5{;M@8F@Jz5$K%AX{cuGZ_E*IzT8H^Fwh1=Kg zJV~O()J#`s*1blPi=^xw25AKVZ~_xSskKuctIZS&2C1DyJP8(c66I2|7DdQxs*Wc0%7 zF%drqp50%h08yBj=GLHozB81>6w{@b{0gXj@$;cFhX?UePdO6Y#9 zP5AFIY6nyZa)}{D4!ZK?q1=Q-2bib=Z1w5Zp+BF|ae*QWCcj>OF0A<=!;f z`-iMrl;Qgthg8T9sum0m;-KaPbpT>V1;Fz2XbNx|guxyMJ_hg^Ld-KPm}Mp8s6CF$ z23?V>{SO`rEVO$wBQOFszr|s$+*qUeoTf=0lAdwb73c>jKy=V;4}A`po$`$JtQr}z ziPVZ_GZfn{YWwPP8PX?Z7Guo(;B%_v%moDg=9kCCdBFX8(~=ju{1y!cSP^vmx2hyM zxSnpXLpuhen-17l0A%^5m**{Kird6KWvrN}0 z*~P%(TBhp?b4(XlxPWI8vKj@uRXDbX9LTZ7WhJ#(RaL~aG93MC7}CnDxlT#6zVY8H zE~Mt-#2Den!pR4CufDBKB%cNjeKYXIjA0OJDkG0%i~XeY2oLFD$hBVtnBW0xMavc_4|eT)p})#45r`5ug*OMX}&{ z4#x$Wyi|jD#7Y#V(P$RK1|-NlLA`PEF$Gm(f#ida*U*O*?+>jUZR#=W)2~yGR7@n# z^$KeY4|Dl{=m5+F3^L`oUdMq(GSlcQSo#El>Ry)A8U9=HocHy&&?nt&7LVbnnS9U~ zTP)#6%o&>=(lXOm^vRY5*@Lduf$0e%A%~48;?V|(BUShUxac4-7cBZZ$(1i z3-E1&d{0NXNTvoFKEGO7fxTGbo^2MFtH6oJ03ndfVS(}VDb-~i0{H?hiv38dAdLj6 zOz&c=hdb<*C03Y$RdE9GMkNafS>;T4r%o`K8uxn`Th!e zk8;)Fa&BlE0MP3zqKM^2DUS*}@07{l3jFgONHzf54Z}bTH_{yNhahvIKT!xHc@EuD z_8%^ZK|n|d9UYA?-$`mBVv4xDHRM@m7-BfpE!s;1Ka$|9Jn+y$`~^Y6yH$PoNKLK% z>{MA!&I3r-zyd2W9Sbj=bo2#$o;obA6=KOH=BZyp?K&}f7AXQW_#pct09~~jcXsQldIuP8@hAjnzv=dx*IxT4L zmnZs5yIYWVoS*<~wty7_ZZeviP zq#LZosX|M`5gsZCy5H>T?96~xqAPk-cxx5yRtMY1m+t)+I}VtYz7-ZkpmWH$DW|0L z7K$L^eGW4zbZE$ztNgDolfSaLCQmU381~HEL^xEOhtu@jqsZ#fW&yleXW>5$t*xFB zYL4I;h%NL1Y@{Lb`wghw5D#KlvHQ%IjR}&J{~hkn+&pzxm8tfwi6~{Ks>6&u*{g_n zo}uN2@5_g^w35DGiw|Zo@&h+OllBGHQb zz4rT!8SK5_4n=t5#;*>~P}PV8`N(@+LCZQ~a@|EMo4%=y{+lLGxk(9WiVce0RM{Oi zmO4{RwBW_-x)Jt;kdN4!LzX+4lGOdYc8ff<>TAPa8EGaHh&?o*$($l}U;d>az3Y=_ zxANw&+w9DSU$*dhOsvT}!(t9YvDgbMZ13gYhkdG+o7rIRltJ||j?V>75h1)CU~@yK zS}{H|;hv`c?;Ef?J3VNrpNv2I{F$IK^=)#ZkQMeLXN_A)uQ8i<8|dG`I|AwzaJg>O zVE8VbZogT+1R2MHPdQ}h@ELIYTmof+>TEKk=x|JIU{}}et-OK>Q-ve((2(d({Y77i_ z;DYArv83l9kv*=}6N|O+O5)%rLE+}+zSrI!sip5Uu_lj;XPK4d*B5+m7vq+Nt*xHC z2)di`{EH+)+@=G(h`gGzvQ=Yl42*kpEWPG4XsM+#kN7Jtak(c6|9?SScKM3Rn!=7p zi$C1Q4A~d7<@h;onX^;!IC~boyeD5@dq0<)Sq@AB2@h zt#2^woEnoptlj3=Ag9sTH~#e36jlH7%sWusInItepkcPuLha$ZWA3b$f)5pnvjK

|;`@Ys-r08%|wBmshjt^#p10M-Qf(WSi!06KXTc$xkNbyU2| zfg*T>Wxfe;dU^JcvUa=7uh3-T00Z{8D8Nd%esYj(toU#;T;44vN=d3dzkmq|r-0=w zB{-2okdXcK3)lRjXWLxvMe5?s>vYd&z-S+8A)>q*Ej9Lh&W87Ka6_}z-RjU~k2}HR zLH{Kgnuq*39SzCfACmg9Wa_RP&-wRuFTmyz$)FZ zm!CpilK0c$)PFBvTy=K6zdrj4MqxN!^$i>g&8d`vBYKCmDPfBpmpJo?+P6a(qSO;D z-AilXOWuR@S|c|!J0INW{c!FGG-&$nrN8gV^3g%lzSsQ1xVvU$hAk?GeA-R z4{S4Mv?rTS{@sb9RAD~HJ$&`TmnseM{1;Wpy8ZwpG4EXTiq*K$C}sEi(<6Y2pg-bX z2OlvwC+xLT=Ti2WF9UTA$!dqKFzlS6-)?BnAYZ(~sxn{~OjE+6C<|IhF#4=^+J9Q{ zceFXrN;fRI2%L!8+o|*~LGT>ndcl)lQc_o(`My<*JUBQ6Q9Gc(3LSV!5EeuaY)Zq4 z9IJuy+N~mT^=y!+)^?m2q;It5%?Il64H)!ty%6HG6H5Z?w!6)XVV>ymL)At+7%`7{ z&P4(V)e@>4h@JLWFk>F6$llCsA2Ypb_^UVHm3a9M%XFnE}qW>+ix?^9ByRc6t@V3yq8oZ^4N#uop%p^!t2BVMqZn*3*LWJMDN{xzg6*FP zG%WNerwyoV&57R6EUvh!dK0pT0 zOu=5M9y?mf!_$+cublNS78MuxAbsRmjX-8as3;E=cmpepYMr1ldgA|Wy9_N z#rq(Hjd1AolA%e0r(B3w-$8-97%zNI|p%Z)E~59~}X`d$ycw$c@$hh$vnr}K~u zMTojuu?BJx&;ukx$i>Yrg>G45|EEbbPTFfKl*KqX*sKROSz@9#1KP^U!@pZgo`H7U z(JZ!dTq$srdNt0$rV25BDKZ-H0frT!x%n0saV;1c!4?xf9Au<<_UhHNeVF$lo`SaL zgXwD{7CeLj>ib5_M%~&XdO zX|015us6kF)`DH=%hdiE9nJb&YNe1ES%-KtgS*5&>^rZdB|q#LZ#S!Zj>`7LUUOWe z`AD9#HTpj;K*N#6pt(Y;ivuHV03PHIEO7)>!jR>>-8HbQ#K5-W8CO{u>xcjq6HRV!AkuBzu}K1(J7C@y zfJradmO%o@OBW0mKI4WOms3Eaf2OAH)uCZqKQC$}|bR(Yiix}P1zm=ElBu1G3NpB1tuJ85y+B;t?C(lbPH?2o-Y&@Z3iH)5d8 zEl`uzRkWdMc5~>CwfFcVombPHS+w_uSvM!%`;#M!YD>XpYS`IMAj)->xgoi@Kx~2l zD=sR6vhzo3;6aB;!Wa99*K9l${14$cHK{IY1)@LyiZH<1ef<1DEBq}!z8moEw=Z72 zhzkHaYF`*-X=UDb2kFhk-mB8iAt4HR{a^azB^2gVes0CDnQR zIa4rM#r z7u=Y$MzuV5XL)JtEl|^|7YnzWQF2OYWi&LVZQ}0>oP|}z{4;cd@i+Lt)f(ZbL6D6w zFp*cS&tN-{ZXg$^9Bj)Y0`V%*nbv z<9Lm}#wB~BXnHJ8PleL?S2aEZOpE8oe$_2z!A2Br*H^NS9r{bFPKvN|uOH7U;B{=8 z^Use}UHmOpZF9dLrNCj+AZ!&~>vp%HY4*4?YEjZj|F-)|@A}2&tMIr#zt2ZBPE_oN zHWn9ODmt*NMbP{Z%^(!#_FLpjcv!VJ8-eMj%NXeQRbCAv(VY6WyZ+(!M(Wm1LR_=h z!?a4XCm<(WLwX^k41X%j-2MPQ^{)dTbs?r-)@NYR1guBMe@O*O1hCd-W@n{S z9}*NmFyi=@hHXupEMxpA3s14(5HEdgQ+Ux8{l49}H$6kaQCosW0XWA|N*@Y~ac2at zDW#RzC6^fABi%H|`siLkD(O~xd%#yR?>9$6oo?6?%ZGj417Xo4gUR9u(v07Xr|Joo z92R^XHO|4k>Yv-CMK4!K3fJ8v&Qwc-voJ&{Wy=`nY6%$)m-2tXA3#q(ks7piS!kDv zDm7s^x=nu|Bq;Mu{Bzmkr&MV#?i;<6vh*d7ZnB`t?|QQGvp+71JxWY6ev|!N!cLNZ zuajG{hkK&Ge~el787vGV%U%oB-IvLD_wvPytGG7{&~M@Xs;MIB-iB=!d9&M(i%_)a!se_V=^CqBu4h!4i*CqqDv^71lZ>gewFP|AL7 zXU7gu$qyMSS|xr~*8AWN4Y(P?9t*utPr677ol7O5~sPoo`k26zm z)Y#e%&+TqEDOk1QvJ^Q@`$}k7W>Etn{EKim{;~;px6@WI5L+#t);ZuSs`bYarWs#X zart~_`8d?Dcv9kzTxe3xZHt|4q2EM-FC!MQ|FR`rNKmLaJ2Pwqk#hH1rxMxtf6Pv$ z$LoI7PRI6?0A0cZj27?S?Zt5cYB|S;`I}(b^BEK&AYNTt!H#+ZR$0$P8ekQY54s6Z z#ApEAhnFcim6n?;EUZsa73b;bZaKfXe@I%GNjT)__#30HryC37x&z}E&ki*EGM>7` z0125;?3%#NmJzgUN_FmPnMC8WdUTk+w{3K<*NYL8 z6xer96JLUD3j~XP2lrfrhWyX7X^w;^I(6AVkD%*3R1t#X}RUAOr;-)Pa$7_4nbQ&Vqb5)toYg(RJgSbS!fj%mv?cOnwi zRB&_S=i=h}JvkYWlS7B{5omp&goFdGwXo?$&i>y=y9)n20famPhzMWYboo+zp~41` zP2U861s#sU!dr4)MIi^y#e>7ZlUQXJmtU)^EnSmpWy8ad+k5Zc#Y#HWUcn_KwCCb# zm*>o;1)CKRRl+YcG&E#Y7W$S9egV5hYvaJ{CT8Qm_y1-c_)PrhHJDz428ZwDCSt(* zW&(alfPa7lO8~!b=y5$@?o8mHdRlB~5FHcKJKo2%x^V1*hQ{%G^3zI5iGBUZLs24P zjSSIAxtR{BD7#&(S&~a9aR`Cu?m&c1z9-$`^+H<=H`&d z#R^oro%O;V0bC&tE-t?FmpIURe|v@w?QG%EGlJU_r8x3@jVkf#zx#djYNJmI3iIo^ z&E7NEL=HXn)C$k0y|uRbdt$;g|MHxTh2=3bGY+`5f;( z?gk35u(fs3$m+cy!~ecz(O&~d__0KS8O$en&SoFfz5s|?Xf>YO4h!y{^YimN zLU%z!n)gPPpd1Ji_MDu&miaDgbtn=xwogh^Zv?QhDHxe!Cud?*dEEIP)u2w_%Px!$ zt8!KcYz1|75=htb0yyYpx(pICNuDb#xY+#Pr@v=tZ2SeTJUKZ$0EfQ9Kc)E746GEv z-UjS};0k?|jl$~UGCVu~^}a+vUZ+rVZKRnohTV-DA#}a9 zHfwl5Wnp4w?)~%UDuDJ#{OH@{dw_nkDH1;|;Q8NIEeMM(_^OqZm3O+iw=}>T2`@b+ zHWpJPcp3uMdMj8Q*j>ILM1vt?oa&)|hUVhnbu;P9LXY#m@# zdp^Di5g@Eis!Va_6xnbONsW*Bu5UgXU*B*~^1ppCX?iQRvk1-PnX`$>caoR*gJWaV z^bI{-UDr^+D)I_w2|VGw>cT%E_Ws3!|DQWqT;ulb+u*_UtD~b4+8@uaU$6T9;RPxW zGMTBXt5;oKoGO%mNETWRHgw@OwE3BYvw?;dsipmegMuQ<+8Y1;OMDdjvC+ibO2U68 zEnqp8>dH?~FFA=e*x&!<8U`MslX*vp3*6eg7p;OiwEsQD6K_VNA zQx`}F;dy5PPDe=oS6_b2b`MVrSpjF?x~_Uu=F`)=8_Qlo2ex^H+E0c{o!2)|oN=ak z=`gA3=_7Jq0wq}scJ0W&8v1tO+ZC3l(fI#+&KK~8A_fM)N_^t;6edK?K}Wb`;08t< za)D924F=?q+~F-h!0E->&aT7!7`8lLr|MAD;+PI{OC85X8%DaZ;N3#1>)9UkJRQE< z(DmG=pr8Qw=f#hA8sN_cZ~T?)60Z@;g% z7gZ$K0vJlr#65+$df@&r8;dh#BUXtpECZ^CB$Fb^u|O4TPk3<(&vpJdS7-O#-~AcZy4St#wN@;s$5b|SZpJE*&fVyW ze4*Y6?H9?IWrT91+zWlupiqj)5efulK7r^a2!$2+p#}w!hb%8%yfc9^quVIJRpux` zFn#zd%(7$63m&!^+DwPUJvDUh6KO}3#*`eEAr(C#qr{M5`ZX3yK)?7Qdj|y_wK7$ z;Js?|Yp;yar?LWScoQ?TO#^fR{{9*E&T3MggNLQ5_hoGZj4|#DD$g#o(S zk+Gqm?1D7yoSPe%M{mTnwgav5j}sC)P<>}^ZS8mXiJFm7bWRg@D2fK4we7&{DvAeA zMn^&0?AFt#PrvS8xAnUDh0ZgyYq{1Q!`0Ejb##23A1VM=pVA9CpjR+*5M>6QC|Dv5 zq|;Uhp1>6)Ws+zC&j7bpf}Al%X|Y-6NsFRua7encg5w|sO?P12z?gZ$w¥#Igh} z-2Sy=@SvS3=|!aor#IY)ZB+4^nho(SNIW1l|dAjJl7 zr)P5wq;><>%D@IZH7X7=g0IfN7;gLhW#jFy=yy>H0430|txMjx^HQcFrxsiD!Ws3& z94NP*V2l3j@&T{axI3s?jjUhF8;9Mp4i*e_VkJ}UIffoEFsBv!p z_3I`;d>jglPWYIh6{zyLb3#yhQz+ zg1mt}MGY1mv=Kt-syYs+p2nJNfRU1ym&f5mliYPkoTs&X4MDH^cSnqA;5u|S!w!rv zA*NR9%)*z6{=y*#97uZl6q|we+1|(dFh+|_Hj)Fp64Z%+rBeHOxFZy2zAXh(tbk4- z12eg@`>#pHBL=w2n)>1e7nWg|R~+bp`fUEc&;EhzJ}ezM`kQZ@yXtL~{==UoxPa4E z8%uE`BCwepK1-^sD=kkH zyYb+tW@RmhAiV_bd`=H@k2<0}n2f-yZf<5n&t9AW4j`LriHV6hhR(5Q!U>TB3{`A# zF(2j|-kR_T^s{`ZrY7);1_s~UEQ+i%z`SYr2)_IsW~?t?zQn>o_(nQ+!qNgLo7ZrU z+Rk^n&n%^o!a#^78JL)uL>xvP(UbK~V#)8e;-$c~yC@IjopWKA_$A~P*6JK_4iNUD zX*fxFqoLgPtHi6R!dn*=@sftSn7nf}yy1}srk!)aCN7=Du3kM*96ti@I?O){pw0k*FIjheiKs&wPA^vY<7p~j=PiE1s zzAymfBe9z!Tnsh~2+(6A;^OFu%_zK@t@$z0k1KEwY5ERolb%B64^gkeqD86Ajh$%Y zg4Pv8u?ByIdB(8k+7EVo_}b6Oc|7uE&N?cNieSbpTrw7P?ofVZjeB0SrQD*Iu`Qv)a9^ zk_JBxp*m?jkQxdQH{9Tda_azE2N`g~G@+(&v9i*I3#s@e*3E@iNrfScn1h>fS~Jg7E~nnNbAO#h3~@J7-2WSxHBO>sH{Ko{$%#3E;X{$2mc4>& zXVXbz+&zj%+uV4bsXdn)0x~3>5hY!|e3?RlgC&$)z0o;-Ybk!ILI%s4_X zKsapxyjN#>YK_QHEX3zKp=NWF8YD`g@XQLF7(w=lum|YPi!% z$mJp;Cj4o&9X*-GJ`RCqvO6=lqcMEzU`CBs;FQ15E>Iz%rxzM@Zdo=g}_rN z2h@;x_J-b5_MCH{Q}pR5=sFv&Dz44Aob&$b1yr0!Ok2^+@P4#{R9dNy zoL`e*&-myl;wOGIZ-re^L$NpV+FF;#&rI~YOCs}`da__OYG%BGgM)LLoc5ZS#1t~6 z;>IW|D`QnikTyE{43JE1#ouqKcqw}j*# zcmT6C7dR}<%*=%K&a#)wLi=)9Ip|$6&DjJM1h5;ZGcX{_qxpAvHatD?Vi8Oc4k~hV z5x=Z3?gEZ~Bd#)jF#r<&5wC0vKZLmfSRGD)Yozxq^CUONRc{`k6WOxG6Q|LIL4u#Z zl$;nXS(gWbR4C~lSKDiYPUjd&$_BZ=P(jw6s3)@+R)FthBVS=%S=z6ae4=&OOpOR46oMO}nnukM9CM0^cde9K^8!6bYO{Kk|LB z(QtjHP~0wC{7SG#do_Fx_$)Ar<(OZ89g<1QywMOCb5J#i?+ALNT)f1(n$WhUp zc}+eL6ISFR;$pa5VCnl^%?q&u-ft%f2*52xbTwB#0#HEbMHyH>q!5sEH^JvmfizC5H|M zL9efG9flrbd>JC263hbyWyApp?4x25)_%+1Kh=+BCA7HF#TH`~F`^8>$Uh)p-PnE9 zTf<0WnE0zD+X_9PQL9`WseJ;#jf^=(WChSPG~G7u_Ws1S3kZRzk(CCjdMMx_5Wo#5 zi-3M8r5z$6AB@#PEN+5E=*!eE)h%f($j{%Pxg1JEGOLW)t7IpWg0hAN6W9lDoD7I9 z=#c0JzL6$CAf@=&201yys7346u2n)yzLl&Dh^erY7N1-F{ojl(oU3c~uw6K*Cx971 zc)SKw&~QxvWucv@YHBKgw#lT0#r;RM*TqCcmK0BZuwKT=>4}j(h>efgZFf6={t1i` z&^OTIWy*KVip(j(<7fUp{&sjcM)P?G1#!Ss@Cyl@JK7`tAMz6;696sHgJtKTycPQ~ zb{o+y?^L6)T&ZX;M*Lo;jH5kP*@q8Wr>^lWUw)eU9tAhkfvLox$W>3D?nr8z zB17D9A&QC=;LOqI<6ctj^*9^_GWQLOKQ%o)ls-R|UVoA0{|Or3aQ}%iBlJbY!M>kA z2{y&Fp#{9f_4z8!1q63*kjFXvF6k(%sEUeYk&YXNmO=ZNg2!KKcIz>?_cvum=)U}_lqK+RSI)W-; z5DRFPuT|IkFY*M@nBuen{ufcLW0%w8_~s&4>~8@~JsQw=%NrL~?Nd{0JH3`lT&yUC zQt}4IRM4PSkZ=h4W_y6a!f^uyz=XocJl476U?xF>h&zv8m^8zfcs*x^6~R?&o3O$ z=O|1hJ`(M#EjaZP3Rv81f%G+&kO(CoNu=Tee*9 z3=~=3eu-R;D_lBU865@rtgS}gUAJ}l`6X7-q713;@*qcW8WFbPlp$OaP!PfS1VaB0 zBPjF>oqgc?V6dSZy#{^}K$;P@gDBtCrx9M>o$#=8a4^MuLK^lY+?d$cC2;f{j4n22F415 zk}Wd7;s{=*)FUF&(b2ikTF8Z113jiQkMV)A+jU8TntOSjx^NIlN=og}HS|08uiAx? zYdA&}93T_Wg9#U|yvL*uRZt#isc4zQi>woF5-J&NY;6(YiJi=^vv9@H>{YxK@Z=( zVT&Pq25}1V`d-$D?P7SD~PSJ&73qUjf|*KymaWc(q5Wo#l34-Z(Mur=R4KHOD% z+7-5&-FM*{bA<_hLuil9AluBUjnZodoO{gU#Y2>rEyi5qdofL!DcE>0@FZGAtRs3j zJ2`n@E4`kPQ7}lyHvxkHl9r*oc#5wV?ZL-S!lnRw5?|L5u;Wk?#-0*7 zlate=L%ZS*P#rk57(fcNb$(d>b6wz`N$xX4EXPRvBDRf2%rf=mH2%h9FIJaY4?qALKs37V-1RmW-lhDy~vs9Xj0fc6->Q+t4kKr@jdZ_IvLi@8a-9@}-Y z^^RgVI^-Y3df-AmJJv$5;i6Ow9Ym~tnUV|XVZfL&j^Wn7S5?Rlh6DjAVfS$8Nzo&n zp<$Crf46YEkmal#UX<2VUO+eaXnWq`Rp%A{WE^Tb4 zOmT*u1o?=d!9pRJx|h=min@DC2HpqrO~J8ak`8U53>FsZIGHJn3{V9!1qj-)JAPv` zBq{yb0>fgLDlf|b^Db#;;R;|kI5hMMxA?sthPke$-6g;aPxSO&@4-ItRv0G1@n0HH zdmoDfjMv6I#&V=72GQM${pTcr7Pv%|{hUVj5dj4@?_6K~<*!XmWZiH&J3s3i<-BRy zhMnqG|AVijP^c)=6W2JqVH14G{^ycZLv~;*auoOipcbHEH;;Sw?wQEDBTPaQBt*v@ zEe>rtUs^@4yamYIvxgCmk3wdvo@*<&1FW6o7Nj7jf_yhjMhWq2SD9)|o2^z5aGl zEYF`0y*ntfk`)Q~`|(0#c@}W&fmqN}K;sw~>iErP&0x*`ZCD=gRm=UB^1X(m2bqBh z&>!laOoI0T#h^HvO^oYMM>%Y;)YX@$mU~P01t}+XkaAa--g|L(kht-k3vRp)UT87c zD^S#cn5QR)yH(wCVE~aigqQdN8S<&C-K!++Elxq|Nu(AC=7~GMZ^!9oW|oMng%4BE z8=E2i@O8wB*`NQx+7B%F@g$e2-b8erR7S-g;q~tOJ3VY|J%w`*Xa%VP9{|F;EeY^~ z*ir$=c`%8Y(0ypm2!h8L@z1am2p}9_Fx|)os-jGC0g6MdFzf`|vx1$*4RD7d`;Pvn zG0&c@OfMW}B_qGO%+e8SgDru*y=&ygg(#+wOHMi+Ea1%W-QsI1F{C05^Zm?+xnWnr zkWH1Ae$~*RU&Po8NdfKo3w%0vE-GY@%EG{hH3&+jq-^(o#M>%|2|iSG=iPrt6kN$U zmf-jCAn>trz}3z_dc?%U?(MLO(6p4^v4bB-09OIYR2W89{0fq;op;4%RL3nWSIF4t z=mwZj(q9cTEjHZf2x%L?7aEErCL}2w2ri*9lXS3@#TQ_fnJ*$BC{)}g3ff+wT~n7` zenEkW_*#sQ550Sri9%A?yBP?ajWe_EXM~gSH+RA4YD|xKg}`hO!dO#ST~(*2hOlK8?227L$euke4wage@J z_zFxy71UHg;Vh{@0JZ*o(?g_jt6N%TKXGIuZ3FfoBK0*mI#>)4#=gi7xiAbD4{Ri= zs1aitMD#|FJ|en67E2YG&2nk5U72QBaOdU6?!)QAAQ8rs zkX2AUfIw8r69~U^#}{BaE;Ex8#Vn*P3s?g!dnAO4T-dV~=)=U416NzZ2#Q#Ut}78! z#ptEr82BN;Xuqw0imT=dvEBG3oXeJB#HSBZg%S7})0m026fI52JRk(2Lc6lFvJP{U z{BC`@1a02&_Aq<9ZD+~D|Ay{#6a*vy65uWZ@P9XIX`*8~{I3gkfi!~#H(~0j?UzX7 zI*t|CryXxS-PqhL3i3)s#5*ny0n!d$6=g(4l?!l68TtnlIN(c4;2?+UEM1VrL9B>5 zQL5;e0Y=!yCc`J6Cjv1L$n$mx8YmvHX*|dvij?XjzG1peU7%pZL{e4$!+9pWJbzT(K4 zx}Y7J&dVAzDejG$%YnTzE;LHG48CFlzn5_^PqpX!cSaaE5NPX2o&-ttq^gwUWEdf^ zX{H!L1yV%yzyUVMG!E2}T=8*MTKZ7vKEwxvjSf8j8Qp4@(t!vZ=v33Eo8xdU*&YeQfXS^xuIui2*P|hMpvnW4;B^{ zCT`dSbfnJA9K;#~3n6`slMgNwx8QhEQ$>MoFO^Kexy7H5t}1Z)Tw^b2-X4S&9YyqH z{z}pZ1%i$BJUkk%3gk!dE&Pb<@BqX+Ftf1%E9$hw+ztzasRqTus=RCv5;KFO+6oaw z$;Gox#WA2r2oNQiH%y~D6nXsUQRD(Ft*z0{=TRZk1<}RsSp72wH1r?QE-NR8^M9$g z<{!*o=8fH4T3hH>NlrN$R_!b{d^bn`fP zc<9uK8cD}x#1t$atQ+bvpao%5US@1Bpjy@jjQ!~4GpwtiEC+T?cKMp6fAot2Dg%SS$O-NX!jtf4_J}h#{q&v z%RHF4v#$7y}_ap*XQS000ngeszIi@3G5^$G_v0<4pTcH>yO zK)m3_{pjuW+u=c^rZxlmGv^e6&Fq$)O`7cgW(Fqf-;jwCA1Zn@Z6NFWs2 z^w2>(z&p-`&;)UMP0b>AcXu$g4Gj%!C&|OrXxQ2O;S^dyV`7^x^5-C6z{M&TFVJ{E zS%|gps8^}3cJ}sYr3`YGWB4Qa4wTxJ*4AD*X7;(E;hl2{0)h7L--QO~*7aT_Q&JcS^5?BQk?Q>+@z@(9Oth1-!uCa+YIaK*!sefCQ%u4J6!FT}Atu7-UHfB!B`?)7pK5FA z!68A~hNG*pvfS_KMmKcAS`RvgXbbDpy!XX>K~T^*)z{ZoN_YfFp9OHlQ}ok$^=uH7 ziXmT(d@0Ff!Qmq0BEnekt%hynvRh?`Y<+!PM%~xw34r26Mnp_lY~ZI* zkQh*FYZP7Se9ex)2;Aj5LZ;ydAdjXwy%~BWfDx!dhqlcRZ!3%X+V}hSoi*QjdV1*p ze3$LcJ7SDCR=e}}$re^tK6~Pn|65qfJ|&Ml5n3c~pt5$x2p@!g#t`;*>j@=18Kvyr z7z+cPf$Ec^98T(2#6i}>=i->5`5ELD!OqkF5MQ!uVNpy;@(j%Qv)o+VR&ZSBJ`Lc- z!S#@7D`beMR;vcM3b(nDXaQkiR%Aud9Fe!?3h0Q-6o>@LX1t?2rljOiISm4UpPPfm z0GPf(X|&CcPa8>O4#VEiFcI?+dP-u~X+l4hWs9NNbEvaJYMhT9wlqP<`Ew6Ajs3Xu6WGk56E}BEE$GhzS)1N-+}k1==4Gi747s45A2g_i!XC z*6|ypcJ35_@&wXSKD!nW4Mf@mElx&WUIpLWAk&w|M)uxDEDYFRm^^AQiK9cvyuiQ@ zNz(*uN=Vawj9x+mp~+#(gJD3xH4n#p9Ds-c1_JKR1ko5wF^*nbG92DDlrBF4)c~&B zA0sGG;=FA6^5HtOk&zJu?nJ2=t!Tti;wzy}F#j2?1q#Ak0vdpiArBwkM13x@Ux3-O zF&_bWmZa$|p%CfLi4zYs`#@kUqO`hATcL^3tH#F3>uu?&so?OWrKC#Uz1x2(fe&F) zSQO5uzo=(8RpYOB5K>RvFhGDpbbNEfyhXmLLS*3(Od#(KVI<7@BGT|O*1%z)RUN!X zhC=`n7=*M3PQaWg8nB5|4eP$8Cv$$T#NL2b3XP5q#xH9hnEO2tHFdz73ZnETzzilf2sTyD?9!eiID~MiIdFUkXtoIPJrT+kxlH<7qS*;ai$F!8s-#q6 zTCA_D`;U$%hzJuvM3TT3z739&`;se;Rcz)>DvPLPk@)ef(#R&3@_)`Ej#y0j()hKs zwUt-W?0w14^xv<>#U6m`lkDJzy5`Sltqvk!e^Q!)^D{@fB*H?%PU3)L&{S!NC5Q#s zUvB(1RgUzVdq{o*^gKZ1F3OUnOZln?kfKzRin8gY$49jG^=v!WQLGSp@UQ`+;ktD) zv%8q-i(XWHfs8cbtAR^wOYknwkH(c9>Sa%%i{$_rjB7E!7Morxl6w z4wUPa#;U8PkZX)S$eqY{LbL0eo<4A-EW>(J2}z|>pitOC4lexfuEoSuM6l67TKGPc za~#%=e5E+wyk068`6G-ldtY^P}Uoiqg#V9>DQy4V!ErRG58|BwliK z)b`AoGmpT1NuGO8M5uhzd)@%m!f7IGqoY7FG>D1wDo>7WD;B`-tX8X_Vy=im zBn@pQ`=n=#_}rvYUmAH=?v-a{V)FBIOP~oPpzu^(seYBO0n3Hh3~KTX%!_d6C%#+> z0Pmh1JJT;yDLw>mN(jmjEr1}^7%QMIFe7b&SqAD}u+6+H zo~=mo7N_jhmh+Bm+Cfht02A;41{|T3DJWS=QDaj)O~nWxMM5e#rlDq}rAaad{d0gZ z+<#<${b~R{AL#&;Df^NSqC^DQ?6|o|Vkw1IP|4dmZHtn{U7Mi?Xt?Pys7L5MlI8*QVe!T*s|U z##Am9hz&fc@}}41tIW-*fUx?S-NZ$Dx2a;c>nXdgX5>igu4LS-?;H19n;|wX4qwC! zh7oKL5IGrkQs&E!^fWQ>(OUpd@J(Bdif(Q>l0byz1gm%b!nkrIWg$nSry%pzefAFm zS)pkj>dzCQdPpe~9=ZFziph%VowL(Iiqiu{(f(`ZUBu8|wD{j|Wq#X{dPSB|=ejHD z_kq51iU>$D5Y!;P8;|*j5eU%xf_VLpIx~ENgeg+@FPRZtw3ZA#eaZci-aqd1vw1&y zfBUlN`N+q{r>y|p3i=@_HT7Q6&UGdmP5@g2jsRNCF3PX;VqyW+ z63S`k&nDT@V(4S>jP%p*U8S$rYkrBAm$c+oS06+#&!Gengl@aLub|?WL8NUYU)^TTIB4hn%k*3g1WE@-) zW#WfQX6M1`)gjZ7= z-Ap}$q2;3!6ZDj)U0tQ+ja&c{n2KF~vzHfyz-#lK=gF3RHd4FFPorpNl}lz6Km#$RsKyRd-sRT&e0#+WWnK8W*?1YANrd!Pi>m6MSlJ)$H#d z{`RcT-SK|-;q?Oci3~9X4(C%YbVUlSHujgf?b&C$Q~unYAK00cQY_bZ(CWF{pPkm< z&JD3cltxcM072?44sI50W+7iAPa}^d9#mCreYQ66&9=>@p{A8@Dhq-T1uNhEDkt;B zbSQS2sNr(Pp`692cw>8^Jzqj`n{W>sob(j<(cdrKoh~kT$XQ)W zMeDJLe)jjP?+@VAg?xTSSE8mWA$aQj?eEJO;+5d&i;owQbfQD9=H9R~# ztJ_xWxTiDsXWinD(U$!?$PfO(LNgNn$UnuDo2#9iJ-L2Tn5*Kj_WfVKqFNOWjNJ2m zRwfH{EI)izUw`M^DZGx=VG7rf!H8mY$8eeTb(>uVY6p+cF zrS*Zl7>AqPqJ+6=X(nem2<%iH+L@1T8$e-4YD&Qp=^VQZ9s!)OTen(D$70tVTb}>s zYZ~=en%YoP+P?AFht+YM8aj{3g8lgMOUf{;rvrK-2KpKJ2;A|?*LvX7e zx7N@gWVEs82Va*~sNqg4kB_3;vd6_w*o`?{8Z(H>X(9^S*ot;49*{p|X}He7;HHeL z0=-}HzSk|jdjjeQehz3@8!2v$-tYTzHf|lSf}+jhXg-;n`33Bz=VafPmvbcs^Ia25 zq-xJzuctYIM_-}jw9^s`iat;_%{RB${h+(W&;~XP54NhtDQTN)*yOF_SMDy!v*V7v zpPj93N0`F9uU6F6*+2tJOV0zm1XMG-*6XfUxN|eB2`V=hlsAei{3z+p-Xw09+$0dq zha>DhHM762?sM?{UUABEM~oO+SRE&WeOkoZBeC_{{9NOFY4KcNo+&@p z2kQ1OO^qE6eXkPB=H|zL@0NKxMd|!A@;mgU%JRJ`92@CWlsE+Z_*LF++$&_hY|+LO zxnG6lF=O0LHd4F)nW@>cwEj9-gP$K9gI6glhwNi`Qb!LNr0JoH%-fj@Si;U^PmeDR zKChLbyw`2|x0~Yp?=x+iCN~Z2o-Qm-OTCv+I#a047bn#l&z^n{dK-$zHe)VEiqZ|F z-*jb`SLgQ2jsCilk~5Z;r=`(%%c{v@_v-qTpL)sG7q$Hw^wW+fGVxtiVdCJ?PO|&* zI!B~y%t@;Pk10t!=1WHji8W~YS~qACcd9K*H?P&r?_yDR?lrMe!OARA270#^*ZHif z1{2AICqEvlfY{;M>tK;$bi!J85uPx&gj?B^GUpiN$t7XQ0@Jl znHi@HCT~qmh};_Uv9>&sH+QyW`jfy{hxUV|sb#|>(|y{iraJO*;bLM5rgi~jRR)}&*Gl~A zxU|On&kNOCysTV%$FG|^A7EgZjF1{S{i5Zf=`K8LuvkWtJnU0lbE=IDiWv$Iz0-fH zSLPR(AYdm~yiIG}4y$@g!#=4wukAb^IUa1+l51;Wcl+Imi)I#Tt2dOGnGZ;CX@b&8giY`j+#rR1*ZolBm1odTdP~PfHBjI=LyA+kxv7 zd|&TD`J99EYQwO9o;*7fvL{%$_SSHK?!l0@{ssmd$KDt$T8Sr;<*S0E&X*Er>4;9ie==f)@R3$6z_-$x4!f6X}_Gka7|rvx7`=B_&ZijS)%)go{Qam zcUYA7?z_vCmyh45=opTVe-fXg4h4d&)34{R19usn74S`Yd?w`eL&d|p8XvA;q)z49AqB+Ec{j`b-trK%gRuLoBM22 zu1fDlZ0@S>J?95Jy}5ThJKEIN@?>bu&ClzwhL>h!UgrIz4_v$BC?AhpLSfMhyxXZ@ z4KA*ib@i)t-A);)^K-6RGuCiDSjS@T`Rh(*XN6a?B{}Hbi78%;&B`y$3hzycr%?7@ z_Ty;yxzwRG?(Qi!bp<1X;Ao%QNu^pJrdq7sp4`8Cs>Xz8^>wK&&2^7Y4h+;kqEnZx z`IHpqSuShy{FFE2%3hmU&Q)DKy^b=mt;g@nUd>XcD>ll#@Iv%%O-{b!zg;SnT{6XFCPN~LQchhLqYQt$t9lAOS^)MfTKCi-+| zZrzsk*9T=3SDhM8){vHw3!F=)Ver5NF zDEDvbw0HBMy?w{rEJB6zvc6ZogzXdy)YuQZ^NEhwE{>3v#JsnA*uvkH7yo`Lb7}ao z{Xk>NG0}6fta!oYYz}pJ&3_x#e9p4%<4>=MlXsq|>>Rom1Uu@nn0lF1S9bNBuYb>4 zyM6t}udPq7RBd^@8^=M3iTcCeNS=9_7DEYU-j-fV`f=BV^O<( zc~sj#gGcOKf+@ar1KlB5_;zvqtq-kbRZ2y~xRR9V1E|JdX%R6|B_>%ClD7T2ySFxS zHRVd1w^Led%j{Sgyc0VA`>!jF^qexmU(d8Mw5m=@(2ht7qbtu6eP=6(&j@ayuKZ^C zl{HJ#4J~B%sBGbqQJQETa&{eg>vQ?V^Kz_^QjaKzpxTY(l3Sj)`ds&RSAyxOn-u20*xr)- zJ~AoQSIB8_R8GJ1bs?(NV zGn04`&6_;B38h{xJhJe*Z_i&UXc6W*mio^U3n|w=!Q*$%Vpisyna&>tpD}3sa2<=m>!y{5Zvslwk)^pzWLT-Pb^&f z1rq`bmcOg4s*j!g?DXr2SykWkl#4>3to1c~^0SYrM)cy>!{z((CH;t-k*0EAEmF{SB39WG4z4cx9;Y-C=qzaM`>q?3cgrQCL;NeMfD<0pguix=`r__&r9=RN| ziBmkuim?gb&oOw@eTX<7lf_vE0w}-t9zL&>_lTTkpB(kL8|X+^Va2e>jpH@$pi)g7 yvBKmdZ*A_A>rMU{T}cc9`R6g)|Cj%|IZw%IS{rnb$;O*JRa0GC?Wu~f*M9*ah~|U< literal 0 HcmV?d00001 diff --git a/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md new file mode 100644 index 0000000000000..5d0b241a578d6 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md @@ -0,0 +1,93 @@ +# roi_pointcloud_fusion + +## Purpose + +The node `roi_pointcloud_fusion` is to cluster the pointcloud based on Region Of Interests (ROIs) detected by a 2D object detector, specific for unknown labeled ROI. + +## Inner-workings / Algorithms + +- The pointclouds are projected onto image planes and extracted as cluster if they are inside the unknown labeled ROIs. +- Since the cluster might contain unrelated points from background, then a refinement step is added to filter the background pointcloud by distance to camera. + +![roi_pointcloud_fusion_image](./images/roi_pointcloud_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------ | -------------------------------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | + +### Output + +| Name | Type | Description | +| ----------------- | -------------------------------------------------------- | -------------------------------------------- | +| `output` | `sensor_msgs::msg::PointCloud2` | output pointcloud as default of interface | +| `output_clusters` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | output clusters | +| `debug/clusters` | `sensor_msgs/msg/PointCloud2` | colored cluster pointcloud for visualization | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| ---------------------- | ------ | -------------------------------------------------------------------------------------------- | +| `min_cluster_size` | int | the minimum number of points that a cluster needs to contain in order to be considered valid | +| `cluster_2d_tolerance` | double | cluster tolerance measured in radial direction | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + +- Currently, the refinement is only based on distance to camera, the roi based clustering is supposed to work well with small object ROIs. Others criteria for refinement might needed in the future. + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 016633fa4ef92..5174aebe069a8 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -29,6 +29,9 @@ #include #include #include +#include +#include +#include #include #include @@ -49,6 +52,8 @@ using autoware_auto_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; +using PointCloud = pcl::PointCloud; +using autoware_auto_perception_msgs::msg::ObjectClassification; template class FusionNode : public rclcpp::Node diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp new file mode 100644 index 0000000000000..2b4eb822a9edb --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -0,0 +1,53 @@ +// 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 IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +namespace image_projection_based_fusion +{ +class RoiPointCloudFusionNode +: public FusionNode +{ +private: + int min_cluster_size_{1}; + bool fuse_unknown_only_{true}; + double cluster_2d_tolerance_; + + rclcpp::Publisher::SharedPtr pub_objects_ptr_; + std::vector output_fused_objects_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; + + /* data */ +public: + explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + +protected: + void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + bool out_of_scope(const DetectedObjectWithFeature & obj); +}; + +} // namespace image_projection_based_fusion +#endif // IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp index 541cf997412c2..d7fd3c3882046 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp @@ -32,18 +32,54 @@ #include #endif +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +#include +#include + +#include "autoware_auto_perception_msgs/msg/shape.hpp" +#include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" + +#include +#include +#include + #include #include +#include namespace image_projection_based_fusion { +using PointCloud = pcl::PointCloud; std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time); Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj); +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center); + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects); + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster); +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj); + } // namespace image_projection_based_fusion #endif // IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml new file mode 100644 index 0000000000000..181f4ccb88320 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 32d7a5633b811..5ff99af2ebb21 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs autoware_point_types cv_bridge + euclidean_cluster image_transport lidar_centerpoint message_filters diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 9ce34d8482fbd..447e66f076c0b 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -420,4 +420,5 @@ void FusionNode::publish(const Msg & output_msg) template class FusionNode; template class FusionNode; template class FusionNode; +template class FusionNode; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 77f6a5c506d92..e9cb76b050427 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -171,12 +171,16 @@ void RoiClusterFusionNode::fuseOnSingleImage( for (const auto & feature_obj : input_roi_msg.feature_objects) { int index = 0; double max_iou = 0.0; + bool is_roi_label_known = + feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; for (const auto & cluster_map : m_cluster_roi) { double iou(0.0), iou_x(0.0), iou_y(0.0); if (use_iou_) { iou = calcIoU(cluster_map.second, feature_obj.feature.roi); } - if (use_iou_x_) { + // use for unknown roi to improve small objects like traffic cone detect + // TODO(badai-nguyen): add option to disable roi_cluster mode + if (use_iou_x_ || !is_roi_label_known) { iou_x = calcIoUX(cluster_map.second, feature_obj.feature.roi); } if (use_iou_y_) { @@ -192,8 +196,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( } } if (!output_cluster_msg.feature_objects.empty()) { - bool is_roi_label_known = feature_obj.object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; bool is_roi_existence_prob_higher = output_cluster_msg.feature_objects.at(index).object.existence_probability <= feature_obj.object.existence_probability; diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp new file mode 100644 index 0000000000000..233e568ebee0b --- /dev/null +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -0,0 +1,165 @@ +// 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 "image_projection_based_fusion/roi_pointcloud_fusion/node.hpp" + +#include "image_projection_based_fusion/utils/geometry.hpp" +#include "image_projection_based_fusion/utils/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif + +#include "euclidean_cluster/utils.hpp" + +namespace image_projection_based_fusion +{ +RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("roi_pointcloud_fusion", options) +{ + fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); + min_cluster_size_ = declare_parameter("min_cluster_size"); + cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); + pub_objects_ptr_ = + this->create_publisher("output_clusters", rclcpp::QoS{1}); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); +} + +void RoiPointCloudFusionNode::preprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + return; +} + +void RoiPointCloudFusionNode::postprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + + pub_objects_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + + DetectedObjectsWithFeature output_msg; + output_msg.header = pointcloud_msg.header; + output_msg.feature_objects = output_fused_objects_; + + if (objects_sub_count > 0) { + pub_objects_ptr_->publish(output_msg); + } + output_fused_objects_.clear(); + // publish debug cluster + if (cluster_debug_pub_->get_subscription_count() > 0) { + sensor_msgs::msg::PointCloud2 debug_cluster_msg; + euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + cluster_debug_pub_->publish(debug_cluster_msg); + } +} +void RoiPointCloudFusionNode::fuseOnSingleImage( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, + __attribute__((unused)) const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, + __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) +{ + if (input_pointcloud_msg.data.empty()) { + return; + } + std::vector output_objs; + // select ROIs for fusion + for (const auto & feature_obj : input_roi_msg.feature_objects) { + if (fuse_unknown_only_) { + bool is_roi_label_unknown = feature_obj.object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + if (is_roi_label_unknown) { + output_objs.push_back(feature_obj); + } + } else { + // TODO(badai-nguyen): selected class from a list + output_objs.push_back(feature_obj); + } + } + if (output_objs.empty()) { + return; + } + + // transform pointcloud to camera optical frame id + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); + geometry_msgs::msg::TransformStamped transform_stamped; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, input_roi_msg.header.frame_id, input_pointcloud_msg.header.frame_id, + input_roi_msg.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + sensor_msgs::msg::PointCloud2 transformed_cloud; + tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); + + std::vector clusters; + clusters.resize(output_objs.size()); + + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cloud, "x"), + iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), + iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), + iter_orig_z(input_pointcloud_msg, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) { + if (*iter_z <= 0.0) { + continue; + } + Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + + for (std::size_t i = 0; i < output_objs.size(); ++i) { + auto & feature_obj = output_objs.at(i); + const auto & check_roi = feature_obj.feature.roi; + auto & cluster = clusters.at(i); + + if ( + check_roi.x_offset <= normalized_projected_point.x() && + check_roi.y_offset <= normalized_projected_point.y() && + check_roi.x_offset + check_roi.width >= normalized_projected_point.x() && + check_roi.y_offset + check_roi.height >= normalized_projected_point.y()) { + cluster.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + } + } + } + + // refine and update output_fused_objects_ + updateOutputFusedObjects( + output_objs, clusters, input_pointcloud_msg.header, input_roi_msg.header, tf_buffer_, + min_cluster_size_, cluster_2d_tolerance_, output_fused_objects_); +} + +bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) + const DetectedObjectWithFeature & obj) +{ + return false; +} +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::RoiPointCloudFusionNode) diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 670c5596001fb..76cd1e3c61e82 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "image_projection_based_fusion/utils/utils.hpp" - namespace image_projection_based_fusion { @@ -39,4 +38,197 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) return a; } +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj) +{ + PointCloud2 ros_cluster; + pcl::toROSMsg(cluster, ros_cluster); + ros_cluster.header = header; + feature_obj.feature.cluster = ros_cluster; + feature_obj.object.kinematics.pose_with_covariance.pose.position = getCentroid(ros_cluster); + feature_obj.object.existence_probability = 1.0f; +} + +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center) +{ + // sort point by distance to camera origin + + auto func = [center](const pcl::PointXYZ & p1, const pcl::PointXYZ & p2) { + return tier4_autoware_utils::calcDistance2d(center, p1) < + tier4_autoware_utils::calcDistance2d(center, p2); + }; + std::sort(cluster.begin(), cluster.end(), func); + PointCloud out_cluster; + for (auto & point : cluster) { + if (out_cluster.empty()) { + out_cluster.push_back(point); + continue; + } + if (tier4_autoware_utils::calcDistance2d(out_cluster.back(), point) < cluster_2d_tolerance) { + out_cluster.push_back(point); + continue; + } + if (out_cluster.size() >= static_cast(min_cluster_size)) { + return out_cluster; + } + out_cluster.clear(); + out_cluster.push_back(point); + } + return out_cluster; +} + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects) +{ + if (output_objs.size() != clusters.size()) { + return; + } + Eigen::Vector3d orig_camera_frame, orig_point_frame; + Eigen::Affine3d camera2lidar_affine; + orig_camera_frame << 0.0, 0.0, 0.0; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer, in_cloud_header.frame_id, in_roi_header.frame_id, in_roi_header.stamp); + if (!transform_stamped_optional) { + return; + } + camera2lidar_affine = transformToEigen(transform_stamped_optional.value().transform); + } + orig_point_frame = camera2lidar_affine * orig_camera_frame; + pcl::PointXYZ camera_orig_point_frame = + pcl::PointXYZ(orig_point_frame.x(), orig_point_frame.y(), orig_point_frame.z()); + + for (std::size_t i = 0; i < output_objs.size(); ++i) { + PointCloud cluster = clusters.at(i); + auto & feature_obj = output_objs.at(i); + if (cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + // TODO(badai-nguyen): change to interface to select refine criteria like closest, largest + // to output refine cluster and centroid + auto refine_cluster = + closest_cluster(cluster, cluster_2d_tolerance, min_cluster_size, camera_orig_point_frame); + if (refine_cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + refine_cluster.width = refine_cluster.points.size(); + refine_cluster.height = 1; + refine_cluster.is_dense = false; + // convert cluster to object + convertCluster2FeatureObject(in_cloud_header, refine_cluster, feature_obj); + output_fused_objects.push_back(feature_obj); + } +} + +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj) +{ + if (cluster.empty()) { + return; + } + pcl::PointXYZ centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + float max_z = -1e6; + float min_z = 1e6; + for (const auto & point : cluster) { + centroid.x += point.x; + centroid.y += point.y; + centroid.z += point.z; + max_z = max_z < point.z ? point.z : max_z; + min_z = min_z > point.z ? point.z : min_z; + } + centroid.x = centroid.x / static_cast(cluster.size()); + centroid.y = centroid.y / static_cast(cluster.size()); + centroid.z = centroid.z / static_cast(cluster.size()); + + std::vector cluster2d; + std::vector cluster2d_convex; + + for (size_t i = 0; i < cluster.size(); ++i) { + cluster2d.push_back( + cv::Point((cluster.at(i).x - centroid.x) * 1000.0, (cluster.at(i).y - centroid.y) * 1000.)); + } + cv::convexHull(cluster2d, cluster2d_convex); + if (cluster2d_convex.empty()) { + return; + } + pcl::PointXYZ polygon_centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + polygon_centroid.x += static_cast(cluster2d_convex.at(i).x) / 1000.0; + polygon_centroid.y += static_cast(cluster2d_convex.at(i).y) / 1000.0; + } + polygon_centroid.x = polygon_centroid.x / static_cast(cluster2d_convex.size()); + polygon_centroid.y = polygon_centroid.y / static_cast(cluster2d_convex.size()); + + autoware_auto_perception_msgs::msg::Shape shape; + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + geometry_msgs::msg::Point32 point; + point.x = cluster2d_convex.at(i).x / 1000.0; + point.y = cluster2d_convex.at(i).y / 1000.0; + point.z = 0.0; + shape.footprint.points.push_back(point); + } + shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + constexpr float eps = 0.01; + shape.dimensions.x = 0; + shape.dimensions.y = 0; + shape.dimensions.z = std::max((max_z - min_z), eps); + feature_obj.object.shape = shape; + feature_obj.object.kinematics.pose_with_covariance.pose.position.x = + centroid.x + polygon_centroid.x; + feature_obj.object.kinematics.pose_with_covariance.pose.position.y = + centroid.y + polygon_centroid.y; + feature_obj.object.kinematics.pose_with_covariance.pose.position.z = + min_z + shape.dimensions.z * 0.5; + feature_obj.object.existence_probability = 1.0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.x = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.y = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.z = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.w = 1; +} + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +{ + geometry_msgs::msg::Point centroid; + centroid.x = 0.0f; + centroid.y = 0.0f; + centroid.z = 0.0f; + for (sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"), + iter_y(pointcloud, "y"), iter_z(pointcloud, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + centroid.x += *iter_x; + centroid.y += *iter_y; + centroid.z += *iter_z; + } + const size_t size = pointcloud.width * pointcloud.height; + centroid.x = centroid.x / static_cast(size); + centroid.y = centroid.y / static_cast(size); + centroid.z = centroid.z / static_cast(size); + return centroid; +} + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) +{ + pcl::PointXYZ closest_point; + double min_dist = 1e6; + pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); + for (std::size_t i = 0; i < cluster.points.size(); ++i) { + pcl::PointXYZ point = cluster.points.at(i); + double dist_closest_point = tier4_autoware_utils::calcDistance2d(point, orig_point); + if (min_dist > dist_closest_point) { + min_dist = dist_closest_point; + closest_point = pcl::PointXYZ(point.x, point.y, point.z); + } + } + return closest_point; +} + } // namespace image_projection_based_fusion From f5176c07bd542b57c645c2764e7eec7f937c837b Mon Sep 17 00:00:00 2001 From: Shunsuke Miura Date: Thu, 14 Sep 2023 02:18:08 +0900 Subject: [PATCH 21/93] fix: camera lidar fusion launch Signed-off-by: Shunsuke Miura --- .../camera_lidar_fusion_based_detection.launch.xml | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 34c2fd4cdc27c..be9b70c622c92 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -331,22 +331,11 @@ - - - - - - - - - - - - + From 982526d2b1550dd1883841184fdb566fbddb81e0 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 28 Aug 2023 11:42:47 +0900 Subject: [PATCH 22/93] refactor(behavior_path_planner): renaming and comment collision check debug (#4749) * refactor(behavior_path_planner): renaming and comment collision check debug Signed-off-by: Zulfaqar Azmi * fix spell check Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../marker_utils/utils.hpp | 34 +++++++++---------- .../src/marker_utils/lane_change/debug.cpp | 15 ++++---- .../scene_module/lane_change/interface.cpp | 8 ++--- .../src/scene_module/lane_change/normal.cpp | 22 ++++++------ .../path_safety_checker/safety_check.cpp | 22 ++++++------ 5 files changed, 50 insertions(+), 51 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 6894edaece828..08ec9fabb2afe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -51,24 +51,22 @@ using visualization_msgs::msg::MarkerArray; struct CollisionCheckDebug { - std::string failed_reason; - std::size_t lane_id{0}; - Pose current_pose{}; - Twist current_twist{}; - Twist object_twist{}; - Pose expected_ego_pose{}; - Pose expected_obj_pose{}; - Pose relative_to_ego{}; - double rss_longitudinal{0.0}; - double ego_to_obj_margin{0.0}; - double longitudinal_offset{0.0}; - double lateral_offset{0.0}; - bool is_front{false}; - bool allow_lane_change{false}; - std::vector lerped_path; - std::vector ego_predicted_path{}; - Polygon2d ego_polygon{}; - Polygon2d obj_polygon{}; + std::string unsafe_reason; ///< Reason indicating unsafe situation. + Pose current_pose{}; ///< Ego vehicle's current pose. + Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. + Twist object_twist{}; ///< Detected object's velocity and rotation. + Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose expected_obj_pose{}; ///< Predicted future pose of object. + double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. + double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. + double extended_polygon_lon_offset{0.0}; ///< Longitudinal offset for extended polygon. + double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. + bool is_front{false}; ///< True if object is in front of ego vehicle. + bool is_safe{false}; ///< True if situation is deemed safe. + std::vector lerped_path; ///< Interpolated ego vehicle path. + std::vector ego_predicted_path{}; ///< Predicted future path of ego vehicle. + Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. + Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. }; using CollisionCheckDebugMap = std::unordered_map; diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index fca53930a0ec5..cd0236f909b14 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -56,12 +56,13 @@ MarkerArray showObjectInfo( std::ostringstream ss; - ss << "Idx: " << ++idx << "\nReason: " << info.failed_reason + ss << "Idx: " << ++idx << "\nReason: " << info.unsafe_reason << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal - << "\nEgo to obj: " << info.ego_to_obj_margin << "\nLateral offset: " << info.lateral_offset - << "\nLongitudinal offset: " << info.longitudinal_offset + << "\nEgo to obj: " << info.inter_vehicle_distance + << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset + << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset << "\nPosition: " << (info.is_front ? "front" : "back") - << "\nSafe: " << (info.allow_lane_change ? "Yes" : "No"); + << "\nSafe: " << (info.is_safe ? "Yes" : "No"); obj_marker.text = ss.str(); @@ -195,8 +196,8 @@ MarkerArray showPolygon( int32_t idx = {0}; for (const auto & [uuid, info] : obj_debug_vec) { - const auto & color = info.allow_lane_change ? green_color : red_color; - const auto & ego_polygon = info.ego_polygon.outer(); + const auto & color = info.is_safe ? green_color : red_color; + const auto & ego_polygon = info.extended_ego_polygon.outer(); const auto poly_z = info.current_pose.position.z; // temporally ego_marker.id = ++id; ego_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); @@ -214,7 +215,7 @@ MarkerArray showPolygon( marker_array.markers.push_back(text_marker); - const auto & obj_polygon = info.obj_polygon.outer(); + const auto & obj_polygon = info.extended_obj_polygon.outer(); obj_marker.id = ++id; obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); obj_marker.points.reserve(obj_polygon.size()); 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 e33b1d9b13873..2f62728e1e323 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 @@ -334,11 +334,11 @@ std::shared_ptr LaneChangeInterface::get_debug_msg_arra for (const auto & [uuid, debug_data] : debug_data) { LaneChangeDebugMsg debug_msg; debug_msg.object_id = uuid; - debug_msg.allow_lane_change = debug_data.allow_lane_change; + debug_msg.allow_lane_change = debug_data.is_safe; debug_msg.is_front = debug_data.is_front; - debug_msg.relative_distance = debug_data.relative_to_ego; - debug_msg.failed_reason = debug_data.failed_reason; - debug_msg.velocity = debug_data.object_twist.linear.x; + debug_msg.failed_reason = debug_data.unsafe_reason; + debug_msg.velocity = + std::hypot(debug_data.object_twist.linear.x, debug_data.object_twist.linear.y); debug_msg_array.lane_change_info.push_back(debug_msg); } lane_change_debug_msg_array_ = debug_msg_array; 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 eee17d79af648..1ab183e0df082 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 @@ -1258,17 +1258,17 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( return std::make_pair(tier4_autoware_utils::toHexString(obj.uuid), debug); }; - const auto updateDebugInfo = - [&debug_data](std::pair & obj, bool is_allowed) { - const auto & key = obj.first; - auto & element = obj.second; - element.allow_lane_change = is_allowed; - if (debug_data.find(key) != debug_data.end()) { - debug_data[key] = element; - } else { - debug_data.insert(obj); - } - }; + const auto updateDebugInfo = [&debug_data]( + std::pair & obj, bool is_safe) { + const auto & key = obj.first; + auto & element = obj.second; + element.is_safe = is_safe; + if (debug_data.find(key) != debug_data.end()) { + debug_data[key] = element; + } else { + debug_data.insert(obj); + } + }; for (const auto & obj : collision_check_objects) { auto current_debug_data = assignDebugData(obj); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 6dd3b627e67f4..7a12f264b45a2 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -81,8 +81,8 @@ Polygon2d createExtendedPolygon( const double lat_offset = width / 2.0 + lat_margin; { - debug.longitudinal_offset = lon_offset; - debug.lateral_offset = lat_offset; + debug.extended_polygon_lon_offset = lon_offset; + debug.extended_polygon_lat_offset = lat_offset; } const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0); @@ -132,8 +132,8 @@ Polygon2d createExtendedPolygon( const double right_lat_offset = min_y - lat_margin; { - debug.longitudinal_offset = lon_offset; - debug.lateral_offset = (left_lat_offset + right_lat_offset) / 2; + debug.extended_polygon_lon_offset = lon_offset; + debug.extended_polygon_lat_offset = (left_lat_offset + right_lat_offset) / 2; } const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0); @@ -267,13 +267,13 @@ bool checkCollision( 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; + debug.extended_ego_polygon = ego_polygon; + debug.extended_obj_polygon = obj_polygon; } // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { - debug.failed_reason = "overlap_polygon"; + debug.unsafe_reason = "overlap_polygon"; return false; } @@ -304,15 +304,15 @@ bool checkCollision( { 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.inter_vehicle_distance = min_lon_length; + debug.extended_ego_polygon = extended_ego_polygon; + debug.extended_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"; + debug.unsafe_reason = "overlap_extended_polygon"; return false; } } From 82fa635eb80af143068626aa3c607b19ab482b30 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 29 Aug 2023 21:39:29 +0900 Subject: [PATCH 23/93] refactor(behavior_path_planner): type aliasing for collision check debug type (#4796) Signed-off-by: Zulfaqar Azmi --- .../marker_utils/lane_change/debug.hpp | 15 ++---- .../marker_utils/utils.hpp | 32 ++++-------- .../scene_module/lane_change/base_class.hpp | 2 +- .../scene_module/lane_change/normal.hpp | 6 +-- .../utils/avoidance/avoidance_module_data.hpp | 2 +- .../path_safety_checker_parameters.hpp | 27 ++++++++++ .../path_safety_checker/safety_check.hpp | 2 +- .../src/marker_utils/lane_change/debug.cpp | 50 ++----------------- .../src/marker_utils/utils.cpp | 23 +++++++++ .../avoidance/avoidance_module.cpp | 2 +- .../src/scene_module/lane_change/normal.cpp | 31 +++--------- .../test/test_safety_check.cpp | 2 +- 12 files changed, 82 insertions(+), 112 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp index 6bcc9cee877b4..808e76a140761 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp @@ -25,20 +25,13 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; -using marker_utils::CollisionCheckDebug; using visualization_msgs::msg::MarkerArray; -MarkerArray showObjectInfo( - const std::unordered_map & obj_debug_vec, std::string && ns); +MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray showAllValidLaneChangePath( const std::vector & lanes, std::string && ns); -MarkerArray showLerpedPose( - const std::unordered_map & obj_debug_vec, std::string && ns); -MarkerArray showEgoPredictedPaths( - const std::unordered_map & obj_debug_vec, std::string && ns); -MarkerArray showPolygon( - const std::unordered_map & obj_debug_vec, std::string && ns); -MarkerArray showPolygonPose( - const std::unordered_map & obj_debug_vec, std::string && ns); +MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); +MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 08ec9fabb2afe..a32a943e65998 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -15,6 +15,7 @@ #define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -29,7 +30,6 @@ #include #include -#include #include namespace marker_utils @@ -39,6 +39,9 @@ using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::DrivableLanes; using behavior_path_planner::ShiftLineArray; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; @@ -49,27 +52,6 @@ using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -struct CollisionCheckDebug -{ - std::string unsafe_reason; ///< Reason indicating unsafe situation. - Pose current_pose{}; ///< Ego vehicle's current pose. - Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. - Twist object_twist{}; ///< Detected object's velocity and rotation. - Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. - Pose expected_obj_pose{}; ///< Predicted future pose of object. - double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. - double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. - double extended_polygon_lon_offset{0.0}; ///< Longitudinal offset for extended polygon. - double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. - bool is_front{false}; ///< True if object is in front of ego vehicle. - bool is_safe{false}; ///< True if situation is deemed safe. - std::vector lerped_path; ///< Interpolated ego vehicle path. - std::vector ego_predicted_path{}; ///< Predicted future path of ego vehicle. - Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. - Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. -}; -using CollisionCheckDebugMap = std::unordered_map; - constexpr std::array, 10> colorsList() { constexpr std::array red = {1., 0., 0.}; @@ -85,12 +67,16 @@ constexpr std::array, 10> colorsList() return {red, green, blue, yellow, aqua, magenta, medium_orchid, light_pink, light_yellow, light_steel_blue}; } - inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); } +CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); + +void updateCollisionCheckDebugMap( + CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 6ca06ff21c4a9..f30bf31d4e010 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -43,11 +43,11 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebugMap; using route_handler::Direction; using tier4_autoware_utils::StopWatch; using tier4_planning_msgs::msg::LaneChangeDebugMsg; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 00c60cbfa8a6d..9f94c75aeba20 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -26,6 +26,8 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; @@ -33,8 +35,6 @@ using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygo using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; -using marker_utils::CollisionCheckDebugMap; using route_handler::Direction; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; @@ -141,7 +141,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, const utils::path_safety_checker::RSSparams & rss_params, - std::unordered_map & debug_data) const; + CollisionCheckDebugMap & debug_data) const; rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); }; 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 27cd89e7756cc..eb2746f93ea83 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 @@ -48,7 +48,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; -using marker_utils::CollisionCheckDebug; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; struct ObjectParameter { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index c4a6a86861e6c..87c93f49901f0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -19,13 +19,18 @@ #include #include +#include +#include +#include +#include #include namespace behavior_path_planner::utils::path_safety_checker { using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; using tier4_autoware_utils::Polygon2d; struct PoseWithVelocity @@ -171,6 +176,28 @@ struct SafetyCheckParams bool publish_debug_marker{false}; ///< Option to publish debug markers. }; +struct CollisionCheckDebug +{ + std::string unsafe_reason; ///< Reason indicating unsafe situation. + Pose current_pose{}; ///< Ego vehicle's current pose. + Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. + Twist object_twist{}; ///< Detected object's velocity and rotation. + Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose expected_obj_pose{}; ///< Predicted future pose of object. + double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. + double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. + double extended_polygon_lon_offset{0.0}; ///< Longitudinal offset for extended polygon. + double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. + bool is_front{false}; ///< True if object is in front of ego vehicle. + bool is_safe{false}; ///< True if situation is deemed safe. + std::vector lerped_path; ///< Interpolated ego vehicle path. + Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. + Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. +}; +using CollisionCheckDebugPair = std::pair; +using CollisionCheckDebugMap = + std::unordered_map; + } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index a0d4282c5f232..ba501bf8e0757 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -43,9 +43,9 @@ namespace behavior_path_planner::utils::path_safety_checker using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_perception_msgs::msg::Shape; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index cd0236f909b14..4a76c7e7dfb11 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -36,8 +36,7 @@ using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; -MarkerArray showObjectInfo( - const std::unordered_map & obj_debug_vec, std::string && ns) +MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) { Marker obj_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, @@ -105,8 +104,7 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes return marker_array; } -MarkerArray showLerpedPose( - const std::unordered_map & obj_debug_vec, std::string && ns) +MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) { MarkerArray marker_array; int32_t id{0}; @@ -128,46 +126,7 @@ MarkerArray showLerpedPose( return marker_array; } -MarkerArray showEgoPredictedPaths( - const std::unordered_map & obj_debug_vec, std::string && ns) -{ - if (obj_debug_vec.empty()) { - return MarkerArray{}; - } - - MarkerArray marker_array; - constexpr auto colors = colorsList(); - constexpr float scale_val = 0.2; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - int32_t id{0}; - for (const auto & [uuid, info] : obj_debug_vec) { - const auto loop_size = std::min(info.ego_predicted_path.size(), colors.size()); - - for (std::size_t idx = 0; idx < loop_size; ++idx) { - const auto & path = info.ego_predicted_path.at(idx).path; - const auto & color = colors.at(idx); - - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::LINE_STRIP, - createMarkerScale(scale_val, scale_val, scale_val), - createMarkerColor(color[0], color[1], color[2], 0.9)); - - marker.points.reserve(path.size()); - - for (const auto & point : path) { - marker.points.push_back(point.position); - } - - marker_array.markers.push_back(marker); - } - } - return marker_array; -} - -MarkerArray showPolygon( - const std::unordered_map & obj_debug_vec, std::string && ns) +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) { if (obj_debug_vec.empty()) { return MarkerArray{}; @@ -235,8 +194,7 @@ MarkerArray showPolygon( return marker_array; } -MarkerArray showPolygonPose( - const std::unordered_map & obj_debug_vec, std::string && ns) +MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) { constexpr auto colors = colorsList(); const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index 80fea95b03caa..d0308bc275a3f 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -23,6 +24,7 @@ namespace marker_utils { using behavior_path_planner::ShiftLine; using behavior_path_planner::utils::calcPathArcLengthArray; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; @@ -32,6 +34,27 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; +CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) +{ + CollisionCheckDebug debug; + debug.current_pose = obj.initial_pose.pose; + debug.current_twist = obj.initial_twist.twist; + return {tier4_autoware_utils::toHexString(obj.uuid), debug}; +} + +void updateCollisionCheckDebugMap( + CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe) +{ + auto & [key, element] = object_debug; + element.is_safe = is_safe; + if (debug_map.find(key) != debug_map.end()) { + debug_map[key] = element; + return; + } + + debug_map.insert(object_debug); +} + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) 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 40fc6ef7b53a6..aa649b138eb82 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 @@ -45,7 +45,7 @@ namespace behavior_path_planner { -using marker_utils::CollisionCheckDebug; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; 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 1ab183e0df082..9c24dbbd9a06c 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 @@ -14,6 +14,7 @@ #include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" @@ -1216,7 +1217,7 @@ bool NormalLaneChange::getAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, const utils::path_safety_checker::RSSparams & rss_params, - std::unordered_map & debug_data) const + CollisionCheckDebugMap & debug_data) const { PathSafetyStatus path_safety_status; @@ -1251,28 +1252,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( target_objects.other_lane.end()); } - const auto assignDebugData = [](const ExtendedPredictedObject & obj) { - CollisionCheckDebug 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 = [&debug_data]( - std::pair & obj, bool is_safe) { - const auto & key = obj.first; - auto & element = obj.second; - element.is_safe = is_safe; - if (debug_data.find(key) != debug_data.end()) { - debug_data[key] = element; - } else { - debug_data.insert(obj); - } - }; - for (const auto & obj : collision_check_objects) { - auto current_debug_data = assignDebugData(obj); - current_debug_data.second.ego_predicted_path.push_back(debug_predicted_path); + auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { @@ -1280,7 +1261,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, current_debug_data.second)) { path_safety_status.is_safe = false; - updateDebugInfo(current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap( + debug_data, current_debug_data, path_safety_status.is_safe); 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 |= @@ -1288,7 +1270,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( path, current_pose, common_parameters.vehicle_info, obj_polygon); } } - updateDebugInfo(current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap( + debug_data, current_debug_data, path_safety_status.is_safe); } return path_safety_status; diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index e1b74636f5ed1..9dfcef487cc81 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -27,10 +27,10 @@ constexpr double epsilon = 1e-6; using autoware_auto_perception_msgs::msg::Shape; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; From bd757086833b43590925d285a24fdb2339d87f47 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Tue, 29 Aug 2023 23:07:20 +0900 Subject: [PATCH 24/93] refactor(behavior_path_planner): use function with color name (#4797) * refactor(behavior_path_planner): use function with color name Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../marker_utils/colors.hpp | 94 +++++++++++++++++++ .../marker_utils/utils.hpp | 15 --- .../src/marker_utils/lane_change/debug.cpp | 32 +++---- 3 files changed, 108 insertions(+), 33 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp new file mode 100644 index 0000000000000..82c387d04e525 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp @@ -0,0 +1,94 @@ +// 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 BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ +#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ + +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "std_msgs/msg/color_rgba.hpp" + +#include + +namespace marker_utils::colors +{ +using std_msgs::msg::ColorRGBA; + +inline ColorRGBA red(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 0., 0., a); +} + +inline ColorRGBA green(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0., 1., 0., a); +} + +inline ColorRGBA blue(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0., 0., 1., a); +} + +inline ColorRGBA yellow(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 1., 0., a); +} + +inline ColorRGBA aqua(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0., 1., 1., a); +} + +inline ColorRGBA magenta(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 0., 1., a); +} + +inline ColorRGBA medium_orchid(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0.729, 0.333, 0.827, a); +} + +inline ColorRGBA light_pink(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 0.713, 0.756, a); +} + +inline ColorRGBA light_yellow(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 1., 0.878, a); +} + +inline ColorRGBA light_steel_blue(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0.690, 0.768, 0.870, a); +} + +inline ColorRGBA white(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 1., 1., a); +} + +inline ColorRGBA grey(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(.5, .5, .5, a); +} + +inline std::vector colors_list(float a = 0.99) +{ + return {red(a), green(a), blue(a), yellow(a), aqua(a), + magenta(a), medium_orchid(a), light_pink(a), light_yellow(a), light_steel_blue(a)}; +} +} // namespace marker_utils::colors + +#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index a32a943e65998..1c68f51cba714 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -52,21 +52,6 @@ using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -constexpr std::array, 10> colorsList() -{ - constexpr std::array red = {1., 0., 0.}; - constexpr std::array green = {0., 1., 0.}; - constexpr std::array blue = {0., 0., 1.}; - constexpr std::array yellow = {1., 1., 0.}; - constexpr std::array aqua = {0., 1., 1.}; - constexpr std::array magenta = {1., 0., 1.}; - constexpr std::array medium_orchid = {0.729, 0.333, 0.827}; - constexpr std::array light_pink = {1, 0.713, 0.756}; - constexpr std::array light_yellow = {1, 1, 0.878}; - constexpr std::array light_steel_blue = {0.690, 0.768, 0.870}; - return {red, green, blue, yellow, aqua, - magenta, medium_orchid, light_pink, light_yellow, light_steel_blue}; -} inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index 4a76c7e7dfb11..aab2be6f3054e 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "behavior_path_planner/marker_utils/colors.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" @@ -33,14 +34,13 @@ namespace marker_utils::lane_change_markers { using geometry_msgs::msg::Point; using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) { Marker obj_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(0.0, 1.0, 1.0, 0.999)); + createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); MarkerArray marker_array; int32_t id{0}; @@ -80,7 +80,7 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes int32_t id{0}; const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - constexpr auto colors = colorsList(); + const auto colors = colors::colors_list(); const auto loop_size = std::min(lanes.size(), colors.size()); marker_array.markers.reserve(loop_size); @@ -91,8 +91,7 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes const auto & color = colors.at(idx); Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), - createMarkerColor(color[0], color[1], color[2], 0.9)); + "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), color); marker.points.reserve(lanes.at(idx).path.points.size()); for (const auto & point : lanes.at(idx).path.points) { @@ -114,7 +113,7 @@ MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::st for (const auto & [uuid, info] : obj_debug_vec) { Marker marker = createDefaultMarker( "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3), - createMarkerColor(1.0, 0.3, 1.0, 0.9)); + colors::magenta()); marker.points.reserve(info.lerped_path.size()); for (const auto & point : info.lerped_path) { @@ -137,17 +136,15 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); Marker ego_marker = createDefaultMarker( "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val), - createMarkerColor(1.0, 1.0, 1.0, 0.9)); + colors::green()); Marker obj_marker = ego_marker; auto text_marker = createDefaultMarker( "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(1.5, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + createMarkerScale(1.5, 1.5, 1.5), colors::white()); MarkerArray marker_array; - constexpr auto red_color = colorsList().at(0); - constexpr auto green_color = colorsList().at(1); const auto reserve_size = obj_debug_vec.size(); marker_array.markers.reserve(reserve_size * 4); @@ -155,11 +152,11 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin int32_t idx = {0}; for (const auto & [uuid, info] : obj_debug_vec) { - const auto & color = info.is_safe ? green_color : red_color; + const auto color = info.is_safe ? colors::green() : colors::red(); const auto & ego_polygon = info.extended_ego_polygon.outer(); const auto poly_z = info.current_pose.position.z; // temporally ego_marker.id = ++id; - ego_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); + ego_marker.color = color; ego_marker.points.reserve(ego_polygon.size()); for (const auto & p : ego_polygon) { ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); @@ -176,7 +173,7 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin const auto & obj_polygon = info.extended_obj_polygon.outer(); obj_marker.id = ++id; - obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); + obj_marker.color = color; obj_marker.points.reserve(obj_polygon.size()); for (const auto & p : obj_polygon) { obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); @@ -196,7 +193,7 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) { - constexpr auto colors = colorsList(); + const auto colors = colors::colors_list(); const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); MarkerArray marker_array; int32_t id{0}; @@ -207,8 +204,7 @@ MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::s for (const auto & [uuid, info] : obj_debug_vec) { const auto & color = colors.at(idx); Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), - createMarkerColor(color[0], color[1], color[2], 0.999)); + "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), color); marker.points.reserve(2); marker.points.push_back(info.expected_ego_pose.position); marker.points.push_back(info.expected_obj_pose.position); @@ -232,7 +228,7 @@ MarkerArray createLaneChangingVirtualWallMarker( { auto wall_marker = createDefaultMarker( "map", now, ns + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(0.1, 5.0, 2.0), createMarkerColor(0.0, 1.0, 0.0, 0.5)); + createMarkerScale(0.1, 5.0, 2.0), colors::green()); wall_marker.pose = lane_changing_pose; wall_marker.pose.position.z += 1.0; marker_array.markers.push_back(wall_marker); @@ -241,7 +237,7 @@ MarkerArray createLaneChangingVirtualWallMarker( { auto text_marker = createDefaultMarker( "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + createMarkerScale(0.0, 0.0, 1.0), colors::white()); text_marker.pose = lane_changing_pose; text_marker.pose.position.z += 2.0; text_marker.text = module_name; From 0133b92fb6e3aa887729e8e047c5a0d064779ca7 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 25 Aug 2023 21:43:08 +0900 Subject: [PATCH 25/93] feat(behavior_path_planner): visualize ego predicted path (#4743) visualize ego predicted path Signed-off-by: kyoichi-sugahara --- .../marker_utils/utils.hpp | 4 ++ .../path_safety_checker/safety_check.hpp | 3 + .../src/marker_utils/utils.cpp | 55 +++++++++++++++++++ .../path_safety_checker/safety_check.cpp | 13 +++++ 4 files changed, 75 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 1c68f51cba714..017747b331475 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -100,6 +100,10 @@ MarkerArray createObjectsMarkerArray( MarkerArray createDrivableLanesMarkerArray( const std::vector & drivable_lanes, std::string && ns); +MarkerArray createPredictedPathMarkerArray( + const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, + std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + } // namespace marker_utils #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index ba501bf8e0757..a9404038d831a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -66,6 +66,9 @@ Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, CollisionCheckDebug & debug); +PredictedPath convertToPredictedPath( + const std::vector & path, const double time_resolution); + double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, const RSSparams & rss_params); diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index d0308bc275a3f..d1148f50c7b5b 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -431,4 +431,59 @@ MarkerArray createDrivableLanesMarkerArray( return msg; } +MarkerArray createPredictedPathMarkerArray( + const PredictedPath & predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, + std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) +{ + if (predicted_path.path.empty()) { + return MarkerArray{}; + } + + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + const auto & path = predicted_path.path; + + Marker marker = createDefaultMarker( + "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), + + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + for (size_t i = 0; i < path.size(); ++i) { + marker.id = i + id; + marker.points.clear(); + + const auto & predicted_path_pose = path.at(i); + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, -half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, -half_width, 0.0) + .position); + marker.points.push_back(marker.points.front()); + + marker_array.markers.push_back(marker); + } + return marker_array; + + marker.points.reserve(path.size()); + for (const auto & point : path) { + marker.points.push_back(point.position); + } + msg.markers.push_back(marker); + return msg; +} + } // namespace marker_utils diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 7a12f264b45a2..2939ca29c0a40 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -152,6 +152,19 @@ Polygon2d createExtendedPolygon( : tier4_autoware_utils::inverseClockwise(polygon); } +PredictedPath convertToPredictedPath( + const std::vector & path, const double time_resolution) +{ + PredictedPath predicted_path; + predicted_path.time_step = rclcpp::Duration::from_seconds(time_resolution); + predicted_path.path.resize(path.size()); + + for (size_t i = 0; i < path.size(); ++i) { + predicted_path.path.at(i) = path.at(i).pose; + } + return predicted_path; +} + double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, const RSSparams & rss_params) From f8233de7d1163224551eed6e10fdf84f991f8c48 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 31 Aug 2023 11:39:06 +0900 Subject: [PATCH 26/93] refactor(behavior_path_plannner): commonize collision check markers (#4798) * refactor(behavior_path_plannner): commonize collision check markers Signed-off-by: Muhammad Zulfaqar Azmi * Update planning/behavior_path_planner/src/marker_utils/utils.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../marker_utils/lane_change/debug.hpp | 4 - .../marker_utils/utils.hpp | 5 + .../path_safety_checker_parameters.hpp | 11 +- .../src/marker_utils/lane_change/debug.cpp | 149 ---------------- .../src/marker_utils/utils.cpp | 161 +++++++++++++++++- .../scene_module/lane_change/interface.cpp | 15 +- .../path_safety_checker/safety_check.cpp | 7 +- 7 files changed, 183 insertions(+), 169 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp index 808e76a140761..d337bae17ca6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp @@ -26,12 +26,8 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; using visualization_msgs::msg::MarkerArray; -MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray showAllValidLaneChangePath( const std::vector & lanes, std::string && ns); -MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); -MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); -MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 017747b331475..c9a15bd5f42f1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -104,6 +104,11 @@ MarkerArray createPredictedPathMarkerArray( const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); } // namespace marker_utils #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 87c93f49901f0..93eb0457d7cf4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -179,10 +179,10 @@ struct SafetyCheckParams struct CollisionCheckDebug { std::string unsafe_reason; ///< Reason indicating unsafe situation. - Pose current_pose{}; ///< Ego vehicle's current pose. Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. - Twist object_twist{}; ///< Detected object's velocity and rotation. Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose current_obj_pose{}; ///< Detected object's current pose. + Twist object_twist{}; ///< Detected object's velocity and rotation. Pose expected_obj_pose{}; ///< Predicted future pose of object. double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. @@ -190,9 +190,10 @@ struct CollisionCheckDebug double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. bool is_front{false}; ///< True if object is in front of ego vehicle. bool is_safe{false}; ///< True if situation is deemed safe. - std::vector lerped_path; ///< Interpolated ego vehicle path. - Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. - Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. + std::vector ego_predicted_path; ///< ego vehicle's predicted path. + std::vector obj_predicted_path; ///< object's predicted path. + Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. + Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. }; using CollisionCheckDebugPair = std::pair; using CollisionCheckDebugMap = diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index aab2be6f3054e..7493982db78b1 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -36,40 +36,6 @@ using geometry_msgs::msg::Point; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerScale; -MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - Marker obj_marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); - - MarkerArray marker_array; - int32_t id{0}; - - marker_array.markers.reserve(obj_debug_vec.size()); - - int idx{0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - obj_marker.id = ++id; - obj_marker.pose = info.current_pose; - - std::ostringstream ss; - - ss << "Idx: " << ++idx << "\nReason: " << info.unsafe_reason - << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal - << "\nEgo to obj: " << info.inter_vehicle_distance - << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset - << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset - << "\nPosition: " << (info.is_front ? "front" : "back") - << "\nSafe: " << (info.is_safe ? "Yes" : "No"); - - obj_marker.text = ss.str(); - - marker_array.markers.push_back(obj_marker); - } - return marker_array; -} - MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) { if (lanes.empty()) { @@ -103,121 +69,6 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes return marker_array; } -MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - MarkerArray marker_array; - int32_t id{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3), - colors::magenta()); - marker.points.reserve(info.lerped_path.size()); - - for (const auto & point : info.lerped_path) { - marker.points.push_back(point.position); - } - - marker_array.markers.push_back(marker); - } - return marker_array; -} - -MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - if (obj_debug_vec.empty()) { - return MarkerArray{}; - } - - constexpr float scale_val{0.2}; - int32_t id{0}; - const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); - Marker ego_marker = createDefaultMarker( - "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val), - colors::green()); - Marker obj_marker = ego_marker; - - auto text_marker = createDefaultMarker( - "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(1.5, 1.5, 1.5), colors::white()); - - MarkerArray marker_array; - - const auto reserve_size = obj_debug_vec.size(); - - marker_array.markers.reserve(reserve_size * 4); - - int32_t idx = {0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto color = info.is_safe ? colors::green() : colors::red(); - const auto & ego_polygon = info.extended_ego_polygon.outer(); - const auto poly_z = info.current_pose.position.z; // temporally - ego_marker.id = ++id; - ego_marker.color = color; - ego_marker.points.reserve(ego_polygon.size()); - for (const auto & p : ego_polygon) { - ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(ego_marker); - - std::ostringstream ss; - text_marker.id = ego_marker.id; - ss << ++idx; - text_marker.text = ss.str(); - text_marker.pose = info.expected_ego_pose; - - marker_array.markers.push_back(text_marker); - - const auto & obj_polygon = info.extended_obj_polygon.outer(); - obj_marker.id = ++id; - obj_marker.color = color; - obj_marker.points.reserve(obj_polygon.size()); - for (const auto & p : obj_polygon) { - obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(obj_marker); - - text_marker.id = obj_marker.id; - text_marker.pose = info.expected_obj_pose; - marker_array.markers.push_back(text_marker); - - ego_marker.points.clear(); - obj_marker.points.clear(); - } - - return marker_array; -} - -MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - const auto colors = colors::colors_list(); - const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); - MarkerArray marker_array; - int32_t id{0}; - size_t idx{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto & color = colors.at(idx); - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), color); - marker.points.reserve(2); - marker.points.push_back(info.expected_ego_pose.position); - marker.points.push_back(info.expected_obj_pose.position); - marker_array.markers.push_back(marker); - ++idx; - if (idx >= loop_size) { - break; - } - } - - return marker_array; -} - MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns) diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index d1148f50c7b5b..aac4d9235146c 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/marker_utils/colors.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -37,7 +38,7 @@ using visualization_msgs::msg::Marker; CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; - debug.current_pose = obj.initial_pose.pose; + debug.current_obj_pose = obj.initial_pose.pose; debug.current_twist = obj.initial_twist.twist; return {tier4_autoware_utils::toHexString(obj.uuid), debug}; } @@ -486,4 +487,162 @@ MarkerArray createPredictedPathMarkerArray( return msg; } +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + if (obj_debug_vec.empty()) { + return MarkerArray{}; + } + + int32_t id{0}; + const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + + constexpr float line_scale_val{0.2}; + const auto line_marker_scale = createMarkerScale(line_scale_val, line_scale_val, line_scale_val); + + auto default_line_marker = [&](const auto & color = colors::green()) { + return createDefaultMarker("map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color); + }; + + constexpr float text_scale_val{1.5}; + const auto text_marker_scale = createMarkerScale(text_scale_val, text_scale_val, text_scale_val); + + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", now, ns + "_text", ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + text_marker_scale, colors::white()); + }; + + auto default_cube_marker = + [&](const auto & width, const auto & depth, const auto & color = colors::green()) { + return createDefaultMarker( + "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(width, depth, 1.0), color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve( + obj_debug_vec.size() * 5); // poly ego, text ego, poly obj, text obj, cube obj + + int32_t idx = {0}; + for (const auto & [uuid, info] : obj_debug_vec) { + const auto color = info.is_safe ? colors::green() : colors::red(); + const auto poly_z = info.current_obj_pose.position.z; // temporally + + const auto insert_polygon_marker = [&](const auto & polygon) { + marker_array.markers.emplace_back(); + auto & polygon_marker = marker_array.markers.back(); + polygon_marker = default_line_marker(color); + polygon_marker.points.reserve(polygon.outer().size()); + for (const auto & p : polygon.outer()) { + polygon_marker.points.push_back(createPoint(p.x(), p.y(), poly_z)); + } + }; + + insert_polygon_marker(info.extended_ego_polygon); + insert_polygon_marker(info.extended_obj_polygon); + + const auto str_idx = std::to_string(++idx); + const auto insert_text_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & text_marker = marker_array.markers.back(); + text_marker = default_text_marker(); + text_marker.text = str_idx; + text_marker.pose = pose; + }; + + insert_text_marker(info.expected_ego_pose); + insert_text_marker(info.expected_obj_pose); + + const auto insert_cube_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & cube_marker = marker_array.markers.back(); + cube_marker = default_cube_marker(1.0, 1.0, color); + cube_marker.pose = pose; + }; + insert_cube_marker(info.current_obj_pose); + } + return marker_array; +} + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; + const auto arrow_marker_scale = createMarkerScale(1.0, 0.3, 0.3); + const auto default_arrow_marker = [&](const auto & color) { + return createDefaultMarker( + "map", current_time, ns, ++id, Marker::ARROW, arrow_marker_scale, color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve(std::accumulate( + obj_debug_vec.cbegin(), obj_debug_vec.cend(), 0UL, + [&](const auto current_sum, const auto & obj_debug) { + const auto & [uuid, info] = obj_debug; + return current_sum + info.ego_predicted_path.size() + info.obj_predicted_path.size() + 2; + })); + + for (const auto & [uuid, info] : obj_debug_vec) { + const auto insert_marker = [&](const auto & path, const auto & color) { + for (const auto & pose : path) { + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose.pose; + } + }; + + insert_marker(info.ego_predicted_path, colors::aqua()); + insert_marker(info.obj_predicted_path, colors::yellow()); + const auto insert_expected_pose_marker = [&](const auto & pose, const auto & color) { + // instead of checking for distance, inserting a new marker might be more efficient + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose; + marker.pose.position.z += 0.05; + }; + + insert_expected_pose_marker(info.expected_ego_pose, colors::red()); + insert_expected_pose_marker(info.expected_obj_pose, colors::red()); + } + return marker_array; +} + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); + }; + + MarkerArray marker_array; + + marker_array.markers.reserve(obj_debug_vec.size()); + + int idx{0}; + + for (const auto & [uuid, info] : obj_debug_vec) { + auto safety_check_info_text = default_text_marker(); + safety_check_info_text.pose = info.current_obj_pose; + + std::ostringstream ss; + + ss << "Idx: " << ++idx << "\nUnsafe reason: " << info.unsafe_reason + << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal + << "\nEgo to obj: " << info.inter_vehicle_distance + << "\nExtended polygon: " << (info.is_front ? "ego" : "object") + << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset + << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset + << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego") + << "\nSafe: " << (info.is_safe ? "Yes" : "No"); + + safety_check_info_text.text = ss.str(); + marker_array.markers.push_back(safety_check_info_text); + } + return marker_array; +} + } // namespace marker_utils 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 2f62728e1e323..a31eeed88a275 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 @@ -297,11 +297,10 @@ void LaneChangeInterface::setObjectDebugVisualization() const if (!parameters_->publish_debug_marker) { return; } + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showLerpedPose; - using marker_utils::lane_change_markers::showObjectInfo; - using marker_utils::lane_change_markers::showPolygon; - using marker_utils::lane_change_markers::showPolygonPose; const auto debug_data = module_type_->getDebugData(); const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); @@ -314,14 +313,14 @@ void LaneChangeInterface::setObjectDebugVisualization() const add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); if (!debug_data.empty()) { - add(showObjectInfo(debug_data, "object_debug_info")); - add(showLerpedPose(debug_data, "ego_predicted_path")); + add(showSafetyCheckInfo(debug_data, "object_debug_info")); + add(showPredictedPath(debug_data, "ego_predicted_path")); add(showPolygon(debug_data, "ego_and_target_polygon_relation")); } if (!debug_after_approval.empty()) { - add(showObjectInfo(debug_after_approval, "object_debug_info_after_approval")); - add(showLerpedPose(debug_after_approval, "ego_predicted_path_after_approval")); + add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval")); + add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval")); add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval")); } } diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 2939ca29c0a40..1838dc6bdc07b 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -253,7 +253,11 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, double hysteresis_factor, CollisionCheckDebug & debug) { - debug.lerped_path.reserve(target_object_path.path.size()); + { + debug.ego_predicted_path = predicted_ego_path; + debug.obj_predicted_path = target_object_path.path; + debug.current_obj_pose = target_object.initial_pose.pose; + } for (const auto & obj_pose_with_poly : target_object_path.path) { const auto & current_time = obj_pose_with_poly.time; @@ -277,7 +281,6 @@ bool checkCollision( const auto & ego_velocity = interpolated_data->velocity; { - debug.lerped_path.push_back(ego_pose); debug.expected_ego_pose = ego_pose; debug.expected_obj_pose = obj_pose; debug.extended_ego_polygon = ego_polygon; From f1033a0309d9c87503523f103710e6553f327a5a Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 5 Sep 2023 21:54:00 +0900 Subject: [PATCH 27/93] fix(operation_transition_manager): trajectgory deviation calculation (#4860) Signed-off-by: Takamasa Horibe --- .../src/state.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index f8434bbdb081f..fe15a5dd8593b 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -121,14 +121,24 @@ bool AutonomousMode::isModeChangeCompleted() const auto closest_point = trajectory_.points.at(*closest_idx); // check for lateral deviation - const auto dist_deviation = calcDistance2d(closest_point.pose, kinematics_.pose.pose); + const auto dist_deviation = + motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position); + if (std::isnan(dist_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (dist_deviation > stable_check_param_.dist_threshold) { RCLCPP_INFO(logger_, "Not stable yet: distance deviation is too large: %f", dist_deviation); return unstable(); } // check for yaw deviation - const auto yaw_deviation = calcYawDeviation(closest_point.pose, kinematics_.pose.pose); + const auto yaw_deviation = + motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose); + if (std::isnan(yaw_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (yaw_deviation > stable_check_param_.yaw_threshold) { RCLCPP_INFO(logger_, "Not stable yet: yaw deviation is too large: %f", yaw_deviation); return unstable(); From 4283e15b274f74797d80f35656f7f5e98a52bce1 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 14 Sep 2023 11:03:12 +0900 Subject: [PATCH 28/93] fix(rtc_manager_panel): fix panel chattering (#4988) Signed-off-by: satoshi-ota --- .../src/rtc_manager_panel.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index ff448e694eb15..058a5d5deb5d6 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -366,7 +366,10 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg rtc_table_->clearContents(); num_rtc_status_ptr_->setText( QString::fromStdString("The Number of RTC Statuses: " + std::to_string(msg->statuses.size()))); - if (msg->statuses.empty()) return; + if (msg->statuses.empty()) { + rtc_table_->update(); + return; + } // this is to stable rtc display not to occupy too much size_t min_display_size{5}; size_t max_display_size{10}; @@ -374,8 +377,17 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg rtc_table_->setRowCount( std::max(min_display_size, std::min(msg->statuses.size(), max_display_size))); int cnt = 0; - for (auto status : msg->statuses) { - if (static_cast(cnt) >= max_display_size) return; + + auto sorted_statuses = msg->statuses; + std::partition(sorted_statuses.begin(), sorted_statuses.end(), [](const auto & status) { + return !status.auto_mode && !uint2bool(status.command_status.type); + }); + + for (auto status : sorted_statuses) { + if (static_cast(cnt) >= max_display_size) { + rtc_table_->update(); + return; + } // uuid { std::stringstream uuid; From 2ba9c591ff5c101b5118c1ed001d87ef5de0faf4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 1 Aug 2023 10:13:00 +0900 Subject: [PATCH 29/93] feat(behavior_velocity_crosswalk_module): suppress sudden stop for stuck vehicle (#4449) * feat(behavior_velocity_crosswalk_module): suppress sudden stop for stuck vehicle Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 5 ++- .../src/manager.cpp | 3 ++ .../src/scene_crosswalk.cpp | 36 ++++++++++++++++--- .../src/scene_crosswalk.hpp | 3 ++ 4 files changed, 41 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 6ff7104bfa992..5fff846785353 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -4,8 +4,8 @@ common: show_processing_time: false # [-] whether to show processing time # param for input data - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal + enable_rtc: true # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. # param for stop position stop_position: @@ -30,6 +30,9 @@ stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck max_stuck_vehicle_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] # param for pass judge logic pass_judge: diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index c774c8876c850..7f5c46f1cf91a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -66,6 +66,9 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); cp.stuck_vehicle_attention_range = node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.min_acc_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_acc"); + cp.max_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.max_jerk"); + cp.min_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_jerk"); // param for pass judge logic cp.ego_pass_first_margin = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index e3fb12effe1a1..759f7fd80eba6 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -30,6 +31,7 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcArcLength; +using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcLongitudinalOffsetPose; @@ -772,6 +774,8 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( const std::vector & path_intersects, const std::optional & stop_pose) const { + const auto & p = planner_param_; + if (path_intersects.size() < 2 || !stop_pose) { return {}; } @@ -782,26 +786,48 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( } const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; - if (planner_param_.stuck_vehicle_velocity < std::hypot(obj_vel.x, obj_vel.y)) { + if (p.stuck_vehicle_velocity < std::hypot(obj_vel.x, obj_vel.y)) { continue; } const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos); - if (planner_param_.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { + if (p.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { continue; } const auto & ego_pos = planner_data_->current_odometry->pose.position; + const auto ego_vel = planner_data_->current_velocity->twist.linear.x; + const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; + const double near_attention_range = calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back()); - const double far_attention_range = - near_attention_range + planner_param_.stuck_vehicle_attention_range; + const double far_attention_range = near_attention_range + p.stuck_vehicle_attention_range; const auto dist_ego2obj = calcSignedArcLength(ego_path.points, ego_pos, obj_pos); if (near_attention_range < dist_ego2obj && dist_ego2obj < far_attention_range) { - return createStopFactor(*stop_pose, {obj_pos}); + // Plan STOP considering min_acc, max_jerk and min_jerk. + const auto min_feasible_dist_ego2stop = calcDecelDistWithJerkAndAccConstraints( + ego_vel, 0.0, ego_acc, p.min_acc_for_stuck_vehicle, p.max_jerk_for_stuck_vehicle, + p.min_jerk_for_stuck_vehicle); + if (!min_feasible_dist_ego2stop) { + continue; + } + + const double dist_ego2stop = + calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position); + const double feasible_dist_ego2stop = std::max(*min_feasible_dist_ego2stop, dist_ego2stop); + const double dist_to_ego = + calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos); + + const auto feasible_stop_pose = + calcLongitudinalOffsetPose(ego_path.points, 0, dist_to_ego + feasible_dist_ego2stop); + if (!feasible_stop_pose) { + continue; + } + + return createStopFactor(*feasible_stop_pose, {obj_pos}); } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d5b8f42dc946b..6fc280d918b70 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -76,6 +76,9 @@ class CrosswalkModule : public SceneModuleInterface double stuck_vehicle_velocity; double max_stuck_vehicle_lateral_offset; double stuck_vehicle_attention_range; + double min_acc_for_stuck_vehicle; + double max_jerk_for_stuck_vehicle; + double min_jerk_for_stuck_vehicle; // param for pass judge logic double ego_pass_first_margin; double ego_pass_later_margin; From 5f1362f8f41364bf86c521b34b004aa022dd0a12 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 1 Aug 2023 18:42:55 +0900 Subject: [PATCH 30/93] feat(crosswalk): disable cancel yield at no traffic signals intersection (#4463) * feat(crosswalk): disable cancel yield at no traffic signals intersection Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/scene_crosswalk.cpp | 7 ++++++- .../src/scene_crosswalk.hpp | 10 ++++++---- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 759f7fd80eba6..ef7ecf90736f1 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -868,6 +868,10 @@ void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) { const auto & objects_ptr = planner_data_->predicted_objects; + const auto traffic_lights_reg_elems = + crosswalk_.regulatoryElementsAs(); + const bool has_traffic_light = !traffic_lights_reg_elems.empty(); + // Check if ego is yielding const bool is_ego_yielding = [&]() { const auto has_reached_stop_point = dist_ego_to_stop < planner_param_.stop_position_threshold; @@ -882,7 +886,8 @@ void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) const auto obj_uuid = toHexString(object.object_id); const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; object_info_manager_.update( - obj_uuid, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, planner_param_); + obj_uuid, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, has_traffic_light, + planner_param_); } object_info_manager_.finalize(); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 6fc280d918b70..a05a32e4d52bf 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -106,7 +106,7 @@ class CrosswalkModule : public SceneModuleInterface void updateState( const rclcpp::Time & now, const double obj_vel, const bool is_ego_yielding, - const PlannerParam & planner_param) + const bool has_traffic_light, const PlannerParam & planner_param) { const bool is_stopped = obj_vel < planner_param.stop_object_velocity; @@ -120,7 +120,9 @@ class CrosswalkModule : public SceneModuleInterface } const bool intent_to_cross = (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk; - if ((is_ego_yielding || planner_param.disable_stop_for_yield_cancel) && !intent_to_cross) { + if ( + (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) && + !intent_to_cross) { state = State::FULLY_STOPPED; } else { // NOTE: Object may start moving @@ -137,7 +139,7 @@ class CrosswalkModule : public SceneModuleInterface void init() { current_uuids_.clear(); } void update( const std::string & uuid, const double obj_vel, const rclcpp::Time & now, - const bool is_ego_yielding, const PlannerParam & planner_param) + const bool is_ego_yielding, const bool has_traffic_light, const PlannerParam & planner_param) { // update current uuids current_uuids_.push_back(uuid); @@ -148,7 +150,7 @@ class CrosswalkModule : public SceneModuleInterface } // update object state - objects.at(uuid).updateState(now, obj_vel, is_ego_yielding, planner_param); + objects.at(uuid).updateState(now, obj_vel, is_ego_yielding, has_traffic_light, planner_param); } void finalize() { From 3e3b875dfab771d07591ede0f69da172846e8d8c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 1 Aug 2023 18:43:50 +0900 Subject: [PATCH 31/93] feat(crosswalk): add option to disable yield for new stopped object (#4465) * feat(crosswalk): disable cancel yield at no traffic signals intersection Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * feat(crosswalk): add option to disable yield for new stopped object Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 3 ++- .../src/manager.cpp | 2 ++ .../src/scene_crosswalk.hpp | 11 +++++++++-- 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 5fff846785353..097b2c6c94aef 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -41,7 +41,8 @@ stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding - disable_stop_for_yield_cancel: false + disable_stop_for_yield_cancel: false # for the crosswalk where there is a traffic signal + disable_yield_for_new_stopped_object: false # for the crosswalk where there is a traffic signal timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 7f5c46f1cf91a..d287787006aff 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -80,6 +80,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); cp.disable_stop_for_yield_cancel = node.declare_parameter(ns + ".pass_judge.disable_stop_for_yield_cancel"); + cp.disable_yield_for_new_stopped_object = + node.declare_parameter(ns + ".pass_judge.disable_yield_for_new_stopped_object"); cp.timeout_no_intention_to_walk = node.declare_parameter(ns + ".pass_judge.timeout_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index a05a32e4d52bf..48fdcf39195ae 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -85,6 +85,7 @@ class CrosswalkModule : public SceneModuleInterface double stop_object_velocity; double min_object_velocity; bool disable_stop_for_yield_cancel; + bool disable_yield_for_new_stopped_object; double timeout_no_intention_to_walk; double timeout_ego_stop_for_yield; // param for input data @@ -101,7 +102,7 @@ class CrosswalkModule : public SceneModuleInterface { // NOTE: FULLY_STOPPED means stopped object which can be ignored. enum class State { STOPPED = 0, FULLY_STOPPED, OTHER }; - State state{State::OTHER}; + State state; std::optional time_to_start_stopped{std::nullopt}; void updateState( @@ -146,7 +147,13 @@ class CrosswalkModule : public SceneModuleInterface // add new object if (objects.count(uuid) == 0) { - objects.emplace(uuid, ObjectInfo{}); + if ( + has_traffic_light && planner_param.disable_stop_for_yield_cancel && + planner_param.disable_yield_for_new_stopped_object) { + objects.emplace(uuid, ObjectInfo{ObjectInfo::State::FULLY_STOPPED}); + } else { + objects.emplace(uuid, ObjectInfo{ObjectInfo::State::OTHER}); + } } // update object state From 0d65a3b47915ee00ba30d4a1674ea2ed69a9cd51 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 1 Aug 2023 20:16:37 +0900 Subject: [PATCH 32/93] fix(crosswalk): fix the doc of TTC-TTV graph (#4480) Signed-off-by: Takayuki Murooka --- .../docs/ttc-ttv.svg | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg b/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg index 7bd11ecf2ee39..04a208eee4166 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg @@ -55,12 +55,12 @@

- TTV = TTC + ego_pass_later_margin + TTV = TTC + ego_pass_first_margin
- TTV = TTC + ego_pass_later_margin + TTV = TTC + ego_pass_first_margin @@ -75,12 +75,12 @@
- TTC = TTV + ego_pass_first_margin + TTC = TTV + ego_pass_later_margin
- TTC = TTV + ego_pass_first_margin + TTC = TTV + ego_pass_later_margin @@ -160,13 +160,13 @@
- ego_pass_later_margin + ego_pass_first_margin [s]
- ego_pass_later_margin [s] + ego_pass_first_margin [s] @@ -181,13 +181,13 @@
- ego_pass_pass_margin + ego_pass_later_margin [s]
- ego_pass_pass_margin [s] + ego_pass_later_margin [s] From 44c712131e20640e64f04a74f4b53962b639e012 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 2 Aug 2023 09:36:40 +0900 Subject: [PATCH 33/93] feat(crosswalk): suppress chattering of GO/STOP decision (#4471) * feat(crosswalk): organize the code Signed-off-by: Takayuki Murooka * suppress chattering Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 2 + .../src/manager.cpp | 4 + .../src/scene_crosswalk.cpp | 122 ++++++++---------- .../src/scene_crosswalk.hpp | 97 ++++++++++---- 4 files changed, 135 insertions(+), 90 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 097b2c6c94aef..11184eedf4aaf 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -37,7 +37,9 @@ # param for pass judge logic pass_judge: ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index d287787006aff..400c3333412f5 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -73,8 +73,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) // param for pass judge logic cp.ego_pass_first_margin = node.declare_parameter(ns + ".pass_judge.ego_pass_first_margin"); + cp.ego_pass_first_additional_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_first_additional_margin"); cp.ego_pass_later_margin = node.declare_parameter(ns + ".pass_judge.ego_pass_later_margin"); + cp.ego_pass_later_additional_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_later_additional_margin"); cp.stop_object_velocity = node.declare_parameter(ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index ef7ecf90736f1..13e2f57b6f06b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -305,7 +305,6 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const std::optional & default_stop_pose) { const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto & objects_ptr = planner_data_->predicted_objects; const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second; @@ -317,45 +316,30 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const auto attention_area = getAttentionArea(sparse_resample_path, crosswalk_attention_range); // Update object state - updateObjectState(dist_ego_to_stop); + updateObjectState( + dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area); // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop std::optional> nearest_stop_info; std::vector stop_factor_points; - const auto ignore_crosswalk = debug_data_.ignore_crosswalk = isRedSignalForPedestrians(); - for (const auto & object : objects_ptr->objects) { - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto obj_uuid = toHexString(object.object_id); - - if (!isCrosswalkUserType(object)) { - continue; - } - - if (ignore_crosswalk) { - continue; - } - - // NOTE: Collision points are calculated on each predicted path - const auto collision_point = - getCollisionPoints(sparse_resample_path, object, attention_area, crosswalk_attention_range); - - for (const auto & cp : collision_point) { - const auto collision_state = - getCollisionState(obj_uuid, cp.time_to_collision, cp.time_to_vehicle); - debug_data_.collision_points.push_back(std::make_pair(cp, collision_state)); - + for (const auto & object : object_info_manager_.getObject()) { + const auto & collision_point = object.collision_point; + if (collision_point) { + const auto & collision_state = object.collision_state; if (collision_state != CollisionState::YIELD) { continue; } - stop_factor_points.push_back(obj_pos); + stop_factor_points.push_back(object.position); const auto dist_ego2cp = - calcSignedArcLength(sparse_resample_path.points, ego_pos, cp.collision_point) - + calcSignedArcLength( + sparse_resample_path.points, ego_pos, collision_point->collision_point) - planner_param_.stop_distance_from_object; if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) { - nearest_stop_info = std::make_pair(cp.collision_point, dist_ego2cp - base_link2front); + nearest_stop_info = + std::make_pair(collision_point->collision_point, dist_ego2cp - base_link2front); } } } @@ -568,9 +552,9 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal return std::make_pair(clamped_near_attention_range, clamped_far_attention_range); } -std::vector CrosswalkModule::getCollisionPoints( - const PathWithLaneId & ego_path, const PredictedObject & object, const Polygon2d & attention_area, - const std::pair & crosswalk_attention_range) +std::optional CrosswalkModule::getCollisionPoint( + const PathWithLaneId & ego_path, const PredictedObject & object, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) { stop_watch_.tic(__func__); @@ -584,6 +568,8 @@ std::vector CrosswalkModule::getCollisionPoints( const auto obj_polygon = createObjectPolygon(object.shape.dimensions.x, object.shape.dimensions.y); + double minimum_stop_dist = std::numeric_limits::max(); + std::optional nearest_collision_point{std::nullopt}; for (const auto & obj_path : object.kinematics.predicted_paths) { for (size_t i = 0; i < obj_path.path.size() - 1; ++i) { const auto & p_obj_front = obj_path.path.at(i); @@ -596,26 +582,26 @@ std::vector CrosswalkModule::getCollisionPoints( continue; } - // Calculate nearest collision point - double minimum_stop_dist = std::numeric_limits::max(); - geometry_msgs::msg::Point nearest_collision_point{}; + // Calculate nearest collision point among collisions with a predicted path + double local_minimum_stop_dist = std::numeric_limits::max(); + geometry_msgs::msg::Point local_nearest_collision_point{}; for (const auto & p : tmp_intersection) { const auto cp = createPoint(p.x(), p.y(), ego_pos.z); const auto dist_ego2cp = calcSignedArcLength(ego_path.points, ego_pos, cp); - if (dist_ego2cp < minimum_stop_dist) { - minimum_stop_dist = dist_ego2cp; - nearest_collision_point = cp; + if (dist_ego2cp < local_minimum_stop_dist) { + local_minimum_stop_dist = dist_ego2cp; + local_nearest_collision_point = cp; } } const auto dist_ego2cp = - calcSignedArcLength(ego_path.points, ego_pos, nearest_collision_point); + calcSignedArcLength(ego_path.points, ego_pos, local_nearest_collision_point); constexpr double eps = 1e-3; const auto dist_obj2cp = calcArcLength(obj_path.path) < eps ? 0.0 - : calcSignedArcLength(obj_path.path, size_t(0), nearest_collision_point); + : calcSignedArcLength(obj_path.path, size_t(0), local_nearest_collision_point); if ( dist_ego2cp < crosswalk_attention_range.first || @@ -623,10 +609,13 @@ std::vector CrosswalkModule::getCollisionPoints( continue; } - ret.push_back( - createCollisionPoint(nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel)); - - debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z)); + // Calculate nearest collision point among predicted paths + if (dist_ego2cp < minimum_stop_dist) { + minimum_stop_dist = dist_ego2cp; + nearest_collision_point = createCollisionPoint( + local_nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); + debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z)); + } break; } @@ -636,7 +625,7 @@ std::vector CrosswalkModule::getCollisionPoints( logger_, planner_param_.show_processing_time, "%s : %f ms", __func__, stop_watch_.toc(__func__, true)); - return ret; + return nearest_collision_point; } CollisionPoint CrosswalkModule::createCollisionPoint( @@ -661,25 +650,6 @@ CollisionPoint CrosswalkModule::createCollisionPoint( return collision_point; } -CollisionState CrosswalkModule::getCollisionState( - const std::string & obj_uuid, const double ttc, const double ttv) const -{ - // First, check if the object can be ignored - const auto obj_state = object_info_manager_.getState(obj_uuid); - if (obj_state == ObjectInfo::State::FULLY_STOPPED) { - return CollisionState::IGNORE; - } - - // Compare time to collision and vehicle - if (ttc + planner_param_.ego_pass_first_margin < ttv) { - return CollisionState::EGO_PASS_FIRST; - } - if (ttv + planner_param_.ego_pass_later_margin < ttc) { - return CollisionState::EGO_PASS_LATER; - } - return CollisionState::YIELD; -} - void CrosswalkModule::applySafetySlowDownSpeed( PathWithLaneId & output, const std::vector & path_intersects) { @@ -864,7 +834,9 @@ std::optional CrosswalkModule::getNearestStopFactor( return {}; } -void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) +void CrosswalkModule::updateObjectState( + const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) { const auto & objects_ptr = planner_data_->predicted_objects; @@ -880,14 +852,34 @@ void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) has_reached_stop_point; }(); + const auto ignore_crosswalk = isRedSignalForPedestrians(); + debug_data_.ignore_crosswalk = ignore_crosswalk; + // Update object state object_info_manager_.init(); for (const auto & object : objects_ptr->objects) { const auto obj_uuid = toHexString(object.object_id); + const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; + + // calculate collision point and state + if (!isCrosswalkUserType(object)) { + continue; + } + if (ignore_crosswalk) { + continue; + } + + const auto collision_point = + getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); object_info_manager_.update( - obj_uuid, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, has_traffic_light, - planner_param_); + obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, + has_traffic_light, collision_point, planner_param_); + + if (collision_point) { + const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); + debug_data_.collision_points.push_back(std::make_pair(*collision_point, collision_state)); + } } object_info_manager_.finalize(); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 48fdcf39195ae..eb468197e6c50 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -81,7 +81,9 @@ class CrosswalkModule : public SceneModuleInterface double min_jerk_for_stuck_vehicle; // param for pass judge logic double ego_pass_first_margin; + double ego_pass_first_additional_margin; double ego_pass_later_margin; + double ego_pass_later_additional_margin; double stop_object_velocity; double min_object_velocity; bool disable_stop_for_yield_cancel; @@ -100,19 +102,22 @@ class CrosswalkModule : public SceneModuleInterface struct ObjectInfo { - // NOTE: FULLY_STOPPED means stopped object which can be ignored. - enum class State { STOPPED = 0, FULLY_STOPPED, OTHER }; - State state; + CollisionState collision_state{}; std::optional time_to_start_stopped{std::nullopt}; - void updateState( - const rclcpp::Time & now, const double obj_vel, const bool is_ego_yielding, - const bool has_traffic_light, const PlannerParam & planner_param) + geometry_msgs::msg::Point position{}; + std::optional collision_point{}; + + void transitState( + const rclcpp::Time & now, const double vel, const bool is_ego_yielding, + const bool has_traffic_light, const std::optional & collision_point, + const PlannerParam & planner_param) { - const bool is_stopped = obj_vel < planner_param.stop_object_velocity; + const bool is_stopped = vel < planner_param.stop_object_velocity; + // Check if the object can be ignored if (is_stopped) { - if (state == State::FULLY_STOPPED) { + if (collision_state == CollisionState::IGNORE) { return; } @@ -124,14 +129,41 @@ class CrosswalkModule : public SceneModuleInterface if ( (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) && !intent_to_cross) { - state = State::FULLY_STOPPED; - } else { - // NOTE: Object may start moving - state = State::STOPPED; + collision_state = CollisionState::IGNORE; + return; } } else { time_to_start_stopped = std::nullopt; - state = State::OTHER; + } + + // Compare time to collision and vehicle + if (collision_point) { + // Check if ego will pass first + const double ego_pass_first_additional_margin = + collision_state == CollisionState::EGO_PASS_FIRST + ? 0.0 + : planner_param.ego_pass_first_additional_margin; + if ( + collision_point->time_to_collision + planner_param.ego_pass_first_margin + + ego_pass_first_additional_margin < + collision_point->time_to_vehicle) { + collision_state = CollisionState::EGO_PASS_FIRST; + return; + } + // Check if ego will pass later + const double ego_pass_later_additional_margin = + collision_state == CollisionState::EGO_PASS_LATER + ? 0.0 + : planner_param.ego_pass_later_additional_margin; + if ( + collision_point->time_to_vehicle + planner_param.ego_pass_later_margin + + ego_pass_later_additional_margin < + collision_point->time_to_collision) { + collision_state = CollisionState::EGO_PASS_LATER; + return; + } + collision_state = CollisionState::YIELD; + return; } } }; @@ -139,8 +171,9 @@ class CrosswalkModule : public SceneModuleInterface { void init() { current_uuids_.clear(); } void update( - const std::string & uuid, const double obj_vel, const rclcpp::Time & now, - const bool is_ego_yielding, const bool has_traffic_light, const PlannerParam & planner_param) + const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel, + const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, + const std::optional & collision_point, const PlannerParam & planner_param) { // update current uuids current_uuids_.push_back(uuid); @@ -150,14 +183,17 @@ class CrosswalkModule : public SceneModuleInterface if ( has_traffic_light && planner_param.disable_stop_for_yield_cancel && planner_param.disable_yield_for_new_stopped_object) { - objects.emplace(uuid, ObjectInfo{ObjectInfo::State::FULLY_STOPPED}); + objects.emplace(uuid, ObjectInfo{CollisionState::IGNORE}); } else { - objects.emplace(uuid, ObjectInfo{ObjectInfo::State::OTHER}); + objects.emplace(uuid, ObjectInfo{CollisionState::YIELD}); } } // update object state - objects.at(uuid).updateState(now, obj_vel, is_ego_yielding, has_traffic_light, planner_param); + objects.at(uuid).transitState( + now, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param); + objects.at(uuid).collision_point = collision_point; + objects.at(uuid).position = position; } void finalize() { @@ -174,7 +210,19 @@ class CrosswalkModule : public SceneModuleInterface objects.erase(obsolete_uuid); } } - ObjectInfo::State getState(const std::string & uuid) const { return objects.at(uuid).state; } + + std::vector getObject() const + { + std::vector object_info_vec; + for (auto object : objects) { + object_info_vec.push_back(object.second); + } + return object_info_vec; + } + CollisionState getCollisionState(const std::string & uuid) const + { + return objects.at(uuid).collision_state; + } std::unordered_map objects; std::vector current_uuids_; @@ -210,9 +258,9 @@ class CrosswalkModule : public SceneModuleInterface const std::vector & path_intersects, const std::optional & stop_pose) const; - std::vector getCollisionPoints( + std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, - const Polygon2d & attention_area, const std::pair & crosswalk_attention_range); + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); std::optional getNearestStopFactor( const PathWithLaneId & ego_path, @@ -248,9 +296,6 @@ class CrosswalkModule : public SceneModuleInterface const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, const geometry_msgs::msg::Vector3 & obj_vel) const; - CollisionState getCollisionState( - const std::string & obj_uuid, const double ttc, const double ttv) const; - float calcTargetVelocity( const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const; @@ -262,7 +307,9 @@ class CrosswalkModule : public SceneModuleInterface const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects) const; - void updateObjectState(const double dist_ego_to_stop); + void updateObjectState( + const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); bool isRedSignalForPedestrians() const; From 9966bd766eabd40ede5cb32116b5941383059dfb Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 2 Aug 2023 11:36:25 +0900 Subject: [PATCH 34/93] feat(crosswalk): add pass judge line (#4492) * feat(crosswalk): add pass judge line Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 1 + .../behavior_velocity_crosswalk_module/src/manager.cpp | 2 ++ .../src/scene_crosswalk.cpp | 10 ++++++++++ .../src/scene_crosswalk.hpp | 1 + 4 files changed, 14 insertions(+) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 11184eedf4aaf..b0e198be68ef6 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -40,6 +40,7 @@ ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering + max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 400c3333412f5..72cc1970fb192 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -79,6 +79,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".pass_judge.ego_pass_later_margin"); cp.ego_pass_later_additional_margin = node.declare_parameter(ns + ".pass_judge.ego_pass_later_additional_margin"); + cp.max_offset_to_crosswalk_for_yield = + node.declare_parameter(ns + ".pass_judge.max_offset_to_crosswalk_for_yield"); cp.stop_object_velocity = node.declare_parameter(ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 13e2f57b6f06b..000a384860634 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -319,6 +319,16 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( updateObjectState( dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area); + // Check if ego moves forward enough to ignore yield. + if (!path_intersects.empty()) { + const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const double dist_ego2crosswalk = + calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()); + if (dist_ego2crosswalk - base_link2front < planner_param_.max_offset_to_crosswalk_for_yield) { + return {}; + } + } + // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop std::optional> nearest_stop_info; diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index eb468197e6c50..2f7de95a3bd95 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -84,6 +84,7 @@ class CrosswalkModule : public SceneModuleInterface double ego_pass_first_additional_margin; double ego_pass_later_margin; double ego_pass_later_additional_margin; + double max_offset_to_crosswalk_for_yield; double stop_object_velocity; double min_object_velocity; bool disable_stop_for_yield_cancel; From 35aa38e86e95c3252ebf96236aa607919656d3b6 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 2 Aug 2023 14:19:50 +0900 Subject: [PATCH 35/93] feat(crosswalk): gradable pass margin (#4494) * feat(crosswalk): gradable pass margin Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 6 ++-- .../src/manager.cpp | 12 ++++--- .../src/scene_crosswalk.hpp | 35 ++++++++++++++++--- 3 files changed, 43 insertions(+), 10 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index b0e198be68ef6..4b0a1ffc4317b 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -36,9 +36,11 @@ # param for pass judge logic pass_judge: - ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_margin_x: [0.0] # [[s]] time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_margin_y: [6.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering - ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 72cc1970fb192..992d1c48eb9f4 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -71,12 +71,16 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.min_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_jerk"); // param for pass judge logic - cp.ego_pass_first_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_first_margin"); + cp.ego_pass_first_margin_x = + node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_x"); + cp.ego_pass_first_margin_y = + node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_y"); cp.ego_pass_first_additional_margin = node.declare_parameter(ns + ".pass_judge.ego_pass_first_additional_margin"); - cp.ego_pass_later_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_later_margin"); + cp.ego_pass_later_margin_x = + node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_x"); + cp.ego_pass_later_margin_y = + node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_y"); cp.ego_pass_later_additional_margin = node.declare_parameter(ns + ".pass_judge.ego_pass_later_additional_margin"); cp.max_offset_to_crosswalk_for_yield = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 2f7de95a3bd95..bd6e0c59277af 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -56,6 +56,24 @@ using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::StopWatch; +namespace +{ +double interpolateEgoPassMargin( + const std::vector & x_vec, const std::vector & y_vec, const double target_x) +{ + for (size_t i = 0; i < x_vec.size(); ++i) { + if (target_x < x_vec.at(i)) { + if (i == 0) { + return y_vec.at(i); + } + const double ratio = (target_x - x_vec.at(i - 1)) / (x_vec.at(i) - x_vec.at(i - 1)); + return y_vec.at(i - 1) + ratio * (y_vec.at(i) - y_vec.at(i - 1)); + } + } + return y_vec.back(); +} +} // namespace + class CrosswalkModule : public SceneModuleInterface { public: @@ -80,9 +98,11 @@ class CrosswalkModule : public SceneModuleInterface double max_jerk_for_stuck_vehicle; double min_jerk_for_stuck_vehicle; // param for pass judge logic - double ego_pass_first_margin; + std::vector ego_pass_first_margin_x; + std::vector ego_pass_first_margin_y; double ego_pass_first_additional_margin; - double ego_pass_later_margin; + std::vector ego_pass_later_margin_x; + std::vector ego_pass_later_margin_y; double ego_pass_later_additional_margin; double max_offset_to_crosswalk_for_yield; double stop_object_velocity; @@ -144,20 +164,27 @@ class CrosswalkModule : public SceneModuleInterface collision_state == CollisionState::EGO_PASS_FIRST ? 0.0 : planner_param.ego_pass_first_additional_margin; + const double ego_pass_first_margin = interpolateEgoPassMargin( + planner_param.ego_pass_first_margin_x, planner_param.ego_pass_first_margin_y, + collision_point->time_to_collision); if ( - collision_point->time_to_collision + planner_param.ego_pass_first_margin + + collision_point->time_to_collision + ego_pass_first_margin + ego_pass_first_additional_margin < collision_point->time_to_vehicle) { collision_state = CollisionState::EGO_PASS_FIRST; return; } + // Check if ego will pass later const double ego_pass_later_additional_margin = collision_state == CollisionState::EGO_PASS_LATER ? 0.0 : planner_param.ego_pass_later_additional_margin; + const double ego_pass_later_margin = interpolateEgoPassMargin( + planner_param.ego_pass_later_margin_x, planner_param.ego_pass_later_margin_y, + collision_point->time_to_vehicle); if ( - collision_point->time_to_vehicle + planner_param.ego_pass_later_margin + + collision_point->time_to_vehicle + ego_pass_later_margin + ego_pass_later_additional_margin < collision_point->time_to_collision) { collision_state = CollisionState::EGO_PASS_LATER; From 70f8e1c13fe16ca9d760127b47629cf67be5656c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 30 Aug 2023 12:58:31 +0900 Subject: [PATCH 36/93] feat(crosswalk): stabler collision point (#4781) * feat(crosswalk): stabler collision point Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../src/debug.cpp | 6 +- .../src/scene_crosswalk.cpp | 136 +++++++++--------- 2 files changed, 72 insertions(+), 70 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index 409898233cbbe..03c61c3b7f723 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -97,7 +97,7 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( for (size_t i = 0; i < debug_data.obj_polygons.size(); ++i) { const auto & points = debug_data.obj_polygons.at(i); auto marker = createDefaultMarker( - "map", now, "object polygon", uid + i++, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), + "map", now, "object polygon", uid + i, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 1.0, 0.999)); marker.points = points; // marker.points.push_back(marker.points.front()); @@ -107,8 +107,8 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( for (size_t i = 0; i < debug_data.ego_polygons.size(); ++i) { const auto & points = debug_data.ego_polygons.at(i); auto marker = createDefaultMarker( - "map", now, "vehicle polygon", uid + i++, Marker::LINE_STRIP, - createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + "map", now, "vehicle polygon", uid + i, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), + createMarkerColor(1.0, 1.0, 0.0, 0.999)); marker.points = points; // marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 000a384860634..5c0d7a805ece4 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -68,39 +68,34 @@ std::vector toGeometryPointVector( return points; } -Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back, - const geometry_msgs::msg::Polygon & base_polygon) +void offsetPolygon2d( + const geometry_msgs::msg::Pose & origin_point, const geometry_msgs::msg::Polygon & polygon, + Polygon2d & offset_polygon) { - Polygon2d one_step_polygon{}; - - { - geometry_msgs::msg::Polygon out_polygon{}; - geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_front); - tf2::doTransform(base_polygon, out_polygon, geometry_tf); - - for (const auto & p : out_polygon.points) { - one_step_polygon.outer().push_back(Point2d(p.x, p.y)); - } + for (const auto & polygon_point : polygon.points) { + const auto offset_pos = + tier4_autoware_utils::calcOffsetPose(origin_point, polygon_point.x, polygon_point.y, 0.0) + .position; + offset_polygon.outer().push_back(Point2d(offset_pos.x, offset_pos.y)); } +} - { - geometry_msgs::msg::Polygon out_polygon{}; - geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_back); - tf2::doTransform(base_polygon, out_polygon, geometry_tf); - - for (const auto & p : out_polygon.points) { - one_step_polygon.outer().push_back(Point2d(p.x, p.y)); - } +template +Polygon2d createMultiStepPolygon( + const T & obj_path_points, const geometry_msgs::msg::Polygon & polygon, const size_t start_idx, + const size_t end_idx) +{ + Polygon2d multi_step_polygon{}; + for (size_t i = start_idx; i <= end_idx; ++i) { + offsetPolygon2d( + tier4_autoware_utils::getPose(obj_path_points.at(i)), polygon, multi_step_polygon); } - Polygon2d hull_polygon{}; - bg::convex_hull(one_step_polygon, hull_polygon); - bg::correct(hull_polygon); + Polygon2d hull_multi_step_polygon{}; + bg::convex_hull(multi_step_polygon, hull_multi_step_polygon); + bg::correct(hull_multi_step_polygon); - return hull_polygon; + return hull_multi_step_polygon; } void sortCrosswalksByDistance( @@ -127,24 +122,12 @@ void sortCrosswalksByDistance( std::sort(crosswalks.begin(), crosswalks.end(), compare); } -std::vector calcOverlappingPoints(const Polygon2d & polygon1, const Polygon2d & polygon2) +std::vector calcOverlappingPoints(const Polygon2d & polygon1, const Polygon2d & polygon2) { // NOTE: If one polygon is fully inside the other polygon, the result is empty. - std::vector intersection{}; - bg::intersection(polygon1, polygon2, intersection); - - if (bg::within(polygon1, polygon2)) { - for (const auto & p : polygon1.outer()) { - intersection.push_back(Point2d{p.x(), p.y()}); - } - } - if (bg::within(polygon2, polygon1)) { - for (const auto & p : polygon2.outer()) { - intersection.push_back(Point2d{p.x(), p.y()}); - } - } - - return intersection; + std::vector intersection_polygons{}; + bg::intersection(polygon1, polygon2, intersection_polygons); + return intersection_polygons; } StopFactor createStopFactor( @@ -581,37 +564,56 @@ std::optional CrosswalkModule::getCollisionPoint( double minimum_stop_dist = std::numeric_limits::max(); std::optional nearest_collision_point{std::nullopt}; for (const auto & obj_path : object.kinematics.predicted_paths) { - for (size_t i = 0; i < obj_path.path.size() - 1; ++i) { - const auto & p_obj_front = obj_path.path.at(i); - const auto & p_obj_back = obj_path.path.at(i + 1); - const auto obj_one_step_polygon = createOneStepPolygon(p_obj_front, p_obj_back, obj_polygon); + size_t start_idx{0}; + bool is_start_idx_initialized{false}; + for (size_t i = 0; i < obj_path.path.size(); ++i) { + // For effective computation, the point and polygon intersection is calculated first. + const auto obj_one_step_polygon = createMultiStepPolygon(obj_path.path, obj_polygon, i, i); + const auto one_step_intersection_polygons = + calcOverlappingPoints(obj_one_step_polygon, attention_area); + if (!one_step_intersection_polygons.empty()) { + if (!is_start_idx_initialized) { + start_idx = i; + is_start_idx_initialized = true; + } + if (i != obj_path.path.size() - 1) { + // NOTE: Even if the object path does not fully cross the ego path, the path should be + // considered. + continue; + } + } + + if (!is_start_idx_initialized) { + continue; + } + + // Calculate multi-step object polygon, and reset start_idx + const size_t start_idx_with_margin = std::max(static_cast(start_idx) - 1, 0); + const size_t end_idx_with_margin = std::min(i + 1, obj_path.path.size() - 1); + const auto obj_multi_step_polygon = createMultiStepPolygon( + obj_path.path, obj_polygon, start_idx_with_margin, end_idx_with_margin); + is_start_idx_initialized = false; // Calculate intersection points between object and attention area - const auto tmp_intersection = calcOverlappingPoints(obj_one_step_polygon, attention_area); - if (tmp_intersection.empty()) { + const auto multi_step_intersection_polygons = + calcOverlappingPoints(obj_multi_step_polygon, attention_area); + if (multi_step_intersection_polygons.empty()) { continue; } // Calculate nearest collision point among collisions with a predicted path - double local_minimum_stop_dist = std::numeric_limits::max(); - geometry_msgs::msg::Point local_nearest_collision_point{}; - for (const auto & p : tmp_intersection) { - const auto cp = createPoint(p.x(), p.y(), ego_pos.z); - const auto dist_ego2cp = calcSignedArcLength(ego_path.points, ego_pos, cp); - - if (dist_ego2cp < local_minimum_stop_dist) { - local_minimum_stop_dist = dist_ego2cp; - local_nearest_collision_point = cp; - } - } + Point2d boost_intersection_center_point; + bg::centroid(multi_step_intersection_polygons.front(), boost_intersection_center_point); + const auto intersection_center_point = createPoint( + boost_intersection_center_point.x(), boost_intersection_center_point.y(), ego_pos.z); const auto dist_ego2cp = - calcSignedArcLength(ego_path.points, ego_pos, local_nearest_collision_point); + calcSignedArcLength(ego_path.points, ego_pos, intersection_center_point); constexpr double eps = 1e-3; const auto dist_obj2cp = calcArcLength(obj_path.path) < eps ? 0.0 - : calcSignedArcLength(obj_path.path, size_t(0), local_nearest_collision_point); + : calcSignedArcLength(obj_path.path, size_t(0), intersection_center_point); if ( dist_ego2cp < crosswalk_attention_range.first || @@ -623,8 +625,9 @@ std::optional CrosswalkModule::getCollisionPoint( if (dist_ego2cp < minimum_stop_dist) { minimum_stop_dist = dist_ego2cp; nearest_collision_point = createCollisionPoint( - local_nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); - debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z)); + intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); + debug_data_.obj_polygons.push_back( + toGeometryPointVector(obj_multi_step_polygon, ego_pos.z)); } break; @@ -732,9 +735,8 @@ Polygon2d CrosswalkModule::getAttentionArea( break; } - const auto & p_ego_front = sparse_resample_path.points.at(j).point.pose; - const auto & p_ego_back = sparse_resample_path.points.at(j + 1).point.pose; - const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, ego_polygon); + const auto ego_one_step_polygon = + createMultiStepPolygon(sparse_resample_path.points, ego_polygon, j, j + 1); debug_data_.ego_polygons.push_back(toGeometryPointVector(ego_one_step_polygon, ego_pos.z)); From a5c88ef33c42229e7af9833b96af027f068fafc0 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 30 Aug 2023 14:50:17 +0900 Subject: [PATCH 37/93] feat(crosswalk): no stop decision with braking distance (#4802) * feat(crosswalk): no stop decision with bracking distance Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 12 +++++++++--- .../src/manager.cpp | 10 ++++++++-- .../src/scene_crosswalk.cpp | 12 +++++++++++- .../src/scene_crosswalk.hpp | 3 +++ 4 files changed, 31 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 4b0a1ffc4317b..b1e5bcb32bd18 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -42,12 +42,18 @@ ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering - max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. + + no_stop_decision: + max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding - disable_stop_for_yield_cancel: false # for the crosswalk where there is a traffic signal - disable_yield_for_new_stopped_object: false # for the crosswalk where there is a traffic signal + disable_stop_for_yield_cancel: true # for the crosswalk where there is a traffic signal + disable_yield_for_new_stopped_object: true # for the crosswalk where there is a traffic signal timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 992d1c48eb9f4..6b479736a3dad 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -83,8 +83,14 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_y"); cp.ego_pass_later_additional_margin = node.declare_parameter(ns + ".pass_judge.ego_pass_later_additional_margin"); - cp.max_offset_to_crosswalk_for_yield = - node.declare_parameter(ns + ".pass_judge.max_offset_to_crosswalk_for_yield"); + cp.max_offset_to_crosswalk_for_yield = node.declare_parameter( + ns + ".pass_judge.no_stop_decision.max_offset_to_crosswalk_for_yield"); + cp.min_acc_for_no_stop_decision = + node.declare_parameter(ns + ".pass_judge.no_stop_decision.min_acc"); + cp.max_jerk_for_no_stop_decision = + node.declare_parameter(ns + ".pass_judge.no_stop_decision.max_jerk"); + cp.min_jerk_for_no_stop_decision = + node.declare_parameter(ns + ".pass_judge.no_stop_decision.min_jerk"); cp.stop_object_velocity = node.declare_parameter(ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 5c0d7a805ece4..0f4c51ecfe166 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -288,6 +288,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const std::optional & default_stop_pose) { const auto & ego_pos = planner_data_->current_odometry->pose.position; + const double ego_vel = planner_data_->current_velocity->twist.linear.x; + const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; + const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second; @@ -303,11 +306,18 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area); // Check if ego moves forward enough to ignore yield. + const auto & p = planner_param_; if (!path_intersects.empty()) { const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const double dist_ego2crosswalk = calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()); - if (dist_ego2crosswalk - base_link2front < planner_param_.max_offset_to_crosswalk_for_yield) { + const auto braking_distance_opt = motion_utils::calcDecelDistWithJerkAndAccConstraints( + ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, + p.min_jerk_for_no_stop_decision); + const double braking_distance = braking_distance_opt ? *braking_distance_opt : 0.0; + if ( + dist_ego2crosswalk - base_link2front - braking_distance < + p.max_offset_to_crosswalk_for_yield) { return {}; } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index bd6e0c59277af..4e172344e5a70 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -105,6 +105,9 @@ class CrosswalkModule : public SceneModuleInterface std::vector ego_pass_later_margin_y; double ego_pass_later_additional_margin; double max_offset_to_crosswalk_for_yield; + double min_acc_for_no_stop_decision; + double max_jerk_for_no_stop_decision; + double min_jerk_for_no_stop_decision; double stop_object_velocity; double min_object_velocity; bool disable_stop_for_yield_cancel; From 3bf467383cf4d4ab87f11077cb382a13febb8cb6 Mon Sep 17 00:00:00 2001 From: ito-san <57388357+ito-san@users.noreply.github.com> Date: Wed, 16 Aug 2023 17:12:37 +0900 Subject: [PATCH 38/93] fix(system_monitor): extend command line to display (#4553) Signed-off-by: ito-san --- .../process_monitor/diag_task.hpp | 129 +++++------------- .../src/process_monitor/process_monitor.cpp | 26 ++-- 2 files changed, 45 insertions(+), 110 deletions(-) diff --git a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp index 183f86baa2a08..15702852a0e50 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp @@ -24,6 +24,25 @@ #include +/** + * @brief Struct for storing process information + */ +struct ProcessInfo +{ + std::string processId; + std::string userName; + std::string priority; + std::string niceValue; + std::string virtualImage; + std::string residentSize; + std::string sharedMemSize; + std::string processStatus; + std::string cpuUsage; + std::string memoryUsage; + std::string cpuTime; + std::string commandName; +}; + class DiagTask : public diagnostic_updater::DiagnosticTask { public: @@ -46,18 +65,18 @@ class DiagTask : public diagnostic_updater::DiagnosticTask if (level_ != DiagStatus::OK) { stat.add("content", content_); } else { - stat.add("COMMAND", command_); - stat.add("%CPU", cpu_); - stat.add("%MEM", mem_); - stat.add("PID", pid_); - stat.add("USER", user_); - stat.add("PR", pr_); - stat.add("NI", ni_); - stat.add("VIRT", virt_); - stat.add("RES", res_); - stat.add("SHR", shr_); - stat.add("S", s_); - stat.add("TIME+", time_); + stat.add("COMMAND", info_.commandName); + stat.add("%CPU", info_.cpuUsage); + stat.add("%MEM", info_.memoryUsage); + stat.add("PID", info_.processId); + stat.add("USER", info_.userName); + stat.add("PR", info_.priority); + stat.add("NI", info_.niceValue); + stat.add("VIRT", info_.virtualImage); + stat.add("RES", info_.residentSize); + stat.add("SHR", info_.sharedMemSize); + stat.add("S", info_.processStatus); + stat.add("TIME+", info_.cpuTime); } stat.summary(level_, message_); } @@ -85,95 +104,17 @@ class DiagTask : public diagnostic_updater::DiagnosticTask } /** - * @brief set process id - * @param [in] pid process id - */ - void setProcessId(const std::string & pid) { pid_ = pid; } - - /** - * @brief set user name - * @param [in] user user name - */ - void setUserName(const std::string & user) { user_ = user; } - - /** - * @brief set priority - * @param [in] pr priority - */ - void setPriority(const std::string & pr) { pr_ = pr; } - - /** - * @brief set nice value - * @param [in] ni nice value - */ - void setNiceValue(const std::string & ni) { ni_ = ni; } - - /** - * @brief set virtual image - * @param [in] virt virtual image - */ - void setVirtualImage(const std::string & virt) { virt_ = virt; } - - /** - * @brief set resident size - * @param [in] res resident size + * @brief Set process information + * @param [in] info Process information */ - void setResidentSize(const std::string & res) { res_ = res; } - - /** - * @brief set shared mem size - * @param [in] shr shared mem size - */ - void setSharedMemSize(const std::string & shr) { shr_ = shr; } - - /** - * @brief set process status - * @param [in] s process status - */ - void setProcessStatus(const std::string & s) { s_ = s; } - - /** - * @brief set CPU usage - * @param [in] cpu CPU usage - */ - void setCPUUsage(const std::string & cpu) { cpu_ = cpu; } - - /** - * @brief set memory usage - * @param [in] mem memory usage - */ - void setMemoryUsage(const std::string & mem) { mem_ = mem; } - - /** - * @brief set CPU time - * @param [in] time CPU time - */ - void setCPUTime(const std::string & time) { time_ = time; } - - /** - * @brief set Command name/line - * @param [in] command Command name/line - */ - void setCommandName(const std::string & command) { command_ = command; } + void setProcessInformation(const struct ProcessInfo & info) { info_ = info; } private: int level_; //!< @brief Diagnostics error level std::string message_; //!< @brief Diagnostics status message std::string error_command_; //!< @brief Error command std::string content_; //!< @brief Error content - - std::string pid_; //!< @brief Process Id - std::string user_; //!< @brief User Name - std::string pr_; //!< @brief Priority - std::string ni_; //!< @brief Nice value - std::string virt_; //!< @brief Virtual Image (kb) - std::string res_; //!< @brief Resident size (kb) - std::string shr_; //!< @brief Shared Mem size (kb) - std::string s_; //!< @brief Process Status - std::string cpu_; //!< @brief CPU usage - std::string mem_; //!< @brief Memory usage - std::string time_; //!< @brief CPU Time - std::string command_; //!< @brief Command name/line + struct ProcessInfo info_; //!< @brief Struct for storing process information }; #endif // SYSTEM_MONITOR__PROCESS_MONITOR__DIAG_TASK_HPP_ diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index f78fc00c430dc..2ce1738ecd960 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -451,27 +451,21 @@ void ProcessMonitor::getTopratedProcesses( return; } - std::vector list; std::string line; int index = 0; while (std::getline(is_out, line) && !line.empty()) { - boost::trim_left(line); - boost::split(list, line, boost::is_space(), boost::token_compress_on); + std::istringstream stream(line); + + ProcessInfo info; + stream >> info.processId >> info.userName >> info.priority >> info.niceValue >> + info.virtualImage >> info.residentSize >> info.sharedMemSize >> info.processStatus >> + info.cpuUsage >> info.memoryUsage >> info.cpuTime; + + std::getline(stream, info.commandName); tasks->at(index)->setDiagnosticsStatus(DiagStatus::OK, "OK"); - tasks->at(index)->setProcessId(list[0]); - tasks->at(index)->setUserName(list[1]); - tasks->at(index)->setPriority(list[2]); - tasks->at(index)->setNiceValue(list[3]); - tasks->at(index)->setVirtualImage(list[4]); - tasks->at(index)->setResidentSize(list[5]); - tasks->at(index)->setSharedMemSize(list[6]); - tasks->at(index)->setProcessStatus(list[7]); - tasks->at(index)->setCPUUsage(list[8]); - tasks->at(index)->setMemoryUsage(list[9]); - tasks->at(index)->setCPUTime(list[10]); - tasks->at(index)->setCommandName(list[11]); + tasks->at(index)->setProcessInformation(info); ++index; } } @@ -521,7 +515,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { From 94ceada57b8aae0be3efb0d0efaf216f5793a4f5 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 14 Aug 2023 22:06:54 +0900 Subject: [PATCH 39/93] feat(lane_change): ignore front parked objects (#4591) Signed-off-by: yutaka Co-authored-by: Fumiya Watanabe --- .../scene_module/lane_change/normal.hpp | 4 + .../utils/lane_change/utils.hpp | 6 - .../src/scene_module/lane_change/normal.cpp | 115 +++++++++++++++++- .../src/utils/lane_change/utils.cpp | 82 ------------- 4 files changed, 116 insertions(+), 91 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 00c60cbfa8a6d..5d13507f5206c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -143,6 +143,10 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams & rss_params, std::unordered_map & debug_data) const; + LaneChangeTargetObjectIndices filterObject( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const lanelet::ConstLanelets & target_backward_lanes) const; + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); }; } // namespace behavior_path_planner 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 95bae3348818a..9eecc84838aad 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 @@ -183,12 +183,6 @@ boost::optional getLeadingStaticObjectIdx( std::optional createPolygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); -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_parameters); - ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters); 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 eee17d79af648..b03eb76f4d764 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 @@ -608,9 +608,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index - const auto target_obj_index = utils::lane_change::filterObject( - objects, current_lanes, target_lanes, target_backward_lanes, current_pose, route_handler, - *lane_change_parameters_); + const auto target_obj_index = filterObject(current_lanes, target_lanes, target_backward_lanes); LaneChangeTargetObjects target_objects; target_objects.current_lane.reserve(target_obj_index.current_lane.size()); @@ -641,6 +639,117 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( return target_objects; } +LaneChangeTargetObjectIndices NormalLaneChange::filterObject( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const lanelet::ConstLanelets & target_backward_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto current_vel = getEgoVelocity(); + const auto & route_handler = *getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + const auto & objects = *planner_data_->dynamic_object; + const auto & object_check_min_road_shoulder_width = + lane_change_parameters_->object_check_min_road_shoulder_width; + const auto & object_shiftable_ratio_threshold = + lane_change_parameters_->object_shiftable_ratio_threshold; + + // Guard + if (objects.objects.empty()) { + return {}; + } + + // Get path + const auto path = + route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto target_path = + route_handler.getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + + const auto current_polygon = + utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); + const auto target_polygon = + utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + const auto target_backward_polygon = utils::lane_change::createPolygon( + target_backward_lanes, 0.0, std::numeric_limits::max()); + + // minimum distance to lane changing start point + const double t_prepare = common_parameters.lane_change_prepare_duration; + const double a_min = lane_change_parameters_->min_longitudinal_acc; + const double min_dist_to_lane_changing_start = std::max( + current_vel * t_prepare, current_vel * t_prepare + 0.5 * a_min * t_prepare * t_prepare); + + LaneChangeTargetObjectIndices filtered_obj_indices; + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & object = objects.objects.at(i); + const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto extended_object = + utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); + + // ignore specific object types + if (!utils::lane_change::isTargetObjectType(object, *lane_change_parameters_)) { + continue; + } + + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + + // calc distance from the current ego position + double max_dist_ego_to_obj = std::numeric_limits::lowest(); + double min_dist_ego_to_obj = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const double dist_ego_to_obj = + motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); + max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); + min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); + } + + // ignore static object that are behind the ego vehicle + if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) { + continue; + } + + // check if the object intersects with target lanes + if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { + // ignore static parked object that are in front of the ego vehicle in target lanes + bool is_parked_object = utils::lane_change::isParkedObject( + target_path, route_handler, extended_object, object_check_min_road_shoulder_width, + object_shiftable_ratio_threshold); + if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) { + continue; + } + + filtered_obj_indices.target_lane.push_back(i); + continue; + } + + // check if the object intersects with target backward lanes + if ( + target_backward_polygon && + boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { + filtered_obj_indices.target_lane.push_back(i); + continue; + } + + // check if the object intersects with current lanes + if ( + current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) && + max_dist_ego_to_obj >= 0.0) { + // check only the objects that are in front of the ego vehicle + filtered_obj_indices.current_lane.push_back(i); + continue; + } + + // ignore all objects that are behind the ego vehicle and not on the current and target + // lanes + if (max_dist_ego_to_obj < 0.0) { + continue; + } + + filtered_obj_indices.other_lane.push_back(i); + } + + return filtered_obj_indices; +} + PathWithLaneId NormalLaneChange::getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, 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 34f5f2065a0f0..a277e72c8660d 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1063,88 +1063,6 @@ std::optional createPolygon( return lanelet::utils::to2D(polygon_3d).basicPolygon(); } -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) -{ - // Guard - if (objects.objects.empty()) { - return {}; - } - - // Get path - const auto path = - route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - - const auto current_polygon = - createPolygon(current_lanes, 0.0, std::numeric_limits::max()); - const auto target_polygon = createPolygon(target_lanes, 0.0, std::numeric_limits::max()); - const auto target_backward_polygon = - createPolygon(target_backward_lanes, 0.0, std::numeric_limits::max()); - - LaneChangeTargetObjectIndices filtered_obj_indices; - for (size_t i = 0; i < objects.objects.size(); ++i) { - const auto & object = objects.objects.at(i); - const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x; - - // ignore specific object types - if (!isTargetObjectType(object, lane_change_parameter)) { - continue; - } - - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - - // calc distance from the current ego position - double max_dist_ego_to_obj = std::numeric_limits::lowest(); - for (const auto & polygon_p : obj_polygon.outer()) { - const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const double dist_ego_to_obj = - motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); - max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); - } - - // ignore static object that are behind the ego vehicle - if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) { - continue; - } - - // check if the object intersects with target lanes - if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { - filtered_obj_indices.target_lane.push_back(i); - continue; - } - - // check if the object intersects with target backward lanes - if ( - target_backward_polygon && - boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { - filtered_obj_indices.target_lane.push_back(i); - continue; - } - - // check if the object intersects with current lanes - if ( - current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) && - max_dist_ego_to_obj >= 0.0) { - // check only the objects that are in front of the ego vehicle - filtered_obj_indices.current_lane.push_back(i); - continue; - } - - // ignore all objects that are behind the ego vehicle and not on the current and target - // lanes - if (max_dist_ego_to_obj < 0.0) { - continue; - } - - filtered_obj_indices.other_lane.push_back(i); - } - - return filtered_obj_indices; -} - ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters) From a9ea5808f3dbb75126f7482bdd5c47a776d62281 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 24 Aug 2023 21:25:34 +0900 Subject: [PATCH 40/93] fix(lane_change): skip safety check for stopping vehicles (#4733) Signed-off-by: Fumiya Watanabe --- .../src/scene_module/lane_change/normal.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) 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 b03eb76f4d764..aae757a49b781 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 @@ -648,10 +648,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; const auto & objects = *planner_data_->dynamic_object; - const auto & object_check_min_road_shoulder_width = - lane_change_parameters_->object_check_min_road_shoulder_width; - const auto & object_shiftable_ratio_threshold = - lane_change_parameters_->object_shiftable_ratio_threshold; // Guard if (objects.objects.empty()) { @@ -710,9 +706,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // check if the object intersects with target lanes if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { // ignore static parked object that are in front of the ego vehicle in target lanes - bool is_parked_object = utils::lane_change::isParkedObject( - target_path, route_handler, extended_object, object_check_min_road_shoulder_width, - object_shiftable_ratio_threshold); + bool is_parked_object = + utils::lane_change::isParkedObject(target_path, route_handler, extended_object, 0.0, 0.0); if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) { continue; } From 71f121814230712787a5fd0f2ab72a7a85a18e2d Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 4 Sep 2023 21:32:23 +0900 Subject: [PATCH 41/93] fix(lane_change): skip parked object filtering (#4868) Signed-off-by: Fumiya Watanabe --- .../src/scene_module/lane_change/normal.cpp | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) 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 aae757a49b781..3368297e90712 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 @@ -644,7 +644,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const lanelet::ConstLanelets & target_backward_lanes) const { const auto current_pose = getEgoPose(); - const auto current_vel = getEgoVelocity(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; const auto & objects = *planner_data_->dynamic_object; @@ -667,12 +666,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto target_backward_polygon = utils::lane_change::createPolygon( target_backward_lanes, 0.0, std::numeric_limits::max()); - // minimum distance to lane changing start point - const double t_prepare = common_parameters.lane_change_prepare_duration; - const double a_min = lane_change_parameters_->min_longitudinal_acc; - const double min_dist_to_lane_changing_start = std::max( - current_vel * t_prepare, current_vel * t_prepare + 0.5 * a_min * t_prepare * t_prepare); - LaneChangeTargetObjectIndices filtered_obj_indices; for (size_t i = 0; i < objects.objects.size(); ++i) { const auto & object = objects.objects.at(i); @@ -705,12 +698,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // check if the object intersects with target lanes if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { - // ignore static parked object that are in front of the ego vehicle in target lanes - bool is_parked_object = - utils::lane_change::isParkedObject(target_path, route_handler, extended_object, 0.0, 0.0); - if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) { - continue; - } + // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target + // lanes filtered_obj_indices.target_lane.push_back(i); continue; From 50ecbd5d49873aef99ddb7c9cffb8692257f1c38 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 13 Sep 2023 21:16:25 +0900 Subject: [PATCH 42/93] fix(lane_change): fix target object filter (#4930) * fix(lane_change): fix target object filter Signed-off-by: Muhammad Zulfaqar Azmi * used lateral distance from centerline instead Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) 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 3368297e90712..ce01f1f8ecc0a 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 @@ -665,6 +665,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); const auto target_backward_polygon = utils::lane_change::createPolygon( target_backward_lanes, 0.0, std::numeric_limits::max()); + const auto dist_ego_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); LaneChangeTargetObjectIndices filtered_obj_indices; for (size_t i = 0; i < objects.objects.size(); ++i) { @@ -691,6 +693,14 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); } + const auto is_lateral_far = [&]() { + const auto dist_object_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet( + current_lanes, object.kinematics.initial_pose_with_covariance.pose); + const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; + return std::abs(lateral) > (common_parameters.vehicle_width / 2); + }; + // ignore static object that are behind the ego vehicle if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) { continue; @@ -701,8 +711,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target // lanes - filtered_obj_indices.target_lane.push_back(i); - continue; + if (max_dist_ego_to_obj >= 0 || is_lateral_far()) { + filtered_obj_indices.target_lane.push_back(i); + continue; + } } // check if the object intersects with target backward lanes From d9b3b20ccad096c2aac7fc5dd88cdf13b91e5a0e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 23 Aug 2023 18:09:34 +0900 Subject: [PATCH 43/93] fix(utils): drivable area generation supports anti-clockwise polygon (#4678) Signed-off-by: satoshi-ota --- planning/behavior_path_planner/src/utils/utils.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 506b3ff2d634b..e64fe41463e48 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1537,6 +1537,10 @@ std::vector calcBound( const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + const auto is_clockwise_polygon = + boost::geometry::is_valid(lanelet::utils::to2D(polygon.basicPolygon())); + const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; + const auto start_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { return p.id() == bound.front().id(); }); @@ -1552,7 +1556,8 @@ std::vector calcBound( // 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)) { + for (const auto & point : + extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration)) { output_points.push_back(point); } From 1fb1a6e9a6dab2e5d7fe77c115b520097dc0eee8 Mon Sep 17 00:00:00 2001 From: Shunsuke Miura Date: Fri, 15 Sep 2023 18:54:18 +0900 Subject: [PATCH 44/93] Revert "Merge pull request #815 from Shin-kyoto/fix/reduce_false_negative" This reverts commit c606feca18ad774bbec2fa9d9345ce03cf7ed2d9, reversing changes made to bdccfb9fcca89e21f46cce912db4276b09e54f29. # Conflicts: # launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml # launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml --- ...ra_lidar_fusion_based_detection.launch.xml | 1 + ...ar_radar_fusion_based_detection.launch.xml | 1 + .../lidar_based_detection.launch.xml | 2 + .../launch/perception.launch.xml | 2 + ...xel_grid_based_euclidean_cluster.launch.py | 96 ++++++++-- ...el_grid_based_euclidean_cluster.launch.xml | 2 + .../pointcloud_preprocessor/CMakeLists.txt | 23 +++ .../crop_box_filter_nodelet.hpp | 1 + .../faster_voxel_grid_downsample_filter.hpp | 93 +++++++++ .../voxel_grid_downsample_filter_nodelet.hpp | 15 +- .../pointcloud_preprocessor/filter.hpp | 14 +- .../ring_outlier_filter_nodelet.hpp | 1 + .../transform_info.hpp | 43 +++++ .../faster_voxel_grid_downsample_filter.cpp | 178 ++++++++++++++++++ .../voxel_grid_downsample_filter_nodelet.cpp | 25 ++- .../pointcloud_preprocessor/src/filter.cpp | 3 +- 16 files changed, 466 insertions(+), 34 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp create mode 100644 sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 34c2fd4cdc27c..150924330b360 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -81,6 +81,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 7d1353433da85..463340efdecfe 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -12,6 +12,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 9ab48a678240d..6d67df5fa7a1f 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -5,6 +5,7 @@ + @@ -33,6 +34,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 58ee60b9409e0..204fb561b10ea 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -58,6 +58,7 @@ + + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index ba6e9e72107c8..82ce5605b67cd 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -16,9 +16,10 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.conditions import UnlessCondition +from launch.substitutions import AndSubstitution from launch.substitutions import AnonName from launch.substitutions import LaunchConfiguration +from launch.substitutions import NotSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -164,7 +165,7 @@ def load_composable_node_param(param_path): ) # set euclidean cluster as a component - euclidean_cluster_component = ComposableNode( + use_downsample_euclidean_cluster_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", @@ -176,40 +177,108 @@ def load_composable_node_param(param_path): parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) + use_map_disuse_downsample_euclidean_component = ComposableNode( + package=pkg, + namespace=ns, + plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + name="euclidean_cluster", + remappings=[ + ("input", "map_filter/pointcloud"), + ("output", LaunchConfiguration("output_clusters")), + ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], + ) + disuse_map_disuse_downsample_euclidean_component = ComposableNode( + package=pkg, + namespace=ns, + plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + name="euclidean_cluster", + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("output", LaunchConfiguration("output_clusters")), + ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], + ) + container = ComposableNodeContainer( name="euclidean_cluster_container", package="rclcpp_components", namespace=ns, executable="component_container", - composable_node_descriptions=[ - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - euclidean_cluster_component, - ], + composable_node_descriptions=[], output="screen", ) - use_map_loader = LoadComposableNodes( + use_map_use_downsample_loader = LoadComposableNodes( composable_node_descriptions=[ compare_map_filter_component, use_map_short_range_crop_box_filter_component, use_map_long_range_crop_box_filter_component, + voxel_grid_filter_component, + outlier_filter_component, + downsample_concat_component, + use_downsample_euclidean_cluster_component, ], target_container=container, - condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition( + AndSubstitution( + LaunchConfiguration("use_pointcloud_map"), + LaunchConfiguration("use_downsample_pointcloud"), + ) + ), ) - disuse_map_loader = LoadComposableNodes( + disuse_map_use_downsample_loader = LoadComposableNodes( composable_node_descriptions=[ disuse_map_short_range_crop_box_filter_component, disuse_map_long_range_crop_box_filter_component, + voxel_grid_filter_component, + outlier_filter_component, + downsample_concat_component, + use_downsample_euclidean_cluster_component, ], target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition( + AndSubstitution( + NotSubstitution(LaunchConfiguration("use_pointcloud_map")), + LaunchConfiguration("use_downsample_pointcloud"), + ) + ), ) - return [container, use_map_loader, disuse_map_loader] + use_map_disuse_downsample_loader = LoadComposableNodes( + composable_node_descriptions=[ + compare_map_filter_component, + use_map_disuse_downsample_euclidean_component, + ], + target_container=container, + condition=IfCondition( + AndSubstitution( + (LaunchConfiguration("use_pointcloud_map")), + NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), + ) + ), + ) + + disuse_map_disuse_downsample_loader = LoadComposableNodes( + composable_node_descriptions=[ + disuse_map_disuse_downsample_euclidean_component, + ], + target_container=container, + condition=IfCondition( + AndSubstitution( + NotSubstitution(LaunchConfiguration("use_pointcloud_map")), + NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), + ) + ), + ) + return [ + container, + use_map_use_downsample_loader, + disuse_map_use_downsample_loader, + use_map_disuse_downsample_loader, + disuse_map_disuse_downsample_loader, + ] def generate_launch_description(): @@ -222,6 +291,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), add_launch_arg("use_pointcloud_map", "false"), + add_launch_arg("use_downsample_pointcloud", "false"), add_launch_arg( "voxel_grid_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 6ae70b2612836..2833fed81c28e 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -4,6 +4,7 @@ + @@ -14,6 +15,7 @@ + diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index b14d3a181e587..c5f15ec4e49d5 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -41,6 +41,21 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base pcl_ros ) +add_library(faster_voxel_grid_downsample_filter SHARED + src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +) + +target_include_directories(faster_voxel_grid_downsample_filter PUBLIC + "$" + "$" +) + +ament_target_dependencies(faster_voxel_grid_downsample_filter + pcl_conversions + rclcpp + sensor_msgs +) + ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/utility/utilities.cpp src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -67,6 +82,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED target_link_libraries(pointcloud_preprocessor_filter pointcloud_preprocessor_filter_base + faster_voxel_grid_downsample_filter ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} @@ -179,6 +195,13 @@ install( RUNTIME DESTINATION bin ) +install( + TARGETS faster_voxel_grid_downsample_filter EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + install( DIRECTORY include/ DESTINATION include/${PROJECT_NAME} diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp index 3b392401de49b..1406679ce0dba 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp @@ -54,6 +54,7 @@ #define POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp new file mode 100644 index 0000000000000..d92b511d40c69 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp @@ -0,0 +1,93 @@ +// 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. + +#pragma once + +#include "pointcloud_preprocessor/transform_info.hpp" + +#include +#include +#include + +#include +#include + +namespace pointcloud_preprocessor +{ + +class FasterVoxelGridDownsampleFilter +{ + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; + +public: + FasterVoxelGridDownsampleFilter(); + void set_voxel_size(float voxel_size_x, float voxel_size_y, float voxel_size_z); + void set_field_offsets(const PointCloud2ConstPtr & input); + void filter( + const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, + const rclcpp::Logger & logger); + +private: + struct Centroid + { + float x; + float y; + float z; + uint32_t point_count_; + + Centroid() : x(0), y(0), z(0) {} + Centroid(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { this->point_count_ = 1; } + + void add_point(float _x, float _y, float _z) + { + this->x += _x; + this->y += _y; + this->z += _z; + this->point_count_++; + } + + Eigen::Vector4f calc_centroid() const + { + Eigen::Vector4f centroid( + (this->x / this->point_count_), (this->y / this->point_count_), + (this->z / this->point_count_), 1.0f); + return centroid; + } + }; + + Eigen::Vector3f inverse_voxel_size_; + std::vector xyz_fields_; + int x_offset_; + int y_offset_; + int z_offset_; + int intensity_offset_; + bool offset_initialized_; + + Eigen::Vector3f get_point_from_global_offset( + const PointCloud2ConstPtr & input, size_t global_offset); + + bool get_min_max_voxel( + const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel); + + std::unordered_map calc_centroids_each_voxel( + const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel, + const Eigen::Vector3i & min_voxel); + + void copy_centroids_to_output( + std::unordered_map & voxel_centroid_map, PointCloud2 & output, + const TransformInfo & transform_info); +}; + +} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp index 7ebf80c2da958..ae285b4fa9239 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -52,6 +52,7 @@ #define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include #include @@ -66,10 +67,16 @@ class VoxelGridDownsampleFilterComponent : public pointcloud_preprocessor::Filte void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + // TODO(atsushi421): Temporary Implementation: Remove this interface when all the filter nodes + // conform to new API + virtual void faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, + const TransformInfo & transform_info); + private: - double voxel_size_x_; - double voxel_size_y_; - double voxel_size_z_; + float voxel_size_x_; + float voxel_size_y_; + float voxel_size_z_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp index f995be741272c..47c0f2b403faa 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -52,6 +52,8 @@ #ifndef POINTCLOUD_PREPROCESSOR__FILTER_HPP_ #define POINTCLOUD_PREPROCESSOR__FILTER_HPP_ +#include "pointcloud_preprocessor/transform_info.hpp" + #include #include #include @@ -134,18 +136,6 @@ class Filter : public rclcpp::Node const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); protected: - struct TransformInfo - { - TransformInfo() - { - eigen_transform = Eigen::Matrix4f::Identity(4, 4); - need_transform = false; - } - - Eigen::Matrix4f eigen_transform; - bool need_transform; - }; - /** \brief The input PointCloud2 subscriber. */ rclcpp::Subscription::SharedPtr sub_input_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index a906eb913d863..ab7d4e0012304 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -17,6 +17,7 @@ #include "autoware_point_types/types.hpp" #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp new file mode 100644 index 0000000000000..a4806555695b5 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp @@ -0,0 +1,43 @@ +// 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. + +#pragma once + +#include + +namespace pointcloud_preprocessor +{ + +/** + * This holds the coordinate transformation information of the point cloud. + * Usage example: + * \code + * if (transform_info.need_transform) { + * point = transform_info.eigen_transform * point; + * } + * \endcode + */ +struct TransformInfo +{ + TransformInfo() + { + eigen_transform = Eigen::Matrix4f::Identity(4, 4); + need_transform = false; + } + + Eigen::Matrix4f eigen_transform; + bool need_transform; +}; + +} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp new file mode 100644 index 0000000000000..7c302eec20431 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -0,0 +1,178 @@ +// 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 "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" + +namespace pointcloud_preprocessor +{ + +FasterVoxelGridDownsampleFilter::FasterVoxelGridDownsampleFilter() +{ + pcl::for_each_type::type>( + pcl::detail::FieldAdder(xyz_fields_)); + offset_initialized_ = false; +} + +void FasterVoxelGridDownsampleFilter::set_voxel_size( + float voxel_size_x, float voxel_size_y, float voxel_size_z) +{ + inverse_voxel_size_ = + Eigen::Array3f::Ones() / Eigen::Array3f(voxel_size_x, voxel_size_y, voxel_size_z); +} + +void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPtr & input) +{ + x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; + y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; + z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; + intensity_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset; + offset_initialized_ = true; +} + +void FasterVoxelGridDownsampleFilter::filter( + const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, + const rclcpp::Logger & logger) +{ + // Check if the field offset has been set + if (!offset_initialized_) { + set_field_offsets(input); + } + + // Compute the minimum and maximum voxel coordinates + Eigen::Vector3i min_voxel, max_voxel; + if (!get_min_max_voxel(input, min_voxel, max_voxel)) { + RCLCPP_ERROR( + logger, + "Voxel size is too small for the input dataset. " + "Integer indices would overflow."); + output = *input; + return; + } + + // Storage for mapping voxel coordinates to centroids + auto voxel_centroid_map = calc_centroids_each_voxel(input, max_voxel, min_voxel); + + // Initialize the output + output.row_step = voxel_centroid_map.size() * input->point_step; + output.data.resize(output.row_step); + output.width = voxel_centroid_map.size(); + pcl_conversions::fromPCL(xyz_fields_, output.fields); + output.is_dense = true; // we filter out invalid points + output.height = input->height; + output.is_bigendian = input->is_bigendian; + output.point_step = input->point_step; + output.header = input->header; + + // Copy the centroids to the output + copy_centroids_to_output(voxel_centroid_map, output, transform_info); +} + +Eigen::Vector3f FasterVoxelGridDownsampleFilter::get_point_from_global_offset( + const PointCloud2ConstPtr & input, size_t global_offset) +{ + Eigen::Vector3f point( + *reinterpret_cast(&input->data[global_offset + x_offset_]), + *reinterpret_cast(&input->data[global_offset + y_offset_]), + *reinterpret_cast(&input->data[global_offset + z_offset_])); + return point; +} + +bool FasterVoxelGridDownsampleFilter::get_min_max_voxel( + const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel) +{ + // Compute the minimum and maximum point coordinates + Eigen::Vector3f min_point, max_point; + min_point.setConstant(FLT_MAX); + max_point.setConstant(-FLT_MAX); + for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); + global_offset += input->point_step) { + Eigen::Vector3f point = get_point_from_global_offset(input, global_offset); + if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) { + min_point = min_point.cwiseMin(point); + max_point = max_point.cwiseMax(point); + } + } + + // Check that the voxel size is not too small, given the size of the data + if ( + ((static_cast((max_point[0] - min_point[0]) * inverse_voxel_size_[0]) + 1) * + (static_cast((max_point[1] - min_point[1]) * inverse_voxel_size_[1]) + 1) * + (static_cast((max_point[2] - min_point[2]) * inverse_voxel_size_[2]) + 1)) > + static_cast(std::numeric_limits::max())) { + return false; + } + + // Compute the minimum and maximum voxel coordinates + min_voxel[0] = static_cast(std::floor(min_point[0] * inverse_voxel_size_[0])); + min_voxel[1] = static_cast(std::floor(min_point[1] * inverse_voxel_size_[1])); + min_voxel[2] = static_cast(std::floor(min_point[2] * inverse_voxel_size_[2])); + max_voxel[0] = static_cast(std::floor(max_point[0] * inverse_voxel_size_[0])); + max_voxel[1] = static_cast(std::floor(max_point[1] * inverse_voxel_size_[1])); + max_voxel[2] = static_cast(std::floor(max_point[2] * inverse_voxel_size_[2])); + + return true; +} + +std::unordered_map +FasterVoxelGridDownsampleFilter::calc_centroids_each_voxel( + const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel, + const Eigen::Vector3i & min_voxel) +{ + std::unordered_map voxel_centroid_map; + // Compute the number of divisions needed along all axis + Eigen::Vector3i div_b = max_voxel - min_voxel + Eigen::Vector3i::Ones(); + // Set up the division multiplier + Eigen::Vector3i div_b_mul(1, div_b[0], div_b[0] * div_b[1]); + + for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); + global_offset += input->point_step) { + Eigen::Vector3f point = get_point_from_global_offset(input, global_offset); + if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) { + // Calculate the voxel index to which the point belongs + int ijk0 = static_cast(std::floor(point[0] * inverse_voxel_size_[0]) - min_voxel[0]); + int ijk1 = static_cast(std::floor(point[1] * inverse_voxel_size_[1]) - min_voxel[1]); + int ijk2 = static_cast(std::floor(point[2] * inverse_voxel_size_[2]) - min_voxel[2]); + uint32_t voxel_id = ijk0 * div_b_mul[0] + ijk1 * div_b_mul[1] + ijk2 * div_b_mul[2]; + + // Add the point to the corresponding centroid + if (voxel_centroid_map.find(voxel_id) == voxel_centroid_map.end()) { + voxel_centroid_map[voxel_id] = Centroid(point[0], point[1], point[2]); + } else { + voxel_centroid_map[voxel_id].add_point(point[0], point[1], point[2]); + } + } + } + + return voxel_centroid_map; +} + +void FasterVoxelGridDownsampleFilter::copy_centroids_to_output( + std::unordered_map & voxel_centroid_map, PointCloud2 & output, + const TransformInfo & transform_info) +{ + size_t output_data_size = 0; + for (const auto & pair : voxel_centroid_map) { + Eigen::Vector4f centroid = pair.second.calc_centroid(); + if (transform_info.need_transform) { + centroid = transform_info.eigen_transform * centroid; + } + *reinterpret_cast(&output.data[output_data_size + x_offset_]) = centroid[0]; + *reinterpret_cast(&output.data[output_data_size + y_offset_]) = centroid[1]; + *reinterpret_cast(&output.data[output_data_size + z_offset_]) = centroid[2]; + *reinterpret_cast(&output.data[output_data_size + intensity_offset_]) = centroid[3]; + output_data_size += output.point_step; + } +} + +} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp index 5b22986a1b6b8..f11f37397a142 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -50,6 +50,8 @@ #include "pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp" +#include "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" + #include #include #include @@ -64,9 +66,9 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent( { // set initial parameters { - voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); - voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); - voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); + voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); + voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); + voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); } using std::placeholders::_1; @@ -74,6 +76,8 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent( std::bind(&VoxelGridDownsampleFilterComponent::paramCallback, this, _1)); } +// TODO(atsushi421): Temporary Implementation: Delete this function definition when all the filter +// nodes conform to new API. void VoxelGridDownsampleFilterComponent::filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { @@ -95,6 +99,19 @@ void VoxelGridDownsampleFilterComponent::filter( output.header = input->header; } +// TODO(atsushi421): Temporary Implementation: Rename this function to `filter()` when all the +// filter nodes conform to new API. Then delete the old `filter()` defined above. +void VoxelGridDownsampleFilterComponent::faster_filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output, const TransformInfo & transform_info) +{ + std::scoped_lock lock(mutex_); + FasterVoxelGridDownsampleFilter faster_voxel_filter; + faster_voxel_filter.set_voxel_size(voxel_size_x_, voxel_size_y_, voxel_size_z_); + faster_voxel_filter.set_field_offsets(input); + faster_voxel_filter.filter(input, output, transform_info, this->get_logger()); +} + rcl_interfaces::msg::SetParametersResult VoxelGridDownsampleFilterComponent::paramCallback( const std::vector & p) { diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index f0b716c73b5f1..d5843be395adf 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -124,7 +124,8 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) // TODO(sykwer): Change the corresponding node to subscribe to `faster_input_indices_callback` // each time a child class supports the faster version. // When all the child classes support the faster version, this workaround is deleted. - std::set supported_nodes = {"CropBoxFilter", "RingOutlierFilter"}; + std::set supported_nodes = { + "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback; From b2af8a5819577c93c7c9eab1a295da5e178a3cec Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Thu, 14 Sep 2023 10:14:32 +0900 Subject: [PATCH 45/93] Merge pull request #833 from tier4/fix/camera_lidar_fusion_launch fix(tier4_perception_launch): camera lidar fusion launch --- .../camera_lidar_fusion_based_detection.launch.xml | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 150924330b360..24edd17253634 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -332,22 +332,11 @@ - - - - - - - - - - - - + From f7322de99ffb947fc869f5b1d7502271fc68db49 Mon Sep 17 00:00:00 2001 From: atsushi yano <55824710+atsushi421@users.noreply.github.com> Date: Tue, 12 Sep 2023 09:59:14 +0900 Subject: [PATCH 46/93] fix(voxel_grid_downsample_filter): support no intensity case (#4922) fix: support for no intensity field Signed-off-by: atsushi421 --- .../faster_voxel_grid_downsample_filter.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp index 7c302eec20431..5829277335433 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -36,7 +36,12 @@ void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPt x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; - intensity_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset; + int intensity_index = pcl::getFieldIndex(*input, "intensity"); + if (intensity_index != -1) { + intensity_offset_ = input->fields[intensity_index].offset; + } else { + intensity_offset_ = z_offset_ + sizeof(float); + } offset_initialized_ = true; } From 597515e01a541d59bee16910242f9efce13504c8 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 4 Sep 2023 20:06:06 +0900 Subject: [PATCH 47/93] refactor(goal_planner): refactor isAllowedGoalModification (#4856) * ractor(goal_planner): refactor isAllowedGoalModification Signed-off-by: kosuke55 * fix build Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../scene_module/goal_planner/manager.hpp | 1 - .../utils/goal_planner/util.hpp | 6 ++-- .../goal_planner/goal_planner_module.cpp | 31 ++++++++++--------- .../src/scene_module/goal_planner/manager.cpp | 8 ++--- .../src/utils/goal_planner/util.cpp | 19 +++++------- 5 files changed, 27 insertions(+), 38 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index 30bbe39c8240f..e11641e24167a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -48,7 +48,6 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface std::shared_ptr parameters_; std::vector> registered_modules_; - bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 3dde96e813aad..29a4a8efa719f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -51,10 +51,8 @@ PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); -bool isAllowedGoalModification( - const std::shared_ptr & route_handler, const bool left_side_parking); -bool checkOriginalGoalIsInShoulder( - const std::shared_ptr & route_handler, const bool left_side_parking); +bool isAllowedGoalModification(const std::shared_ptr & route_handler); +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); // debug MarkerArray createPullOverAreaMarkerArray( diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 97e047be749b5..60f0e89a9e091 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -305,10 +305,9 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); - const double request_length = - goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_) - ? calcModuleRequestLength() - : parameters_->minimum_request_length; + const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) + ? calcModuleRequestLength() + : parameters_->minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; @@ -317,7 +316,7 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } @@ -404,8 +403,7 @@ ModuleStatus GoalPlannerModule::updateState() { // finish module only when the goal is fixed if ( - !goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_) && + !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler) && hasFinishedGoalPlanner()) { return ModuleStatus::SUCCESS; } @@ -503,7 +501,7 @@ void GoalPlannerModule::generateGoalCandidates() // calculate goal candidates const Pose goal_pose = route_handler->getGoalPose(); refined_goal_pose_ = calcRefinedGoal(goal_pose); - if (goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { + if (goal_planner_utils::isAllowedGoalModification(route_handler)) { goal_searcher_->setPlannerData(planner_data_); goal_candidates_ = goal_searcher_->search(refined_goal_pose_); } else { @@ -518,8 +516,9 @@ BehaviorModuleOutput GoalPlannerModule::plan() { generateGoalCandidates(); - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + path_reference_ = getPreviousModuleOutput().reference_path; + + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return planWithGoalModification(); } else { fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -835,8 +834,12 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + resetPathCandidate(); + resetPathReference(); + + path_reference_ = getPreviousModuleOutput().reference_path; + + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return planWaitingApprovalWithGoalModification(); } else { fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -1445,9 +1448,7 @@ void GoalPlannerModule::setDebugData() } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 9e94a234a7f48..5923597af8da8 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -241,8 +241,6 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } parameters_ = std::make_shared(p); - - left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; } void GoalPlannerModuleManager::updateModuleParams( @@ -263,8 +261,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const { // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -275,8 +272,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const { // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 19b6bd27ea07c..01fc4b182e211 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -181,25 +181,20 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } -bool isAllowedGoalModification( - const std::shared_ptr & route_handler, const bool left_side_parking) +bool isAllowedGoalModification(const std::shared_ptr & route_handler) { - return route_handler->isAllowedGoalModification() || - checkOriginalGoalIsInShoulder(route_handler, left_side_parking); + return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); } -bool checkOriginalGoalIsInShoulder( - const std::shared_ptr & route_handler, const bool left_side_parking) +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) { const Pose & goal_pose = route_handler->getGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking); - lanelet::ConstLanelet target_lane{}; - lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); + lanelet::ConstLanelet closest_shoulder_lane{}; + lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane); - return route_handler->isShoulderLanelet(target_lane) && - lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1); + return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); } } // namespace goal_planner_utils From f30699d7c325d25711e79acfd7627face616d7f8 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 4 Sep 2023 22:43:34 +0900 Subject: [PATCH 48/93] fix(behavior_path_planner): fix checkOriginalGoalIsInShoulder (#4878) Signed-off-by: kosuke55 --- .../behavior_path_planner/src/utils/goal_planner/util.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 01fc4b182e211..8b48d1a867126 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -192,9 +192,11 @@ bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_h const auto shoulder_lanes = route_handler->getShoulderLanelets(); lanelet::ConstLanelet closest_shoulder_lane{}; - lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane); + if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { + return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + } - return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + return false; } } // namespace goal_planner_utils From ff976baf2a7cf4b9939ed2880247e0e388552a06 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 5 Sep 2023 10:06:27 +0900 Subject: [PATCH 49/93] fix(start_planner): start pose candidates (#4880) Signed-off-by: kosuke55 --- .../src/scene_module/start_planner/start_planner_module.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 98796d52c6e73..a802b43ee4d49 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -415,6 +415,9 @@ void StartPlannerModule::planWithPriority( } const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { + // Set back_finished flag based on the current index + status_.back_finished = i == 0; + // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); @@ -494,8 +497,6 @@ void StartPlannerModule::planWithPriority( for (const auto & p : order_priority) { if (is_safe_with_pose_planner(p.first, p.second)) { const std::lock_guard lock(mutex_); - // Set back_finished flag based on the current index - status_.back_finished = p.first == 0; return; } } From 78879eb9463676bc60f0da49155eab1831676013 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 5 Sep 2023 14:54:28 +0900 Subject: [PATCH 50/93] fix(goal_planner): fix goal search for narrow shoulder lane (#4816) Signed-off-by: kosuke55 --- .../behavior_path_planner/utils/utils.hpp | 4 +- .../goal_planner/goal_planner_module.cpp | 15 +- .../src/utils/goal_planner/goal_searcher.cpp | 21 ++- .../behavior_path_planner/src/utils/utils.cpp | 162 ++++++++++++------ 4 files changed, 139 insertions(+), 63 deletions(-) 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 5ab03c34e7e14..07d93c143ee74 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 @@ -320,8 +320,8 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, const bool left_side); std::optional getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose, const bool left_side); + const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, + const double base_link2rear, const Pose & vehicle_pose, const bool left_side); // misc diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 60f0e89a9e091..f560c82223c13 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -354,6 +354,10 @@ double GoalPlannerModule::calcModuleRequestLength() const Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const { + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); @@ -385,15 +389,18 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const center_pose.orientation = tf2::toMsg(tf_quat); } - const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); - if (!distance_from_left_bound) { + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking_); + if (!distance_from_bound) { RCLCPP_ERROR(getLogger(), "fail to calculate refined goal"); return goal_pose; } + const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - distance_from_left_bound.value() + parameters_->margin_from_boundary; + sign * (distance_from_bound.value() + parameters_->margin_from_boundary); + const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); return refined_goal_pose; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 1e57dcc319dc5..e12ee576a5d4b 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -53,6 +53,9 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double lateral_offset_interval = parameters_.lateral_offset_interval; const double max_lateral_offset = parameters_.max_lateral_offset; const double ignore_distance_from_lane_start = parameters_.ignore_distance_from_lane_start; + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); @@ -76,7 +79,10 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; for (const auto & p : center_line_path.points) { - const Pose & center_pose = p.point.pose; + // todo(kosuke55): fix orientation for inverseTransformPoint temporarily + Pose center_pose = p.point.pose; + center_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); // ignore goal_pose near lane start const double distance_from_lane_start = @@ -85,13 +91,14 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) continue; } - const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); - if (!distance_from_left_bound) continue; + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking_); + if (!distance_from_bound) continue; - const double sign = left_side_parking_ ? 1.0 : -1.0; + const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - sign * (std::abs(distance_from_left_bound.value()) - margin_from_boundary); + -distance_from_bound.value() + sign * margin_from_boundary; const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( @@ -102,7 +109,7 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) double lateral_offset = 0.0; for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { lateral_offset = dy; - search_pose = calcOffsetPose(original_search_pose, 0, -sign * dy, 0); + search_pose = calcOffsetPose(original_search_pose, 0, sign * dy, 0); const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 506b3ff2d634b..f93309ef739c2 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2284,64 +2284,126 @@ double getSignedDistanceFromBoundary( } std::optional getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose, const bool left_side) -{ - double min_distance = std::numeric_limits::max(); - - const auto transformed_footprint = - transformVector(footprint, tier4_autoware_utils::pose2transform(vehicle_pose)); - bool found_neighbor_bound = false; - for (const auto & vehicle_corner_point : transformed_footprint) { - // convert point of footprint to pose + const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, + const double base_link2rear, const Pose & vehicle_pose, const bool left_side) +{ + // Depending on which side is selected, calculate the transformed coordinates of the front and + // rear vehicle corners + Point rear_corner_point, front_corner_point; + if (left_side) { + Point front_left, rear_left; + rear_left.x = -base_link2rear; + rear_left.y = vehicle_width / 2; + front_left.x = base_link2front; + front_left.y = vehicle_width / 2; + rear_corner_point = tier4_autoware_utils::transformPoint(rear_left, vehicle_pose); + front_corner_point = tier4_autoware_utils::transformPoint(front_left, vehicle_pose); + } else { + Point front_right, rear_right; + rear_right.x = -base_link2rear; + rear_right.y = -vehicle_width / 2; + front_right.x = base_link2front; + front_right.y = -vehicle_width / 2; + rear_corner_point = tier4_autoware_utils::transformPoint(rear_right, vehicle_pose); + front_corner_point = tier4_autoware_utils::transformPoint(front_right, vehicle_pose); + } + + const auto combined_lane = lanelet::utils::combineLaneletsShape(lanelets); + const auto & bound_line_2d = left_side ? lanelet::utils::to2D(combined_lane.leftBound3d()) + : lanelet::utils::to2D(combined_lane.rightBound3d()); + + // Initialize the lateral distance to the maximum (for left side) or the minimum (for right side) + // possible value. + double lateral_distance = + left_side ? -std::numeric_limits::max() : std::numeric_limits::max(); + + // Find the closest bound segment that contains the corner point in the X-direction + // and calculate the lateral distance from that segment. + const auto calcLateralDistanceFromBound = [&]( + const Point & vehicle_corner_point, + boost::optional & lateral_distance, + boost::optional & segment_idx) { Pose vehicle_corner_pose{}; - vehicle_corner_pose.position = tier4_autoware_utils::toMsg(vehicle_corner_point.to_3d()); + vehicle_corner_pose.position = vehicle_corner_point; vehicle_corner_pose.orientation = vehicle_pose.orientation; - // calculate distance to the bound directly next to footprint points - lanelet::ConstLanelet closest_lanelet{}; - if (lanelet::utils::query::getClosestLanelet(lanelets, vehicle_corner_pose, &closest_lanelet)) { - const auto & bound_line_2d = left_side ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) - : lanelet::utils::to2D(closest_lanelet.rightBound3d()); - - for (size_t i = 1; i < bound_line_2d.size(); ++i) { - const Point p_front = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i - 1]); - const Point p_back = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); - - const Point inverse_p_front = - tier4_autoware_utils::inverseTransformPoint(p_front, vehicle_corner_pose); - const Point inverse_p_back = - tier4_autoware_utils::inverseTransformPoint(p_back, vehicle_corner_pose); - - const double dy_front = inverse_p_front.y; - const double dy_back = inverse_p_back.y; - const double dx_front = inverse_p_front.x; - const double dx_back = inverse_p_back.x; - // is in segment - if (dx_front < 0 && dx_back > 0) { - const double lateral_distance_from_pose_to_segment = - (dy_front * dx_back + dy_back * -dx_front) / (dx_back - dx_front); - - if (std::abs(lateral_distance_from_pose_to_segment) < std::abs(min_distance)) { - min_distance = lateral_distance_from_pose_to_segment; - } - - found_neighbor_bound = true; - break; - } + // Euclidean distance to find the closest segment containing the corner point. + double min_distance = std::numeric_limits::max(); + + for (size_t i = 0; i < bound_line_2d.size() - 1; i++) { + const Point p1 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); + const Point p2 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i + 1]); + + const Point inverse_p1 = tier4_autoware_utils::inverseTransformPoint(p1, vehicle_corner_pose); + const Point inverse_p2 = tier4_autoware_utils::inverseTransformPoint(p2, vehicle_corner_pose); + const double dx_p1 = inverse_p1.x; + const double dx_p2 = inverse_p2.x; + const double dy_p1 = inverse_p1.y; + const double dy_p2 = inverse_p2.y; + + // Calculate the Euclidean distances between vehicle's corner and the current and next points. + const double distance1 = tier4_autoware_utils::calcDistance2d(p1, vehicle_corner_point); + const double distance2 = tier4_autoware_utils::calcDistance2d(p2, vehicle_corner_point); + + // If one of the bound points is behind and the other is in front of the vehicle corner point + // and any of these points is closer than the current minimum distance, + // then update minimum distance, lateral distance and the segment index. + if (dx_p1 < 0 && dx_p2 > 0 && (distance1 < min_distance || distance2 < min_distance)) { + min_distance = std::min(distance1, distance2); + // Update lateral distance using the formula derived from similar triangles in the lateral + // cross-section view. + lateral_distance = -1.0 * (dy_p1 * dx_p2 + dy_p2 * -dx_p1) / (dx_p2 - dx_p1); + segment_idx = i; } } - } + }; - if (!found_neighbor_bound) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "neighbor shoulder bound to footprint is not found."); + // Calculate the lateral distance for both the rear and front corners of the vehicle. + boost::optional rear_segment_idx{}; + boost::optional rear_lateral_distance{}; + calcLateralDistanceFromBound(rear_corner_point, rear_lateral_distance, rear_segment_idx); + boost::optional front_segment_idx{}; + boost::optional front_lateral_distance{}; + calcLateralDistanceFromBound(front_corner_point, front_lateral_distance, front_segment_idx); + + // If no closest bound segment was found for both corners, return an empty optional. + if (!rear_lateral_distance && !front_lateral_distance) { return {}; } - - // min_distance is the distance from corner_pose to bound, so reverse this value - return -min_distance; + // If only one of them found the closest bound, return the found lateral distance. + if (!rear_lateral_distance) { + return *front_lateral_distance; + } else if (!front_lateral_distance) { + return *rear_lateral_distance; + } + // If both corners found their closest bound, return the maximum (for left side) or the minimum + // (for right side) lateral distance. + lateral_distance = left_side ? std::max(*rear_lateral_distance, *front_lateral_distance) + : std::min(*rear_lateral_distance, *front_lateral_distance); + + // Iterate through all segments between the segments closest to the rear and front corners. + // Update the lateral distance in case any of these inner segments are closer to the vehicle. + for (size_t i = *rear_segment_idx + 1; i < *front_segment_idx; i++) { + Pose bound_pose; + bound_pose.position = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); + bound_pose.orientation = vehicle_pose.orientation; + + const Point inverse_rear_point = + tier4_autoware_utils::inverseTransformPoint(rear_corner_point, bound_pose); + const Point inverse_front_point = + tier4_autoware_utils::inverseTransformPoint(front_corner_point, bound_pose); + const double dx_rear = inverse_rear_point.x; + const double dx_front = inverse_front_point.x; + const double dy_rear = inverse_rear_point.y; + const double dy_front = inverse_front_point.y; + + const double current_lateral_distance = + (dy_rear * dx_front + dy_front * -dx_rear) / (dx_front - dx_rear); + lateral_distance = left_side ? std::max(lateral_distance, current_lateral_distance) + : std::min(lateral_distance, current_lateral_distance); + } + + return lateral_distance; } double getArcLengthToTargetLanelet( From b2557112c856faa2172771262fc25184e3278917 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 5 Sep 2023 15:36:19 +0900 Subject: [PATCH 51/93] fix(route_handler): fix getter for pose (#4884) Signed-off-by: kosuke55 --- planning/route_handler/src/route_handler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 238cd87db872f..25e5862d1190c 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -426,7 +426,7 @@ Pose RouteHandler::getStartPose() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getStartPose: Route has not been set yet"); - Pose(); + return Pose(); } return route_ptr_->start_pose; } @@ -440,7 +440,7 @@ Pose RouteHandler::getGoalPose() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getGoalPose: Route has not been set yet"); - Pose(); + return Pose(); } return route_ptr_->goal_pose; } From 9b02b5d7299d884cbf19b93d3ba2519c113f1e18 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 5 Sep 2023 17:17:14 +0900 Subject: [PATCH 52/93] fix(behavior_path_planner): save original route goal pose (#4876) * fix(behavior_path_planner): save original route goal pose Signed-off-by: kosuke55 * fix return Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.cpp | 6 +++--- .../src/utils/goal_planner/util.cpp | 2 +- .../include/route_handler/route_handler.hpp | 2 ++ planning/route_handler/src/route_handler.cpp | 14 ++++++++++++++ 4 files changed, 20 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f560c82223c13..26639c838714a 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -271,7 +271,7 @@ bool GoalPlannerModule::isExecutionRequested() const const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose & goal_pose = route_handler->getGoalPose(); + const Pose & goal_pose = route_handler->getOriginalGoalPose(); // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; @@ -506,7 +506,7 @@ void GoalPlannerModule::generateGoalCandidates() } // calculate goal candidates - const Pose goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getOriginalGoalPose(); refined_goal_pose_ = calcRefinedGoal(goal_pose); if (goal_planner_utils::isAllowedGoalModification(route_handler)) { goal_searcher_->setPlannerData(planner_data_); @@ -1543,7 +1543,7 @@ void GoalPlannerModule::printParkingPositionError() const bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const { const auto & route_handler = planner_data_->route_handler; - const Pose & goal_pose = route_handler->getGoalPose(); + const Pose & goal_pose = route_handler->getOriginalGoalPose(); const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 8b48d1a867126..0d58a54431be0 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -57,7 +57,7 @@ PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWith lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side) { - const Pose goal_pose = route_handler.getGoalPose(); + const Pose goal_pose = route_handler.getOriginalGoalPose(); lanelet::ConstLanelet target_shoulder_lane{}; if (route_handler::RouteHandler::getPullOverTarget( diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 865350d831c4b..c7120510dc547 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -98,6 +98,7 @@ class RouteHandler Pose getGoalPose() const; Pose getStartPose() const; Pose getOriginalStartPose() const; + Pose getOriginalGoalPose() const; lanelet::Id getGoalLaneId() const; bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const; std::vector getLanesBeforePose( @@ -376,6 +377,7 @@ class RouteHandler // save original(not modified) route start pose for start planer execution Pose original_start_pose_; + Pose original_goal_pose_; // non-const methods void setLaneletsFromRouteMsg(); diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 25e5862d1190c..3443416e85e12 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -176,6 +176,7 @@ void RouteHandler::setRoute(const LaneletRoute & route_msg) // if get not modified route but new route, reset original start pose if (!route_ptr_ || route_ptr_->uuid != route_msg.uuid) { original_start_pose_ = route_msg.start_pose; + original_goal_pose_ = route_msg.goal_pose; } route_ptr_ = std::make_shared(route_msg); is_handler_ready_ = false; @@ -433,6 +434,10 @@ Pose RouteHandler::getStartPose() const Pose RouteHandler::getOriginalStartPose() const { + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalStartPose: Route has not been set yet"); + return Pose(); + } return original_start_pose_; } @@ -445,6 +450,15 @@ Pose RouteHandler::getGoalPose() const return route_ptr_->goal_pose; } +Pose RouteHandler::getOriginalGoalPose() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalGoalPose: Route has not been set yet"); + return Pose(); + } + return original_goal_pose_; +} + lanelet::Id RouteHandler::getGoalLaneId() const { if (!route_ptr_ || route_ptr_->segments.empty()) { From ff2b0a59017e3d62a577b192dd4a7a2952d5a103 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 5 Sep 2023 21:24:17 +0900 Subject: [PATCH 53/93] feat(goal_planner): set ignore_distance_from_lane_start 0.0 (#4889) Signed-off-by: kosuke55 --- .../config/goal_planner/goal_planner.param.yaml | 2 +- .../docs/behavior_path_planner_goal_planner_design.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 39d05a7fb761b..3e6481931f051 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -17,7 +17,7 @@ longitudinal_margin: 3.0 max_lateral_offset: 0.5 lateral_offset_interval: 0.25 - ignore_distance_from_lane_start: 15.0 + ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.5 # occupancy grid map diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 99dadc959f5a5..6ceeb4c22925b 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -174,7 +174,7 @@ searched for in certain range of the shoulder lane. | longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | | max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | | lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | ## **Path Generation** From 76906281c44802e233f0f360023e099c19d616c3 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 6 Sep 2023 02:16:59 +0900 Subject: [PATCH 54/93] refactor(start/goal_planner): minor refactor like const and reference (#4897) Signed-off-by: kosuke55 --- .../utils/start_planner/freespace_pull_out.hpp | 2 +- .../utils/start_planner/geometric_pull_out.hpp | 2 +- .../utils/start_planner/pull_out_planner_base.hpp | 2 +- .../utils/start_planner/shift_pull_out.hpp | 2 +- .../src/scene_module/goal_planner/goal_planner_module.cpp | 4 ++-- .../src/scene_module/start_planner/start_planner_module.cpp | 4 ++-- .../geometric_parallel_parking/geometric_parallel_parking.cpp | 1 - .../src/utils/start_planner/freespace_pull_out.cpp | 4 ++-- .../src/utils/start_planner/geometric_pull_out.cpp | 2 +- .../src/utils/start_planner/shift_pull_out.cpp | 2 +- 10 files changed, 12 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp index abdf17c9c6cfe..2b2de183b2dac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -41,7 +41,7 @@ class FreespacePullOut : public PullOutPlannerBase PlannerType getPlannerType() override { return PlannerType::FREESPACE; } - boost::optional plan(Pose start_pose, Pose end_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & end_pose) override; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp index 7ace770dc7ff1..a5ff52e74038f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp @@ -29,7 +29,7 @@ class GeometricPullOut : public PullOutPlannerBase explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 6e6e5111e5dd9..cb662efd767bf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -60,7 +60,7 @@ class PullOutPlannerBase } virtual PlannerType getPlannerType() = 0; - virtual boost::optional plan(Pose start_pose, Pose goal_pose) = 0; + virtual boost::optional plan(const Pose & start_pose, const Pose & goal_pose) = 0; protected: std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 2042593064c51..0842d0a17bd97 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp @@ -37,7 +37,7 @@ class ShiftPullOut : public PullOutPlannerBase std::shared_ptr & lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 26639c838714a..3730749e571b1 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -271,7 +271,7 @@ bool GoalPlannerModule::isExecutionRequested() const const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose & goal_pose = route_handler->getOriginalGoalPose(); + const Pose goal_pose = route_handler->getOriginalGoalPose(); // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; @@ -1111,7 +1111,7 @@ bool GoalPlannerModule::isStuck() bool GoalPlannerModule::hasFinishedCurrentPath() { - const auto & current_path_end = getCurrentPath().points.back(); + const auto current_path_end = getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index a802b43ee4d49..e1b0bbd4abc5f 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -107,7 +107,7 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { // Execute when current pose is near route start pose - const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; if ( tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > @@ -116,7 +116,7 @@ bool StartPlannerModule::isExecutionRequested() const } // Check if ego arrives at goal - const Pose & goal_pose = planner_data_->route_handler->getGoalPose(); + const Pose goal_pose = planner_data_->route_handler->getGoalPose(); if ( tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < parameters_->th_arrived_distance) { diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index a5ccfc9517771..40d5e1dc7a41f 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -445,7 +445,6 @@ std::vector GeometricParallelParking::planOneTrial( if (std::abs(end_pose_offset) > 0) { PathPointWithLaneId straight_point{}; straight_point.point.pose = goal_pose; - // setLaneIds(straight_point); path_turn_right.points.push_back(straight_point); } diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index ba34901d9df6a..36fb6381ac967 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -44,7 +44,7 @@ FreespacePullOut::FreespacePullOut( } } -boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +boost::optional FreespacePullOut::plan(const Pose & start_pose, const Pose & end_pose) { const auto & route_handler = planner_data_->route_handler; const double backward_path_length = planner_data_->parameters.backward_path_length; @@ -86,7 +86,7 @@ boost::optional FreespacePullOut::plan(const Pose start_pose, const } // push back generate road lane path between end pose and goal pose to last path - const auto & goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getGoalPose(); constexpr double offset_from_end_pose = 1.0; const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 721ffd3064840..912853e53ede3 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -36,7 +36,7 @@ GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParame planner_.setParameters(parallel_parking_parameters_); } -boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional GeometricPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { PullOutPath output; 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 08dcc0254deb1..ee9c4323aa06c 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 @@ -40,7 +40,7 @@ ShiftPullOut::ShiftPullOut( { } -boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional ShiftPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; const auto & common_parameters = planner_data_->parameters; From 8ad2bfaffdedfd372ba2fcf3d1d9bbf125cad7a4 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 6 Sep 2023 02:18:12 +0900 Subject: [PATCH 55/93] refactor(start/goal_planner): use combineLanelets (#4894) Signed-off-by: kosuke55 --- .../start_planner/start_planner_module.cpp | 12 +----------- .../src/utils/goal_planner/geometric_pull_over.cpp | 9 ++++----- .../src/utils/start_planner/geometric_pull_out.cpp | 10 ---------- 3 files changed, 5 insertions(+), 26 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index e1b0bbd4abc5f..e12352eec36d3 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -147,17 +147,7 @@ bool StartPlannerModule::isExecutionRequested() const /*forward_only_in_route*/ true); const auto pull_out_lanes = start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - auto lanes = current_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } + const auto lanes = utils::combineLanelets(current_lanes, pull_out_lanes); if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { return false; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 6a5ccc827db29..dd9dc46703ea7 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -47,13 +47,12 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } - auto lanes = road_lanes; - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); const auto & p = parallel_parking_parameters_; const double max_steer_angle = @@ -62,7 +61,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) planner_.setPlannerData(planner_data_); const bool found_valid_path = - planner_.planPullOver(goal_pose, road_lanes, shoulder_lanes, is_forward_); + planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_); if (!found_valid_path) { return {}; } diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 912853e53ede3..4bf4a3b747b8d 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -47,16 +47,6 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); - auto lanes = road_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } planner_.setTurningRadius( planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); From d65d8ee4f9f1269090233b29159d74a73fa28225 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 6 Sep 2023 02:21:37 +0900 Subject: [PATCH 56/93] feat(goal_planner): do not use minimum_request_length for fixed goal planner (#4831) Signed-off-by: kosuke55 --- .../goal_planner/goal_planner.param.yaml | 2 +- ...havior_path_planner_goal_planner_design.md | 36 ++++++++++--------- .../goal_planner/goal_planner_parameters.hpp | 2 +- .../goal_planner/goal_planner_module.cpp | 13 +++++-- .../src/scene_module/goal_planner/manager.cpp | 12 +++---- 5 files changed, 38 insertions(+), 27 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 3e6481931f051..213276c4bd1da 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -2,7 +2,6 @@ ros__parameters: goal_planner: # general params - minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. @@ -36,6 +35,7 @@ # pull over pull_over: + minimum_request_length: 100.0 pull_over_velocity: 3.0 pull_over_minimum_velocity: 1.38 decide_path_distance: 10.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 6ceeb4c22925b..51f8f9a851152 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -86,8 +86,8 @@ Either one is activated when all conditions are met. ### fixed_goal_planner -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. - Route is set with `allow_goal_modification=false` by default. +- ego-vehicle is in the same lane as the goal. @@ -95,7 +95,7 @@ Either one is activated when all conditions are met. #### pull over on road lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Route is set with `allow_goal_modification=true` . - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service. - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz. @@ -105,7 +105,7 @@ Either one is activated when all conditions are met. #### pull over on shoulder lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Goal is set in the `road_shoulder`. @@ -118,17 +118,11 @@ Either one is activated when all conditions are met. ## General parameters for goal_planner -| Name | Unit | Type | Description | Default value | -| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 100.0 | -| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| Name | Unit | Type | Description | Default value | +| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | ## **collision check** @@ -175,11 +169,21 @@ searched for in certain range of the shoulder lane. | max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | | lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | | ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -## **Path Generation** +## **Pull Over** There are three path generation methods. -The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane. +The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | ### **shift parking** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index f87944641f59b..79d3a0c6e9401 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -40,7 +40,6 @@ enum class ParkingPolicy { struct GoalPlannerParameters { // general params - double minimum_request_length; double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; @@ -71,6 +70,7 @@ struct GoalPlannerParameters double object_recognition_collision_check_max_extra_stopping_margin; // pull over general params + double pull_over_minimum_request_length; double pull_over_velocity; double pull_over_minimum_velocity; double decide_path_distance; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 3730749e571b1..8d211844609db 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -302,12 +302,19 @@ bool GoalPlannerModule::isExecutionRequested() const return false; } + // if goal modification is not allowed + // 1) goal_pose is in current_lanes, plan path to the original fixed goal + // 2) goal_pose is NOT in current_lanes, do not execute goal_planner + if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + return goal_is_in_current_lanes; + } + // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) ? calcModuleRequestLength() - : parameters_->minimum_request_length; + : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; @@ -343,13 +350,13 @@ double GoalPlannerModule::calcModuleRequestLength() const { const auto min_stop_distance = calcFeasibleDecelDistance(0.0); if (!min_stop_distance) { - return parameters_->minimum_request_length; + return parameters_->pull_over_minimum_request_length; } const double minimum_request_length = *min_stop_distance + parameters_->backward_goal_search_length + approximate_pull_over_distance_; - return std::max(minimum_request_length, parameters_->minimum_request_length); + return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 5923597af8da8..d6e9fc3bb3737 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -34,11 +34,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // general params { - std::string ns = "goal_planner."; - p.minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); + p.th_arrived_distance = node->declare_parameter(base_ns + "th_arrived_distance"); + p.th_stopped_time = node->declare_parameter(base_ns + "th_stopped_time"); } // goal search @@ -95,7 +93,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // pull over general params { - std::string ns = "goal_planner.pull_over."; + const std::string ns = base_ns + "pull_over."; + p.pull_over_minimum_request_length = + node->declare_parameter(ns + "minimum_request_length"); p.pull_over_velocity = node->declare_parameter(ns + "pull_over_velocity"); p.pull_over_minimum_velocity = node->declare_parameter(ns + "pull_over_minimum_velocity"); From d4d1caab53d14689361520abf0ae6990cadce5a6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 6 Sep 2023 02:23:00 +0900 Subject: [PATCH 57/93] fix(start_planner): fix start pose candidats when the ego is close to the lane end (#4893) Signed-off-by: kosuke55 --- .../start_planner/start_planner_module.hpp | 7 +- .../start_planner/start_planner_module.cpp | 79 +++++++++++-------- 2 files changed, 50 insertions(+), 36 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 24fd27d9a7a78..52cd2ae5862c4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -122,7 +122,8 @@ class StartPlannerModule : public SceneModuleInterface std::mutex mutex_; PathWithLaneId getFullPath() const; - std::vector searchPullOutStartPoses(); + PathWithLaneId calcStartPoseCandidatesBackwardPath() const; + std::vector searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const; std::shared_ptr lane_departure_checker_; @@ -132,8 +133,8 @@ class StartPlannerModule : public SceneModuleInterface void incrementPathIndex(); PathWithLaneId getCurrentPath() const; void planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority); + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority); PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index e12352eec36d3..c8cb2f7c0b24c 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -396,8 +396,8 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const } void StartPlannerModule::planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority) + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority) { // check if start pose candidates are valid if (start_pose_candidates.empty()) { @@ -405,12 +405,13 @@ void StartPlannerModule::planWithPriority( } const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { - // Set back_finished flag based on the current index - status_.back_finished = i == 0; - // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); + // Set back_finished to true if the current start pose is same to refined_start_pose + status_.back_finished = + tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; + planner->setPlannerData(planner_data_); const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); // not found safe path @@ -486,7 +487,6 @@ void StartPlannerModule::planWithPriority( for (const auto & p : order_priority) { if (is_safe_with_pose_planner(p.first, p.second)) { - const std::lock_guard lock(mutex_); return; } } @@ -604,9 +604,24 @@ void StartPlannerModule::updatePullOutStatus() const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); + // refine start pose with pull out lanes. + // 1) backward driving is not allowed: use refined pose just as start pose. + // 2) backward driving is allowed: use refined pose to check if backward driving is needed. + const PathWithLaneId start_pose_candidates_path = calcStartPoseCandidatesBackwardPath(); + const auto refined_start_pose = calcLongitudinalOffsetPose( + start_pose_candidates_path.points, planner_data_->self_odometry->pose.pose.position, 0.0); + if (!refined_start_pose) return; + // search pull out start candidates backward - std::vector start_pose_candidates = searchPullOutStartPoses(); - planWithPriority(start_pose_candidates, goal_pose, parameters_->search_priority); + const std::vector start_pose_candidates = std::invoke([&]() -> std::vector { + if (parameters_->enable_back) { + return searchPullOutStartPoses(start_pose_candidates_path); + } + return {*refined_start_pose}; + }); + + planWithPriority( + start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); checkBackFinished(); if (!status_.back_finished) { @@ -616,20 +631,34 @@ void StartPlannerModule::updatePullOutStatus() } } -std::vector StartPlannerModule::searchPullOutStartPoses() +PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const { - std::vector pull_out_start_pose{}; - const Pose & current_pose = planner_data_->self_odometry->pose.pose; // get backward shoulder path const auto arc_position_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); const double check_distance = parameters_->max_back_distance + 30.0; // buffer - auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath( + auto path = planner_data_->route_handler->getCenterLinePath( status_.pull_out_lanes, arc_position_pose.length - check_distance, arc_position_pose.length + check_distance); + // lateral shift to current_pose + const double distance_from_center_line = arc_position_pose.distance; + for (auto & p : path.points) { + p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); + } + + return path; +} + +std::vector StartPlannerModule::searchPullOutStartPoses( + const PathWithLaneId & start_pose_candidates) const +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + + std::vector pull_out_start_pose{}; + // filter pull out lanes stop objects const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( @@ -637,28 +666,12 @@ std::vector StartPlannerModule::searchPullOutStartPoses() const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); - // lateral shift to current_pose - const double distance_from_center_line = arc_position_pose.distance; - for (auto & p : backward_shoulder_path.points) { - p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); - } - - // if backward driving is disable, just refine current pose to the lanes - if (!parameters_->enable_back) { - const auto refined_pose = - calcLongitudinalOffsetPose(backward_shoulder_path.points, current_pose.position, 0); - if (refined_pose) { - pull_out_start_pose.push_back(*refined_pose); - } - return pull_out_start_pose; - } - // check collision between footprint and object at the backed pose const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); for (double back_distance = 0.0; back_distance <= parameters_->max_back_distance; back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( - backward_shoulder_path.points, current_pose.position, -back_distance); + start_pose_candidates.points, current_pose.position, -back_distance); if (!backed_pose) { continue; } @@ -666,10 +679,10 @@ std::vector StartPlannerModule::searchPullOutStartPoses() // check the back pose is near the lane end const double length_to_backed_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length; - double length_to_lane_end = 0.0; - for (const auto & lane : status_.pull_out_lanes) { - length_to_lane_end += lanelet::utils::getLaneletLength2d(lane); - } + + const double length_to_lane_end = std::accumulate( + std::begin(status_.pull_out_lanes), std::end(status_.pull_out_lanes), 0.0, + [](double acc, const auto & lane) { return acc + lanelet::utils::getLaneletLength2d(lane); }); const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose; if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) { RCLCPP_WARN_THROTTLE( From 7d3a6a30c52c57259a95b653e5cd5764bec09a35 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 6 Sep 2023 02:57:51 +0900 Subject: [PATCH 58/93] feat(start_planner): run start planner even if ego is out of lane (#4895) Signed-off-by: kosuke55 --- .../start_planner/start_planner_module.cpp | 20 ------------------- 1 file changed, 20 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index c8cb2f7c0b24c..14908b7e5fc23 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -133,26 +133,6 @@ bool StartPlannerModule::isExecutionRequested() const return false; } - // Create vehicle footprint - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - const auto vehicle_footprint = transformVector( - local_vehicle_footprint, - tier4_autoware_utils::pose2transform(planner_data_->self_odometry->pose.pose)); - - // Check if ego is not out of lanes - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - const auto lanes = utils::combineLanelets(current_lanes, pull_out_lanes); - - if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { - return false; - } - return true; } From 9b64aa01ced69e47f1e847317a7c8d92ecd8d913 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 6 Sep 2023 18:01:41 +0900 Subject: [PATCH 59/93] fix(behavior_path_planner): fix functions related to pull over lanes (#4857) * fix(behavior_path_planner): fix functions related to pull over lanes Signed-off-by: kosuke55 * add buffer Signed-off-by: kosuke55 * add comments Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../utils/goal_planner/util.hpp | 4 +- .../goal_planner/goal_planner_module.cpp | 20 ++++--- .../goal_planner/freespace_pull_over.cpp | 5 +- .../goal_planner/geometric_pull_over.cpp | 5 +- .../src/utils/goal_planner/goal_searcher.cpp | 10 ++-- .../utils/goal_planner/shift_pull_over.cpp | 8 +-- .../src/utils/goal_planner/util.cpp | 27 +++++----- .../include/route_handler/route_handler.hpp | 12 +++-- planning/route_handler/src/route_handler.cpp | 54 +++++++++++++++---- 9 files changed, 97 insertions(+), 48 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 29a4a8efa719f..0adda77a2ad72 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -46,7 +46,9 @@ using visualization_msgs::msg::MarkerArray; // TODO(sugahara) move to util PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side); +lanelet::ConstLanelets getPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance); PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 8d211844609db..03ad8b752de7b 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -330,8 +330,9 @@ bool GoalPlannerModule::isExecutionRequested() const // if (A) or (B) is met execute pull over // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); if (!isCrossingPossible(current_lane, target_lane)) { @@ -365,8 +366,9 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const const double base_link2front = planner_data_->parameters.base_link2front; const double base_link2rear = planner_data_->parameters.base_link2rear; - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::Lanelet closest_pull_over_lanelet{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet); @@ -616,8 +618,9 @@ void GoalPlannerModule::setLanes() planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, /*forward_only_in_route*/ false); - status_.pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + status_.pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); status_.lanes = utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes); } @@ -1552,8 +1555,9 @@ bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const const auto & route_handler = planner_data_->route_handler; const Pose & goal_pose = route_handler->getOriginalGoalPose(); - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index f4aabb13b8916..9e55b9b859c68 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -63,8 +63,9 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index dd9dc46703ea7..f58c33a8f88b5 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -47,8 +47,9 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index e12ee576a5d4b..19dec4d3c97d1 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -57,8 +57,9 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double base_link2front = planner_data_->parameters.base_link2front; const double base_link2rear = planner_data_->parameters.base_link2rear; - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); auto lanes = utils::getExtendedCurrentLanes( planner_data_, backward_length, forward_length, /*forward_only_in_route*/ false); @@ -157,8 +158,9 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const } // check margin with pull over lane objects - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); const auto [shoulder_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( *(planner_data_->dynamic_object), pull_over_lanes); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 2f5c1d9b05b7f..46e683dd34e58 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -49,16 +49,16 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, backward_search_length, forward_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = - generatePullOverPath(road_lanes, shoulder_lanes, goal_pose, lateral_jerk); + generatePullOverPath(road_lanes, pull_over_lanes, goal_pose, lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 0d58a54431be0..aa3380c2b99df 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -55,36 +55,37 @@ PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWith return path; } -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side) +lanelet::ConstLanelets getPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance) { const Pose goal_pose = route_handler.getOriginalGoalPose(); + // Buffer to get enough lanes in front of the goal, need much longer than the pull over distance. + // In the case of loop lanes, it may not be possible to extend the lane forward. + // todo(kosuek55): automatically calculates this distance. + const double backward_distance_with_buffer = backward_distance + 100; + lanelet::ConstLanelet target_shoulder_lane{}; if (route_handler::RouteHandler::getPullOverTarget( route_handler.getShoulderLanelets(), goal_pose, &target_shoulder_lane)) { // pull over on shoulder lane - return route_handler.getShoulderLaneletSequence(target_shoulder_lane, goal_pose); + return route_handler.getShoulderLaneletSequence( + target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance); } - // pull over on road lane lanelet::ConstLanelet closest_lane{}; route_handler.getClosestLaneletWithinRoute(goal_pose, &closest_lane); - lanelet::ConstLanelet outermost_lane{}; if (left_side) { - outermost_lane = route_handler.getMostLeftLanelet(closest_lane); + outermost_lane = route_handler.getMostLeftLanelet(closest_lane, false, true); } else { - outermost_lane = route_handler.getMostRightLanelet(closest_lane); - } - - lanelet::ConstLanelet outermost_shoulder_lane; - if (route_handler.getLeftShoulderLanelet(outermost_lane, &outermost_shoulder_lane)) { - return route_handler.getShoulderLaneletSequence(outermost_shoulder_lane, goal_pose); + outermost_lane = route_handler.getMostRightLanelet(closest_lane, false, true); } - const bool dist = std::numeric_limits::max(); constexpr bool only_route_lanes = false; - return route_handler.getLaneletSequence(outermost_lane, dist, dist, only_route_lanes); + return route_handler.getLaneletSequence( + outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes); } PredictedObjects filterObjectsByLateralDistance( diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index c7120510dc547..061953f30005c 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -122,7 +122,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -132,7 +133,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const; @@ -195,7 +197,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ lanelet::ConstLanelet getMostRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -205,7 +208,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ lanelet::ConstLanelet getMostLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Searches the furthest linestring to the right side of the lanelet diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 3443416e85e12..7c2b4c2af7974 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -528,7 +528,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter( while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelet next_lanelet; if (!getNextLaneletWithinRoute(current_lanelet, &next_lanelet)) { - break; + if (only_route_lanes) { + break; + } + const auto next_lanes = getNextLanelets(current_lanelet); + if (next_lanes.empty()) { + break; + } + next_lanelet = next_lanes.front(); } // loop check if (lanelet.id() == next_lanelet.id()) { @@ -556,7 +563,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelets candidate_lanelets; if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { - break; + if (only_route_lanes) { + break; + } + const auto prev_lanes = getPreviousLanelets(current_lanelet); + if (prev_lanes.empty()) { + break; + } + candidate_lanelets = prev_lanes; } // loop check if (std::any_of( @@ -947,7 +961,8 @@ bool RouteHandler::isBijectiveConnection( } boost::optional RouteHandler::getRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // right road lanelet of shoulder lanelet if (isShoulderLanelet(lanelet)) { @@ -959,6 +974,14 @@ boost::optional RouteHandler::getRightLanelet( return boost::none; } + // right shoulder lanelet + if (get_shoulder_lane) { + lanelet::ConstLanelet right_shoulder_lanelet; + if (getRightShoulderLanelet(lanelet, &right_shoulder_lanelet)) { + return right_shoulder_lanelet; + } + } + // routable lane const auto & right_lane = routing_graph_ptr_->right(lanelet); if (right_lane) { @@ -1019,7 +1042,8 @@ bool RouteHandler::getLeftLaneletWithinRoute( } boost::optional RouteHandler::getLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // left road lanelet of shoulder lanelet if (isShoulderLanelet(lanelet)) { @@ -1031,6 +1055,14 @@ boost::optional RouteHandler::getLeftLanelet( return boost::none; } + // left shoulder lanelet + if (get_shoulder_lane) { + lanelet::ConstLanelet left_shoulder_lanelet; + if (getLeftShoulderLanelet(lanelet, &left_shoulder_lanelet)) { + return left_shoulder_lanelet; + } + } + // routable lane const auto & left_lane = routing_graph_ptr_->left(lanelet); if (left_lane) { @@ -1206,26 +1238,28 @@ lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLane } lanelet::ConstLanelet RouteHandler::getMostRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // recursively compute the width of the lanes - const auto & same = getRightLanelet(lanelet, enable_same_root); + const auto & same = getRightLanelet(lanelet, enable_same_root, get_shoulder_lane); if (same) { - return getMostRightLanelet(same.get(), enable_same_root); + return getMostRightLanelet(same.get(), enable_same_root, get_shoulder_lane); } return lanelet; } lanelet::ConstLanelet RouteHandler::getMostLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet, enable_same_root); + const auto & same = getLeftLanelet(lanelet, enable_same_root, get_shoulder_lane); if (same) { - return getMostLeftLanelet(same.get(), enable_same_root); + return getMostLeftLanelet(same.get(), enable_same_root, get_shoulder_lane); } return lanelet; From 275b9973c6f0e415de29d129e566e07f27c2a3b6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 7 Sep 2023 15:18:20 +0900 Subject: [PATCH 60/93] fix(start_planner): fix drivable area without shoulder lanes (#4909) fix(start_planner): fix pull out lanes without shoulder lanes Signed-off-by: kosuke55 --- .../start_planner/start_planner_module.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 14908b7e5fc23..601c11986ef12 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -535,10 +535,15 @@ std::vector StartPlannerModule::generateDrivableLanes( const PathWithLaneId & path) const { const auto path_road_lanes = getPathRoadLanes(path); - if (!path_road_lanes.empty()) { - return utils::generateDrivableLanesWithShoulderLanes( - getPathRoadLanes(path), status_.pull_out_lanes); + lanelet::ConstLanelets shoulder_lanes; + const auto & rh = planner_data_->route_handler; + std::copy_if( + status_.pull_out_lanes.begin(), status_.pull_out_lanes.end(), + std::back_inserter(shoulder_lanes), + [&rh](const auto & pull_out_lane) { return rh->isShoulderLanelet(pull_out_lane); }); + + return utils::generateDrivableLanesWithShoulderLanes(getPathRoadLanes(path), shoulder_lanes); } // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes From 5614761225e877bf0b0aac33cece312de1bdfb55 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 10 Sep 2023 15:37:50 +0900 Subject: [PATCH 61/93] fix(start_planner): prevent approval when stop path (#4932) Signed-off-by: kosuke55 --- .../scene_module/start_planner/start_planner_module.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 601c11986ef12..ca05d10981b39 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -138,6 +138,10 @@ bool StartPlannerModule::isExecutionRequested() const bool StartPlannerModule::isExecutionReady() const { + if (!status_.is_safe) { + return false; + } + return true; } @@ -167,6 +171,7 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -184,6 +189,7 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -280,6 +286,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -290,6 +297,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } From 726fd7ff18b13240ca6cbaa0fb002ed834403810 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 12 Sep 2023 11:51:52 +0900 Subject: [PATCH 62/93] fix(start_planner): crop backward path to start from narrow space (#4929) * fix(start_planner): crop backward path to start from narrow space Signed-off-by: kosuke55 * check out of lanes Signed-off-by: kosuke55 * refactor Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../utils/start_planner/shift_pull_out.cpp | 43 ++++++++++++++++--- 1 file changed, 37 insertions(+), 6 deletions(-) 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 ee9c4323aa06c..fc113cfb91c06 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 @@ -96,17 +96,48 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P shift_path.points.begin() + collision_check_end_idx + 1); } - // check lane departure + // extract shoulder lanes from pull out lanes + lanelet::ConstLanelets shoulder_lanes; + std::copy_if( + pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), + [&route_handler](const auto & pull_out_lane) { + return route_handler->isShoulderLanelet(pull_out_lane); + }); const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_out_lanes); + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::expandLanelets( + const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); + dp.drivable_area_types_to_skip)); + + // crop backward path + // removes points which are out of lanes up to the start pose. + // this ensures that the backward_path stays within the drivable area when starting from a + // narrow place. + const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + PathWithLaneId cropped_path{}; + for (size_t i = 0; i < shift_path.points.size(); ++i) { + const Pose pose = shift_path.points.at(i).point.pose; + const auto transformed_vehicle_footprint = + transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); + const bool is_out_of_lane = + LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint); + if (i <= start_segment_idx) { + if (!is_out_of_lane) { + cropped_path.points.push_back(shift_path.points.at(i)); + } + } else { + cropped_path.points.push_back(shift_path.points.at(i)); + } + } + shift_path.points = cropped_path.points; + + // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane( - utils::transformToLanelets(expanded_lanes), path_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) { continue; } From bd16bba657148c35c9461d63bc84321d525ff0ab Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 12 Sep 2023 11:52:13 +0900 Subject: [PATCH 63/93] refactor(start_planner): refactor drivable lanes (#4951) Signed-off-by: kosuke55 --- .../src/scene_module/start_planner/start_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index ca05d10981b39..16d80309f1ecd 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -551,7 +551,7 @@ std::vector StartPlannerModule::generateDrivableLanes( std::back_inserter(shoulder_lanes), [&rh](const auto & pull_out_lane) { return rh->isShoulderLanelet(pull_out_lane); }); - return utils::generateDrivableLanesWithShoulderLanes(getPathRoadLanes(path), shoulder_lanes); + return utils::generateDrivableLanesWithShoulderLanes(path_road_lanes, shoulder_lanes); } // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes From f373778d912a22f051e9ec4f667102015c3f0c71 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 12 Sep 2023 11:52:35 +0900 Subject: [PATCH 64/93] fix(start_planner): fix path update (#4931) Signed-off-by: kosuke55 --- .../src/scene_module/start_planner/start_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 16d80309f1ecd..a0589eca8dd56 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -582,7 +582,7 @@ void StartPlannerModule::updatePullOutStatus() // skip updating if enough time has not passed for preventing chattering between back and // start_planner - if (!has_received_new_route && !last_pull_out_start_update_time_ && !status_.back_finished) { + if (!has_received_new_route) { if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } From 8a379bb4def5cb4eed2ae6675d62a59a5b7acbc7 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 12 Sep 2023 17:38:16 +0900 Subject: [PATCH 65/93] fix(start/goal_planner): fix inverse order path points (#4950) fix(start_planner): fix inverse order path points Signed-off-by: kosuke55 --- .../src/utils/goal_planner/shift_pull_over.cpp | 4 ++++ .../src/utils/start_planner/shift_pull_out.cpp | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 46e683dd34e58..be084ebb19ca0 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -170,6 +171,9 @@ boost::optional ShiftPullOver::generatePullOverPath( shifted_path.path.points.push_back(p); } + shifted_path.path = + utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); + // set the same z as the goal for (auto & p : shifted_path.path.points) { p.point.pose.position.z = goal_pose.position.z; 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 fc113cfb91c06..1beab97d60b25 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 @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -294,6 +295,9 @@ std::vector ShiftPullOut::calcPullOutPaths( continue; } + shifted_path.path = + utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); + // set velocity const size_t pull_out_end_idx = findNearestIndex(shifted_path.path.points, shift_end_pose_ptr->position); From bb8943024b00728b041a13c80c90d414e44c734f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 12 Sep 2023 20:08:17 +0900 Subject: [PATCH 66/93] fix(start_planner): keep max back distace whithin lanes (#4962) * fix(start_planner): keep max back distace whithin lanes Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp Co-authored-by: Kyoichi Sugahara * Update planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp * style(pre-commit): autofix --------- Signed-off-by: kosuke55 Co-authored-by: Kyoichi Sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../scene_module/start_planner/start_planner_module.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index a0589eca8dd56..cbdd0931569f3 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -659,9 +659,16 @@ std::vector StartPlannerModule::searchPullOutStartPoses( const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); + // Set the maximum backward distance less than the distance from the vehicle's base_link to the + // lane's rearmost point to prevent lane departure. + const double s_current = + lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose).length; + const double max_back_distance = std::clamp( + s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance); + // check collision between footprint and object at the backed pose const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - for (double back_distance = 0.0; back_distance <= parameters_->max_back_distance; + for (double back_distance = 0.0; back_distance <= max_back_distance; back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( start_pose_candidates.points, current_pose.position, -back_distance); From d982b3afa59f2d2d63078e418619470fe689e257 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 13 Sep 2023 00:10:20 +0900 Subject: [PATCH 67/93] refactor(goal_planner): remove unused variables (#4966) Signed-off-by: kosuke55 --- .../src/utils/goal_planner/goal_searcher.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 19dec4d3c97d1..4c71929db0abe 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -73,10 +73,6 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), parameters_.goal_search_interval); - const auto [shoulder_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; for (const auto & p : center_line_path.points) { From 8f4ab70b1fb5988fcdd5ddb10dfdd5e42f1465c6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 13 Sep 2023 00:11:42 +0900 Subject: [PATCH 68/93] fix(goal_planner): fix occpancy grid initializing check (#4969) Signed-off-by: kosuke55 --- .../src/scene_module/goal_planner/goal_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 03ad8b752de7b..9ee05882e5236 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1179,7 +1179,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const { - if (parameters_->use_occupancy_grid || !occupancy_grid_map_) { + if (parameters_->use_occupancy_grid && occupancy_grid_map_) { const bool check_out_of_range = false; if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { return true; From 940cdfc0cf5ec831768afb1e3fe28ac381d39368 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Sun, 17 Sep 2023 01:34:38 +0900 Subject: [PATCH 69/93] fix Signed-off-by: kosuke55 --- .../start_planner/start_planner_module.hpp | 3 ++- .../behavior_path_planner/utils/utils.hpp | 17 +++++++++++++++++ .../src/scene_module/goal_planner/manager.cpp | 9 +++++---- .../src/utils/goal_planner/shift_pull_over.cpp | 2 +- .../src/utils/start_planner/shift_pull_out.cpp | 3 +-- .../behavior_path_planner/src/utils/utils.cpp | 15 +++++++++++++++ 6 files changed, 41 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 52cd2ae5862c4..44b31dcad6ff8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -54,7 +54,8 @@ struct PullOutStatus PathWithLaneId backward_path{}; lanelet::ConstLanelets pull_out_lanes{}; bool is_safe{false}; - bool back_finished{false}; + bool back_finished{false}; // if backward driving is not required, this is also set to true + // todo: rename to clear variable name. Pose pull_out_start_pose{}; PullOutStatus() {} 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 07d93c143ee74..6a25a44c32ee9 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 @@ -412,6 +412,23 @@ std::optional getPolygonByPoint( lanelet::ConstLanelets combineLanelets( const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes); + +/** + * @brief removeInverseOrderPathPoints function + * + * This function is designed to handle a situation that can arise when shifting paths on a curve, + * where the positions of the path points may become inverted (i.e., a point further along the path + * comes to be located before an earlier point). It corrects for this by using the function + * tier4_autoware_utils::isDrivingForward(p1, p2) to check if each pair of adjacent points is in + * the correct order (with the second point being 'forward' of the first). Any points which fail + * this test are omitted from the returned PathWithLaneId. + * + * @param path A path with points possibly in incorrect order. + * @return Returns a new PathWithLaneId that has all points in the correct order. + */ +PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path); + } // namespace behavior_path_planner::utils + #endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index d6e9fc3bb3737..b10ba23ad0173 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -34,9 +34,10 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // general params { - p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); - p.th_arrived_distance = node->declare_parameter(base_ns + "th_arrived_distance"); - p.th_stopped_time = node->declare_parameter(base_ns + "th_stopped_time"); + std::string ns = "goal_planner."; + p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); + p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); + p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); } // goal search @@ -93,7 +94,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // pull over general params { - const std::string ns = base_ns + "pull_over."; + const std::string ns = "goal_planner.pull_over."; p.pull_over_minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); p.pull_over_velocity = node->declare_parameter(ns + "pull_over_velocity"); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index be084ebb19ca0..4971f21e419ea 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -172,7 +172,7 @@ boost::optional ShiftPullOver::generatePullOverPath( } shifted_path.path = - utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); + utils::removeInverseOrderPathPoints(shifted_path.path); // set the same z as the goal for (auto & p : shifted_path.path.points) { 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 1beab97d60b25..628dfe9eea4bf 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 @@ -16,7 +16,6 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -296,7 +295,7 @@ std::vector ShiftPullOut::calcPullOutPaths( } shifted_path.path = - utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); + utils::removeInverseOrderPathPoints(shifted_path.path); // set velocity const size_t pull_out_end_idx = diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index f93309ef739c2..e1cf3b77e2787 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -3259,4 +3259,19 @@ lanelet::ConstLanelets combineLanelets( return combined_lanes; } +PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path) +{ + PathWithLaneId fixed_path; + fixed_path.header = path.header; + fixed_path.points.push_back(path.points.at(0)); + for (size_t i = 1; i < path.points.size(); i++) { + const auto p1 = path.points.at(i - 1).point.pose; + const auto p2 = path.points.at(i).point.pose; + if (tier4_autoware_utils::isDrivingForward(p1, p2)) { + fixed_path.points.push_back(path.points.at(i)); + } + } + return fixed_path; +} + } // namespace behavior_path_planner::utils From 08dd7a1b2a06d381b8fa4345f8930259c1bc74c1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 13 Sep 2023 00:45:25 +0900 Subject: [PATCH 70/93] feat(goal_planner): use only static objects in pull over lanes to path generation (#4968) Signed-off-by: kosuke55 include safety checker Signed-off-by: kosuke55 --- .../goal_planner/goal_planner.param.yaml | 1 + .../goal_planner/goal_planner_parameters.hpp | 1 + .../goal_planner/goal_planner_module.cpp | 11 ++++++++++- .../src/scene_module/goal_planner/manager.cpp | 1 + .../src/utils/goal_planner/goal_searcher.cpp | 18 ++++++++++++++---- 5 files changed, 27 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 213276c4bd1da..9e7cd2e63b342 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -32,6 +32,7 @@ use_object_recognition: true object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker object_recognition_collision_check_max_extra_stopping_margin: 1.0 + th_moving_object_velocity: 1.0 # pull over pull_over: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index 79d3a0c6e9401..72da00daa9ae5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -68,6 +68,7 @@ struct GoalPlannerParameters bool use_object_recognition; double object_recognition_collision_check_margin; double object_recognition_collision_check_max_extra_stopping_margin; + double th_moving_object_velocity; // pull over general params double pull_over_minimum_request_length; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 9ee05882e5236..f2e55f81185ac 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -1187,12 +1188,20 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const } if (parameters_->use_object_recognition) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_->th_moving_object_velocity); const auto common_parameters = planner_data_->parameters; const double base_link2front = common_parameters.base_link2front; const double base_link2rear = common_parameters.base_link2rear; const double vehicle_width = common_parameters.vehicle_width; if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( - path, *(planner_data_->dynamic_object), base_link2front, base_link2rear, vehicle_width, + path, pull_over_lane_stop_objects, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { return true; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index b10ba23ad0173..1863b169ffefd 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -90,6 +90,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.object_recognition_collision_check_max_extra_stopping_margin = node->declare_parameter( ns + "object_recognition_collision_check_max_extra_stopping_margin"); + p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); } // pull over general params diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 4c71929db0abe..3f819a574d99e 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -153,16 +153,18 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const continue; } - // check margin with pull over lane objects + // check longitudinal margin with pull over lane objects const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); - const auto [shoulder_lane_objects, others] = + const auto [pull_over_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_.th_moving_object_velocity); constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( - goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, + goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, parameters_.object_recognition_collision_check_margin, filter_inside); if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { goal_candidate.is_safe = false; @@ -186,8 +188,16 @@ bool GoalSearcher::checkCollision(const Pose & pose) const } if (parameters_.use_object_recognition) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_.th_moving_object_velocity); if (utils::checkCollisionBetweenFootprintAndObjects( - vehicle_footprint_, pose, *(planner_data_->dynamic_object), + vehicle_footprint_, pose, pull_over_lane_stop_objects, parameters_.object_recognition_collision_check_margin)) { return true; } From 314b95b17512b18921eb5563c541b661a5ad265a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 14 Sep 2023 16:55:40 +0900 Subject: [PATCH 71/93] feat(goal_planner): add options of occupancy grid map to use only for goal search (#4970) Signed-off-by: kosuke55 --- .../config/goal_planner/goal_planner.param.yaml | 7 ++++--- .../behavior_path_planner_goal_planner_design.md | 15 ++++++++------- .../goal_planner/goal_planner_parameters.hpp | 5 +++-- .../goal_planner/goal_planner_module.cpp | 6 ++++-- .../src/scene_module/goal_planner/manager.cpp | 9 ++++++--- .../src/utils/goal_planner/goal_searcher.cpp | 6 ++++-- 6 files changed, 29 insertions(+), 19 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 9e7cd2e63b342..513288948c1c0 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -21,15 +21,16 @@ # occupancy grid map occupancy_grid: - use_occupancy_grid: true - use_occupancy_grid_for_longitudinal_margin: false + use_occupancy_grid_for_goal_search: true + use_occupancy_grid_for_goal_longitudinal_margin: false + use_occupancy_grid_for_path_collision_check: false occupancy_grid_collision_check_margin: 0.0 theta_size: 360 obstacle_threshold: 60 # object recognition object_recognition: - use_object_recognition: true + use_object_recognition: false object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker object_recognition_collision_check_max_extra_stopping_margin: 1.0 th_moving_object_velocity: 1.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 51f8f9a851152..de5f66628f1c9 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -132,13 +132,14 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio #### Parameters for occupancy grid based collision check -| Name | Unit | Type | Description | Default value | -| :----------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | -| use_occupancy_grid | [-] | bool | flag whether to use occupancy grid for collision check | true | -| use_occupancy_grid_for_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | -| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | -| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | -| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | +| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | +| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | +| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | +| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | ### **object recognition based collision check** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index 72da00daa9ae5..a09684ecb1fe4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -58,8 +58,9 @@ struct GoalPlannerParameters double margin_from_boundary; // occupancy grid map - bool use_occupancy_grid; - bool use_occupancy_grid_for_longitudinal_margin; + bool use_occupancy_grid_for_goal_search; + bool use_occupancy_grid_for_goal_longitudinal_margin; + bool use_occupancy_grid_for_path_collision_check; double occupancy_grid_collision_check_margin; int theta_size; int obstacle_threshold; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f2e55f81185ac..d162ebc036da1 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -252,7 +252,9 @@ void GoalPlannerModule::initializeOccupancyGridMap() void GoalPlannerModule::processOnEntry() { // Initialize occupancy grid map - if (parameters_->use_occupancy_grid) { + if ( + parameters_->use_occupancy_grid_for_goal_search || + parameters_->use_occupancy_grid_for_path_collision_check) { initializeOccupancyGridMap(); } } @@ -1180,7 +1182,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const { - if (parameters_->use_occupancy_grid && occupancy_grid_map_) { + if (parameters_->use_occupancy_grid_for_path_collision_check && occupancy_grid_map_) { const bool check_out_of_range = false; if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { return true; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 1863b169ffefd..12bbf273f445f 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -72,9 +72,12 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // occupancy grid map { std::string ns = "goal_planner.occupancy_grid."; - p.use_occupancy_grid = node->declare_parameter(ns + "use_occupancy_grid"); - p.use_occupancy_grid_for_longitudinal_margin = - node->declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); + p.use_occupancy_grid_for_goal_search = + node->declare_parameter(ns + "use_occupancy_grid_for_goal_search"); + p.use_occupancy_grid_for_path_collision_check = + node->declare_parameter(ns + "use_occupancy_grid_for_path_collision_check"); + p.use_occupancy_grid_for_goal_longitudinal_margin = + node->declare_parameter(ns + "use_occupancy_grid_for_goal_longitudinal_margin"); p.occupancy_grid_collision_check_margin = node->declare_parameter(ns + "occupancy_grid_collision_check_margin"); p.theta_size = node->declare_parameter(ns + "theta_size"); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 3f819a574d99e..2f67382eab8de 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -177,7 +177,7 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const bool GoalSearcher::checkCollision(const Pose & pose) const { - if (parameters_.use_occupancy_grid) { + if (parameters_.use_occupancy_grid_for_goal_search) { const Pose pose_grid_coords = global2local(occupancy_grid_map_->getMap(), pose); const auto idx = pose2index( occupancy_grid_map_->getMap(), pose_grid_coords, occupancy_grid_map_->getParam().theta_size); @@ -208,7 +208,9 @@ bool GoalSearcher::checkCollision(const Pose & pose) const bool GoalSearcher::checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const { - if (parameters_.use_occupancy_grid && parameters_.use_occupancy_grid_for_longitudinal_margin) { + if ( + parameters_.use_occupancy_grid_for_goal_search && + parameters_.use_occupancy_grid_for_goal_longitudinal_margin) { constexpr bool check_out_of_range = false; const double offset = std::max( parameters_.longitudinal_margin - parameters_.occupancy_grid_collision_check_margin, 0.0); From 586bd26114a6d8a376c88cddb280c9b669b367e9 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sat, 16 Sep 2023 16:50:16 +0000 Subject: [PATCH 72/93] style(pre-commit): autofix --- .../include/behavior_path_planner/utils/utils.hpp | 1 - .../src/utils/goal_planner/shift_pull_over.cpp | 3 +-- .../src/utils/start_planner/shift_pull_out.cpp | 3 +-- 3 files changed, 2 insertions(+), 5 deletions(-) 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 6a25a44c32ee9..8c700dfb1d7af 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 @@ -430,5 +430,4 @@ PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path); } // namespace behavior_path_planner::utils - #endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 4971f21e419ea..05a787910da45 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -171,8 +171,7 @@ boost::optional ShiftPullOver::generatePullOverPath( shifted_path.path.points.push_back(p); } - shifted_path.path = - utils::removeInverseOrderPathPoints(shifted_path.path); + shifted_path.path = utils::removeInverseOrderPathPoints(shifted_path.path); // set the same z as the goal for (auto & p : shifted_path.path.points) { 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 628dfe9eea4bf..bd1a158c99383 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 @@ -294,8 +294,7 @@ std::vector ShiftPullOut::calcPullOutPaths( continue; } - shifted_path.path = - utils::removeInverseOrderPathPoints(shifted_path.path); + shifted_path.path = utils::removeInverseOrderPathPoints(shifted_path.path); // set velocity const size_t pull_out_end_idx = From 78b4a3969dd104636a594bd996705a9f4bbf220b Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Tue, 19 Sep 2023 09:38:20 +0900 Subject: [PATCH 73/93] refactor(perception): rearrange clustering pipeline (#4999) * fix: change downsample filter Signed-off-by: badai-nguyen * fix: remove downsamle after compare map Signed-off-by: badai-nguyen * fix: add low range cropbox Signed-off-by: badai-nguyen * refactor: use_pointcloud_map Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen * fix: add roi based clustering option Signed-off-by: badai-nguyen * chore: change node name Signed-off-by: badai-nguyen * fix: launch argument pasrer Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- ...ra_lidar_fusion_based_detection.launch.xml | 23 +- ...ar_radar_fusion_based_detection.launch.xml | 22 +- .../detection/detection.launch.xml | 7 +- .../lidar_based_detection.launch.xml | 14 +- .../lidar_radar_based_detection.launch.xml | 2 +- .../detection/pointcloud_map_filter.launch.py | 39 ++- .../launch/perception.launch.xml | 10 +- .../config/compare_map.param.yaml | 20 -- .../config/outlier.param.yaml | 8 - .../config/voxel_grid.param.yaml | 7 - ...el_grid_based_euclidean_cluster.param.yaml | 14 +- .../launch/euclidean_cluster.launch.py | 73 ++---- .../launch/euclidean_cluster.launch.xml | 6 +- ...xel_grid_based_euclidean_cluster.launch.py | 239 ++---------------- ...el_grid_based_euclidean_cluster.launch.xml | 12 +- 15 files changed, 103 insertions(+), 393 deletions(-) delete mode 100644 perception/euclidean_cluster/config/compare_map.param.yaml delete mode 100644 perception/euclidean_cluster/config/outlier.param.yaml delete mode 100644 perception/euclidean_cluster/config/voxel_grid.param.yaml diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 24edd17253634..cf18f6c691878 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -11,13 +11,12 @@ - - + @@ -59,15 +58,16 @@ --> - + - + + @@ -75,19 +75,18 @@ - - + + - - - - + + + - + @@ -120,7 +119,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 463340efdecfe..2bd007b658978 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -11,8 +11,6 @@ - - @@ -78,15 +76,16 @@ - + - + + @@ -94,13 +93,12 @@ - - + + - - - - + + + @@ -151,7 +149,7 @@ - + @@ -184,7 +182,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index ca5d1d0f7e8bb..ab55047132482 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -5,7 +5,6 @@ - @@ -65,6 +64,8 @@ + + @@ -94,6 +95,8 @@ + + @@ -107,6 +110,7 @@ + @@ -119,6 +123,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 6d67df5fa7a1f..ebe96200b6883 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -4,37 +4,33 @@ - - - + - + + - - - + - - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index a47e6a86f1c7b..4756fc9338e46 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -3,7 +3,6 @@ - @@ -19,6 +18,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index a5d8d2113d2db..93d395ca3e466 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -35,7 +35,6 @@ def __init__(self, context): ) with open(pointcloud_map_filter_param_path, "r") as f: self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] - self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"] @@ -46,47 +45,40 @@ def __init__(self, context): ] self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"] self.publish_debug_pcd = self.pointcloud_map_filter_param["publish_debug_pcd"] + self.use_pointcloud_map = LaunchConfiguration("use_pointcloud_map").perform(context) def create_pipeline(self): - if self.use_down_sample_filter: - return self.create_down_sample_pipeline() + if self.use_pointcloud_map == "true": + return self.create_compare_map_pipeline() else: - return self.create_normal_pipeline() + return self.create_no_compare_map_pipeline() - def create_normal_pipeline(self): + def create_no_compare_map_pipeline(self): components = [] components.append( ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name="voxel_based_compare_map_filter", + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), - ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { - "distance_threshold": self.distance_threshold, - "downsize_ratio_z_axis": self.downsize_ratio_z_axis, - "timer_interval_ms": self.timer_interval_ms, - "use_dynamic_map_loading": self.use_dynamic_map_loading, - "map_update_distance_threshold": self.map_update_distance_threshold, - "map_loader_radius": self.map_loader_radius, - "publish_debug_pcd": self.publish_debug_pcd, - "input_frame": "map", + "voxel_size_x": self.voxel_size, + "voxel_size_y": self.voxel_size, + "voxel_size_z": self.voxel_size, } ], extra_arguments=[ - {"use_intra_process_comms": False}, + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} ], - ) + ), ) return components - def create_down_sample_pipeline(self): + def create_compare_map_pipeline(self): components = [] down_sample_topic = ( "/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud" @@ -94,7 +86,7 @@ def create_down_sample_pipeline(self): components.append( ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), @@ -177,6 +169,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") + add_launch_arg("use_pointcloud_map", "true") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 204fb561b10ea..acc14b1dcbe24 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -1,10 +1,8 @@ - - @@ -58,8 +56,9 @@ - + + - - - + + diff --git a/perception/euclidean_cluster/config/compare_map.param.yaml b/perception/euclidean_cluster/config/compare_map.param.yaml deleted file mode 100644 index 3dd303464a2c1..0000000000000 --- a/perception/euclidean_cluster/config/compare_map.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - - # distance threshold for compare compare - distance_threshold: 0.5 - - # publish voxelized map pointcloud for debug - publish_debug_pcd: False - - # use dynamic map loading - use_dynamic_map_loading: True - - # time interval to check dynamic map loading - timer_interval_ms: 100 - - # distance threshold for dynamic map update - map_update_distance_threshold: 10.0 - - # radius map for dynamic map loading - map_loader_radius: 150.0 diff --git a/perception/euclidean_cluster/config/outlier.param.yaml b/perception/euclidean_cluster/config/outlier.param.yaml deleted file mode 100644 index 1962fba1f332a..0000000000000 --- a/perception/euclidean_cluster/config/outlier.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.3 - voxel_size_y: 0.3 - voxel_size_z: 100.0 - voxel_points_threshold: 3 diff --git a/perception/euclidean_cluster/config/voxel_grid.param.yaml b/perception/euclidean_cluster/config/voxel_grid.param.yaml deleted file mode 100644 index 3ff32bfbb7146..0000000000000 --- a/perception/euclidean_cluster/config/voxel_grid.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.15 - voxel_size_y: 0.15 - voxel_size_z: 0.15 diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml index 2f3de2b789eb3..26b027f0077aa 100644 --- a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml +++ b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml @@ -7,9 +7,11 @@ max_cluster_size: 3000 use_height: false input_frame: "base_link" - max_x: 70.0 - min_x: -70.0 - max_y: 70.0 - min_y: -70.0 - max_z: 4.5 - min_z: -4.5 + + # low height crop box filter param + max_x: 200.0 + min_x: -200.0 + max_y: 200.0 + min_y: -200.0 + max_z: 2.0 + min_z: -10.0 diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index db86bbf80d250..66c25396a656e 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -35,61 +35,35 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "voxel_grid_filtered/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), - ("map", LaunchConfiguration("input_map")), - ("output", "compare_map_filtered/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[ - { - "distance_threshold": 0.5, - "timer_interval_ms": 100, - "use_dynamic_map_loading": True, - "downsize_ratio_z_axis": 0.5, - "map_update_distance_threshold": 10.0, - "map_loader_radius": 150.0, - "publish_debug_pcd": True, - "input_frame": "map", - } + ("output", "low_height/pointcloud"), ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_euclidean_cluster_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "compare_map_filtered/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], ) - disuse_map_euclidean_cluster_component = ComposableNode( + disuse_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), + ("input", LaunchConfiguration("input_pointcloud")), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], @@ -100,26 +74,26 @@ def load_composable_node_param(param_path): namespace=ns, package="rclcpp_components", executable="component_container", - composable_node_descriptions=[voxel_grid_filter_component], + composable_node_descriptions=[], output="screen", ) - use_map_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_loader = LoadComposableNodes( - composable_node_descriptions=[disuse_map_euclidean_cluster_component], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) - return [container, use_map_loader, disuse_map_loader] + return [container, use_low_height_pointcloud_loader, disuse_low_height_pointcloud_loader] def generate_launch_description(): @@ -131,14 +105,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml index 7159fb4c42793..fd1ea82befae0 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml @@ -3,16 +3,14 @@ - - + - - + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 82ce5605b67cd..00bcd4422bd3c 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -16,10 +16,8 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.substitutions import AndSubstitution -from launch.substitutions import AnonName +from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration -from launch.substitutions import NotSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -36,159 +34,31 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - namespace=ns, - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("map", LaunchConfiguration("input_map")), - ("output", "map_filter/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[load_composable_node_param("compare_map_param_path")], - ) - - # separate range of pointcloud when map_filter used - use_map_short_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", - remappings=[ - ("input", "map_filter/pointcloud"), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - use_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # disuse_map_voxel_grid_filter - disuse_map_short_range_crop_box_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", namespace=ns, plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - disuse_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), - remappings=[ - ("input", "short_range/pointcloud"), - ("output", "downsampled/short_range/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set outlier filter as a component - outlier_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", - name="outlier_filter", - remappings=[ - ("input", "downsampled/short_range/pointcloud"), - ("output", "outlier_filter/pointcloud"), - ], - parameters=[load_composable_node_param("outlier_param_path")], - ) - - # concat with-outlier pointcloud and without-outlier pcl - downsample_concat_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concat_downsampled_pcl", - remappings=[("output", "downsampled/concatenated/pointcloud")], - parameters=[ - { - "input_topics": ["long_range/pointcloud", "outlier_filter/pointcloud"], - "output_frame": "base_link", - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # set euclidean cluster as a component - use_downsample_euclidean_cluster_component = ComposableNode( - package=pkg, - namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", - name="euclidean_cluster", - remappings=[ - ("input", "downsampled/concatenated/pointcloud"), - ("output", LaunchConfiguration("output_clusters")), + ("output", "low_height/pointcloud"), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_disuse_downsample_euclidean_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ - ("input", "map_filter/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - disuse_map_disuse_downsample_euclidean_component = ComposableNode( + + disuse_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", @@ -209,75 +79,24 @@ def load_composable_node_param(param_path): output="screen", ) - use_map_use_downsample_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_short_range_crop_box_filter_component, - use_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition( - AndSubstitution( - LaunchConfiguration("use_pointcloud_map"), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_use_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_short_range_crop_box_filter_component, - disuse_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, - ], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), - ) - - use_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - compare_map_filter_component, - use_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - (LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), - ) - - disuse_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) return [ container, - use_map_use_downsample_loader, - disuse_map_use_downsample_loader, - use_map_disuse_downsample_loader, - disuse_map_disuse_downsample_loader, + use_low_height_pointcloud_loader, + disuse_low_height_pointcloud_loader, ] @@ -290,29 +109,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg("use_downsample_pointcloud", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), - add_launch_arg( - "outlier_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/outlier.param.yaml", - ], - ), - add_launch_arg( - "compare_map_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/compare_map.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "voxel_grid_based_euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 2833fed81c28e..b6a426dabfd12 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -3,22 +3,14 @@ - - - - - + - - - - - + From a9dfe085b3e06e1e7ca33d79ac73f2314f5a9aa6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 19 Sep 2023 11:24:18 +0900 Subject: [PATCH 74/93] feat(interface): add new option `last_keep` (#4811) (#845) * feat(interface): add new option last_keep * feat(planner_manager): keep last module * update param config --------- Signed-off-by: satoshi-ota Signed-off-by: kosuke55 Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../config/scene_module_manager.param.yaml | 10 ++++ .../behavior_path_planner/parameters.hpp | 1 + .../scene_module_manager_interface.hpp | 5 ++ .../src/behavior_path_planner_node.cpp | 1 + .../src/planner_manager.cpp | 52 ++++++++++++++----- 5 files changed, 55 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index e89c5539a0965..9895d9b5e473f 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -8,6 +8,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 6 max_module_size: 1 @@ -16,6 +17,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 6 max_module_size: 1 @@ -24,6 +26,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 5 max_module_size: 1 @@ -32,6 +35,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 5 max_module_size: 1 @@ -40,6 +44,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 0 max_module_size: 1 @@ -48,6 +53,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 2 max_module_size: 1 @@ -56,6 +62,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: true priority: 1 max_module_size: 1 @@ -64,6 +71,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 4 max_module_size: 1 @@ -72,6 +80,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 3 max_module_size: 1 @@ -80,5 +89,6 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 7 max_module_size: 1 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 0cad130ffffdb..689a3db66a196 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -28,6 +28,7 @@ struct ModuleConfigParameters bool enable_rtc{false}; bool enable_simultaneous_execution_as_approved_module{false}; bool enable_simultaneous_execution_as_candidate_module{false}; + bool keep_last{false}; uint8_t priority{0}; uint8_t max_module_size{0}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index 00882866d6d2b..cabd22823531d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -54,6 +54,7 @@ class SceneModuleManagerInterface enable_simultaneous_execution_as_candidate_module_( config.enable_simultaneous_execution_as_candidate_module), enable_rtc_(config.enable_rtc), + keep_last_(config.keep_last), max_module_num_(config.max_module_size), priority_(config.priority) { @@ -220,6 +221,8 @@ class SceneModuleManagerInterface return enable_simultaneous_execution_as_candidate_module_; } + bool isKeepLast() const { return keep_last_; } + void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } void reset() @@ -281,6 +284,8 @@ class SceneModuleManagerInterface private: bool enable_rtc_; + bool keep_last_; + size_t max_module_num_; size_t priority_; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 1302b579d2875..31da3e892ffe5 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -282,6 +282,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() declare_parameter(ns + "enable_simultaneous_execution_as_approved_module"); config.enable_simultaneous_execution_as_candidate_module = declare_parameter(ns + "enable_simultaneous_execution_as_candidate_module"); + config.keep_last = declare_parameter(ns + "keep_last"); config.priority = declare_parameter(ns + "priority"); config.max_module_size = declare_parameter(ns + "max_module_size"); return config; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index a3309968dd62c..cb65a9775aa3c 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -445,6 +445,17 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisKeepLast() && getManager(b)->isKeepLast(); + }); + } + /** * execute all approved modules. */ @@ -461,22 +472,32 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisWaitingApproval(); }; + const auto not_keep_last_module = std::find_if( + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), + [this](const auto & m) { return !getManager(m)->isKeepLast(); }); - const auto itr = std::find_if( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), waiting_approval_modules); + // convert reverse iterator -> iterator + const auto begin_itr = not_keep_last_module != approved_module_ptrs_.rend() + ? std::next(not_keep_last_module).base() + : approved_module_ptrs_.begin(); - if (itr != approved_module_ptrs_.end()) { + const auto waiting_approval_modules_itr = std::find_if( + begin_itr, approved_module_ptrs_.end(), + [](const auto & m) { return m->isWaitingApproval(); }); + + if (waiting_approval_modules_itr != approved_module_ptrs_.end()) { clearCandidateModules(); - candidate_module_ptrs_.push_back(*itr); + candidate_module_ptrs_.push_back(*waiting_approval_modules_itr); + + debug_info_.emplace_back( + *waiting_approval_modules_itr, Action::MOVE, "Back To Waiting Approval"); std::for_each( - std::next(itr), approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); }); + waiting_approval_modules_itr, approved_module_ptrs_.end(), + [&results](const auto & m) { results.erase(m->name()); }); - debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval"); + approved_module_ptrs_.erase(waiting_approval_modules_itr); } - - approved_module_ptrs_.erase(itr, approved_module_ptrs_.end()); } /** @@ -506,12 +527,15 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname(); - const auto approved_modules_output = [&output_module_name, &results]() { - if (results.count(output_module_name) == 0) { - return results.at("root"); + const auto approved_modules_output = [&results, this]() { + const auto itr = std::find_if( + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), + [&results](const auto & m) { return results.count(m->name()) != 0; }); + + if (itr != approved_module_ptrs_.rend()) { + return results.at((*itr)->name()); } - return results.at(output_module_name); + return results.at("root"); }(); const auto not_success_itr = std::find_if( From dd572e489e0b2438f854f6e26b3e861e4410d490 Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Tue, 19 Sep 2023 14:19:27 +0900 Subject: [PATCH 75/93] Revert "fix(system_monitor): extend command line to display (#4553)" This reverts commit 3bf467383cf4d4ab87f11077cb382a13febb8cb6. --- .../process_monitor/diag_task.hpp | 129 +++++++++++++----- .../src/process_monitor/process_monitor.cpp | 26 ++-- 2 files changed, 110 insertions(+), 45 deletions(-) diff --git a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp index 15702852a0e50..183f86baa2a08 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp @@ -24,25 +24,6 @@ #include -/** - * @brief Struct for storing process information - */ -struct ProcessInfo -{ - std::string processId; - std::string userName; - std::string priority; - std::string niceValue; - std::string virtualImage; - std::string residentSize; - std::string sharedMemSize; - std::string processStatus; - std::string cpuUsage; - std::string memoryUsage; - std::string cpuTime; - std::string commandName; -}; - class DiagTask : public diagnostic_updater::DiagnosticTask { public: @@ -65,18 +46,18 @@ class DiagTask : public diagnostic_updater::DiagnosticTask if (level_ != DiagStatus::OK) { stat.add("content", content_); } else { - stat.add("COMMAND", info_.commandName); - stat.add("%CPU", info_.cpuUsage); - stat.add("%MEM", info_.memoryUsage); - stat.add("PID", info_.processId); - stat.add("USER", info_.userName); - stat.add("PR", info_.priority); - stat.add("NI", info_.niceValue); - stat.add("VIRT", info_.virtualImage); - stat.add("RES", info_.residentSize); - stat.add("SHR", info_.sharedMemSize); - stat.add("S", info_.processStatus); - stat.add("TIME+", info_.cpuTime); + stat.add("COMMAND", command_); + stat.add("%CPU", cpu_); + stat.add("%MEM", mem_); + stat.add("PID", pid_); + stat.add("USER", user_); + stat.add("PR", pr_); + stat.add("NI", ni_); + stat.add("VIRT", virt_); + stat.add("RES", res_); + stat.add("SHR", shr_); + stat.add("S", s_); + stat.add("TIME+", time_); } stat.summary(level_, message_); } @@ -104,17 +85,95 @@ class DiagTask : public diagnostic_updater::DiagnosticTask } /** - * @brief Set process information - * @param [in] info Process information + * @brief set process id + * @param [in] pid process id + */ + void setProcessId(const std::string & pid) { pid_ = pid; } + + /** + * @brief set user name + * @param [in] user user name + */ + void setUserName(const std::string & user) { user_ = user; } + + /** + * @brief set priority + * @param [in] pr priority + */ + void setPriority(const std::string & pr) { pr_ = pr; } + + /** + * @brief set nice value + * @param [in] ni nice value + */ + void setNiceValue(const std::string & ni) { ni_ = ni; } + + /** + * @brief set virtual image + * @param [in] virt virtual image + */ + void setVirtualImage(const std::string & virt) { virt_ = virt; } + + /** + * @brief set resident size + * @param [in] res resident size */ - void setProcessInformation(const struct ProcessInfo & info) { info_ = info; } + void setResidentSize(const std::string & res) { res_ = res; } + + /** + * @brief set shared mem size + * @param [in] shr shared mem size + */ + void setSharedMemSize(const std::string & shr) { shr_ = shr; } + + /** + * @brief set process status + * @param [in] s process status + */ + void setProcessStatus(const std::string & s) { s_ = s; } + + /** + * @brief set CPU usage + * @param [in] cpu CPU usage + */ + void setCPUUsage(const std::string & cpu) { cpu_ = cpu; } + + /** + * @brief set memory usage + * @param [in] mem memory usage + */ + void setMemoryUsage(const std::string & mem) { mem_ = mem; } + + /** + * @brief set CPU time + * @param [in] time CPU time + */ + void setCPUTime(const std::string & time) { time_ = time; } + + /** + * @brief set Command name/line + * @param [in] command Command name/line + */ + void setCommandName(const std::string & command) { command_ = command; } private: int level_; //!< @brief Diagnostics error level std::string message_; //!< @brief Diagnostics status message std::string error_command_; //!< @brief Error command std::string content_; //!< @brief Error content - struct ProcessInfo info_; //!< @brief Struct for storing process information + + std::string pid_; //!< @brief Process Id + std::string user_; //!< @brief User Name + std::string pr_; //!< @brief Priority + std::string ni_; //!< @brief Nice value + std::string virt_; //!< @brief Virtual Image (kb) + std::string res_; //!< @brief Resident size (kb) + std::string shr_; //!< @brief Shared Mem size (kb) + std::string s_; //!< @brief Process Status + std::string cpu_; //!< @brief CPU usage + std::string mem_; //!< @brief Memory usage + std::string time_; //!< @brief CPU Time + std::string command_; //!< @brief Command name/line }; #endif // SYSTEM_MONITOR__PROCESS_MONITOR__DIAG_TASK_HPP_ diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 2ce1738ecd960..f78fc00c430dc 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -451,21 +451,27 @@ void ProcessMonitor::getTopratedProcesses( return; } + std::vector list; std::string line; int index = 0; while (std::getline(is_out, line) && !line.empty()) { - std::istringstream stream(line); - - ProcessInfo info; - stream >> info.processId >> info.userName >> info.priority >> info.niceValue >> - info.virtualImage >> info.residentSize >> info.sharedMemSize >> info.processStatus >> - info.cpuUsage >> info.memoryUsage >> info.cpuTime; - - std::getline(stream, info.commandName); + boost::trim_left(line); + boost::split(list, line, boost::is_space(), boost::token_compress_on); tasks->at(index)->setDiagnosticsStatus(DiagStatus::OK, "OK"); - tasks->at(index)->setProcessInformation(info); + tasks->at(index)->setProcessId(list[0]); + tasks->at(index)->setUserName(list[1]); + tasks->at(index)->setPriority(list[2]); + tasks->at(index)->setNiceValue(list[3]); + tasks->at(index)->setVirtualImage(list[4]); + tasks->at(index)->setResidentSize(list[5]); + tasks->at(index)->setSharedMemSize(list[6]); + tasks->at(index)->setProcessStatus(list[7]); + tasks->at(index)->setCPUUsage(list[8]); + tasks->at(index)->setMemoryUsage(list[9]); + tasks->at(index)->setCPUTime(list[10]); + tasks->at(index)->setCommandName(list[11]); ++index; } } @@ -515,7 +521,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { From c67945d353e7dd9ed8186d5df7283dfabdd68c77 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 19 Sep 2023 15:48:25 +0900 Subject: [PATCH 76/93] feat(behavior_path_planner): add always executable module option (#4785) (#850) * feat(behavior_path_planner): add always executable module option * fix conditions * rename itr * Update planning/behavior_path_planner/src/planner_manager.cpp * Update planning/behavior_path_planner/src/planner_manager.cpp * Update planning/behavior_path_planner/src/planner_manager.cpp * fix candidate break * Revert "fix candidate break" This reverts commit 0071bbdd8aa4d510a67f6d9bc32e23716f7aa416. * remove continue * check manager_ptr->isAlwaysExecutableModule first * common similar process * fix getRequestModules --------- Signed-off-by: kosuke55 Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../scene_module/goal_planner/manager.hpp | 2 + .../scene_module_manager_interface.hpp | 19 +++ .../src/planner_manager.cpp | 132 ++++++++++++++---- .../src/scene_module/goal_planner/manager.cpp | 20 +++ 4 files changed, 142 insertions(+), 31 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index e11641e24167a..4574536f9e771 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -40,6 +40,8 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; + bool isAlwaysExecutableModule() const override; + bool isSimultaneousExecutableAsApprovedModule() const override; bool isSimultaneousExecutableAsCandidateModule() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index cabd22823531d..4ab09e0acd8f3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -211,13 +211,32 @@ class SceneModuleManagerInterface bool canLaunchNewModule() const { return registered_modules_.size() < max_module_num_; } + /** + * Determine if the module is always executable, regardless of the state of other modules. + * + * When this returns true: + * - The module can be executed even if other modules are not marked as 'simultaneously + * executable'. + * - Conversely, the presence of this module will not prevent other modules + * from executing, even if they are not marked as 'simultaneously executable'. + */ + virtual bool isAlwaysExecutableModule() const { return false; } + virtual bool isSimultaneousExecutableAsApprovedModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + return enable_simultaneous_execution_as_approved_module_; } virtual bool isSimultaneousExecutableAsCandidateModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + return enable_simultaneous_execution_as_candidate_module_; } diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index cb65a9775aa3c..3e9411b141fb3 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -188,22 +188,6 @@ std::vector PlannerManager::getRequestModules( std::vector request_modules{}; - /** - * check whether it is possible to push back more modules to approved modules. - */ - { - const auto find_block_module = [this](const auto & m) { - return !getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }; - - const auto itr = - std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module); - - if (itr != approved_module_ptrs_.end()) { - return {}; - } - } - const auto toc = [this](const auto & name) { processing_time_.at(name) += stop_watch_.toc(name, true); }; @@ -212,14 +196,62 @@ std::vector PlannerManager::getRequestModules( stop_watch_.tic(manager_ptr->getModuleName()); /** - * don't launch candidate module if approved modules already exist. + * determine the execution capability of modules based on existing approved modules. */ - if (!approved_module_ptrs_.empty()) { - if (!manager_ptr->isSimultaneousExecutableAsApprovedModule()) { - toc(manager_ptr->getModuleName()); + // Condition 1: always executable module can be added regardless of the existence of other + // modules, so skip checking the existence of other modules. + // in other cases, need to check the existence of other modules and which module can be added. + const bool has_non_always_executable_module = std::any_of( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + if (!manager_ptr->isAlwaysExecutableModule() && has_non_always_executable_module) { + // pairs of find_block_module and is_executable + std::vector, std::function>> + conditions; + + // Condition 2: do not add modules that are neither always nor simultaneous executable + // if there exists at least one approved module that is simultaneous but not always + // executable. (only modules that are either always executable or simultaneous executable can + // be added) + conditions.push_back( + {[&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }}); + + // Condition 3: do not add modules that are not always executable if there exists + // at least one approved module that is neither always nor simultaneous executable. + // (only modules that are always executable can be added) + conditions.push_back( + {[&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return false; }}); + + bool skip_module = false; + for (const auto & condition : conditions) { + const auto & find_block_module = condition.first; + const auto & is_executable = condition.second; + + const auto itr = std::find_if( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module); + + if (itr != approved_module_ptrs_.end() && !is_executable()) { + toc(manager_ptr->getModuleName()); + skip_module = true; + continue; + } + } + if (skip_module) { continue; } } + // else{ + // Condition 4: if none of the above conditions are met, any module can be added. + // (when the approved modules are either empty or consist only of always executable modules.) + // } /** * launch new candidate module. @@ -342,20 +374,58 @@ std::pair PlannerManager::runRequestModule * remove non-executable modules. */ for (const auto & module_ptr : sorted_request_modules) { - if (!getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule()) { - if (executable_modules.empty()) { - executable_modules.push_back(module_ptr); - break; - } + // Condition 1: always executable module can be added regardless of the existence of other + // modules. + if (getManager(module_ptr)->isAlwaysExecutableModule()) { + executable_modules.push_back(module_ptr); + continue; } - const auto itr = - std::find_if(executable_modules.begin(), executable_modules.end(), [this](const auto & m) { - return !getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }); - - if (itr == executable_modules.end()) { + // Condition 4: If the executable modules are either empty or consist only of always executable + // modules, any module can be added. + const bool has_non_always_executable_module = std::any_of( + executable_modules.begin(), executable_modules.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + if (!has_non_always_executable_module) { executable_modules.push_back(module_ptr); + continue; + } + + // pairs of find_block_module and is_executable + std::vector, std::function>> + conditions; + + // Condition 3: Only modules that are always executable can be added + // if there exists at least one executable module that is neither always nor simultaneous + // executable. + conditions.push_back( + {[this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return false; }}); + + // Condition 2: Only modules that are either always executable or simultaneous executable can be + // added if there exists at least one executable module that is simultaneous but not always + // executable. + conditions.push_back( + {[this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }}); + + for (const auto & condition : conditions) { + const auto & find_block_module = condition.first; + const auto & is_executable = condition.second; + + const auto itr = + std::find_if(executable_modules.begin(), executable_modules.end(), find_block_module); + + if (itr != executable_modules.end() && is_executable()) { + executable_modules.push_back(module_ptr); + break; + } } } diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 12bbf273f445f..99abb044ab80c 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -262,8 +262,24 @@ void GoalPlannerModuleManager::updateModuleParams( }); } +bool GoalPlannerModuleManager::isAlwaysExecutableModule() const +{ + // enable AlwaysExecutable whenever goal modification is not allowed + // because only minor path refinements are made for fixed goals + if (!goal_planner_utils::isAllowedGoalModification( + planner_data_->route_handler, left_side_parking_)) { + return true; + } + + return false; +} + bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { @@ -275,6 +291,10 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { From cb599b9d302d1408267ef895c8b8768685cea805 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 19 Sep 2023 15:51:08 +0900 Subject: [PATCH 77/93] fix(goal_planner): build Signed-off-by: kosuke55 --- .../src/scene_module/goal_planner/manager.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 99abb044ab80c..332a8aa65c5d8 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -266,8 +266,7 @@ bool GoalPlannerModuleManager::isAlwaysExecutableModule() const { // enable AlwaysExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } From 309cbebf72dc0b0f3a98d954651ff2915029274d Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 22 Aug 2023 19:50:33 +0900 Subject: [PATCH 78/93] feat(behavior_path_planner): add status_is_safe_dynamic_objects (#4691) * add_status_is_safe_dynamic_objects Signed-off-by: kyoichi-sugahara * revert unnecessary changes Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../start_planner/start_planner_module.hpp | 6 +++--- .../start_planner/start_planner_module.cpp | 20 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 44b31dcad6ff8..fd3b6ed419171 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -53,9 +53,9 @@ struct PullOutStatus PlannerType planner_type{PlannerType::NONE}; PathWithLaneId backward_path{}; lanelet::ConstLanelets pull_out_lanes{}; - bool is_safe{false}; - bool back_finished{false}; // if backward driving is not required, this is also set to true - // todo: rename to clear variable name. + bool is_safe_static_objects{false}; // current path is safe against static objects + bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects + bool back_finished{false}; Pose pull_out_start_pose{}; PullOutStatus() {} diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index cbdd0931569f3..bca7ff28f6ac0 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -184,7 +184,7 @@ BehaviorModuleOutput StartPlannerModule::plan() } BehaviorModuleOutput output; - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); const auto output = generateStopOutput(); @@ -291,7 +291,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() } BehaviorModuleOutput output; - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); clearWaitingApproval(); @@ -409,7 +409,7 @@ void StartPlannerModule::planWithPriority( // use current path if back is not needed if (status_.back_finished) { const std::lock_guard lock(mutex_); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.pull_out_path = *pull_out_path; status_.pull_out_start_pose = pull_out_start_pose; status_.planner_type = planner->getPlannerType(); @@ -430,7 +430,7 @@ void StartPlannerModule::planWithPriority( // Update status variables with the next path information { const std::lock_guard lock(mutex_); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.pull_out_path = *pull_out_path_next; status_.pull_out_start_pose = pull_out_start_pose_next; status_.planner_type = planner->getPlannerType(); @@ -482,7 +482,7 @@ void StartPlannerModule::planWithPriority( // not found safe path if (status_.planner_type != PlannerType::FREESPACE) { const std::lock_guard lock(mutex_); - status_.is_safe = false; + status_.is_safe_static_objects = false; status_.planner_type = PlannerType::NONE; } } @@ -717,7 +717,7 @@ bool StartPlannerModule::isOverlappedWithLane( bool StartPlannerModule::hasFinishedPullOut() const { - if (!status_.back_finished || !status_.is_safe) { + if (!status_.back_finished || !status_.is_safe_static_objects) { return false; } @@ -804,7 +804,7 @@ bool StartPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return true; } @@ -1015,7 +1015,7 @@ bool StartPlannerModule::planFreespacePath() status_.pull_out_path = *freespace_path; status_.pull_out_start_pose = current_pose; status_.planner_type = freespace_planner_->getPlannerType(); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.back_finished = true; return true; } @@ -1066,8 +1066,8 @@ void StartPlannerModule::setDebugData() const const auto header = planner_data_->route_handler->getRouteHeader(); { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); From 7fe5d84208002e013da709fb52669a2d93201141 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 23 Aug 2023 16:29:26 +0900 Subject: [PATCH 79/93] feat(behavior_path_planner): add PullOutPath member variables for ego predicted path generation (#4669) * add_pull_out_member_variables Signed-off-by: kyoichi-sugahara * update pull_out path struct Signed-off-by: kyoichi-sugahara * update pull_out path struct Signed-off-by: kyoichi-sugahara * minor update Signed-off-by: kyoichi-sugahara * fix calculation Signed-off-by: kyoichi-sugahara * fix build error Signed-off-by: kyoichi-sugahara * fix code Signed-off-by: kyoichi-sugahara * fix code Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../utils/start_planner/pull_out_path.hpp | 3 ++ .../start_planner/start_planner_module.cpp | 3 +- .../start_planner/geometric_pull_out.cpp | 35 ++++++++++++++++++- .../utils/start_planner/shift_pull_out.cpp | 5 +++ 4 files changed, 44 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp index d68985ec27350..608b918e839fd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp @@ -19,6 +19,7 @@ #include +#include #include namespace behavior_path_planner @@ -27,6 +28,8 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; struct PullOutPath { std::vector partial_paths{}; + // accelerate with constant acceleration to the target velocity + std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; Pose end_pose{}; }; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index bca7ff28f6ac0..7c5e87f3e15a9 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -106,6 +106,7 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { + // TODO(Sugahara): if required lateral shift distance is small, don't engage this module. // Execute when current pose is near route start pose const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -138,7 +139,7 @@ bool StartPlannerModule::isExecutionRequested() const bool StartPlannerModule::isExecutionReady() const { - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return false; } diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 4bf4a3b747b8d..56d8eb0ff858a 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -70,15 +70,48 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con parameters_.collision_check_margin)) { return {}; } + const double velocity = parallel_parking_parameters_.forward_parking_velocity; if (parameters_.divide_pull_out_path) { output.partial_paths = planner_.getPaths(); + /* + Calculate the acceleration required to reach the forward parking velocity at the center of + the front path, assuming constant acceleration and deceleration. + v v + | | + | /\ | /\ + | / \ | / \ + | / \ | / \ + | / \ | / \ + |/________\_____ x |/________\______ t + 0 a_l/2 a_l 0 t_c 2*t_c + Notes: + a_l represents "arc_length_on_path_front" + t_c represents "time_to_center" + */ // insert stop velocity to first arc path end output.partial_paths.front().points.back().point.longitudinal_velocity_mps = 0.0; + const double arc_length_on_first_arc_path = + motion_utils::calcArcLength(output.partial_paths.front().points); + const double time_to_center = arc_length_on_first_arc_path / (2 * velocity); + const double average_velocity = arc_length_on_first_arc_path / (time_to_center * 2); + const double average_acceleration = average_velocity / (time_to_center * 2); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(average_velocity, average_acceleration)); + const double arc_length_on_second_arc_path = + motion_utils::calcArcLength(planner_.getArcPaths().at(1).points); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path))); } else { - auto partial_paths = planner_.getPaths(); + const auto partial_paths = planner_.getPaths(); const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); + + // Calculate the acceleration required to reach the forward parking velocity at the center of + // the path, assuming constant acceleration and deceleration. + const double arc_length_on_path = motion_utils::calcArcLength(combined_path.points); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; 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 bd1a158c99383..787009299add1 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 @@ -194,9 +194,12 @@ std::vector ShiftPullOut::calcPullOutPaths( // non_shifted_path for when shift length or pull out distance is too short const PullOutPath non_shifted_path = std::invoke([&]() { PullOutPath non_shifted_path{}; + // In non_shifted_path, to minimize safety checks, 0 is assigned to prevent the predicted_path + // of the ego vehicle from becoming too large. non_shifted_path.partial_paths.push_back(road_lane_reference_path); non_shifted_path.start_pose = start_pose; non_shifted_path.end_pose = start_pose; + non_shifted_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(0, 0)); return non_shifted_path; }); @@ -316,6 +319,8 @@ std::vector ShiftPullOut::calcPullOutPaths( candidate_path.partial_paths.push_back(shifted_path.path); candidate_path.start_pose = shift_line.start; candidate_path.end_pose = shift_line.end; + candidate_path.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(terminal_velocity, longitudinal_acc)); candidate_paths.push_back(candidate_path); } From 5fcdd754e6f0eac11b0ef4d7973617f048cd0ed5 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 25 Aug 2023 23:14:18 +0900 Subject: [PATCH 80/93] feat(behavior_path_planner): update filter objects by velocity (#4742) --- .../path_safety_checker/objects_filtering.hpp | 35 +++++++++++++------ .../path_safety_checker/objects_filtering.cpp | 16 ++++++--- 2 files changed, 35 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index b4575eb4d3b7e..8a3b7094c69d1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -55,24 +55,37 @@ PredictedObjects filterObjects( const std::shared_ptr & params); /** - * @brief Filter objects based on their velocity. + * @brief Filters objects based on their velocity. * - * @param objects The predicted objects to filter. - * @param lim_v Velocity limit for filtering. - * @return PredictedObjects The filtered objects. + * Depending on the remove_above_threshold parameter, this function either removes objects with + * velocities above the given threshold or only keeps those objects. It uses the helper function + * filterObjectsByVelocity() to do the actual filtering. + * + * @param objects The objects to be filtered. + * @param velocity_threshold The velocity threshold for the filtering. + * @param remove_above_threshold If true, objects with velocities above the threshold are removed. + * If false, only objects with velocities above the threshold are + * kept. + * @return A new collection of objects that have been filtered according to the rules. */ -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, const double velocity_threshold, + const bool remove_above_threshold = true); /** - * @brief Filter objects based on a velocity range. + * @brief Helper function to filter objects based on their velocity. * - * @param objects The predicted objects to filter. - * @param min_v Minimum velocity for filtering. - * @param max_v Maximum velocity for filtering. - * @return PredictedObjects The filtered objects. + * This function iterates over all objects and calculates their velocity norm. If the velocity norm + * is within the velocity_threshold and max_velocity range, the object is added to a new collection. + * This new collection is then returned. + * + * @param objects The objects to be filtered. + * @param velocity_threshold The minimum velocity for an object to be included in the output. + * @param max_velocity The maximum velocity for an object to be included in the output. + * @return A new collection of objects that have been filtered according to the rules. */ PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v); + const PredictedObjects & objects, double velocity_threshold, double max_velocity); /** * @brief Filter objects based on their position relative to a current_pose. diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index 33afe0fe6642f..a775e7c16efed 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -37,7 +37,7 @@ PredictedObjects filterObjects( PredictedObjects filtered_objects; - filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold); + filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false); filterObjectsByClass(filtered_objects, target_object_types); @@ -51,13 +51,19 @@ PredictedObjects filterObjects( return filtered_objects; } -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, const double velocity_threshold, + const bool remove_above_threshold) { - return filterObjectsByVelocity(objects, -lim_v, lim_v); + if (remove_above_threshold) { + return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold); + } else { + return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::max()); + } } PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v) + const PredictedObjects & objects, double velocity_threshold, double max_velocity) { PredictedObjects filtered; filtered.header = objects.header; @@ -65,7 +71,7 @@ PredictedObjects filterObjectsByVelocity( const auto v_norm = std::hypot( obj.kinematics.initial_twist_with_covariance.twist.linear.x, obj.kinematics.initial_twist_with_covariance.twist.linear.y); - if (min_v < v_norm && v_norm < max_v) { + if (velocity_threshold < v_norm && v_norm < max_velocity) { filtered.objects.push_back(obj); } } From 7b685e168d8883fc9c6f8cd2fa1f3b9efc40bb26 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 19 Sep 2023 18:11:16 +0900 Subject: [PATCH 81/93] fix(obstacle_cruise_planner): fix backward virtual wall Signed-off-by: kosuke55 --- .../motion_utils/marker/marker_helper.hpp | 9 +++++--- .../marker/virtual_wall_marker_creator.hpp | 3 ++- .../motion_utils/src/marker/marker_helper.cpp | 21 +++++++++++-------- .../marker/virtual_wall_marker_creator.cpp | 2 +- .../src/planner_interface.cpp | 8 +++---- 5 files changed, 25 insertions(+), 18 deletions(-) diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index 6433f1b9ae2f9..99f7dd4333bae 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp @@ -28,15 +28,18 @@ using geometry_msgs::msg::Pose; visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); diff --git a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp index 331b3dd5275fd..3f92f41b8738d 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -38,6 +38,7 @@ struct VirtualWall std::string ns{}; VirtualWallType style = stop; double longitudinal_offset{}; + bool is_driving_forward{true}; }; typedef std::vector VirtualWalls; @@ -54,7 +55,7 @@ class VirtualWallMarkerCreator using create_wall_function = std::function; + const std::string & ns_prefix, const bool is_driving_forward)>; VirtualWalls virtual_walls; std::unordered_map marker_count_per_namespace; diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 25d8616949b02..7998dbeabe7f9 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -86,10 +86,11 @@ namespace motion_utils { visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "stop_", now, id, createMarkerColor(1.0, 0.0, 0.0, 0.5)); @@ -97,10 +98,11 @@ visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, createMarkerColor(1.0, 1.0, 0.0, 0.5)); @@ -108,10 +110,11 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5)); diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp index 63c456ab24fb0..87736f8a2982b 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -64,7 +64,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( } auto markers = create_fn( virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, - virtual_wall.ns); + virtual_wall.ns, virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { marker.id = marker_count_per_namespace[marker.ns].current++; marker_array.markers.push_back(marker); diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 1aaa897986bcd..8bb897dc8ae89 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -323,7 +323,7 @@ std::vector PlannerInterface::generateStopTrajectory( // virtual wall marker for stop obstacle const auto markers = motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, - abs_ego_offset); + abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle); @@ -479,7 +479,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", - planner_data.current_time, i, abs_ego_offset); + planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); } @@ -487,14 +487,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( if (slow_down_start_idx) { const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", - planner_data.current_time, i * 2, abs_ego_offset); + planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", - planner_data.current_time, i * 2 + 1, abs_ego_offset); + planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } From cca56a37f2c2730860d28f895ffb181a7afed81a Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 19 Sep 2023 18:18:59 +0900 Subject: [PATCH 82/93] fix(obstacle_cruise): fix stop margin after backward terminal Signed-off-by: kosuke55 --- planning/obstacle_cruise_planner/src/node.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index ab2092a3120ca..f6fe3896088cf 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -129,11 +129,11 @@ double calcObstacleMaxLength(const Shape & shape) } TrajectoryPoint getExtendTrajectoryPoint( - const double extend_distance, const TrajectoryPoint & goal_point) + const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) { TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = - tier4_autoware_utils::calcOffsetPose(goal_point.pose, extend_distance, 0.0, 0.0); + extend_trajectory_point.pose = tier4_autoware_utils::calcOffsetPose( + goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; @@ -145,6 +145,8 @@ std::vector extendTrajectoryPoints( const double step_length) { auto output_points = input_points; + const auto is_driving_forward_opt = motion_utils::isDrivingForwardWithTwist(input_points); + const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (extend_distance < std::numeric_limits::epsilon()) { return output_points; @@ -154,11 +156,13 @@ std::vector extendTrajectoryPoints( double extend_sum = 0.0; while (extend_sum <= (extend_distance - step_length)) { - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_sum, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); extend_sum += step_length; } - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_distance, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); return output_points; From 7bc2614a29ee6cb675e7fc99a11d23152280f74a Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sun, 3 Sep 2023 19:25:36 +0900 Subject: [PATCH 83/93] refactor(behavior_path_planner): consolidate common function which is used by start and goal planner (#4804) * refactor code * consolidate function * add utility * util to utils * define parameters * fix typo * fix arg name * commonize calcFeasibleDecelDistance * define modifyVelocityByDirection * add description --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/behavior_path_planner/CMakeLists.txt | 1 + .../goal_planner/goal_planner_module.hpp | 9 +- .../geometric_parallel_parking.hpp | 5 + .../goal_planner/goal_planner_parameters.hpp | 13 ++ .../goal_planner/pull_over_planner_base.hpp | 3 + .../path_safety_checker_parameters.hpp | 2 +- .../common_module_data.hpp | 45 ++++++ .../utils/start_goal_planner_common/utils.hpp | 94 +++++++++++ .../start_planner_parameters.hpp | 13 ++ .../goal_planner/goal_planner_module.cpp | 95 +++++------ .../geometric_parallel_parking.cpp | 15 ++ .../goal_planner/freespace_pull_over.cpp | 15 +- .../goal_planner/geometric_pull_over.cpp | 1 + .../utils/goal_planner/shift_pull_over.cpp | 1 + .../path_safety_checker/objects_filtering.cpp | 6 +- .../src/utils/path_utils.cpp | 1 + .../utils/start_goal_planner_common/utils.cpp | 148 ++++++++++++++++++ .../start_planner/geometric_pull_out.cpp | 10 +- 18 files changed, 406 insertions(+), 71 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp create mode 100644 planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 0352096d02b2b..f62e418371401 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -32,6 +32,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/path_utils.cpp src/utils/path_safety_checker/safety_check.cpp src/utils/path_safety_checker/objects_filtering.cpp + src/utils/start_goal_planner_common/utils.cpp src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 5177feaaf61f4..65263905b9a24 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -65,7 +65,7 @@ enum class PathType { FREESPACE, }; -struct PUllOverStatus +struct PullOverStatus { std::shared_ptr pull_over_path{}; std::shared_ptr lane_parking_pull_over_path{}; @@ -76,7 +76,8 @@ struct PUllOverStatus lanelet::ConstLanelets pull_over_lanes{}; std::vector lanes{}; // current + pull_over bool has_decided_path{false}; - bool is_safe{false}; + bool is_safe_static_objects{false}; // current path is safe against static objects + bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects bool prev_is_safe{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; @@ -127,7 +128,8 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; }; private: - PUllOverStatus status_; + + PullOverStatus status_; std::shared_ptr parameters_; @@ -198,7 +200,6 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); - boost::optional calcFeasibleDecelDistance(const double target_velocity) const; void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index cb769a1f2884c..9cae5e18f1352 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -94,6 +94,10 @@ class GeometricParallelParking std::vector getArcPaths() const { return arc_paths_; } std::vector getPaths() const { return paths_; } + std::vector> getPairsTerminalVelocityAndAccel() const + { + return pairs_terminal_velocity_and_accel_; + } PathWithLaneId getPathByIdx(size_t const idx) const; PathWithLaneId getCurrentPath() const; PathWithLaneId getFullPath() const; @@ -112,6 +116,7 @@ class GeometricParallelParking std::vector arc_paths_; std::vector paths_; + std::vector> pairs_terminal_velocity_and_accel_; size_t current_path_idx_ = 0; void clearPaths(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index a09684ecb1fe4..4e40267ede4df 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -101,6 +102,18 @@ struct GoalPlannerParameters AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + // stop condition + double maximum_deceleration_for_stop; + double maximum_jerk_for_stop; + + // hysteresis parameter + double hysteresis_factor_expand_rate; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; + utils::path_safety_checker::SafetyCheckParams safety_check_params; + // debug bool print_debug_info; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 4b800917d4aec..20788e53309ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -26,6 +26,7 @@ #include #include +#include #include using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -46,6 +47,8 @@ struct PullOverPath { PullOverPlannerType type{PullOverPlannerType::NONE}; std::vector partial_paths{}; + // accelerate with constant acceleration to the target velocity + std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; Pose end_pose{}; std::vector debug_poses{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 93eb0457d7cf4..475b38f5bb824 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -170,7 +170,7 @@ struct ObjectsFilteringParams struct SafetyCheckParams { bool enable_safety_check; ///< Enable safety checks. - double backward_lane_length; ///< Length of the backward lane for path generation. + double backward_path_length; ///< Length of the backward lane for path generation. double forward_path_length; ///< Length of the forward path lane for path generation. RSSparams rss_params; ///< Parameters related to the RSS model. bool publish_debug_marker{false}; ///< Option to publish debug markers. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp new file mode 100644 index 0000000000000..4ba2cb08b6341 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -0,0 +1,45 @@ +// 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 BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ + +#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include + +#include + +#include + +namespace behavior_path_planner +{ +using autoware_auto_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +/* + * Common data for start/goal_planner module + */ +struct StartGoalPlannerData +{ + // filtered objects + PredictedObjects filtered_objects; + TargetObjectsOnLane target_objects_on_lane; + std::vector ego_predicted_path; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp new file mode 100644 index 0000000000000..60bbb3c78b401 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -0,0 +1,94 @@ +// 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 BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" +#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using behavior_path_planner::StartPlannerParameters; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity); + +/** + * @brief Update path velocities based on driving direction. + * + * This function updates the longitudinal velocity of each point in the provided paths, + * based on whether the vehicle is driving forward or backward. It also sets the terminal + * velocity and acceleration for each path. + * + * @param paths A vector of paths with lane IDs to be updated. + * @param terminal_vel_acc_pairs A vector of pairs, where each pair contains the terminal + * velocity and acceleration for a corresponding path. + * @param target_velocity The target velocity for ego vehicle predicted path. + * @param acceleration The acceleration for ego vehicle predicted path. + */ +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params); + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel); + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx); + +} // namespace behavior_path_planner::utils::start_goal_planner_common + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 1e26bef0c85be..771fbd93f7196 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -76,6 +77,18 @@ struct StartPlannerParameters PlannerCommonParam freespace_planner_common_parameters; AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + + // stop condition + double maximum_deceleration_for_stop; + double maximum_jerk_for_stop; + + // hysteresis parameter + double hysteresis_factor_expand_rate; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; + utils::path_safety_checker::SafetyCheckParams safety_check_params; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index d162ebc036da1..ae7e3772f7672 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -35,6 +36,7 @@ #include #include +using behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; @@ -118,7 +120,7 @@ GoalPlannerModule::GoalPlannerModule( void GoalPlannerModule::resetStatus() { - PUllOverStatus initial_status{}; + PullOverStatus initial_status{}; status_ = initial_status; pull_over_path_candidates_.clear(); closest_start_pose_.reset(); @@ -352,7 +354,8 @@ bool GoalPlannerModule::isExecutionReady() const double GoalPlannerModule::calcModuleRequestLength() const { - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { return parameters_->pull_over_minimum_request_length; } @@ -459,7 +462,7 @@ bool GoalPlannerModule::planFreespacePath() mutex_.lock(); status_.pull_over_path = std::make_shared(*freespace_path); status_.current_path_idx = 0; - status_.is_safe = true; + status_.is_safe_static_objects = true; modified_goal_pose_ = goal_candidate; last_path_update_time_ = std::make_unique(clock_->now()); debug_data_.freespace_planner.is_planning = false; @@ -496,7 +499,7 @@ void GoalPlannerModule::returnToLaneParking() } mutex_.lock(); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.has_decided_path = false; status_.pull_over_path = status_.lane_parking_pull_over_path; status_.current_path_idx = 0; @@ -554,7 +557,7 @@ void GoalPlannerModule::selectSafePullOverPath() const auto pull_over_path_candidates = pull_over_path_candidates_; const auto goal_candidates = goal_candidates_; mutex_.unlock(); - status_.is_safe = false; + status_.is_safe_static_objects = false; for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe const auto goal_candidate_it = std::find_if( @@ -573,7 +576,7 @@ void GoalPlannerModule::selectSafePullOverPath() continue; } - status_.is_safe = true; + status_.is_safe_static_objects = true; mutex_.lock(); status_.pull_over_path = std::make_shared(pull_over_path); status_.current_path_idx = 0; @@ -585,7 +588,7 @@ void GoalPlannerModule::selectSafePullOverPath() } // decelerate before the search area start - if (status_.is_safe) { + if (status_.is_safe_static_objects) { const auto search_start_offset_pose = calcLongitudinalOffsetPose( status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - @@ -596,7 +599,9 @@ void GoalPlannerModule::selectSafePullOverPath() decelerateBeforeSearchStart(*search_start_offset_pose, first_path); } else { // if already passed the search start offset pose, set pull_over_velocity to first_path. - const auto min_decel_distance = calcFeasibleDecelDistance(parameters_->pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + parameters_->pull_over_velocity); for (auto & p : first_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -630,7 +635,7 @@ void GoalPlannerModule::setLanes() void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { - if (status_.is_safe) { + if (status_.is_safe_static_objects) { // clear stop pose when the path is safe and activated if (isActivated()) { resetWallPoses(); @@ -659,7 +664,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - status_.prev_is_safe = status_.is_safe; + status_.prev_is_safe = status_.is_safe_static_objects; } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) @@ -707,7 +712,7 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (status_.is_safe) { + if (status_.is_safe_static_objects) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); modified_goal.pose = modified_goal_pose_->goal_pose; @@ -755,7 +760,7 @@ bool GoalPlannerModule::hasDecidedPath() const } // if path is not safe, not decided - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return false; } @@ -876,7 +881,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = status_.is_safe + path_candidate_ = status_.is_safe_static_objects ? std::make_shared(status_.pull_over_path->getFullPath()) : out.path; path_reference_ = getPreviousModuleOutput().reference_path; @@ -970,17 +975,19 @@ PathWithLaneId GoalPlannerModule::generateStopPath() reference_path.points, refined_goal_pose_.position, -parameters_->backward_goal_search_length - common_parameters.base_link2front - approximate_pull_over_distance_); - if (!status_.is_safe && !closest_start_pose_ && !search_start_offset_pose) { + if (!status_.is_safe_static_objects && !closest_start_pose_ && !search_start_offset_pose) { return generateFeasibleStopPath(); } - const Pose stop_pose = status_.is_safe ? status_.pull_over_path->start_pose - : (closest_start_pose_ ? closest_start_pose_.value() - : *search_start_offset_pose); + const Pose stop_pose = + status_.is_safe_static_objects + ? status_.pull_over_path->start_pose + : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_offset_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; @@ -997,7 +1004,9 @@ PathWithLaneId GoalPlannerModule::generateStopPath() decelerateBeforeSearchStart(*search_start_offset_pose, reference_path); } else { // if already passed the search start offset pose, set pull_over_velocity to reference_path. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); for (auto & p : reference_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -1025,7 +1034,8 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); // calc minimum stop distance under maximum deceleration - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { return stop_path; } @@ -1110,7 +1120,7 @@ bool GoalPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return true; } @@ -1233,7 +1243,8 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c return false; } - const auto current_to_stop_distance = calcFeasibleDecelDistance(0.0); + const auto current_to_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!current_to_stop_distance) { return false; } @@ -1269,30 +1280,6 @@ void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) } } -boost::optional GoalPlannerModule::calcFeasibleDecelDistance( - const double target_velocity) const -{ - const auto v_now = planner_data_->self_odometry->twist.twist.linear.x; - const auto a_now = planner_data_->self_acceleration->accel.accel.linear.x; - const auto a_lim = parameters_->maximum_deceleration; // positive value - const auto j_lim = parameters_->maximum_jerk; - - if (v_now < target_velocity) { - return 0.0; - } - - auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( - v_now, target_velocity, a_now, -a_lim, j_lim, -1.0 * j_lim); - - if (!min_stop_distance) { - return {}; - } - - min_stop_distance = std::max(*min_stop_distance, 0.0); - - return min_stop_distance; -} - double GoalPlannerModule::calcSignedArcLengthFromEgo( const PathWithLaneId & path, const Pose & pose) const { @@ -1317,7 +1304,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith const float decel_vel = std::min(point.point.longitudinal_velocity_mps, static_cast(distance_to_stop / time)); const double distance_from_ego = calcSignedArcLengthFromEgo(path, point.point.pose); - const auto min_decel_distance = calcFeasibleDecelDistance(decel_vel); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, decel_vel); // when current velocity already lower than decel_vel, min_decel_distance will be 0.0, // and do not need to decelerate. @@ -1335,7 +1323,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith } const double stop_point_length = calcSignedArcLength(path.points, 0, stop_pose.position); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (min_stop_distance && *min_stop_distance < stop_point_length) { const auto stop_point = utils::insertStopPoint(stop_point_length, path); @@ -1349,7 +1338,9 @@ void GoalPlannerModule::decelerateBeforeSearchStart( const Pose & current_pose = planner_data_->self_odometry->pose.pose; // slow down before the search area. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); if (min_decel_distance) { const double distance_to_search_start = calcSignedArcLengthFromEgo(path, search_start_offset_pose); @@ -1489,7 +1480,7 @@ void GoalPlannerModule::setDebugData() } // Visualize path and related pose - if (status_.is_safe) { + if (status_.is_safe_static_objects) { add(createPoseMarkerArray( status_.pull_over_path->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( @@ -1509,8 +1500,8 @@ void GoalPlannerModule::setDebugData() // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 40d5e1dc7a41f..99d109dd0bece 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -163,6 +163,7 @@ void GeometricParallelParking::clearPaths() current_path_idx_ = 0; arc_paths_.clear(); paths_.clear(); + pairs_terminal_velocity_and_accel_.clear(); } bool GeometricParallelParking::planPullOver( @@ -182,6 +183,7 @@ bool GeometricParallelParking::planPullOver( if (is_forward) { // When turning forward to the right, the front left goes out, // so reduce the steer angle at that time for seach no lane departure path. + // TODO(Sugahara): define in the config constexpr double start_pose_offset = 0.0; constexpr double min_steer_rad = 0.05; constexpr double steer_interval = 0.1; @@ -480,6 +482,19 @@ std::vector GeometricParallelParking::planOneTrial( paths_.push_back(path_turn_left); paths_.push_back(path_turn_right); + // set terminal velocity and acceleration(temporary implementation) + if (is_forward) { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + } else { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + } + // set pull_over start and end pose // todo: make start and end pose for pull_out start_pose_ = start_pose; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index 9e55b9b859c68..9322350877ad1 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include #include @@ -55,8 +56,7 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) const Pose end_pose = use_back_ ? goal_pose : tier4_autoware_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); - const bool found_path = planner_->makePlan(current_pose, end_pose); - if (!found_path) { + if (!planner_->makePlan(current_pose, end_pose)) { return {}; } @@ -114,18 +114,21 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) addPose(goal_pose); } - utils::correctDividedPathVelocity(partial_paths); + std::vector> pairs_terminal_velocity_and_accel{}; + pairs_terminal_velocity_and_accel.resize(partial_paths.size()); + utils::start_goal_planner_common::modifyVelocityByDirection( + partial_paths, pairs_terminal_velocity_and_accel, velocity_, 0); + // Check if driving forward for each path, return empty if not for (auto & path : partial_paths) { - const auto is_driving_forward = motion_utils::isDrivingForward(path.points); - if (!is_driving_forward) { - // path points is less than 2 + if (!motion_utils::isDrivingForward(path.points)) { return {}; } } PullOverPath pull_over_path{}; pull_over_path.partial_paths = partial_paths; + pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel; pull_over_path.start_pose = current_pose; pull_over_path.end_pose = goal_pose; pull_over_path.type = getPlannerType(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index f58c33a8f88b5..ed0106d891bb4 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -75,6 +75,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths = planner_.getPaths(); + pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel(); pull_over_path.start_pose = planner_.getStartPose(); pull_over_path.end_pose = planner_.getArcEndPose(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 05a787910da45..75b2a8237687d 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -207,6 +207,7 @@ boost::optional ShiftPullOver::generatePullOverPath( PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths.push_back(shifted_path.path); + pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index a775e7c16efed..23016b01cb9ab 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -343,13 +343,13 @@ TargetObjectsOnLane createTargetObjectsOnLane( }; // TODO(Sugahara): Consider shoulder and other lane objects - if (object_lane_configuration.check_current_lane) { + if (object_lane_configuration.check_current_lane && !current_lanes.empty()) { append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); } - if (object_lane_configuration.check_left_lane) { + if (object_lane_configuration.check_left_lane && !all_left_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_left_lane, all_left_lanelets); } - if (object_lane_configuration.check_right_lane) { + if (object_lane_configuration.check_right_lane && !all_right_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); } diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index ec14a064bf51b..c4bfb62276d42 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -418,6 +418,7 @@ void correctDividedPathVelocity(std::vector & divided_paths) { for (auto & path : divided_paths) { const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + // If the number of points in the path is less than 2, don't correct the velocity if (!is_driving_forward) { continue; } diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp new file mode 100644 index 0000000000000..f29711c82b3ce --- /dev/null +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -0,0 +1,148 @@ +// 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 "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using motion_utils::calcDecelDistWithJerkAndAccConstraints; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity) +{ + const auto v_now = planner_data->self_odometry->twist.twist.linear.x; + const auto a_now = planner_data->self_acceleration->accel.accel.linear.x; + + if (v_now < target_velocity) { + return 0.0; + } + + auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( + v_now, target_velocity, a_now, -acc_lim, jerk_lim, -1.0 * jerk_lim); + + if (!min_stop_distance) { + return {}; + } + + min_stop_distance = std::max(*min_stop_distance, 0.0); + + return min_stop_distance; +} + +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration) +{ + assert(paths.size() == terminal_vel_acc_pairs.size()); + + auto path_itr = std::begin(paths); + auto pair_itr = std::begin(terminal_vel_acc_pairs); + + for (; path_itr != std::end(paths); ++path_itr, ++pair_itr) { + const auto is_driving_forward = motion_utils::isDrivingForward(path_itr->points); + + // If the number of points in the path is less than 2, don't insert stop velocity and + // set pairs_terminal_velocity_and_accel to 0 + if (!is_driving_forward) { + *pair_itr = std::make_pair(0.0, 0.0); + continue; + } + + if (*is_driving_forward) { + for (auto & point : path_itr->points) { + // TODO(Sugahara): velocity calculation can be improved by considering the acceleration + point.point.longitudinal_velocity_mps = std::abs(point.point.longitudinal_velocity_mps); + } + // TODO(Sugahara): Consider the calculation of the target velocity and acceleration for ego's + // predicted path when ego will stop at the end of the path + *pair_itr = std::make_pair(target_velocity, acceleration); + } else { + for (auto & point : path_itr->points) { + point.point.longitudinal_velocity_mps = -std::abs(point.point.longitudinal_velocity_mps); + } + *pair_itr = std::make_pair(-target_velocity, -acceleration); + } + path_itr->points.back().point.longitudinal_velocity_mps = 0.0; + } +} + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params) +{ + ego_predicted_path_params = + std::make_shared(start_planner_params->ego_predicted_path_params); +} +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params) +{ + ego_predicted_path_params = + std::make_shared(goal_planner_params->ego_predicted_path_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params) +{ + safety_check_params = + std::make_shared(start_planner_params->safety_check_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params) +{ + safety_check_params = + std::make_shared(goal_planner_params->safety_check_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params) +{ + objects_filtering_params = + std::make_shared(start_planner_params->objects_filtering_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params) +{ + objects_filtering_params = + std::make_shared(goal_planner_params->objects_filtering_params); +} + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel) +{ + ego_predicted_path_params->target_velocity = pairs_terminal_velocity_and_accel.first; + ego_predicted_path_params->acceleration = pairs_terminal_velocity_and_accel.second; +} + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx) +{ + if (pairs_terminal_velocity_and_accel.size() <= current_path_idx) { + return std::make_pair(0.0, 0.0); + } + return pairs_terminal_velocity_and_accel.at(current_path_idx); +} + +} // namespace behavior_path_planner::utils::start_goal_planner_common diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 56d8eb0ff858a..628f1cfd7e421 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -59,11 +59,10 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_out_lanes); - const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_out_lane_objects, parameters_.th_moving_object_velocity); + const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + const auto [pull_out_lane_stop_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_out_lanes); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, @@ -113,6 +112,7 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con output.pairs_terminal_velocity_and_accel.push_back( std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } + output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; From b2cde5286fc1aaae7786e44d09749163d608663d Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 4 Sep 2023 12:17:18 +0900 Subject: [PATCH 84/93] refactor(behavior_path_planner): add vehicle_info_ as member variable in goal_planner (#4859) add vehicle_info_ Signed-off-by: kyoichi-sugahara --- .../scene_module/goal_planner/goal_planner_module.hpp | 2 ++ .../src/scene_module/goal_planner/goal_planner_module.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 65263905b9a24..1ddad7f392ee9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -133,6 +133,8 @@ class GoalPlannerModule : public SceneModuleInterface std::shared_ptr parameters_; + vehicle_info_util::VehicleInfo vehicle_info_; + // planner std::vector> pull_over_planners_; std::unique_ptr freespace_planner_; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index ae7e3772f7672..2e2d3a0594546 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -54,10 +54,12 @@ GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + parameters_{parameters}, + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); + lane_departure_checker.setVehicleInfo(vehicle_info_); occupancy_grid_map_ = std::make_shared(); From 9437f57fb1d346e7a872c83b5f758e415f81e8ee Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 5 Sep 2023 20:08:50 +0900 Subject: [PATCH 85/93] feat(behavior_path_planner): add safety check against dynamic objects for start/goal planner (#4873) * add safety check for start/goal planner Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara * update comments Signed-off-by: kyoichi-sugahara * add const Signed-off-by: kosuke55 * fix empty check Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Signed-off-by: kosuke55 Co-authored-by: kosuke55 Signed-off-by: kyoichi-sugahara --- .../goal_planner/goal_planner.param.yaml | 60 +++++++ .../start_planner/start_planner.param.yaml | 60 +++++++ .../goal_planner/goal_planner_module.hpp | 22 +++ .../start_planner/start_planner_module.hpp | 24 +++ .../utils/start_goal_planner_common/utils.hpp | 6 +- .../utils/start_planner/util.hpp | 4 + .../goal_planner/goal_planner_module.cpp | 98 ++++++++++++ .../src/scene_module/goal_planner/manager.cpp | 148 ++++++++++++++++-- .../scene_module/start_planner/manager.cpp | 116 ++++++++++++++ .../start_planner/start_planner_module.cpp | 94 ++++++++++- .../utils/start_goal_planner_common/utils.cpp | 9 +- 11 files changed, 619 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 513288948c1c0..709f271ac265c 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -106,6 +106,66 @@ neighbor_radius: 8.0 margin: 1.0 + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 + # debug debug: print_debug_info: false diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index bea78664c65a3..b62262423fa72 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -72,3 +72,63 @@ max_planning_time: 150.0 neighbor_radius: 8.0 margin: 1.0 + + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 1ddad7f392ee9..7441ee4e19d65 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -24,6 +24,9 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -57,6 +60,12 @@ using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStar; using freespace_planning_algorithms::RRTStarParam; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; + enum class PathType { NONE = 0, SHIFT, @@ -131,8 +140,14 @@ class GoalPlannerModule : public SceneModuleInterface PullOverStatus status_; + mutable StartGoalPlannerData goal_planner_data_; + std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; + vehicle_info_util::VehicleInfo vehicle_info_; // planner @@ -270,6 +285,13 @@ class GoalPlannerModule : public SceneModuleInterface // rtc std::pair calcDistanceToPathChange() const; + // safety check + SafetyCheckParams createSafetyCheckParams() const; + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; + // debug void setDebugData(); void printParkingPositionError() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index fd3b6ed419171..f3314e9896cbd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -17,12 +17,15 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -43,6 +46,11 @@ namespace behavior_path_planner { +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; @@ -88,6 +96,13 @@ class StartPlannerModule : public SceneModuleInterface void setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; + if (parameters->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } void resetStatus(); @@ -102,10 +117,14 @@ class StartPlannerModule : public SceneModuleInterface private: std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; vehicle_info_util::VehicleInfo vehicle_info_; std::vector> start_planners_; PullOutStatus status_; + mutable StartGoalPlannerData start_planner_data_; std::deque odometry_buffer_; @@ -148,6 +167,10 @@ class StartPlannerModule : public SceneModuleInterface bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. @@ -156,6 +179,7 @@ class StartPlannerModule : public SceneModuleInterface // generate BehaviorPathOutput with stopping path and update status BehaviorModuleOutput generateStopOutput(); + SafetyCheckParams createSafetyCheckParams() const; // freespace planner void onFreespacePlannerTimer(); bool planFreespacePath(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp index 60bbb3c78b401..bea671c266899 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -63,7 +63,7 @@ void updateEgoPredictedPathParams( void updateEgoPredictedPathParams( std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updateSafetyCheckParams( std::shared_ptr & safety_check_params, @@ -71,7 +71,7 @@ void updateSafetyCheckParams( void updateSafetyCheckParams( std::shared_ptr & safety_check_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updateObjectsFilteringParams( std::shared_ptr & objects_filtering_params, @@ -79,7 +79,7 @@ void updateObjectsFilteringParams( void updateObjectsFilteringParams( std::shared_ptr & objects_filtering_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updatePathProperty( std::shared_ptr & ego_predicted_path_params, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 63e374e074a5a..d85e6181764d5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -16,6 +16,9 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include @@ -36,6 +39,7 @@ namespace behavior_path_planner::start_planner_utils 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::utils::path_safety_checker::EgoPredictedPathParams; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 2e2d3a0594546..e6b4b9756eb61 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -351,6 +351,22 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { + // TODO(Sugahara): should check safe or not but in the current flow, it is not possible. + if (status_.pull_over_path == nullptr) { + return true; + } + + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } @@ -1450,12 +1466,83 @@ bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) return isCrossingPossible(start_pose, end_pose, lanes); } +void GoalPlannerModule::updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const +{ + goal_planner_data_.filtered_objects = filtered_objects; + goal_planner_data_.target_objects_on_lane = target_objects_on_lane; + goal_planner_data_.ego_predicted_path = ego_predicted_path; +} + +bool GoalPlannerModule::isSafePath() const +{ + const auto & pull_over_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const double forward_path_length = planner_data_->parameters.forward_path_length; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const auto & pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, backward_path_length, forward_path_length); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); + const auto & common_param = planner_data_->parameters; + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_over_path->pairs_terminal_velocity_and_accel, status_.current_path_idx); + RCLCPP_DEBUG( + getLogger(), "pairs_terminal_velocity_and_accel: %f, %f", terminal_velocity_and_accel.first, + terminal_velocity_and_accel.second); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto & ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity, + ego_seg_idx); + + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, pull_over_lanes, current_pose.position, + objects_filtering_params_); + + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path); + + for (const auto & object : target_objects_on_lane.on_current_lane) { + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::path_safety_checker::checkCollision( + pull_over_path, ego_predicted_path, object, obj_path, common_param, + safety_check_params_->rss_params, hysteresis_factor, collision)) { + return false; + } + } + } + + return true; +} + void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; @@ -1497,6 +1584,17 @@ void GoalPlannerModule::setDebugData() add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + if (goal_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + goal_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (goal_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } } // Visualize planner type text diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 332a8aa65c5d8..f88a506a93faf 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -32,17 +32,17 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( { GoalPlannerParameters p; + const std::string base_ns = "goal_planner."; // general params { - std::string ns = "goal_planner."; - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); + p.th_arrived_distance = node->declare_parameter(base_ns + "th_arrived_distance"); + p.th_stopped_time = node->declare_parameter(base_ns + "th_stopped_time"); } // goal search { - std::string ns = "goal_planner.goal_search."; + const std::string ns = base_ns + "goal_search."; p.search_priority = node->declare_parameter(ns + "search_priority"); p.forward_goal_search_length = node->declare_parameter(ns + "forward_goal_search_length"); @@ -86,7 +86,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // object recognition { - std::string ns = "goal_planner.object_recognition."; + const std::string ns = base_ns + "object_recognition."; p.use_object_recognition = node->declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = node->declare_parameter(ns + "object_recognition_collision_check_margin"); @@ -98,7 +98,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // pull over general params { - const std::string ns = "goal_planner.pull_over."; + const std::string ns = base_ns + "pull_over."; p.pull_over_minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); p.pull_over_velocity = node->declare_parameter(ns + "pull_over_velocity"); @@ -111,7 +111,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // shift parking { - std::string ns = "goal_planner.pull_over.shift_parking."; + const std::string ns = base_ns + "pull_over.shift_parking."; p.enable_shift_parking = node->declare_parameter(ns + "enable_shift_parking"); p.shift_sampling_num = node->declare_parameter(ns + "shift_sampling_num"); p.maximum_lateral_jerk = node->declare_parameter(ns + "maximum_lateral_jerk"); @@ -123,7 +123,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking forward { - std::string ns = "goal_planner.pull_over.parallel_parking.forward."; + const std::string ns = base_ns + "pull_over.parallel_parking.forward."; p.enable_arc_forward_parking = node->declare_parameter(ns + "enable_arc_forward_parking"); p.parallel_parking_parameters.after_forward_parking_straight_distance = node->declare_parameter(ns + "after_forward_parking_straight_distance"); @@ -139,7 +139,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking backward { - std::string ns = "goal_planner.pull_over.parallel_parking.backward."; + const std::string ns = base_ns + "pull_over.parallel_parking.backward."; p.enable_arc_backward_parking = node->declare_parameter(ns + "enable_arc_backward_parking"); p.parallel_parking_parameters.after_backward_parking_straight_distance = @@ -156,7 +156,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking general params { - std::string ns = "goal_planner.pull_over.freespace_parking."; + const std::string ns = base_ns + "pull_over.freespace_parking."; p.enable_freespace_parking = node->declare_parameter(ns + "enable_freespace_parking"); p.freespace_parking_algorithm = node->declare_parameter(ns + "freespace_parking_algorithm"); @@ -179,7 +179,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking search config { - std::string ns = "goal_planner.pull_over.freespace_parking.search_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.search_configs."; p.freespace_parking_common_parameters.theta_size = node->declare_parameter(ns + "theta_size"); p.freespace_parking_common_parameters.angle_goal_range = @@ -196,14 +196,14 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking costmap configs { - std::string ns = "goal_planner.pull_over.freespace_parking.costmap_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.costmap_configs."; p.freespace_parking_common_parameters.obstacle_threshold = node->declare_parameter(ns + "obstacle_threshold"); } // freespace parking astar { - std::string ns = "goal_planner.pull_over.freespace_parking.astar."; + const std::string ns = base_ns + "pull_over.freespace_parking.astar."; p.astar_parameters.only_behind_solutions = node->declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); @@ -213,7 +213,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking rrtstar { - std::string ns = "goal_planner.pull_over.freespace_parking.rrtstar."; + const std::string ns = base_ns + "pull_over.freespace_parking.rrtstar."; p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = node->declare_parameter(ns + "use_informed_sampling"); @@ -223,9 +223,125 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string path_safety_check_ns = "goal_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // debug { - std::string ns = "goal_planner.debug."; + const std::string ns = base_ns + "debug."; p.print_debug_info = node->declare_parameter(ns + "print_debug_info"); } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index ec18d55a31069..25f1f79affd4f 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -148,6 +148,122 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string base_ns = "start_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = base_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = base_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = base_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 7c5e87f3e15a9..458c80015abb3 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -17,7 +17,9 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -139,10 +141,21 @@ bool StartPlannerModule::isExecutionRequested() const bool StartPlannerModule::isExecutionReady() const { - if (!status_.is_safe_static_objects) { - return false; + if (status_.pull_out_path.partial_paths.empty()) { + return true; } + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } @@ -926,6 +939,70 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return turn_signal; } +void StartPlannerModule::updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const +{ + start_planner_data_.filtered_objects = filtered_objects; + start_planner_data_.target_objects_on_lane = target_objects_on_lane; + start_planner_data_.ego_predicted_path = ego_predicted_path; +} + +bool StartPlannerModule::isSafePath() const +{ + // TODO(Sugahara): should safety check for backward path + + const auto & pull_out_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points); + const auto & common_param = planner_data_->parameters; + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto & ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity, + ego_seg_idx); + + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_); + + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + current_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path); + + for (const auto & object : target_objects_on_lane.on_current_lane) { + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::path_safety_checker::checkCollision( + pull_out_path, ego_predicted_path, object, obj_path, common_param, + safety_check_params_->rss_params, hysteresis_factor, collision)) { + return false; + } + } + } + + return true; +} + bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; @@ -1043,8 +1120,10 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons void StartPlannerModule::setDebugData() const { + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; @@ -1062,6 +1141,17 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + if (start_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (start_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } // Visualize planner type text const auto header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp index f29711c82b3ce..cff6e9f81f02e 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -131,8 +131,15 @@ void updatePathProperty( std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel) { + // If acceleration is close to 0, the ego predicted path will be too short, so a minimum value is + // necessary to ensure a reasonable path length. + // TODO(Sugahara): define them as parameter + const double min_accel_for_ego_predicted_path = 1.0; + const double acceleration = + std::max(pairs_terminal_velocity_and_accel.second, min_accel_for_ego_predicted_path); + ego_predicted_path_params->target_velocity = pairs_terminal_velocity_and_accel.first; - ego_predicted_path_params->acceleration = pairs_terminal_velocity_and_accel.second; + ego_predicted_path_params->acceleration = acceleration; } std::pair getPairsTerminalVelocityAndAccel( From b81b10c9a2a6a7dc145385d9ed33b09ebebefa43 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 12 Sep 2023 16:03:35 +0900 Subject: [PATCH 86/93] feat(behavior_path_planner): add processOnEntry in start_planner (#4953) * add process on entry in start_planner Signed-off-by: kyoichi-sugahara * delete description Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../goal_planner/goal_planner_module.hpp | 13 +++++----- .../start_planner/start_planner_module.hpp | 4 +++ .../goal_planner/goal_planner_module.cpp | 25 +++++++++++++------ .../start_planner/start_planner_module.cpp | 22 ++++++++++++---- 4 files changed, 46 insertions(+), 18 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 7441ee4e19d65..517cba520af8f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -81,12 +81,12 @@ struct PullOverStatus size_t current_path_idx{0}; bool require_increment_{true}; // if false, keep current path idx. std::shared_ptr prev_stop_path{nullptr}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets pull_over_lanes{}; - std::vector lanes{}; // current + pull_over - bool has_decided_path{false}; - bool is_safe_static_objects{false}; // current path is safe against static objects - bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects + lanelet::ConstLanelets current_lanes{}; // TODO(someone): explain + lanelet::ConstLanelets pull_over_lanes{}; // TODO(someone): explain + std::vector lanes{}; // current + pull_over + bool has_decided_path{false}; // if true, the path is decided and safe against static objects + bool is_safe_static_objects{false}; // current path is safe against *static* objects + bool is_safe_dynamic_objects{false}; // current path is safe against *dynamic* objects bool prev_is_safe{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; @@ -286,6 +286,7 @@ class GoalPlannerModule : public SceneModuleInterface std::pair calcDistanceToPathChange() const; // safety check + void initializeSafetyCheckParameters(); SafetyCheckParams createSafetyCheckParams() const; void updateSafetyCheckTargetObjectsData( const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index f3314e9896cbd..1bec51b7fc0b3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -91,6 +91,7 @@ class StartPlannerModule : public SceneModuleInterface BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; + void processOnEntry() override; void processOnExit() override; void setParameters(const std::shared_ptr & parameters) @@ -116,6 +117,9 @@ class StartPlannerModule : public SceneModuleInterface bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: + + void initializeSafetyCheckParameters(); + std::shared_ptr parameters_; mutable std::shared_ptr ego_predicted_path_params_; mutable std::shared_ptr objects_filtering_params_; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index e6b4b9756eb61..dbad42b4ca294 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -253,6 +253,15 @@ void GoalPlannerModule::initializeOccupancyGridMap() occupancy_grid_map_->setParam(occupancy_grid_map_param); } +void GoalPlannerModule::initializeSafetyCheckParameters() +{ + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); +} + void GoalPlannerModule::processOnEntry() { // Initialize occupancy grid map @@ -261,6 +270,10 @@ void GoalPlannerModule::processOnEntry() parameters_->use_occupancy_grid_for_path_collision_check) { initializeOccupancyGridMap(); } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + } } void GoalPlannerModule::processOnExit() @@ -357,11 +370,6 @@ bool GoalPlannerModule::isExecutionReady() const } if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { - utils::start_goal_planner_common::updateEgoPredictedPathParams( - ego_predicted_path_params_, parameters_); - utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); - utils::start_goal_planner_common::updateObjectsFilteringParams( - objects_filtering_params_, parameters_); if (!isSafePath()) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); return false; @@ -690,7 +698,6 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); - output.reference_path = getPreviousModuleOutput().reference_path; status_.prev_stop_path = output.path; // set stop path as pull over path mutex_.lock(); @@ -705,10 +712,10 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) } else { // not_safe -> not_safe: use previous stop path output.path = status_.prev_stop_path; - output.reference_path = getPreviousModuleOutput().reference_path; RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } + output.reference_path = getPreviousModuleOutput().reference_path; } void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const @@ -1095,6 +1102,10 @@ bool GoalPlannerModule::incrementPathIndex() PathWithLaneId GoalPlannerModule::getCurrentPath() const { + if (status_.pull_over_path == nullptr) { + return PathWithLaneId{}; + } + if (status_.pull_over_path->partial_paths.size() <= status_.current_path_idx) { return PathWithLaneId{}; } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 458c80015abb3..c6102bc5490bb 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -99,6 +99,14 @@ BehaviorModuleOutput StartPlannerModule::run() return plan(); } +void StartPlannerModule::processOnEntry() +{ + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + } +} + void StartPlannerModule::processOnExit() { resetPathCandidate(); @@ -146,11 +154,6 @@ bool StartPlannerModule::isExecutionReady() const } if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { - utils::start_goal_planner_common::updateEgoPredictedPathParams( - ego_predicted_path_params_, parameters_); - utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); - utils::start_goal_planner_common::updateObjectsFilteringParams( - objects_filtering_params_, parameters_); if (!isSafePath()) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); return false; @@ -269,6 +272,15 @@ CandidateOutput StartPlannerModule::planCandidate() const return CandidateOutput{}; } +void StartPlannerModule::initializeSafetyCheckParameters() +{ + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); +} + PathWithLaneId StartPlannerModule::getFullPath() const { // combine partial pull out path From fc95426c9a4dfec047dd604a0242b9ea9929bece Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sat, 16 Sep 2023 18:17:49 +0900 Subject: [PATCH 87/93] fix(behavior_path_planner): define hysteresis_factor_expand_rate (#5002) * define hysteresis_factor_expand_rate Signed-off-by: kyoichi-sugahara * add hysteresis_factor_expand_rate in SafetyCheckParams Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * revert unnecessary change Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/goal_planner/goal_planner.param.yaml | 2 ++ .../config/start_planner/start_planner.param.yaml | 2 ++ .../path_safety_checker/path_safety_checker_parameters.hpp | 4 +++- .../src/scene_module/goal_planner/manager.cpp | 2 ++ .../src/scene_module/start_planner/manager.cpp | 2 ++ .../src/scene_module/start_planner/start_planner_module.cpp | 2 +- 6 files changed, 12 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 709f271ac265c..779e7af61699f 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -162,6 +162,8 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 1.0 longitudinal_velocity_delta_time: 1.0 + # hysteresis factor to expand/shrink polygon with the value + hysteresis_factor_expand_rate: 1.0 # temporary backward_path_length: 30.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index b62262423fa72..586676fbbb0f0 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -129,6 +129,8 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 1.0 longitudinal_velocity_delta_time: 1.0 + # hysteresis factor to expand/shrink polygon + hysteresis_factor_expand_rate: 1.0 # temporary backward_path_length: 30.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 475b38f5bb824..b2433c076b1a8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -169,7 +169,9 @@ struct ObjectsFilteringParams */ struct SafetyCheckParams { - bool enable_safety_check; ///< Enable safety checks. + bool enable_safety_check; ///< Enable safety checks. + double + hysteresis_factor_expand_rate; ///< Hysteresis factor to expand/shrink polygon with the value. double backward_path_length; ///< Length of the backward lane for path generation. double forward_path_length; ///< Length of the forward path lane for path generation. RSSparams rss_params; ///< Parameters related to the RSS model. diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index f88a506a93faf..a3b0c6adf9721 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -316,6 +316,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( { p.safety_check_params.enable_safety_check = node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); p.safety_check_params.backward_path_length = node->declare_parameter(safety_check_ns + "backward_path_length"); p.safety_check_params.forward_path_length = diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 25f1f79affd4f..6d2f0aa50cb07 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -241,6 +241,8 @@ StartPlannerModuleManager::StartPlannerModuleManager( { p.safety_check_params.enable_safety_check = node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); p.safety_check_params.backward_path_length = node->declare_parameter(safety_check_ns + "backward_path_length"); p.safety_check_params.forward_path_length = diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index c6102bc5490bb..619811430e535 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -995,7 +995,7 @@ bool StartPlannerModule::isSafePath() const current_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = - status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path); From 0910de58064969647586e66e84166b3431df1a52 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 19 Sep 2023 10:43:47 +0000 Subject: [PATCH 88/93] style(pre-commit): autofix --- .../scene_module/goal_planner/goal_planner_module.hpp | 1 - .../scene_module/start_planner/start_planner_module.hpp | 1 - 2 files changed, 2 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 517cba520af8f..2f465901a7085 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -137,7 +137,6 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; }; private: - PullOverStatus status_; mutable StartGoalPlannerData goal_planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 1bec51b7fc0b3..e332821607672 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -117,7 +117,6 @@ class StartPlannerModule : public SceneModuleInterface bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: - void initializeSafetyCheckParameters(); std::shared_ptr parameters_; From f4f4a1480548b703afd2ddf4e99ba846aa97a781 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 19 Sep 2023 20:19:50 +0900 Subject: [PATCH 89/93] perf(planner_manger): remove lifo limitation (#4458) (#855) * feat(route_handler): add function to get closest preferred lanelet * perf(planner_manager): remove module immediately once its status is success --------- Signed-off-by: satoshi-ota Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../behavior_path_planner/planner_manager.hpp | 36 +++-------- .../src/planner_manager.cpp | 59 ++++--------------- .../include/route_handler/route_handler.hpp | 2 + planning/route_handler/src/route_handler.cpp | 7 +++ 4 files changed, 30 insertions(+), 74 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 8820ef70c37bf..6ca2ecf638da6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -348,41 +348,21 @@ class PlannerManager candidate_module_ptrs_.clear(); } - /** - * @brief stop and remove not RUNNING modules in candidate_module_ptrs_. - */ - void clearNotRunningCandidateModules() - { - const auto it = std::remove_if( - candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) { - if (m->getCurrentStatus() != ModuleStatus::RUNNING) { - deleteExpiredModules(m); - return true; - } - return false; - }); - candidate_module_ptrs_.erase(it, candidate_module_ptrs_.end()); - } - - /** - * @brief check if there is any RUNNING module in candidate_module_ptrs_. - */ - bool hasAnyRunningCandidateModule() - { - return std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](auto & m) { - return m->getCurrentStatus() == ModuleStatus::RUNNING; - }); - } - /** * @brief get current root lanelet. the lanelet is used for reference path generation. * @param planner data. * @return root lanelet. */ - lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr & data) const + lanelet::ConstLanelet updateRootLanelet( + const std::shared_ptr & data, bool success_lane_change = false) const { lanelet::ConstLanelet ret{}; - data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + if (success_lane_change) { + data->route_handler->getClosestPreferredLaneletWithinRoute( + data->self_odometry->pose.pose, &ret); + } else { + data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + } RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id()); return ret; } diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 3e9411b141fb3..d3ee7bcc093ec 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -509,8 +509,6 @@ std::pair PlannerManager::runRequestModule BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr & data) { - const bool has_any_running_candidate_module = hasAnyRunningCandidateModule(); - std::unordered_map results; BehaviorModuleOutput output = getReferencePath(data); results.emplace("root", output); @@ -608,59 +606,28 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() != ModuleStatus::SUCCESS; }); - - // convert reverse iterator -> iterator - const auto success_itr = std::prev(not_success_itr).base() - 1; - /** - * there is no succeeded module. return. + * remove success module immediately. if lane change module has succeeded, update root lanelet. */ - if (success_itr == approved_module_ptrs_.end()) { - return approved_modules_output; - } - - const auto lane_change_itr = std::find_if( - success_itr, approved_module_ptrs_.end(), - [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); - - /** - * remove success modules according to Last In First Out(LIFO) policy. when the next module is in - * ModuleStatus::RUNNING, the previous module keeps running even if it is in - * ModuleStatus::SUCCESS. - */ - if (lane_change_itr == approved_module_ptrs_.end() && !has_any_running_candidate_module) { - std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) { - debug_info_.emplace_back(m, Action::DELETE, "From Approved"); - deleteExpiredModules(m); - }); + { + const auto success_module_itr = std::partition( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [](const auto & m) { return m->getCurrentStatus() != ModuleStatus::SUCCESS; }); - approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); - clearNotRunningCandidateModules(); + const auto success_lane_change = std::any_of( + success_module_itr, approved_module_ptrs_.end(), + [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); - return approved_modules_output; - } + if (success_lane_change) { + root_lanelet_ = updateRootLanelet(data, true); + } - /** - * as an exception, when there is lane change module is in succeeded modules, it doesn't remove - * any modules if module whose status is NOT ModuleStatus::SUCCESS exists. this is because the - * root lanelet is updated at the moment of lane change module's unregistering, and that causes - * change First In module's input. - */ - if (not_success_itr == approved_module_ptrs_.rend() && !has_any_running_candidate_module) { - std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) { + std::for_each(success_module_itr, approved_module_ptrs_.end(), [&](auto & m) { debug_info_.emplace_back(m, Action::DELETE, "From Approved"); deleteExpiredModules(m); }); - approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); - clearNotRunningCandidateModules(); - - root_lanelet_ = updateRootLanelet(data); - - return approved_modules_output; + approved_module_ptrs_.erase(success_module_itr, approved_module_ptrs_.end()); } return approved_modules_output; diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 061953f30005c..9c36c8bec30e7 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -307,6 +307,8 @@ class RouteHandler bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; + bool getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; bool getClosestLaneletWithConstrainsWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, const double yaw_threshold) const; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 7c2b4c2af7974..b4e7be025dcaf 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -825,6 +825,13 @@ bool RouteHandler::getClosestLaneletWithinRoute( return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet); } +bool RouteHandler::getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const +{ + return lanelet::utils::query::getClosestLanelet( + preferred_lanelets_, search_pose, closest_lanelet); +} + bool RouteHandler::getClosestLaneletWithConstrainsWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, const double yaw_threshold) const From 8d0d52afbd57d5442ee77839e22766c4cb7ea6d4 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 19 Sep 2023 23:16:00 +0900 Subject: [PATCH 90/93] fix(mission_planner/behavior_path_planner): use transient local for modified goal (#5012) (#852) fix(mission_planner/behavior_path_planner): use trasient local for modified goal Signed-off-by: kosuke55 --- .../src/behavior_path_planner_node.cpp | 4 +++- .../src/mission_planner/mission_planner.cpp | 9 +++++---- .../src/mission_planner/mission_planner.hpp | 4 ++-- 3 files changed, 10 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 31da3e892ffe5..3f33bcd7b64dc 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -66,7 +66,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod turn_signal_publisher_ = create_publisher("~/output/turn_indicators_cmd", 1); hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); - modified_goal_publisher_ = create_publisher("~/output/modified_goal", 1); + const auto durable_qos = rclcpp::QoS(1).transient_local(); + modified_goal_publisher_ = + create_publisher("~/output/modified_goal", durable_qos); stop_reason_publisher_ = create_publisher("~/output/stop_reasons", 1); debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 0c9b27a685583..41fe1d552b9f4 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -90,12 +90,14 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); - auto qos_transient_local = rclcpp::QoS{1}.transient_local(); + const auto durable_qos = rclcpp::QoS(1).transient_local(); vector_map_subscriber_ = create_subscription( - "input/vector_map", qos_transient_local, + "input/vector_map", durable_qos, std::bind(&MissionPlanner::onMap, this, std::placeholders::_1)); + sub_modified_goal_ = create_subscription( + "input/modified_goal", durable_qos, + std::bind(&MissionPlanner::on_modified_goal, this, std::placeholders::_1)); - const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_marker_ = create_publisher("debug/route_marker", durable_qos); const auto adaptor = component_interface_utils::NodeAdaptor(this); @@ -110,7 +112,6 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) adaptor.init_srv(srv_change_route_points_, this, &MissionPlanner::on_change_route_points); adaptor.init_srv(srv_set_mrm_route_, this, &MissionPlanner::on_set_mrm_route); adaptor.init_srv(srv_clear_mrm_route_, this, &MissionPlanner::on_clear_mrm_route); - adaptor.init_sub(sub_modified_goal_, this, &MissionPlanner::on_modified_goal); change_state(RouteState::Message::UNSET); } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 4d09f72f81899..ad8ca9cb07fbf 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -72,8 +72,10 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr sub_modified_goal_; Odometry::ConstSharedPtr odometry_; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); @@ -124,8 +126,6 @@ class MissionPlanner : public rclcpp::Node HADMapBin::ConstSharedPtr map_ptr_{nullptr}; void onMap(const HADMapBin::ConstSharedPtr msg); - component_interface_utils::Subscription::SharedPtr sub_modified_goal_; - void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg); void on_change_route( const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res); From 75a263c663e8f7f6d1a76c2237e44725076b2f21 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Wed, 20 Sep 2023 00:55:37 +0900 Subject: [PATCH 91/93] fix(pull_out): add empty check (#5041) Signed-off-by: tomoya.kimura --- .../geometric_parallel_parking/geometric_parallel_parking.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 99d109dd0bece..840597e1ec95d 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -282,6 +282,10 @@ bool GeometricParallelParking::planPullOut( PathWithLaneId road_center_line_path = planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); + if (road_center_line_path.points.empty()) { + continue; + } + // check the continuity of straight path and arc path const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose; const Pose & arc_path_last_pose = arc_paths.back().points.back().point.pose; From 92e1f962fb81d72da93b68149d014fb44bf87fdd Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 21 Sep 2023 19:02:55 +0900 Subject: [PATCH 92/93] =?UTF-8?q?feat(start=5Fplanner):=20prevent=20start?= =?UTF-8?q?=20planner=20execution=20in=20the=20middle=20of=E2=80=A6=20(#86?= =?UTF-8?q?0)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit feat(start_planner): prevent start planner execution in the middle of the road (#5004) prevent start planner execution in the middle of the road Signed-off-by: Daniel Sanchez Co-authored-by: danielsanchezaran --- .../start_planner/start_planner.param.yaml | 1 + ...avior_path_planner_start_planner_design.md | 21 ++++++++++--------- .../start_planner_parameters.hpp | 1 + .../scene_module/start_planner/manager.cpp | 2 ++ .../start_planner/start_planner_module.cpp | 12 ++++++++--- 5 files changed, 24 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 586676fbbb0f0..216ba4ccebbc9 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -7,6 +7,7 @@ collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 th_moving_object_velocity: 1.0 + th_distance_to_middle_of_the_road: 0.1 # shift pull out enable_shift_pull_out: true check_shift_path_lane_departure: false diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 2b98775e4fe81..0b0e01378b61a 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -59,16 +59,17 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | +| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | ## Safety check with static obstacles diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 771fbd93f7196..9e9173c3917a0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -39,6 +39,7 @@ struct StartPlannerParameters double th_stopped_velocity; double th_stopped_time; double th_turn_signal_on_lateral_offset; + double th_distance_to_middle_of_the_road; double intersection_search_length; double length_ratio_for_turn_signal_deactivation_near_intersection; double collision_check_margin; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 6d2f0aa50cb07..f5cd60e315e68 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -36,6 +36,8 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); p.th_turn_signal_on_lateral_offset = node->declare_parameter(ns + "th_turn_signal_on_lateral_offset"); + p.th_distance_to_middle_of_the_road = + node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 619811430e535..89e9200fee250 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -116,10 +116,16 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { - // TODO(Sugahara): if required lateral shift distance is small, don't engage this module. - // Execute when current pose is near route start pose - const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const double lateral_distance_to_center_lane = + lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; + + if (std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road) { + return false; + } + + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); if ( tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > parameters_->th_arrived_distance) { From e70187f09210f8004b8393aaaabb919f1cefd0af Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 25 Sep 2023 18:05:25 +0900 Subject: [PATCH 93/93] feat(avoidance): imporove avoidance near goal (#863) * feat(avoidance): add parameter to configurate avoidance return point (#4467) Signed-off-by: satoshi-ota * fix(utils): improve monotonic bound generation logic (#4706) Signed-off-by: satoshi-ota * feat(avoidance): insert return dead line point (#5029) Signed-off-by: satoshi-ota * fix(avoidance): exit if there is no avoidable object (#5051) Signed-off-by: satoshi-ota Signed-off-by: kyoichi-sugahara --------- Signed-off-by: satoshi-ota Signed-off-by: kyoichi-sugahara Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../config/avoidance/avoidance.param.yaml | 1 + .../avoidance/avoidance_module.hpp | 2 + .../utils/avoidance/avoidance_module_data.hpp | 3 + .../avoidance/avoidance_module.cpp | 97 +++++++++++++++++-- .../src/scene_module/avoidance/manager.cpp | 1 + .../behavior_path_planner/src/utils/utils.cpp | 18 +++- 6 files changed, 113 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 53db7ee81c40e..c6072e8a50ce3 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -171,6 +171,7 @@ # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] + remain_buffer_distance: 30.0 # [m] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index cda680b9dec7c..9edb53fe8af63 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -223,6 +223,8 @@ class AvoidanceModule : public SceneModuleInterface */ void insertStopPoint(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + void insertReturnDeadLine(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + /** * @brief insert stop point in output path. * @param target path. 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 eb2746f93ea83..ec0ec4696a44d 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 @@ -231,6 +231,9 @@ struct AvoidanceParameters // nominal avoidance sped double nominal_avoidance_speed; + // module try to return original path to keep this distance from edge point of the path. + double remain_buffer_distance; + // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. double soft_road_shoulder_margin{1.0}; 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 aa649b138eb82..8f2e505fcb76e 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 @@ -146,7 +146,10 @@ ModuleStatus AvoidanceModule::updateState() { const auto & data = avoidance_data_; const auto is_plan_running = isAvoidancePlanRunning(); - const bool has_avoidance_target = !data.target_objects.empty(); + const bool has_avoidance_target = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { + return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + }); if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "previous module lane is updated."); @@ -670,6 +673,8 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif throw std::domain_error("invalid behavior"); } + insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); + setStopReason(StopReason::AVOIDANCE, path.path); } @@ -1013,9 +1018,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; // The end_margin also has the purpose of preventing the return path from NOT being // triggered at the end point. - const auto end_margin = 1.0; - const auto return_remaining_distance = - std::max(data.arclength_from_ego.back() - o.longitudinal - offset - end_margin, 0.0); + const auto return_remaining_distance = std::max( + data.arclength_from_ego.back() - o.longitudinal - offset - + parameters_->remain_buffer_distance, + 0.0); al_return.start_shift_length = feasible_shift_length.get(); al_return.end_shift_length = 0.0; @@ -1795,14 +1801,14 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) return; } - const auto remaining_distance = arclength_from_ego.back(); + const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance; // If the avoidance point has already been set, the return shift must be set after the point. const auto last_sl_distance = avoidance_data_.arclength_from_ego.at(last_sl.end_idx); // check if there is enough distance for return. - if (last_sl_distance + 1.0 > remaining_distance) { // tmp: add some small number (+1.0) - DEBUG_PRINT("No enough distance for return."); + if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 1000, "No enough distance for return."); return; } @@ -2929,6 +2935,77 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const return object.longitudinal - std::min(variable + constant, p->stop_max_distance); } +void AvoidanceModule::insertReturnDeadLine( + const bool use_constraints_for_decel, ShiftedPath & shifted_path) const +{ + const auto & data = avoidance_data_; + + if (!planner_data_->route_handler->isInGoalRouteSection(data.current_lanelets.back())) { + RCLCPP_DEBUG(getLogger(), "goal is far enough."); + return; + } + + const auto shift_length = path_shifter_.getLastShiftLength(); + + if (std::abs(shift_length) < 1e-3) { + RCLCPP_DEBUG(getLogger(), "don't have to consider return shift."); + return; + } + + const auto min_return_distance = helper_.getMinAvoidanceDistance(shift_length); + + const auto to_goal = calcSignedArcLength( + shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); + const auto to_stop_line = to_goal - min_return_distance - parameters_->remain_buffer_distance; + + // If we don't need to consider deceleration constraints, insert a deceleration point + // and return immediately + if (!use_constraints_for_decel) { + utils::avoidance::insertDecelPoint( + getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, + stop_pose_); + return; + } + + // If the stop distance is not enough for comfortable stop, don't insert wait point. + const auto is_comfortable_stop = helper_.getFeasibleDecelDistance(0.0) < to_stop_line; + if (!is_comfortable_stop) { + RCLCPP_DEBUG(getLogger(), "stop distance is not enough."); + return; + } + + utils::avoidance::insertDecelPoint( + getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); + + // insert slow down speed. + const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( + shift_length, helper_.getLateralMinJerkLimit(), to_stop_line); + if (current_target_velocity < getEgoSpeed()) { + RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed."); + return; + } + + const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); + for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { + const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); + + // slow down speed is inserted only in front of the object. + const auto shift_longitudinal_distance = to_stop_line - distance_from_ego; + if (shift_longitudinal_distance < 0.0) { + break; + } + + // target speed with nominal jerk limits. + const double v_target = PathShifter::calcFeasibleVelocityFromJerk( + shift_length, helper_.getLateralMinJerkLimit(), shift_longitudinal_distance); + const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; + const double v_insert = + std::max(v_target - parameters_->buf_slow_down_speed, parameters_->min_slow_down_speed); + + shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert); + } +} + void AvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { @@ -3059,6 +3136,12 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto object = data.target_objects.front(); + const auto enough_space = + object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + if (!enough_space) { + return; + } + // calculate shift length for front object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.object.classification); 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 a9e8982a9a48a..14f98d026c493 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -197,6 +197,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( std::string ns = "avoidance.avoidance.longitudinal."; p.prepare_time = get_parameter(node, ns + "prepare_time"); p.min_prepare_distance = get_parameter(node, ns + "min_prepare_distance"); + p.remain_buffer_distance = get_parameter(node, ns + "remain_buffer_distance"); p.min_slow_down_speed = get_parameter(node, ns + "min_slow_down_speed"); p.buf_slow_down_speed = get_parameter(node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = get_parameter(node, ns + "nominal_avoidance_speed"); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 3648b8670b171..2945c74d51804 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1691,8 +1691,22 @@ void makeBoundLongitudinallyMonotonic( continue; } - for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { - set_orientation(ret, j, getPose(path_points.at(i)).orientation); + if (i + 1 == path_points.size()) { + for (size_t j = intersect_idx.get(); j < bound_with_pose.size(); j++) { + if (j + 1 == bound_with_pose.size()) { + const auto yaw = + calcAzimuthAngle(bound_with_pose.at(j - 1).position, bound_with_pose.at(j).position); + set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); + } else { + const auto yaw = + calcAzimuthAngle(bound_with_pose.at(j).position, bound_with_pose.at(j + 1).position); + set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); + } + } + } else { + for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { + set_orientation(ret, j, getPose(path_points.at(i)).orientation); + } } constexpr size_t OVERLAP_CHECK_NUM = 3;