From efe4e1670e679057d91e18eaeaff312c920c1f9a Mon Sep 17 00:00:00 2001 From: taisa1 <38522559+taisa1@users.noreply.github.com> Date: Mon, 11 Mar 2024 17:14:15 +0900 Subject: [PATCH 1/8] perf(common_ground_filter): performance tuning (#5861) * perf(scan_ground_filter): performance tuning Signed-off-by: taisa1 * perf(scan_ground_filter): improved performance Signed-off-by: taisa1 * chore: refactoring Signed-off-by: taisa1 * chore: refactoring Signed-off-by: taisa1 * refactor: reflect review Signed-off-by: taisa1 * style(pre-commit): autofix * fix: remove unnecessary comment Signed-off-by: taisa1 * test: change unit test to use faster_filter Signed-off-by: taisa1 * test: fixed compile error Signed-off-by: taisa1 * fix: add lacking parameter callback Signed-off-by: taisa1 * fix: uninitialized valuabe Signed-off-by: atsushi421 --------- Signed-off-by: taisa1 Signed-off-by: atsushi421 Co-authored-by: taisa1 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: atsushi yano <55824710+atsushi421@users.noreply.github.com> Co-authored-by: atsushi421 --- .../scan_ground_filter_nodelet.hpp | 86 ++-- .../src/scan_ground_filter_nodelet.cpp | 402 +++++++++++------- .../test/test_scan_ground_filter.cpp | 3 +- .../pointcloud_preprocessor/src/filter.cpp | 2 +- 4 files changed, 310 insertions(+), 183 deletions(-) diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index 980565fe2144b..d017d06929702 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -16,6 +16,7 @@ #define GROUND_SEGMENTATION__SCAN_GROUND_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include @@ -50,7 +51,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter // classified point label // (0: not classified, 1: ground, 2: not ground, 3: follow previous point, // 4: unkown(currently not used), 5: virtual ground) - enum class PointLabel { + enum class PointLabel : uint16_t { INIT = 0, GROUND, NON_GROUND, @@ -59,19 +60,15 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter VIRTUAL_GROUND, OUT_OF_RANGE }; - struct PointRef + + struct PointData { - float grid_size; // radius of grid - uint16_t grid_id; // id of grid in vertical - float radius; // cylindrical coords on XY Plane - float theta; // angle deg on XY plane - size_t radial_div; // index of the radial division to which this point belongs to + float radius; // cylindrical coords on XY Plane PointLabel point_state{PointLabel::INIT}; - + uint16_t grid_id; // id of grid in vertical size_t orig_index; // index of this point in the source pointcloud - pcl::PointXYZ * orig_point; }; - using PointCloudRefVector = std::vector; + using PointCloudVector = std::vector; struct GridCenter { @@ -144,18 +141,39 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter pcl::PointIndices getIndices() { return pcl_indices; } std::vector getHeightList() { return height_list; } + + pcl::PointIndices & getIndicesRef() { return pcl_indices; } + std::vector & getHeightListRef() { return height_list; } }; void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + // TODO(taisa1): 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 pointcloud_preprocessor::TransformInfo & transform_info); + tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; + int x_offset_; + int y_offset_; + int z_offset_; + int intensity_offset_; + bool offset_initialized_; + + void set_field_offsets(const PointCloud2ConstPtr & input); + + void get_point_from_global_offset( + const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset); + const uint16_t gnd_grid_continual_thresh_ = 3; bool elevation_grid_mode_; float non_ground_height_threshold_; float grid_size_rad_; + float tan_grid_size_rad_; float grid_size_m_; float low_priority_region_x_; uint16_t gnd_grid_buffer_size_; @@ -163,14 +181,17 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter float grid_mode_switch_angle_rad_; float virtual_lidar_z_; float detection_range_z_max_; - float center_pcl_shift_; // virtual center of pcl to center mass - float grid_mode_switch_radius_; // non linear grid size switching distance - double global_slope_max_angle_rad_; // radians - double local_slope_max_angle_rad_; // radians + float center_pcl_shift_; // virtual center of pcl to center mass + float grid_mode_switch_radius_; // non linear grid size switching distance + double global_slope_max_angle_rad_; // radians + double local_slope_max_angle_rad_; // radians + double global_slope_max_ratio_; + double local_slope_max_ratio_; double radial_divider_angle_rad_; // distance in rads between dividers double split_points_distance_tolerance_; // distance in meters between concentric divisions - double // minimum height threshold regardless the slope, - split_height_distance_; // useful for close points + double split_points_distance_tolerance_square_; + double // minimum height threshold regardless the slope, + split_height_distance_; // useful for close points bool use_virtual_ground_point_; bool use_recheck_ground_cluster_; // to enable recheck ground cluster size_t radial_dividers_num_; @@ -186,23 +207,25 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter */ /*! - * Convert pcl::PointCloud to sorted PointCloudRefVector + * Convert sensor_msgs::msg::PointCloud2 to sorted PointCloudVector * @param[in] in_cloud Input Point Cloud to be organized in radial segments * @param[out] out_radial_ordered_points_manager Vector of Points Clouds, * each element will contain the points ordered */ void convertPointcloud( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points_manager); + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points_manager); void convertPointcloudGridScan( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points_manager); + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points_manager); /*! * Output ground center of front wheels as the virtual ground point * @param[out] point Virtual ground origin point */ void calcVirtualGroundOrigin(pcl::PointXYZ & point); + float calcGridSize(const PointData & p); + /*! * Classifies Points in the PointCloud as Ground and Not Ground * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud @@ -214,14 +237,19 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter void initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids); - void checkContinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); - void checkDiscontinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); - void checkBreakGndGrid(PointRef & p, const std::vector & gnd_grids_list); + void checkContinuousGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + void checkDiscontinuousGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + void checkBreakGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); void classifyPointCloud( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud_ptr, + std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); void classifyPointCloudGridScan( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud_ptr, + std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); /*! * Re-classifies point of ground cluster based on their height @@ -237,11 +265,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter * and the other removed as indicated in the indices * @param in_cloud_ptr Input PointCloud to which the extraction will be performed * @param in_indices Indices of the points to be both removed and kept - * @param out_object_cloud_ptr Resulting PointCloud with the indices kept + * @param out_object_cloud Resulting PointCloud with the indices kept */ void extractObjectPoints( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr); + const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, + PointCloud2 & out_object_cloud); /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index f164e0102e0e9..7a0333d95075b 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -48,8 +48,12 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); + global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); + local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); + split_points_distance_tolerance_square_ = + split_points_distance_tolerance_ * split_points_distance_tolerance_; split_height_distance_ = declare_parameter("split_height_distance"); use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); @@ -60,6 +64,12 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & grid_mode_switch_radius_ / grid_size_m_; // changing the mode of grid division virtual_lidar_z_ = vehicle_info_.vehicle_height_m; grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); + + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + offset_initialized_ = false; } using std::placeholders::_1; @@ -77,92 +87,117 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & } } +inline void ScanGroundFilterComponent::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; + 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; +} + +inline void ScanGroundFilterComponent::get_point_from_global_offset( + const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset) +{ + point.x = *reinterpret_cast(&input->data[global_offset + x_offset_]); + point.y = *reinterpret_cast(&input->data[global_offset + y_offset_]); + point.z = *reinterpret_cast(&input->data[global_offset + z_offset_]); +} + void ScanGroundFilterComponent::convertPointcloudGridScan( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { out_radial_ordered_points.resize(radial_dividers_num_); - PointRef current_point; - uint16_t back_steps_num = 1; + PointData current_point; + + const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; + const auto inv_grid_size_rad = 1.0f / grid_size_rad_; + const auto inv_grid_size_m = 1.0f / grid_size_m_; + + const auto grid_id_offset = + grid_mode_switch_grid_id_ - grid_mode_switch_angle_rad_ * inv_grid_size_rad; + const auto x_shift = vehicle_info_.wheel_base_m / 2.0f + center_pcl_shift_; - grid_size_rad_ = - normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); - for (size_t i = 0; i < in_cloud->points.size(); ++i) { - auto x{ - in_cloud->points[i].x - vehicle_info_.wheel_base_m / 2.0f - - center_pcl_shift_}; // base on front wheel center - // auto y{in_cloud->points[i].y}; - auto radius{static_cast(std::hypot(x, in_cloud->points[i].y))}; - auto theta{normalizeRadian(std::atan2(x, in_cloud->points[i].y), 0.0)}; + const size_t in_cloud_data_size = in_cloud->data.size(); + const size_t in_cloud_point_step = in_cloud->point_step; + + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto x{input_point.x - x_shift}; // base on front wheel center + auto radius{static_cast(std::hypot(x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(x, input_point.y), 0.0)}; // divide by vertical angle - auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; - auto radial_div{ - static_cast(std::floor(normalizeDegree(theta / radial_divider_angle_rad_, 0.0)))}; + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; uint16_t grid_id = 0; - float curr_grid_size = 0.0f; if (radius <= grid_mode_switch_radius_) { - grid_id = static_cast(radius / grid_size_m_); - curr_grid_size = grid_size_m_; + grid_id = static_cast(radius * inv_grid_size_m); } else { - grid_id = grid_mode_switch_grid_id_ + (gamma - grid_mode_switch_angle_rad_) / grid_size_rad_; - if (grid_id <= grid_mode_switch_grid_id_ + back_steps_num) { - curr_grid_size = grid_size_m_; - } else { - curr_grid_size = std::tan(gamma) - std::tan(gamma - grid_size_rad_); - curr_grid_size *= virtual_lidar_z_; - } + auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; + grid_id = grid_id_offset + gamma * inv_grid_size_rad; } current_point.grid_id = grid_id; - current_point.grid_size = curr_grid_size; current_point.radius = radius; - current_point.theta = theta; - current_point.radial_div = radial_div; current_point.point_state = PointLabel::INIT; - current_point.orig_index = i; - current_point.orig_point = &in_cloud->points[i]; + current_point.orig_index = point_index; // radial divisions out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; } // sort by distance for (size_t i = 0; i < radial_dividers_num_; ++i) { std::sort( out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointRef & a, const PointRef & b) { return a.radius < b.radius; }); + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); } } + void ScanGroundFilterComponent::convertPointcloud( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { out_radial_ordered_points.resize(radial_dividers_num_); - PointRef current_point; + PointData current_point; + + const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; + + const size_t in_cloud_data_size = in_cloud->data.size(); + const size_t in_cloud_point_step = in_cloud->point_step; - for (size_t i = 0; i < in_cloud->points.size(); ++i) { - auto radius{static_cast(std::hypot(in_cloud->points[i].x, in_cloud->points[i].y))}; - auto theta{normalizeRadian(std::atan2(in_cloud->points[i].x, in_cloud->points[i].y), 0.0)}; - auto radial_div{ - static_cast(std::floor(normalizeDegree(theta / radial_divider_angle_rad_, 0.0)))}; + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + // Point + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto radius{static_cast(std::hypot(input_point.x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(input_point.x, input_point.y), 0.0)}; + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; current_point.radius = radius; - current_point.theta = theta; - current_point.radial_div = radial_div; current_point.point_state = PointLabel::INIT; - current_point.orig_index = i; - current_point.orig_point = &in_cloud->points[i]; + current_point.orig_index = point_index; // radial divisions out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; } - // sort by distance for (size_t i = 0; i < radial_dividers_num_; ++i) { std::sort( out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointRef & a, const PointRef & b) { return a.radius < b.radius; }); + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); } } @@ -173,6 +208,21 @@ void ScanGroundFilterComponent::calcVirtualGroundOrigin(pcl::PointXYZ & point) point.z = 0; } +inline float ScanGroundFilterComponent::calcGridSize(const PointData & p) +{ + float grid_size = grid_size_m_; + uint16_t back_steps_num = 1; + + if ( + p.radius > grid_mode_switch_radius_ && p.grid_id > grid_mode_switch_grid_id_ + back_steps_num) { + // equivalent to grid_size = (std::tan(gamma) - std::tan(gamma - grid_size_rad_)) * + // virtual_lidar_z_ when gamma = normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f) + grid_size = p.radius - (p.radius - tan_grid_size_rad_ * virtual_lidar_z_) / + (1 + p.radius * tan_grid_size_rad_ / virtual_lidar_z_); + } + return grid_size; +} + void ScanGroundFilterComponent::initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids) { @@ -193,96 +243,94 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( } void ScanGroundFilterComponent::checkContinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { float next_gnd_z = 0.0f; - float curr_gnd_slope_rad = 0.0f; + float curr_gnd_slope_ratio = 0.0f; float gnd_buff_z_mean = 0.0f; - float gnd_buff_z_max = 0.0f; float gnd_buff_radius = 0.0f; for (auto it = gnd_grids_list.end() - gnd_grid_buffer_size_ - 1; it < gnd_grids_list.end() - 1; ++it) { gnd_buff_radius += it->radius; gnd_buff_z_mean += it->avg_height; - gnd_buff_z_max += it->max_height; } gnd_buff_radius /= static_cast(gnd_grid_buffer_size_ - 1); - gnd_buff_z_max /= static_cast(gnd_grid_buffer_size_ - 1); gnd_buff_z_mean /= static_cast(gnd_grid_buffer_size_ - 1); float tmp_delta_mean_z = gnd_grids_list.back().avg_height - gnd_buff_z_mean; float tmp_delta_radius = gnd_grids_list.back().radius - gnd_buff_radius; - curr_gnd_slope_rad = std::atan(tmp_delta_mean_z / tmp_delta_radius); - curr_gnd_slope_rad = curr_gnd_slope_rad < -global_slope_max_angle_rad_ - ? -global_slope_max_angle_rad_ - : curr_gnd_slope_rad; - curr_gnd_slope_rad = curr_gnd_slope_rad > global_slope_max_angle_rad_ - ? global_slope_max_angle_rad_ - : curr_gnd_slope_rad; + curr_gnd_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; + curr_gnd_slope_ratio = curr_gnd_slope_ratio < -global_slope_max_ratio_ ? -global_slope_max_ratio_ + : curr_gnd_slope_ratio; + curr_gnd_slope_ratio = + curr_gnd_slope_ratio > global_slope_max_ratio_ ? global_slope_max_ratio_ : curr_gnd_slope_ratio; - next_gnd_z = std::tan(curr_gnd_slope_rad) * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; + next_gnd_z = curr_gnd_slope_ratio * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; float gnd_z_local_thresh = std::tan(DEG2RAD(5.0)) * (p.radius - gnd_grids_list.back().radius); - tmp_delta_mean_z = p.orig_point->z - (gnd_grids_list.end() - 2)->avg_height; + tmp_delta_mean_z = p_orig_point.z - (gnd_grids_list.end() - 2)->avg_height; tmp_delta_radius = p.radius - (gnd_grids_list.end() - 2)->radius; - float local_slope = std::atan(tmp_delta_mean_z / tmp_delta_radius); + float local_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; if ( - abs(p.orig_point->z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || - abs(local_slope) <= local_slope_max_angle_rad_) { + abs(p_orig_point.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || + abs(local_slope_ratio) <= local_slope_max_ratio_) { p.point_state = PointLabel::GROUND; - } else if (p.orig_point->z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { + } else if (p_orig_point.z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { p.point_state = PointLabel::NON_GROUND; } } + void ScanGroundFilterComponent::checkDiscontinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { - float tmp_delta_max_z = p.orig_point->z - gnd_grids_list.back().max_height; - float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; + float tmp_delta_max_z = p_orig_point.z - gnd_grids_list.back().max_height; + float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; - float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius); + float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; if ( - abs(local_slope) < local_slope_max_angle_rad_ || + abs(local_slope_ratio) < local_slope_max_ratio_ || abs(tmp_delta_avg_z) < non_ground_height_threshold_ || abs(tmp_delta_max_z) < non_ground_height_threshold_) { p.point_state = PointLabel::GROUND; - } else if (local_slope > global_slope_max_angle_rad_) { + } else if (local_slope_ratio > global_slope_max_ratio_) { p.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::checkBreakGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { - float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; + float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; - float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius); - if (abs(local_slope) < global_slope_max_angle_rad_) { + float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; + if (abs(local_slope_ratio) < global_slope_max_ratio_) { p.point_state = PointLabel::GROUND; - } else if (local_slope > global_slope_max_angle_rad_) { + } else if (local_slope_ratio > global_slope_max_ratio_) { p.point_state = PointLabel::NON_GROUND; } } + void ScanGroundFilterComponent::recheckGroundCluster( PointsCentroid & gnd_cluster, const float non_ground_threshold, pcl::PointIndices & non_ground_indices) { const float min_gnd_height = gnd_cluster.getMinHeight(); - pcl::PointIndices gnd_indices = gnd_cluster.getIndices(); - std::vector height_list = gnd_cluster.getHeightList(); + const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef(); + const std::vector & height_list = gnd_cluster.getHeightListRef(); for (size_t i = 0; i < height_list.size(); ++i) { if (height_list.at(i) >= min_gnd_height + non_ground_threshold) { non_ground_indices.indices.push_back(gnd_indices.indices.at(i)); } } } + void ScanGroundFilterComponent::classifyPointCloudGridScan( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); @@ -299,42 +347,41 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( // check the first point in ray auto * p = &in_radial_ordered_clouds[i][0]; - PointRef * prev_p; - prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point + auto * prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point bool initialized_first_gnd_grid = false; bool prev_list_init = false; - - for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { - p = &in_radial_ordered_clouds[i][j]; - float global_slope_p = std::atan(p->orig_point->z / p->radius); + pcl::PointXYZ p_orig_point, prev_p_orig_point; + for (auto & point : in_radial_ordered_clouds[i]) { + prev_p = p; + prev_p_orig_point = p_orig_point; + p = &point; + get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); + float global_slope_ratio_p = p_orig_point.z / p->radius; float non_ground_height_threshold_local = non_ground_height_threshold_; - if (p->orig_point->x < low_priority_region_x_) { + if (p_orig_point.x < low_priority_region_x_) { non_ground_height_threshold_local = - non_ground_height_threshold_ * abs(p->orig_point->x / low_priority_region_x_); + non_ground_height_threshold_ * abs(p_orig_point.x / low_priority_region_x_); } // classify first grid's point cloud if ( - !initialized_first_gnd_grid && global_slope_p >= global_slope_max_angle_rad_ && - p->orig_point->z > non_ground_height_threshold_local) { + !initialized_first_gnd_grid && global_slope_ratio_p >= global_slope_max_ratio_ && + p_orig_point.z > non_ground_height_threshold_local) { out_no_ground_indices.indices.push_back(p->orig_index); p->point_state = PointLabel::NON_GROUND; - prev_p = p; continue; } if ( - !initialized_first_gnd_grid && abs(global_slope_p) < global_slope_max_angle_rad_ && - abs(p->orig_point->z) < non_ground_height_threshold_local) { - ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); + !initialized_first_gnd_grid && abs(global_slope_ratio_p) < global_slope_max_ratio_ && + abs(p_orig_point.z) < non_ground_height_threshold_local) { + ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); p->point_state = PointLabel::GROUND; initialized_first_gnd_grid = static_cast(p->grid_id - prev_p->grid_id); - prev_p = p; continue; } if (!initialized_first_gnd_grid) { - prev_p = p; continue; } @@ -366,53 +413,50 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( ground_cluster.initialize(); } // classify - if (p->orig_point->z - gnd_grids.back().avg_height > detection_range_z_max_) { + if (p_orig_point.z - gnd_grids.back().avg_height > detection_range_z_max_) { p->point_state = PointLabel::OUT_OF_RANGE; - prev_p = p; continue; } - float points_xy_distance = std::hypot( - p->orig_point->x - prev_p->orig_point->x, p->orig_point->y - prev_p->orig_point->y); + float points_xy_distance_square = + (p_orig_point.x - prev_p_orig_point.x) * (p_orig_point.x - prev_p_orig_point.x) + + (p_orig_point.y - prev_p_orig_point.y) * (p_orig_point.y - prev_p_orig_point.y); if ( prev_p->point_state == PointLabel::NON_GROUND && - points_xy_distance < split_points_distance_tolerance_ && - p->orig_point->z > prev_p->orig_point->z) { + points_xy_distance_square < split_points_distance_tolerance_square_ && + p_orig_point.z > prev_p_orig_point.z) { p->point_state = PointLabel::NON_GROUND; out_no_ground_indices.indices.push_back(p->orig_index); - prev_p = p; continue; } - - if (global_slope_p > global_slope_max_angle_rad_) { + if (global_slope_ratio_p > global_slope_max_ratio_) { out_no_ground_indices.indices.push_back(p->orig_index); - prev_p = p; continue; } // gnd grid is continuous, the last gnd grid is close uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id + gnd_grid_buffer_size_ + gnd_grid_continual_thresh_; + float curr_grid_size = calcGridSize(*p); if ( p->grid_id < next_gnd_grid_id_thresh && - p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { - checkContinuousGndGrid(*p, gnd_grids); - - } else if (p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { - checkDiscontinuousGndGrid(*p, gnd_grids); + p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkContinuousGndGrid(*p, p_orig_point, gnd_grids); + } else if ( + p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkDiscontinuousGndGrid(*p, p_orig_point, gnd_grids); } else { - checkBreakGndGrid(*p, gnd_grids); + checkBreakGndGrid(*p, p_orig_point, gnd_grids); } if (p->point_state == PointLabel::NON_GROUND) { out_no_ground_indices.indices.push_back(p->orig_index); } else if (p->point_state == PointLabel::GROUND) { - ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); + ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); } - prev_p = p; } } } void ScanGroundFilterComponent::classifyPointCloud( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); @@ -430,16 +474,15 @@ void ScanGroundFilterComponent::classifyPointCloud( PointsCentroid ground_cluster, non_ground_cluster; float local_slope = 0.0f; PointLabel prev_point_label = PointLabel::INIT; - pcl::PointXYZ prev_gnd_point(0, 0, 0); + pcl::PointXYZ prev_gnd_point(0, 0, 0), p_orig_point, prev_p_orig_point; // loop through each point in the radial div for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { - const float global_slope_max_angle = global_slope_max_angle_rad_; const float local_slope_max_angle = local_slope_max_angle_rad_; + prev_p_orig_point = p_orig_point; auto * p = &in_radial_ordered_clouds[i][j]; - auto * p_prev = &in_radial_ordered_clouds[i][j - 1]; - + get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); if (j == 0) { - bool is_front_side = (p->orig_point->x > virtual_ground_point.x); + bool is_front_side = (p_orig_point.x > virtual_ground_point.x); if (use_virtual_ground_point_ && is_front_side) { prev_gnd_point = virtual_ground_point; } else { @@ -449,22 +492,22 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_gnd_slope = 0.0f; ground_cluster.initialize(); non_ground_cluster.initialize(); - points_distance = calcDistance3d(*p->orig_point, prev_gnd_point); + points_distance = calcDistance3d(p_orig_point, prev_gnd_point); } else { - points_distance = calcDistance3d(*p->orig_point, *p_prev->orig_point); + points_distance = calcDistance3d(p_orig_point, prev_p_orig_point); } float radius_distance_from_gnd = p->radius - prev_gnd_radius; - float height_from_gnd = p->orig_point->z - prev_gnd_point.z; - float height_from_obj = p->orig_point->z - non_ground_cluster.getAverageHeight(); + float height_from_gnd = p_orig_point.z - prev_gnd_point.z; + float height_from_obj = p_orig_point.z - non_ground_cluster.getAverageHeight(); bool calculate_slope = false; bool is_point_close_to_prev = (points_distance < (p->radius * radial_divider_angle_rad_ + split_points_distance_tolerance_)); - float global_slope = std::atan2(p->orig_point->z, p->radius); + float global_slope_ratio = p_orig_point.z / p->radius; // check points which is far enough from previous point - if (global_slope > global_slope_max_angle) { + if (global_slope_ratio > global_slope_max_ratio_) { p->point_state = PointLabel::NON_GROUND; calculate_slope = false; } else if ( @@ -479,7 +522,7 @@ void ScanGroundFilterComponent::classifyPointCloud( calculate_slope = true; } if (is_point_close_to_prev) { - height_from_gnd = p->orig_point->z - ground_cluster.getAverageHeight(); + height_from_gnd = p_orig_point.z - ground_cluster.getAverageHeight(); radius_distance_from_gnd = p->radius - ground_cluster.getAverageRadius(); } if (calculate_slope) { @@ -514,58 +557,66 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_point_label = p->point_state; if (p->point_state == PointLabel::GROUND) { prev_gnd_radius = p->radius; - prev_gnd_point = pcl::PointXYZ(p->orig_point->x, p->orig_point->y, p->orig_point->z); - ground_cluster.addPoint(p->radius, p->orig_point->z); + prev_gnd_point = pcl::PointXYZ(p_orig_point.x, p_orig_point.y, p_orig_point.z); + ground_cluster.addPoint(p->radius, p_orig_point.z); prev_gnd_slope = ground_cluster.getAverageSlope(); } // update the non ground state if (p->point_state == PointLabel::NON_GROUND) { - non_ground_cluster.addPoint(p->radius, p->orig_point->z); + non_ground_cluster.addPoint(p->radius, p_orig_point.z); } } } } void ScanGroundFilterComponent::extractObjectPoints( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr) + const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, + PointCloud2 & out_object_cloud) { + size_t output_data_size = 0; for (const auto & i : in_indices.indices) { - out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]); + std::memcpy( + &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step], + in_cloud_ptr->point_step * sizeof(uint8_t)); + *reinterpret_cast(&out_object_cloud.data[output_data_size + intensity_offset_]) = + 1; // set intensity to 1 + output_data_size += in_cloud_ptr->point_step; } } -void ScanGroundFilterComponent::filter( +void ScanGroundFilterComponent::faster_filter( const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + PointCloud2 & output, + [[maybe_unused]] const pointcloud_preprocessor::TransformInfo & transform_info) { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr current_sensor_cloud_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*input, *current_sensor_cloud_ptr); - std::vector radial_ordered_points; + if (!offset_initialized_) { + set_field_offsets(input); + } + std::vector radial_ordered_points; pcl::PointIndices no_ground_indices; - pcl::PointCloud::Ptr no_ground_cloud_ptr(new pcl::PointCloud); - no_ground_cloud_ptr->points.reserve(current_sensor_cloud_ptr->points.size()); if (elevation_grid_mode_) { - convertPointcloudGridScan(current_sensor_cloud_ptr, radial_ordered_points); - classifyPointCloudGridScan(radial_ordered_points, no_ground_indices); + convertPointcloudGridScan(input, radial_ordered_points); + classifyPointCloudGridScan(input, radial_ordered_points, no_ground_indices); } else { - convertPointcloud(current_sensor_cloud_ptr, radial_ordered_points); - classifyPointCloud(radial_ordered_points, no_ground_indices); + convertPointcloud(input, radial_ordered_points); + classifyPointCloud(input, radial_ordered_points, no_ground_indices); } - - extractObjectPoints(current_sensor_cloud_ptr, no_ground_indices, no_ground_cloud_ptr); - - auto no_ground_cloud_msg_ptr = std::make_shared(); - pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr); - - no_ground_cloud_msg_ptr->header = input->header; - output = *no_ground_cloud_msg_ptr; - + output.row_step = no_ground_indices.indices.size() * input->point_step; + output.data.resize(output.row_step); + output.width = no_ground_indices.indices.size(); + output.fields.assign(input->fields.begin(), input->fields.begin() + 3); + output.is_dense = true; + output.height = input->height; + output.is_bigendian = input->is_bigendian; + output.point_step = input->point_step; + output.header = input->header; + + extractObjectPoints(input, no_ground_indices, output); if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); @@ -576,20 +627,62 @@ void ScanGroundFilterComponent::filter( } } +// TODO(taisa1): Temporary Implementation: Delete this function definition when all the filter +// nodes conform to new API. +void ScanGroundFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + (void)input; + (void)indices; + (void)output; +} + rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( const std::vector & p) { + if (get_param(p, "grid_size_m", grid_size_m_)) { + grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_m to: %f.", grid_size_m_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + } + if (get_param(p, "grid_mode_switch_radius", grid_mode_switch_radius_)) { + grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; + grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_mode_switch_radius to: %f.", grid_mode_switch_radius_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_angle_rad to: %f.", grid_mode_switch_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + } double global_slope_max_angle_deg{get_parameter("global_slope_max_angle_deg").as_double()}; if (get_param(p, "global_slope_max_angle_deg", global_slope_max_angle_deg)) { global_slope_max_angle_rad_ = deg2rad(global_slope_max_angle_deg); + global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting global_slope_max_angle_rad to: %f.", global_slope_max_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting global_slope_max_ratio to: %f.", global_slope_max_ratio_); } double local_slope_max_angle_deg{get_parameter("local_slope_max_angle_deg").as_double()}; if (get_param(p, "local_slope_max_angle_deg", local_slope_max_angle_deg)) { local_slope_max_angle_rad_ = deg2rad(local_slope_max_angle_deg); + local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting local_slope_max_angle_rad to: %f.", local_slope_max_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting local_slope_max_ratio to: %f.", local_slope_max_ratio_); } double radial_divider_angle_deg{get_parameter("radial_divider_angle_deg").as_double()}; if (get_param(p, "radial_divider_angle_deg", radial_divider_angle_deg)) { @@ -600,9 +693,14 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( RCLCPP_DEBUG(get_logger(), "Setting radial_dividers_num to: %zu.", radial_dividers_num_); } if (get_param(p, "split_points_distance_tolerance", split_points_distance_tolerance_)) { + split_points_distance_tolerance_square_ = + split_points_distance_tolerance_ * split_points_distance_tolerance_; RCLCPP_DEBUG( get_logger(), "Setting split_points_distance_tolerance to: %f.", split_points_distance_tolerance_); + RCLCPP_DEBUG( + get_logger(), "Setting split_points_distance_tolerance_square to: %f.", + split_points_distance_tolerance_square_); } if (get_param(p, "split_height_distance", split_height_distance_)) { RCLCPP_DEBUG(get_logger(), "Setting split_height_distance to: %f.", split_height_distance_); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index 9df9c7e9c7433..444a38ea44754 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -131,7 +131,8 @@ class ScanGroundFilterTest : public ::testing::Test // wrapper function to test private function filter void filter(sensor_msgs::msg::PointCloud2 & out_cloud) { - scan_ground_filter_->filter(input_msg_ptr_, nullptr, out_cloud); + pointcloud_preprocessor::TransformInfo transform_info; + scan_ground_filter_->faster_filter(input_msg_ptr_, nullptr, out_cloud, transform_info); } void parse_yaml() diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index d5843be395adf..57a563c28a4a6 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -125,7 +125,7 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) // 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"}; + "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback; From 8bdb5422c682c1a2e9fa211e3c06be3a5f73ca55 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Mon, 11 Mar 2024 19:09:03 +0900 Subject: [PATCH 2/8] fix(lane_departure_checker): empty lanelet polygon (#6588) * fix union sometimes returning empty polygon Signed-off-by: Daniel Sanchez * fix union of polygons Signed-off-by: Daniel Sanchez * fix back launcher Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../lane_departure_checker.hpp | 2 +- .../lane_departure_checker.cpp | 44 +++++++++++-------- .../src/shift_pull_out.cpp | 2 +- 3 files changed, 27 insertions(+), 21 deletions(-) diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index be2411cd3268b..c658cf4497973 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -121,7 +121,7 @@ class LaneDepartureChecker std::vector> getLaneletsFromPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; - std::optional getFusedLaneletPolygonForPath( + std::optional getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; bool checkPathWillLeaveLane( diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 811e1652fcb4a..c48383a17ab4b 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -321,33 +321,39 @@ std::vector> LaneDepartureChecker::getLanele lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0); } -std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( +std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); + auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d { + tier4_autoware_utils::Polygon2d p; + auto & outer = p.outer(); + + for (const auto & p : poly) { + tier4_autoware_utils::Point2d p2d(p.x(), p.y()); + outer.push_back(p2d); + } + boost::geometry::correct(p); + return p; + }; + // Fuse lanelets into a single BasicPolygon2d - auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional { + auto fused_lanelets = [&]() -> std::optional { if (lanelets_distance_pair.empty()) return std::nullopt; - if (lanelets_distance_pair.size() == 1) - return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); + tier4_autoware_utils::MultiPolygon2d lanelet_unions; + tier4_autoware_utils::MultiPolygon2d result; - lanelet::BasicPolygon2d merged_polygon = - lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); - for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) { + for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) { const auto & route_lanelet = lanelets_distance_pair.at(i).second; - const auto & poly = route_lanelet.polygon2d().basicPolygon(); - - std::vector lanelet_union_temp; - boost::geometry::union_(poly, merged_polygon, lanelet_union_temp); - - // Update merged_polygon by accumulating all merged results - merged_polygon.clear(); - for (const auto & temp_poly : lanelet_union_temp) { - merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end()); - } + const auto & p = route_lanelet.polygon2d().basicPolygon(); + tier4_autoware_utils::Polygon2d poly = to_polygon2d(p); + boost::geometry::union_(lanelet_unions, poly, result); + lanelet_unions = result; + result.clear(); } - if (merged_polygon.empty()) return std::nullopt; - return merged_polygon; + + if (lanelet_unions.empty()) return std::nullopt; + return lanelet_unions.front(); }(); return fused_lanelets; diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index f5674cfb288d0..274fe19e04618 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -110,7 +110,7 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes( lanelet_map_ptr, shift_path, start_segment_idx); - + if (cropped_path.points.empty()) continue; shift_path.points = cropped_path.points; shift_path.header = planner_data_->route_handler->getRouteHeader(); From b404c8f6f7c132842698d25be0f2433941c41c75 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Mon, 11 Mar 2024 22:35:27 +0900 Subject: [PATCH 3/8] fix(mrm_handler): fix bug in operation mode availability timeout (#6513) * fix operation mode availability timeout Signed-off-by: veqcc --- .../include/mrm_handler/mrm_handler_core.hpp | 2 + .../src/mrm_handler/mrm_handler_core.cpp | 43 +++++++++++++------ 2 files changed, 33 insertions(+), 12 deletions(-) diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index b4a00f0de7af5..1dd0f6b081337 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -144,6 +144,8 @@ class MrmHandler : public rclcpp::Node // Heartbeat rclcpp::Time stamp_operation_mode_availability_; std::optional stamp_autonomous_become_unavailable_ = std::nullopt; + bool is_operation_mode_availability_timeout; + void checkOperationModeAvailabilityTimeout(); // Algorithm bool is_emergency_holding_ = false; diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index fccc0cf4f3031..e8e692f755e2d 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -94,6 +94,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler") mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; + is_operation_mode_availability_timeout = false; // Timer const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); @@ -376,24 +377,29 @@ bool MrmHandler::isDataReady() return true; } -void MrmHandler::onTimer() +void MrmHandler::checkOperationModeAvailabilityTimeout() { - if (!isDataReady()) { - return; - } - const bool is_operation_mode_availability_timeout = + if ( (this->now() - stamp_operation_mode_availability_).seconds() > - param_.timeout_operation_mode_availability; - if (is_operation_mode_availability_timeout) { + param_.timeout_operation_mode_availability) { + is_operation_mode_availability_timeout = true; RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "heartbeat operation_mode_availability is timeout"); - mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING; - publishHazardCmd(); - publishGearCmd(); + "operation_mode_availability is timeout"); + } else { + is_operation_mode_availability_timeout = false; + } +} + +void MrmHandler::onTimer() +{ + if (!isDataReady()) { return; } + // Check whether operation_mode_availability is timeout + checkOperationModeAvailabilityTimeout(); + // Update Emergency State updateMrmState(); @@ -495,6 +501,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB // State machine if (mrm_state_.behavior == MrmState::NONE) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -511,6 +520,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (mrm_state_.behavior == MrmState::PULL_OVER) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -527,6 +539,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (isStopped() && operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -543,6 +558,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (mrm_state_.behavior == MrmState::EMERGENCY_STOP) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (isStopped() && operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -569,7 +587,8 @@ bool MrmHandler::isStopped() bool MrmHandler::isEmergency() const { - return !operation_mode_availability_->autonomous || is_emergency_holding_; + return !operation_mode_availability_->autonomous || is_emergency_holding_ || + is_operation_mode_availability_timeout; } bool MrmHandler::isArrivedAtGoal() From 5adc394e04b1a74d0ed40651400412f728baf094 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Mon, 11 Mar 2024 17:24:21 +0100 Subject: [PATCH 4/8] build(perception_online_evaluator): add lanelet_extension dependency (#6592) Signed-off-by: Esteve Fernandez --- evaluator/perception_online_evaluator/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index 54281358219d1..18dde41c99566 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -21,6 +21,7 @@ diagnostic_msgs eigen geometry_msgs + lanelet2_extension libgoogle-glog-dev motion_utils nav_msgs From c1b8f46af472e73cbefcd80a8ef048d576c12d98 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 12 Mar 2024 09:53:17 +0900 Subject: [PATCH 5/8] fix(avoidance): ignore objects are in intersection when the ego stops by red traffic signal (#6565) fix(avoidance): don't avoid objects are farther than red signal traffic light Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/scene.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index eed1f324727fb..384fd4a0d2fa3 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -321,10 +321,19 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::getTargetLanelets; using utils::avoidance::separateObjectsByPath; using utils::avoidance::updateRoadShoulderDistance; + using utils::traffic_light::calcDistanceToRedTrafficLight; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. constexpr double MARGIN = 10.0; - const auto forward_detection_range = helper_->getForwardDetectionRange(); + const auto forward_detection_range = [&]() { + const auto to_traffic_light = calcDistanceToRedTrafficLight( + data.current_lanelets, helper_->getPreviousReferencePath(), planner_data_); + if (!to_traffic_light.has_value()) { + return helper_->getForwardDetectionRange(); + } + return std::min(helper_->getForwardDetectionRange(), to_traffic_light.value()); + }(); + const auto [object_within_target_lane, object_outside_target_lane] = separateObjectsByPath( helper_->getPreviousReferencePath(), helper_->getPreviousSplineShiftPath().path, planner_data_, data, parameters_, forward_detection_range + MARGIN, debug); From 94bbe3a09b85e5bc329ee2d278a3893fd1797b8b Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 12 Mar 2024 10:38:56 +0900 Subject: [PATCH 6/8] fix(perception_online_evaluator): fix build error (#6595) Signed-off-by: satoshi-ota --- .../test/test_perception_online_evaluator_node.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 1bfd27828c98e..16c46d0eecd92 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -79,7 +79,9 @@ class EvalTest : public ::testing::Test marker_sub_ = rclcpp::create_subscription( eval_node, "perception_online_evaluator/markers", 10, - [this]([[maybe_unused]] const MarkerArray::SharedPtr msg) { has_received_marker_ = true; }); + [this]([[maybe_unused]] const MarkerArray::ConstSharedPtr msg) { + has_received_marker_ = true; + }); uuid_ = generateUUID(); } From 1de37a3bd6016312b5284149d2a2e5adb10a2412 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 12 Mar 2024 11:00:27 +0900 Subject: [PATCH 7/8] feat(map_based_prediction, etc): publish processing time in various perception nodes (#6574) * feat: fix processing time publisher Signed-off-by: yoshiri * feat: add processing_time in detection_by_tracker Signed-off-by: yoshiri * feat: add processing time publisher to shape estimator Signed-off-by: yoshiri * feat: add latency time for association merger Signed-off-by: yoshiri * feat: add processing time publisher for tracker merger Signed-off-by: yoshiri * chore: rename pipeline latency in multi object tracker Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../include/detection_by_tracker/debugger.hpp | 23 +++++++++++++++++ .../src/detection_by_tracker_core.cpp | 2 ++ perception/map_based_prediction/README.md | 12 +++++---- .../map_based_prediction_node.hpp | 10 ++++---- .../src/map_based_prediction_node.cpp | 25 ++++++++++--------- .../src/multi_object_tracker_core.cpp | 2 +- .../include/object_merger/node.hpp | 6 +++++ .../src/object_association_merger/node.cpp | 13 ++++++++++ perception/shape_estimation/src/node.cpp | 12 +++++++++ perception/shape_estimation/src/node.hpp | 6 +++++ .../decorative_tracker_merger.hpp | 4 +++ .../src/decorative_tracker_merger.cpp | 12 +++++++++ 12 files changed, 104 insertions(+), 23 deletions(-) diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp index fb7642356e8d9..0acba3d2dd58d 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -61,6 +63,11 @@ class Debugger divided_objects_pub_ = node->create_publisher( "debug/divided_objects", 1); + processing_time_publisher_ = + std::make_unique(node, "detection_by_tracker"); + stop_watch_ptr_ = + std::make_unique>(); + this->startStopWatch(); } ~Debugger() {} @@ -80,6 +87,19 @@ class Debugger { divided_objects_pub_->publish(removeFeature(input)); } + void startStopWatch() + { + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + void startMeasureProcessingTime() { stop_watch_ptr_->tic("processing_time"); } + void publishProcessingTime() + { + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); + } private: rclcpp::Publisher::SharedPtr @@ -90,6 +110,9 @@ class Debugger merged_objects_pub_; rclcpp::Publisher::SharedPtr divided_objects_pub_; + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; autoware_auto_perception_msgs::msg::DetectedObjects removeFeature( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 2595315e1f9b2..7afc3e41e4683 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -206,6 +206,7 @@ void DetectionByTracker::setMaxSearchRange() void DetectionByTracker::onObjects( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg) { + debugger_->startMeasureProcessingTime(); autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; detected_objects.header = input_msg->header; @@ -250,6 +251,7 @@ void DetectionByTracker::onObjects( } objects_pub_->publish(detected_objects); + debugger_->publishProcessingTime(); } void DetectionByTracker::divideUnderSegmentedObjects( diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index a58e19db70304..7989a5fbf6af9 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -218,11 +218,13 @@ If the target object is inside the road or crosswalk, this module outputs one or ### Output -| Name | Type | Description | -| ------------------------ | ------------------------------------------------------ | ------------------------------------------------------------------------------------- | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | -| `~/output/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | -| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | +| Name | Type | Description | +| ---------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------------------------- | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | +| `~/output/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | +| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | +| `~/debug/processing_time_ms` | `std_msgs::msg::Float64` | processing time of this module. | +| `~/debug/cyclic_time_ms` | `std_msgs::msg::Float64` | cyclic time of this module. | ## Parameters diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 321342c3d8367..26dee3eacb08d 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -21,6 +21,7 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include +#include #include #include #include @@ -33,7 +34,6 @@ #include #include #include -#include #include #include @@ -125,11 +125,14 @@ class MapBasedPredictionNode : public rclcpp::Node // ROS Publisher and Subscriber rclcpp::Publisher::SharedPtr pub_objects_; rclcpp::Publisher::SharedPtr pub_debug_markers_; - rclcpp::Publisher::SharedPtr pub_calculation_time_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_traffic_signals_; + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + // Object History std::unordered_map> objects_history_; std::map, rclcpp::Time> stopped_times_against_green_; @@ -196,9 +199,6 @@ class MapBasedPredictionNode : public rclcpp::Node std::vector distance_set_for_no_intention_to_walk_; std::vector timeout_set_for_no_intention_to_walk_; - // Stop watch - StopWatch stop_watch_; - // Member Functions void mapCallback(const HADMapBin::ConstSharedPtr msg); void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg); diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 21697bb1b5f1a..549619950cf82 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -646,14 +646,6 @@ ObjectClassification::_label_type changeLabelForPrediction( } } -StringStamped createStringStamped(const rclcpp::Time & now, const double data) -{ - StringStamped msg; - msg.stamp = now; - msg.data = std::to_string(data); - return msg; -} - // NOTE: These two functions are copied from the route_handler package. lanelet::Lanelets getRightOppositeLanelets( const std::shared_ptr & lanelet_map_ptr, @@ -816,10 +808,15 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); - pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); + processing_time_publisher_ = + std::make_unique(this, "map_based_prediction"); set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); + + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( @@ -893,7 +890,7 @@ void MapBasedPredictionNode::trafficSignalsCallback(const TrafficSignalArray::Co void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { - stop_watch_.tic(); + stop_watch_ptr_->toc("processing_time", true); // Guard for map pointer and frame transformation if (!lanelet_map_ptr_) { return; @@ -1116,8 +1113,12 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Publish Results pub_objects_->publish(output); pub_debug_markers_->publish(debug_markers); - const auto calculation_time_msg = createStringStamped(now(), stop_watch_.toc()); - pub_calculation_time_->publish(calculation_time_msg); + const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + processing_time_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + processing_time_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); } bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 1d4d50c7eab4c..d955745bb8c47 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -148,7 +148,7 @@ void TrackerDebugger::publishProcessingTime() processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); processing_time_publisher_->publish( - "debug/elapsed_time_from_sensor_input_ms", elapsed_time_from_sensor_input_ * 1e3); + "debug/pipeline_latency_ms", elapsed_time_from_sensor_input_ * 1e3); } } diff --git a/perception/object_merger/include/object_merger/node.hpp b/perception/object_merger/include/object_merger/node.hpp index 8341ce490ca72..ba89a04553476 100644 --- a/perception/object_merger/include/object_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -18,6 +18,8 @@ #include "object_merger/data_association/data_association.hpp" #include +#include +#include #include @@ -81,6 +83,10 @@ class ObjectAssociationMergerNode : public rclcpp::Node std::map generalized_iou_threshold; std::map distance_threshold_map; } overlapped_judge_param_; + + // debug publisher + std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; }; } // namespace object_association diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 1ffc2791e8f4e..15b1939911394 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -118,6 +118,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio merged_object_pub_ = create_publisher( "output/object", rclcpp::QoS{1}); + + // Debug publisher + processing_time_publisher_ = + std::make_unique(this, "object_association_merger"); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } void ObjectAssociationMergerNode::objectsCallback( @@ -128,6 +135,7 @@ void ObjectAssociationMergerNode::objectsCallback( if (merged_object_pub_->get_subscription_count() < 1) { return; } + stop_watch_ptr_->toc("processing_time", true); /* transform to base_link coordinate */ autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects0, transformed_objects1; @@ -215,6 +223,11 @@ void ObjectAssociationMergerNode::objectsCallback( // publish output msg merged_object_pub_->publish(output_msg); + // publish processing time + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } } // namespace object_association diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 624ca604d8fbf..22f31fffec21a 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -50,10 +50,17 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer); estimator_ = std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); + + processing_time_publisher_ = + std::make_unique(this, "shape_estimation"); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg) { + stop_watch_ptr_->toc("processing_time", true); // Guard if (pub_->get_subscription_count() < 1) { return; @@ -108,6 +115,11 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared // Publish pub_->publish(output_msg); + + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } #include diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp index 95ee4afe8d9e8..87df44f08f836 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/node.hpp @@ -18,6 +18,8 @@ #include "shape_estimation/shape_estimator.hpp" #include +#include +#include #include #include @@ -33,6 +35,10 @@ class ShapeEstimationNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_; rclcpp::Subscription::SharedPtr sub_; + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + void callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg); std::unique_ptr estimator_; diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp index 0509fb2a07dc5..56843f4c5e1e0 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -20,6 +20,8 @@ #include "tracking_object_merger/utils/utils.hpp" #include +#include +#include #include #include @@ -87,6 +89,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_object_pub_; bool publish_interpolated_sub_objects_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; /* handle objects */ std::unordered_map> diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index b22b8d5fa2948..af0de2a02bbe7 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -151,6 +151,13 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio set3dDataAssociation("lidar-radar", data_association_map_); // radar-radar association matrix set3dDataAssociation("radar-radar", data_association_map_); + + // debug publisher + processing_time_publisher_ = + std::make_unique(this, "decorative_object_merger_node"); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } void DecorativeTrackerMergerNode::set3dDataAssociation( @@ -182,6 +189,7 @@ void DecorativeTrackerMergerNode::set3dDataAssociation( void DecorativeTrackerMergerNode::mainObjectsCallback( const TrackedObjects::ConstSharedPtr & main_objects) { + stop_watch_ptr_->toc("processing_time", true); // try to merge sub object if (!sub_objects_buffer_.empty()) { // get interpolated sub objects @@ -214,6 +222,10 @@ void DecorativeTrackerMergerNode::mainObjectsCallback( this->decorativeMerger(main_sensor_type_, main_objects); merged_object_pub_->publish(getTrackedObjects(main_objects->header)); + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } /** From 70bdf7f558d22b7f946c1a3ec1d230c03d3e5fac Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 12 Mar 2024 12:36:57 +0900 Subject: [PATCH 8/8] fix(intersection_occlusion): estimate dynamic occlusion source with triangle (#6585) Signed-off-by: Mamoru Sobue --- .../src/debug.cpp | 16 ++++++++ .../src/scene_intersection.hpp | 4 ++ .../src/scene_intersection_occlusion.cpp | 38 +++++++++++++++---- 3 files changed, 51 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index b323cdf4cb5da..bd2ad1935406b 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -389,6 +389,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + if (debug_data_.nearest_occlusion_triangle) { + const auto [p1, p2, p3] = debug_data_.nearest_occlusion_triangle.value(); + const auto color = debug_data_.static_occlusion ? green : red; + geometry_msgs::msg::Polygon poly; + poly.points.push_back( + geometry_msgs::build().x(p1.x).y(p1.y).z(p1.z)); + poly.points.push_back( + geometry_msgs::build().x(p2.x).y(p2.y).z(p2.z)); + poly.points.push_back( + geometry_msgs::build().x(p3.x).y(p3.y).z(p3.z)); + appendMarkerArray( + debug::createPolygonMarkerArray( + poly, "nearest_occlusion_triangle", lane_id_, now, 0.3, 0.0, 0.0, std::get<0>(color), + std::get<1>(color), std::get<2>(color)), + &debug_marker_array, now); + } if (debug_data_.traffic_light_observation) { const auto GREEN = autoware_perception_msgs::msg::TrafficSignalElement::GREEN; const auto YELLOW = autoware_perception_msgs::msg::TrafficSignalElement::AMBER; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8917b4bac579b..fcb1ef3a1985a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -235,6 +235,10 @@ class IntersectionModule : public SceneModuleInterface std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; + std::optional< + std::tuple> + nearest_occlusion_triangle{std::nullopt}; + bool static_occlusion{false}; std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; std::optional> diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 00ac333adeff1..2a66aedd36ac7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -334,13 +334,14 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( } return nearest; }; - struct NearestOcclusionPoint + struct NearestOcclusionInterval { int64 division_index{0}; int64 point_index{0}; double dist{0.0}; geometry_msgs::msg::Point point; geometry_msgs::msg::Point projection; + geometry_msgs::msg::Point visible_end; } nearest_occlusion_point; double min_dist = std::numeric_limits::infinity(); for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { @@ -370,6 +371,8 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( continue; } double acc_dist = 0.0; + bool found_min_dist_for_this_division = false; + bool is_prev_occluded = false; auto acc_dist_it = projection_it; for (auto point_it = projection_it; point_it != division.end(); point_it++) { const double dist = @@ -386,11 +389,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( if (acc_dist < min_dist) { min_dist = acc_dist; nearest_occlusion_point = { - division_index, std::distance(division.begin(), point_it), acc_dist, + division_index, + std::distance(division.begin(), point_it), + acc_dist, tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z), + tier4_autoware_utils::createPoint( + projection_it->x(), projection_it->y(), + origin.z) /* initialize with projection point at first*/}; + found_min_dist_for_this_division = true; + } else if (found_min_dist_for_this_division && is_prev_occluded) { + // although this cell is not "nearest" cell, we have found the "nearest" cell on this + // division previously in this iteration, and the iterated cells are still OCCLUDED since + // then + nearest_occlusion_point.visible_end = + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z); } } + is_prev_occluded = (pixel == OCCLUDED); } } @@ -400,16 +416,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( debug_data_.nearest_occlusion_projection = std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); - LineString2d ego_occlusion_line; - ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); - ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + debug_data_.nearest_occlusion_triangle = std::make_tuple( + current_pose.position, nearest_occlusion_point.point, nearest_occlusion_point.visible_end); + Polygon2d ego_occlusion_triangle; + ego_occlusion_triangle.outer().emplace_back(current_pose.position.x, current_pose.position.y); + ego_occlusion_triangle.outer().emplace_back( + nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + ego_occlusion_triangle.outer().emplace_back( + nearest_occlusion_point.visible_end.x, nearest_occlusion_point.visible_end.y); + bg::correct(ego_occlusion_triangle); for (const auto & attention_object_info : object_info_manager_.allObjects()) { const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); - if (bg::intersects(obj_poly, ego_occlusion_line)) { + if (bg::intersects(obj_poly, ego_occlusion_triangle)) { + debug_data_.static_occlusion = false; return DynamicallyOccluded{min_dist}; } } + debug_data_.static_occlusion = true; return StaticallyOccluded{min_dist}; } } // namespace behavior_velocity_planner