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 5dcc02e4f0cc6..2d983dc47545c 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 @@ -5,23 +5,32 @@ + + + + + + + + + @@ -65,6 +74,7 @@ + @@ -82,21 +92,21 @@ - + - + - + - + - + - + - + - + @@ -183,21 +193,21 @@ - + - + - + - + - + - + - + - + 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 23f3bd0b532a0..3a24ec2fdf500 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,6 +4,7 @@ + @@ -59,6 +60,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 3efe7831b3c19..bfb30e860f070 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/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index bafacc01e6324..dc3e33faafb94 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 @@ -38,6 +38,7 @@ def __init__(self, context): 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"] self.timer_interval_ms = self.pointcloud_map_filter_param["timer_interval_ms"] self.use_dynamic_map_loading = self.pointcloud_map_filter_param["use_dynamic_map_loading"] self.map_update_distance_threshold = self.pointcloud_map_filter_param[ @@ -69,6 +70,7 @@ def create_normal_pipeline(self): 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, @@ -125,6 +127,7 @@ def create_down_sample_pipeline(self): 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, diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 896d1e41f5507..204fb561b10ea 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -33,23 +33,32 @@ + + + + + + + + + + diff --git a/perception/compare_map_segmentation/README.md b/perception/compare_map_segmentation/README.md index 67aceafef3a16..ad891b9ad6afa 100644 --- a/perception/compare_map_segmentation/README.md +++ b/perception/compare_map_segmentation/README.md @@ -76,14 +76,15 @@ This filter is a combination of the distance_based_compare_map_filter and voxel_ #### Parameters -| Name | Type | Description | Default value | -| :------------------------------ | :---- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `use_dynamic_map_loading` | bool | map loading mode selection, `true` for dynamic map loading, `false` for static map loading, recommended for no-split map pointcloud | true | -| `distance_threshold` | float | Threshold distance to compare input points with map points [m] | 0.5 | -| `map_update_distance_threshold` | float | Threshold of vehicle movement distance when map update is necessary (in dynamic map loading) [m] | 10.0 | -| `map_loader_radius` | float | Radius of map need to be loaded (in dynamic map loading) [m] | 150.0 | -| `timer_interval_ms` | int | Timer interval to check if the map update is necessary (in dynamic map loading) [ms] | 100 | -| `publish_debug_pcd` | bool | Enable to publish voxelized updated map in `debug/downsampled_map/pointcloud` for debugging. It might cause additional computation cost | false | +| Name | Type | Description | Default value | +| :------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `use_dynamic_map_loading` | bool | map loading mode selection, `true` for dynamic map loading, `false` for static map loading, recommended for no-split map pointcloud | true | +| `distance_threshold` | float | Threshold distance to compare input points with map points [m] | 0.5 | +| `map_update_distance_threshold` | float | Threshold of vehicle movement distance when map update is necessary (in dynamic map loading) [m] | 10.0 | +| `map_loader_radius` | float | Radius of map need to be loaded (in dynamic map loading) [m] | 150.0 | +| `timer_interval_ms` | int | Timer interval to check if the map update is necessary (in dynamic map loading) [ms] | 100 | +| `publish_debug_pcd` | bool | Enable to publish voxelized updated map in `debug/downsampled_map/pointcloud` for debugging. It might cause additional computation cost | false | +| `downsize_ratio_z_axis` | double | Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis | 0.5 | ## Assumptions / Known limits diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp index 8090ea2e4221a..301dc75839f8e 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp @@ -42,7 +42,7 @@ class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader public: DistanceBasedStaticMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) - : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + : VoxelGridStaticMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex) { RCLCPP_INFO(logger_, "DistanceBasedStaticMapLoader initialized.\n"); } @@ -57,7 +57,7 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader DistanceBasedDynamicMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) - : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + : VoxelGridDynamicMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex, main_callback_group) { RCLCPP_INFO(logger_, "DistanceBasedDynamicMapLoader initialized.\n"); } diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp index f05f8100873cd..3a5e183767620 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp @@ -32,8 +32,9 @@ class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader { public: explicit VoxelBasedApproximateStaticMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) - : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex) + : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) { RCLCPP_INFO(logger_, "VoxelBasedApproximateStaticMapLoader initialized.\n"); } @@ -44,9 +45,11 @@ class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader { public: VoxelBasedApproximateDynamicMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) - : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + : VoxelGridDynamicMapLoader( + node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group) { RCLCPP_INFO(logger_, "VoxelBasedApproximateDynamicMapLoader initialized.\n"); } diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp index d60e4af75e89c..0cfc8a64ab2dd 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp @@ -42,8 +42,9 @@ class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader public: explicit VoxelDistanceBasedStaticMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) - : VoxelGridStaticMapLoader(node, leaf_size, tf_map_input_frame, mutex) + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex) + : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) { RCLCPP_INFO(logger_, "VoxelDistanceBasedStaticMapLoader initialized.\n"); } @@ -59,9 +60,11 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader /* data */ public: explicit VoxelDistanceBasedDynamicMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) - : VoxelGridDynamicMapLoader(node, leaf_size, tf_map_input_frame, mutex, main_callback_group) + : VoxelGridDynamicMapLoader( + node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group) { RCLCPP_INFO(logger_, "VoxelDistanceBasedDynamicMapLoader initialized.\n"); } diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp index e3f57c8f9b9a0..9a9c0d1534a5e 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp @@ -96,6 +96,8 @@ class VoxelGridMapLoader rclcpp::Logger logger_; std::mutex * mutex_ptr_; double voxel_leaf_size_; + double voxel_leaf_size_z_; + double downsize_ratio_z_axis_; rclcpp::Publisher::SharedPtr downsampled_map_pub_; bool debug_ = false; @@ -104,7 +106,9 @@ class VoxelGridMapLoader typedef typename pcl::Filter::PointCloud PointCloud; typedef typename PointCloud::Ptr PointCloudPtr; explicit VoxelGridMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex); + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex); + virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0; bool is_close_to_neighbor_voxels( const pcl::PointXYZ & point, const double distance_threshold, VoxelGridPointXYZ & voxel, @@ -132,7 +136,8 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader public: explicit VoxelGridStaticMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex); + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex); virtual void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map); virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); }; @@ -192,7 +197,8 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader public: explicit VoxelGridDynamicMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group); void onEstimatedPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose); void onPoseInitializerCallback(const InitializationState::Message::ConstSharedPtr msg); @@ -293,7 +299,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader auto map_cell_voxel_input_tmp_ptr = std::make_shared>(map_cell_pc_tmp); - map_cell_voxel_grid_tmp.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); + map_cell_voxel_grid_tmp.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_z_); map_cell_downsampled_pc_ptr_tmp.reset(new pcl::PointCloud); map_cell_voxel_grid_tmp.setInputCloud(map_cell_voxel_input_tmp_ptr); map_cell_voxel_grid_tmp.setSaveLeafLayout(true); diff --git a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml index b2d86800fe435..2fc2884fd5df9 100644 --- a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml @@ -5,6 +5,7 @@ + @@ -15,6 +16,7 @@ + diff --git a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp index 830503467a02a..34a0f17bdb389 100644 --- a/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_approximate_compare_map_filter_nodelet.cpp @@ -83,14 +83,20 @@ VoxelBasedApproximateCompareMapFilterComponent::VoxelBasedApproximateCompareMapF distance_threshold_ = declare_parameter("distance_threshold"); bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); + double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis"); + if (downsize_ratio_z_axis <= 0.0) { + RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive"); + return; + } if (use_dynamic_map_loading) { rclcpp::CallbackGroup::SharedPtr main_callback_group; main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); voxel_based_approximate_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_, + main_callback_group); } else { voxel_based_approximate_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_); } } diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp index cfce105bc909a..82c3576dd951c 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp @@ -18,6 +18,7 @@ #include #include +#include #include namespace compare_map_segmentation @@ -40,18 +41,24 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( distance_threshold_ = declare_parameter("distance_threshold"); bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); - + double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis"); + if (downsize_ratio_z_axis <= 0.0) { + RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive"); + return; + } set_map_in_voxel_grid_ = false; if (use_dynamic_map_loading) { rclcpp::CallbackGroup::SharedPtr main_callback_group; main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); voxel_grid_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_, + main_callback_group); } else { voxel_grid_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_); } tf_input_frame_ = *(voxel_grid_map_loader_->tf_map_input_frame_); + RCLCPP_INFO(this->get_logger(), "tf_map_input_frame: %s", tf_input_frame_.c_str()); } void VoxelBasedCompareMapFilterComponent::filter( diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index 5a6ebc401c3d3..32e5367fbcc38 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -112,14 +112,20 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC distance_threshold_ = declare_parameter("distance_threshold"); bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); + double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis"); + if (downsize_ratio_z_axis <= 0.0) { + RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive"); + return; + } if (use_dynamic_map_loading) { rclcpp::CallbackGroup::SharedPtr main_callback_group; main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); voxel_distance_based_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_, + main_callback_group); } else { voxel_distance_based_map_loader_ = std::make_unique( - this, distance_threshold_, &tf_input_frame_, &mutex_); + this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_); } } diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index 9f1133db901bf..426dc8e706dc9 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -15,8 +15,11 @@ #include "compare_map_segmentation/voxel_grid_map_loader.hpp" VoxelGridMapLoader::VoxelGridMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) -: logger_(node->get_logger()), voxel_leaf_size_(leaf_size) + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex) +: logger_(node->get_logger()), + voxel_leaf_size_(leaf_size), + downsize_ratio_z_axis_(downsize_ratio_z_axis) { tf_map_input_frame_ = tf_map_input_frame; mutex_ptr_ = mutex; @@ -69,6 +72,7 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( VoxelGridPointXYZ & voxel) const { // check map downsampled pc + double distance_threshold_z = downsize_ratio_z_axis_ * distance_threshold; if (map == NULL) { return false; } @@ -77,7 +81,7 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y - distance_threshold, point.z - distance_threshold), point, + pcl::PointXYZ(point.x, point.y - distance_threshold, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -87,22 +91,22 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y - distance_threshold, point.z + distance_threshold), point, + pcl::PointXYZ(point.x, point.y - distance_threshold, point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y, point.z - distance_threshold), point, distance_threshold, + pcl::PointXYZ(point.x, point.y, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y, point.z + distance_threshold), point, distance_threshold, + pcl::PointXYZ(point.x, point.y, point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y + distance_threshold, point.z - distance_threshold), point, + pcl::PointXYZ(point.x, point.y + distance_threshold, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -112,14 +116,15 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( return true; } if (is_in_voxel( - pcl::PointXYZ(point.x, point.y + distance_threshold, point.z + distance_threshold), point, + pcl::PointXYZ(point.x, point.y + distance_threshold, point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( pcl::PointXYZ( - point.x - distance_threshold, point.y - distance_threshold, point.z - distance_threshold), + point.x - distance_threshold, point.y - distance_threshold, + point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -130,12 +135,13 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( } if (is_in_voxel( pcl::PointXYZ( - point.x - distance_threshold, point.y - distance_threshold, point.z + distance_threshold), + point.x - distance_threshold, point.y - distance_threshold, + point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( - pcl::PointXYZ(point.x - distance_threshold, point.y, point.z - distance_threshold), point, + pcl::PointXYZ(point.x - distance_threshold, point.y, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -145,13 +151,14 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( return true; } if (is_in_voxel( - pcl::PointXYZ(point.x - distance_threshold, point.y, point.z + distance_threshold), point, + pcl::PointXYZ(point.x - distance_threshold, point.y, point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( pcl::PointXYZ( - point.x - distance_threshold, point.y + distance_threshold, point.z - distance_threshold), + point.x - distance_threshold, point.y + distance_threshold, + point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -162,14 +169,16 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( } if (is_in_voxel( pcl::PointXYZ( - point.x - distance_threshold, point.y + distance_threshold, point.z + distance_threshold), + point.x - distance_threshold, point.y + distance_threshold, + point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( pcl::PointXYZ( - point.x + distance_threshold, point.y - distance_threshold, point.z - distance_threshold), + point.x + distance_threshold, point.y - distance_threshold, + point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -180,12 +189,13 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( } if (is_in_voxel( pcl::PointXYZ( - point.x + distance_threshold, point.y - distance_threshold, point.z + distance_threshold), + point.x + distance_threshold, point.y - distance_threshold, + point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( - pcl::PointXYZ(point.x + distance_threshold, point.y, point.z - distance_threshold), point, + pcl::PointXYZ(point.x + distance_threshold, point.y, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -195,13 +205,14 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( return true; } if (is_in_voxel( - pcl::PointXYZ(point.x + distance_threshold, point.y, point.z + distance_threshold), point, + pcl::PointXYZ(point.x + distance_threshold, point.y, point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } if (is_in_voxel( pcl::PointXYZ( - point.x + distance_threshold, point.y + distance_threshold, point.z - distance_threshold), + point.x + distance_threshold, point.y + distance_threshold, + point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } @@ -212,11 +223,11 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( } if (is_in_voxel( pcl::PointXYZ( - point.x + distance_threshold, point.y + distance_threshold, point.z + distance_threshold), + point.x + distance_threshold, point.y + distance_threshold, + point.z + distance_threshold_z), point, distance_threshold, map, voxel)) { return true; } - return false; } @@ -231,7 +242,7 @@ bool VoxelGridMapLoader::is_in_voxel( const double dist_y = map->points.at(voxel_index).y - target_point.y; const double dist_z = map->points.at(voxel_index).z - target_point.z; const double sqr_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; - if (sqr_distance < distance_threshold * distance_threshold) { + if (sqr_distance < distance_threshold * distance_threshold * downsize_ratio_z_axis_) { return true; } } @@ -239,9 +250,11 @@ bool VoxelGridMapLoader::is_in_voxel( } VoxelGridStaticMapLoader::VoxelGridStaticMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) -: VoxelGridMapLoader(node, leaf_size, tf_map_input_frame, mutex) + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex) +: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) { + voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_; sub_map_ = node->create_subscription( "map", rclcpp::QoS{1}.transient_local(), std::bind(&VoxelGridStaticMapLoader::onMapCallback, this, std::placeholders::_1)); @@ -257,7 +270,7 @@ void VoxelGridStaticMapLoader::onMapCallback( *tf_map_input_frame_ = map_pcl_ptr->header.frame_id; (*mutex_ptr_).lock(); voxel_map_ptr_.reset(new pcl::PointCloud); - voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); + voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_z_); voxel_grid_.setInputCloud(map_pcl_ptr); voxel_grid_.setSaveLeafLayout(true); voxel_grid_.filter(*voxel_map_ptr_); @@ -277,10 +290,12 @@ bool VoxelGridStaticMapLoader::is_close_to_map( } VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( - rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) -: VoxelGridMapLoader(node, leaf_size, tf_map_input_frame, mutex) +: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) { + voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_; auto timer_interval_ms = node->declare_parameter("timer_interval_ms"); map_update_distance_threshold_ = node->declare_parameter("map_update_distance_threshold"); map_loader_radius_ = node->declare_parameter("map_loader_radius"); diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index 1dfeea9ba9f8f..f4256422e6c8b 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -64,6 +64,7 @@ def load_composable_node_param(param_path): "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, 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 b07eaee9785f7..a2c7c5966da62 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/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 29cc087ed3132..8c0ba744491e7 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -1,12 +1,19 @@ cmake_minimum_required(VERSION 3.14) project(image_projection_based_fusion) -add_compile_options(-Wno-unknown-pragmas) +# add_compile_options(-Wno-unknown-pragmas) +add_compile_options(-Wno-unknown-pragmas -fopenmp) find_package(autoware_cmake REQUIRED) find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) autoware_package() +find_package(OpenMP REQUIRED) +if(OpenMP_FOUND) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + # Build non-CUDA dependent nodes ament_auto_add_library(${PROJECT_NAME} SHARED src/fusion_node.cpp @@ -117,7 +124,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) else() message(STATUS "diff ${FILE_NAME}") message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v2/${FILE_NAME} + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v3/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) @@ -125,7 +132,7 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) 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/pointpainting/v2/${FILE_NAME} + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v3/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) @@ -138,8 +145,8 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) endfunction() # default model - download(pts_voxel_encoder_pointpainting.onnx 438dfecd962631ec8f011e0dfa2c6160) - download(pts_backbone_neck_head_pointpainting.onnx e590a0b2bdcd35e01340cf4521bf149e) + download(pts_voxel_encoder_pointpainting.onnx e674271096f6fbbe0f808eef87f5a5ab) + download(pts_backbone_neck_head_pointpainting.onnx d4527a4da08959c2bf9c997112a1de35) find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) diff --git a/perception/image_projection_based_fusion/config/pointpainting.param.yaml b/perception/image_projection_based_fusion/config/pointpainting.param.yaml index 700724a817ceb..56346b5a3eb41 100755 --- a/perception/image_projection_based_fusion/config/pointpainting.param.yaml +++ b/perception/image_projection_based_fusion/config/pointpainting.param.yaml @@ -1,10 +1,17 @@ /**: ros__parameters: - class_names: ["CAR", "PEDESTRIAN", "BICYCLE"] - point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + point_feature_size: 9 # x, y, z, time-lag and car, pedestrian, bicycle max_voxel_size: 40000 - point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] - voxel_size: [0.32, 0.32, 8.0] + point_cloud_range: [-102.4, -102.4, -4.0, 102.4, 102.4, 6.0] + voxel_size: [0.32, 0.32, 10.0] downsample_factor: 1 - encoder_in_feature_size: 12 - yaw_norm_thresholds: [0.3, 0.0, 0.3] + encoder_in_feature_size: 14 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + # omp params + omp_num_threads: 1 diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp index feed6437f6a68..d7de10fed068f 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -17,6 +17,7 @@ #include "image_projection_based_fusion/fusion_node.hpp" #include "image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp" +#include "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" #include #include @@ -52,11 +53,15 @@ class PointPaintingFusionNode : public FusionNode tan_h_; // horizontal field of view + int omp_num_threads_{1}; float score_threshold_{0.0}; std::vector class_names_; + std::map class_index_; + std::map> isClassTable_; std::vector pointcloud_range; bool has_twist_{false}; + centerpoint::NonMaximumSuppression iou_bev_nms_; centerpoint::DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp old mode 100755 new mode 100644 index 9ca1667edb1a0..7d7f290f71a4c --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp @@ -24,7 +24,8 @@ cudaError_t generateFeatures_launch( const float * voxel_features, const float * voxel_num_points, const int * coords, const std::size_t num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, const float voxel_size_y, const float voxel_size_z, const float range_min_x, - const float range_min_y, const float range_min_z, float * features, cudaStream_t stream); + const float range_min_y, const float range_min_z, float * features, + const std::size_t encoder_in_feature_size, cudaStream_t stream); } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index a31f246fb5af1..0e18e9ee42a47 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -43,7 +43,7 @@ - + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index ef2c4644ae444..083a40044489b 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_auto_perception_msgs + autoware_point_types cv_bridge image_transport lidar_centerpoint diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 03f625603ba91..f76af5060316c 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -34,7 +34,9 @@ #include #endif -#include +// static int publish_counter = 0; +static double processing_time_ms = 0; + namespace image_projection_based_fusion { @@ -161,9 +163,29 @@ void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) template void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) { + if (sub_std_pair_.second != nullptr) { + stop_watch_ptr_->toc("processing_time", true); + timer_->cancel(); + publish(*(sub_std_pair_.second)); + postprocess(*(sub_std_pair_.second)); + sub_std_pair_.second = nullptr; + std::fill(is_fused_.begin(), is_fused_.end(), false); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", + processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + processing_time_ms = 0; + } + } + std::lock_guard lock(mutex_); auto period = std::chrono::duration_cast( - std::chrono::duration(timeout_ms_)); + std::chrono::duration(timeout_ms_)); try { setPeriod(period.count()); } catch (rclcpp::exceptions::RCLError & ex) { @@ -238,39 +260,25 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ // Msg if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { timer_->cancel(); - postprocess(*output_msg); publish(*output_msg); + postprocess(*output_msg); std::fill(is_fused_.begin(), is_fused_.end(), false); + sub_std_pair_.second = nullptr; // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + processing_time_ms = stop_watch_ptr_->toc("processing_time", true); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + processing_time_ms = 0; } } else { - if (sub_std_pair_.second != nullptr) { - timer_->cancel(); - postprocess(*(sub_std_pair_.second)); - publish(*(sub_std_pair_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); - } - } - sub_std_pair_.first = int64_t(timestamp_nsec); sub_std_pair_.second = output_msg; + processing_time_ms = stop_watch_ptr_->toc("processing_time", true); } } @@ -278,6 +286,8 @@ template void FusionNode::roiCallback( const DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg, const std::size_t roi_i) { + stop_watch_ptr_->toc("processing_time", true); + int64_t timestamp_nsec = (*input_roi_msg).header.stamp.sec * (int64_t)1e9 + (*input_roi_msg).header.stamp.nanosec; @@ -312,8 +322,8 @@ void FusionNode::roiCallback( if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { timer_->cancel(); - postprocess(*(sub_std_pair_.second)); publish(*(sub_std_pair_.second)); + postprocess(*(sub_std_pair_.second)); std::fill(is_fused_.begin(), is_fused_.end(), false); sub_std_pair_.second = nullptr; @@ -322,8 +332,13 @@ void FusionNode::roiCallback( const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", + processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + processing_time_ms = 0; } } + processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); return; } } @@ -345,19 +360,25 @@ void FusionNode::timer_callback() if (mutex_.try_lock()) { // timeout, postprocess cached msg if (sub_std_pair_.second != nullptr) { - postprocess(*(sub_std_pair_.second)); + stop_watch_ptr_->toc("processing_time", true); + publish(*(sub_std_pair_.second)); + postprocess(*(sub_std_pair_.second)); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", + processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + processing_time_ms = 0; + } } std::fill(is_fused_.begin(), is_fused_.end(), false); sub_std_pair_.second = nullptr; - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - } - mutex_.unlock(); } else { try { diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index f3486d65741f6..3641b42db461a 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -14,6 +14,8 @@ #include "image_projection_based_fusion/pointpainting_fusion/node.hpp" +#include "autoware_point_types/types.hpp" + #include #include #include @@ -23,12 +25,64 @@ #include #include +#include + +#include + namespace image_projection_based_fusion { +inline bool isInsideBbox( + float proj_x, float proj_y, sensor_msgs::msg::RegionOfInterest roi, float zc) +{ + return proj_x >= roi.x_offset * zc && proj_x <= (roi.x_offset + roi.width) * zc && + proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; +} + +inline bool isVehicle(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + label2d == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + label2d == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + label2d == autoware_auto_perception_msgs::msg::ObjectClassification::BUS; +} + +inline bool isCar(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::CAR; +} + +inline bool isTruck(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + label2d == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; +} + +inline bool isBus(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::BUS; +} + +inline bool isPedestrian(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; +} + +inline bool isBicycle(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE || + label2d == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; +} + +inline bool isUnknown(int label2d) +{ + return label2d == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; +} + PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) : FusionNode("pointpainting_fusion", options) { + omp_num_threads_ = this->declare_parameter("omp_num_threads", 1); const float score_threshold = static_cast(this->declare_parameter("score_threshold", 0.4)); const float circle_nms_dist_threshold = @@ -46,7 +100,27 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt const std::string encoder_engine_path = this->declare_parameter("encoder_engine_path", ""); const std::string head_onnx_path = this->declare_parameter("head_onnx_path", ""); const std::string head_engine_path = this->declare_parameter("head_engine_path", ""); + class_names_ = this->declare_parameter>("class_names"); + std::vector classes_{"CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"}; + if (std::find(class_names_.begin(), class_names_.end(), "TRUCK") != class_names_.end()) { + isClassTable_["CAR"] = std::bind(&isCar, std::placeholders::_1); + } else { + isClassTable_["CAR"] = std::bind(&isVehicle, std::placeholders::_1); + } + isClassTable_["TRUCK"] = std::bind(&isTruck, std::placeholders::_1); + isClassTable_["BUS"] = std::bind(&isBus, std::placeholders::_1); + isClassTable_["BICYCLE"] = std::bind(&isBicycle, std::placeholders::_1); + isClassTable_["PEDESTRIAN"] = std::bind(&isPedestrian, std::placeholders::_1); + for (const auto & cls : classes_) { + auto it = find(class_names_.begin(), class_names_.end(), cls); + if (it != class_names_.end()) { + int index = it - class_names_.begin(); + class_index_[cls] = index + 1; + } else { + isClassTable_.erase(cls); + } + } has_twist_ = this->declare_parameter("has_twist", false); const std::size_t point_feature_size = static_cast(this->declare_parameter("point_feature_size")); @@ -78,6 +152,16 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + { + centerpoint::NMSParams p; + p.nms_type_ = centerpoint::NMS_TYPE::IoU_BEV; + p.target_class_names_ = + this->declare_parameter>("iou_nms_target_class_names"); + p.search_distance_2d_ = this->declare_parameter("iou_nms_search_distance_2d"); + p.iou_threshold_ = this->declare_parameter("iou_nms_threshold"); + iou_bev_nms_.setParameters(p); + } + centerpoint::NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); centerpoint::NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); centerpoint::DensificationParam densification_param( @@ -102,16 +186,15 @@ void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted // set fields sensor_msgs::PointCloud2Modifier pcd_modifier(painted_pointcloud_msg); pcd_modifier.clear(); + pcd_modifier.reserve(tmp.width); painted_pointcloud_msg.width = tmp.width; - constexpr int num_fields = 7; + constexpr int num_fields = 5; pcd_modifier.setPointCloud2Fields( num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "intensity", 1, sensor_msgs::msg::PointField::FLOAT32, "CAR", 1, - sensor_msgs::msg::PointField::FLOAT32, "PEDESTRIAN", 1, sensor_msgs::msg::PointField::FLOAT32, - "BICYCLE", 1, sensor_msgs::msg::PointField::FLOAT32); + "intensity", 1, sensor_msgs::msg::PointField::FLOAT32, "CLASS", 1, + sensor_msgs::msg::PointField::FLOAT32); painted_pointcloud_msg.point_step = num_fields * sizeof(float); - // filter points out of range const auto painted_point_step = painted_pointcloud_msg.point_step; size_t j = 0; @@ -133,6 +216,7 @@ void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted j += painted_point_step; } } + painted_pointcloud_msg.data.resize(j); painted_pointcloud_msg.width = static_cast( painted_pointcloud_msg.data.size() / painted_pointcloud_msg.height / @@ -148,6 +232,11 @@ void PointPaintingFusionNode::fuseOnSingleImage( const sensor_msgs::msg::CameraInfo & camera_info, sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + auto num_bbox = (input_roi_msg.feature_objects).size(); + if (num_bbox == 0) { + return; + } + std::vector debug_image_rois; std::vector debug_image_points; @@ -163,77 +252,77 @@ void PointPaintingFusionNode::fuseOnSingleImage( transform_stamped = transform_stamped_optional.value(); } - // projection matrix - Eigen::Matrix4d camera_projection; - camera_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); - // transform sensor_msgs::msg::PointCloud2 transformed_pointcloud; tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); + std::chrono::system_clock::time_point start, end; + start = std::chrono::system_clock::now(); + + const auto x_offset = + transformed_pointcloud.fields.at(static_cast(autoware_point_types::PointIndex::X)) + .offset; + const auto y_offset = + transformed_pointcloud.fields.at(static_cast(autoware_point_types::PointIndex::Y)) + .offset; + const auto z_offset = + transformed_pointcloud.fields.at(static_cast(autoware_point_types::PointIndex::Z)) + .offset; + const auto class_offset = painted_pointcloud_msg.fields.at(4).offset; + const auto p_step = transformed_pointcloud.point_step; + // projection matrix + Eigen::Matrix3f camera_projection; // use only x,y,z + camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6); + /** dc : don't care + +x | f x1 x2 dc ||xc| +y = | y1 f y2 dc ||yc| +dc | dc dc dc dc ||zc| + |dc| + **/ + + auto objects = input_roi_msg.feature_objects; + int iterations = transformed_pointcloud.data.size() / transformed_pointcloud.point_step; // iterate points - sensor_msgs::PointCloud2Iterator iter_car(painted_pointcloud_msg, "CAR"); - sensor_msgs::PointCloud2Iterator iter_ped(painted_pointcloud_msg, "PEDESTRIAN"); - sensor_msgs::PointCloud2Iterator iter_bic(painted_pointcloud_msg, "BICYCLE"); - - for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_pointcloud, "x"), - iter_y(transformed_pointcloud, "y"), iter_z(transformed_pointcloud, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_car, ++iter_ped, ++iter_bic) { - // filter the points outside of the horizontal field of view - if ( - *iter_z <= 0.0 || (*iter_x / *iter_z) > tan_h_.at(image_id) || - (*iter_x / *iter_z) < -tan_h_.at(image_id)) { + // Requires 'OMP_NUM_THREADS=N' + omp_set_num_threads(omp_num_threads_); +#pragma omp parallel for + for (int i = 0; i < iterations; i++) { + int stride = p_step * i; + unsigned char * data = &transformed_pointcloud.data[0]; + unsigned char * output = &painted_pointcloud_msg.data[0]; + float p_x = *reinterpret_cast(&data[stride + x_offset]); + float p_y = *reinterpret_cast(&data[stride + y_offset]); + float p_z = *reinterpret_cast(&data[stride + z_offset]); + if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { continue; } // project - Eigen::Vector4d projected_point = - camera_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()); - + Eigen::Vector3f normalized_projected_point = camera_projection * Eigen::Vector3f(p_x, p_y, p_z); // iterate 2d bbox - for (const auto & feature_object : input_roi_msg.feature_objects) { + for (const auto & feature_object : objects) { sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; // paint current point if it is inside bbox + int label2d = feature_object.object.classification.front().label; if ( - normalized_projected_point.x() >= roi.x_offset && - normalized_projected_point.x() <= roi.x_offset + roi.width && - normalized_projected_point.y() >= roi.y_offset && - normalized_projected_point.y() <= roi.y_offset + roi.height && - feature_object.object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { - switch (feature_object.object.classification.front().label) { - case autoware_auto_perception_msgs::msg::ObjectClassification::CAR: - *iter_car = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK: - *iter_car = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER: - *iter_car = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::BUS: - *iter_car = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN: - *iter_ped = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE: - *iter_bic = 1.0; - break; - case autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE: - *iter_bic = 1.0; - break; + !isUnknown(label2d) && + isInsideBbox(normalized_projected_point.x(), normalized_projected_point.y(), roi, p_z)) { + data = &painted_pointcloud_msg.data[0]; + auto p_class = reinterpret_cast(&output[stride + class_offset]); + for (const auto & cls : isClassTable_) { + *p_class = cls.second(label2d) ? class_index_[cls.first] : *p_class; } } +#if 0 + // Parallelizing loop don't support push_back if (debugger_) { debug_image_points.push_back(normalized_projected_point); } +#endif } } + for (const auto & feature_object : input_roi_msg.feature_objects) { debug_image_rois.push_back(feature_object.feature.roi); } @@ -247,24 +336,35 @@ void PointPaintingFusionNode::fuseOnSingleImage( void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) { + const auto objects_sub_count = + obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + std::vector det_boxes3d; bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d); if (!is_success) { return; } - autoware_auto_perception_msgs::msg::DetectedObjects output_obj_msg; - output_obj_msg.header = painted_pointcloud_msg.header; + std::vector raw_objects; + raw_objects.reserve(det_boxes3d.size()); for (const auto & box3d : det_boxes3d) { - if (box3d.score < score_threshold_) { - continue; - } autoware_auto_perception_msgs::msg::DetectedObject obj; - centerpoint::box3DToDetectedObject(box3d, class_names_, has_twist_, obj); - output_obj_msg.objects.emplace_back(obj); + box3DToDetectedObject(box3d, class_names_, has_twist_, obj); + raw_objects.emplace_back(obj); } - obj_pub_ptr_->publish(output_obj_msg); + autoware_auto_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = painted_pointcloud_msg.header; + output_msg.objects = iou_bev_nms_.apply(raw_objects); + + detection_class_remapper_.mapClasses(output_msg); + + if (objects_sub_count > 0) { + obj_pub_ptr_->publish(output_msg); + } } bool PointPaintingFusionNode::out_of_scope(__attribute__((unused)) const DetectedObjects & obj) diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index 81593361de06f..f0e4de9c988e2 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -65,7 +65,7 @@ bool PointPaintingTRT::preprocess( voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_, config_.max_voxel_size_, config_.voxel_size_x_, config_.voxel_size_y_, config_.voxel_size_z_, config_.range_min_x_, config_.range_min_y_, config_.range_min_z_, encoder_in_features_d_.get(), - stream_)); + config_.encoder_in_feature_size_, stream_)); return true; } diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu index de590c5d91984..d9dce27fb0ecc 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu @@ -37,7 +37,8 @@ namespace { const std::size_t MAX_POINT_IN_VOXEL_SIZE = 32; // the same as max_point_in_voxel_size_ in config const std::size_t WARPS_PER_BLOCK = 4; -const std::size_t ENCODER_IN_FEATURE_SIZE = 12; // same as encoder_in_feature_size_ in config.hpp +const std::size_t ENCODER_IN_FEATURE_SIZE = 14; // same as encoder_in_feature_size_ in config.hpp +const int POINT_FEATURE_SIZE = 9; // cspell: ignore divup std::size_t divup(const std::size_t a, const std::size_t b) @@ -59,7 +60,8 @@ namespace image_projection_based_fusion __global__ void generateFeatures_kernel( const float * voxel_features, const float * voxel_num_points, const int * coords, const std::size_t num_voxels, const float voxel_x, const float voxel_y, const float voxel_z, - const float range_min_x, const float range_min_y, const float range_min_z, float * features) + const float range_min_x, const float range_min_y, const float range_min_z, float * features, + const std::size_t encoder_in_feature_size) { // voxel_features (float): (max_num_voxels, max_num_points_per_voxel, point_feature_size) // voxel_num_points (int): (max_num_voxels) @@ -71,7 +73,6 @@ __global__ void generateFeatures_kernel( if (pillar_idx >= num_voxels) return; // load src - const int POINT_FEATURE_SIZE = 7; __shared__ float pillarSM[WARPS_PER_BLOCK][MAX_POINT_IN_VOXEL_SIZE][POINT_FEATURE_SIZE]; __shared__ float3 pillarSumSM[WARPS_PER_BLOCK]; __shared__ int3 cordsSM[WARPS_PER_BLOCK]; @@ -86,7 +87,7 @@ __global__ void generateFeatures_kernel( for (std::size_t i = 0; i < POINT_FEATURE_SIZE; ++i) { pillarSM[pillar_idx_inBlock][point_idx][i] = voxel_features - [POINT_FEATURE_SIZE * pillar_idx * MAX_POINT_IN_VOXEL_SIZE + POINT_FEATURE_SIZE * point_idx + + [(POINT_FEATURE_SIZE)*pillar_idx * MAX_POINT_IN_VOXEL_SIZE + (POINT_FEATURE_SIZE)*point_idx + i]; } __syncthreads(); @@ -123,46 +124,30 @@ __global__ void generateFeatures_kernel( // store output if (point_idx < pointsNumSM[pillar_idx_inBlock]) { - // 0 ~ 5 - pillarOutSM[pillar_idx_inBlock][point_idx][0] = pillarSM[pillar_idx_inBlock][point_idx][0]; - pillarOutSM[pillar_idx_inBlock][point_idx][1] = pillarSM[pillar_idx_inBlock][point_idx][1]; - pillarOutSM[pillar_idx_inBlock][point_idx][2] = pillarSM[pillar_idx_inBlock][point_idx][2]; - pillarOutSM[pillar_idx_inBlock][point_idx][3] = pillarSM[pillar_idx_inBlock][point_idx][3]; - pillarOutSM[pillar_idx_inBlock][point_idx][4] = pillarSM[pillar_idx_inBlock][point_idx][4]; - pillarOutSM[pillar_idx_inBlock][point_idx][5] = pillarSM[pillar_idx_inBlock][point_idx][5]; - pillarOutSM[pillar_idx_inBlock][point_idx][6] = pillarSM[pillar_idx_inBlock][point_idx][6]; + for (std::size_t i = 0; i < POINT_FEATURE_SIZE; ++i) { + pillarOutSM[pillar_idx_inBlock][point_idx][i] = pillarSM[pillar_idx_inBlock][point_idx][i]; + } // change index - pillarOutSM[pillar_idx_inBlock][point_idx][7] = mean.x; - pillarOutSM[pillar_idx_inBlock][point_idx][8] = mean.y; - pillarOutSM[pillar_idx_inBlock][point_idx][9] = mean.z; + pillarOutSM[pillar_idx_inBlock][point_idx][POINT_FEATURE_SIZE] = mean.x; + pillarOutSM[pillar_idx_inBlock][point_idx][POINT_FEATURE_SIZE + 1] = mean.y; + pillarOutSM[pillar_idx_inBlock][point_idx][POINT_FEATURE_SIZE + 2] = mean.z; - pillarOutSM[pillar_idx_inBlock][point_idx][10] = center.x; - pillarOutSM[pillar_idx_inBlock][point_idx][11] = center.y; + pillarOutSM[pillar_idx_inBlock][point_idx][POINT_FEATURE_SIZE + 3] = center.x; + pillarOutSM[pillar_idx_inBlock][point_idx][POINT_FEATURE_SIZE + 4] = center.y; } else { - pillarOutSM[pillar_idx_inBlock][point_idx][0] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][1] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][2] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][3] = 0; - - pillarOutSM[pillar_idx_inBlock][point_idx][4] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][5] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][6] = 0; - - pillarOutSM[pillar_idx_inBlock][point_idx][7] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][8] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][9] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][10] = 0; - pillarOutSM[pillar_idx_inBlock][point_idx][11] = 0; + for (std::size_t i = 0; i < encoder_in_feature_size; ++i) { + pillarOutSM[pillar_idx_inBlock][point_idx][i] = 0; + } } __syncthreads(); - for (int i = 0; i < ENCODER_IN_FEATURE_SIZE; i++) { + for (int i = 0; i < encoder_in_feature_size; i++) { int outputSMId = pillar_idx_inBlock * MAX_POINT_IN_VOXEL_SIZE * ENCODER_IN_FEATURE_SIZE + i * MAX_POINT_IN_VOXEL_SIZE + point_idx; - int outputId = pillar_idx * MAX_POINT_IN_VOXEL_SIZE * ENCODER_IN_FEATURE_SIZE + + int outputId = pillar_idx * MAX_POINT_IN_VOXEL_SIZE * encoder_in_feature_size + i * MAX_POINT_IN_VOXEL_SIZE + point_idx; features[outputId] = ((float *)pillarOutSM)[outputSMId]; } @@ -172,13 +157,14 @@ cudaError_t generateFeatures_launch( const float * voxel_features, const float * voxel_num_points, const int * coords, const std::size_t num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, const float voxel_size_y, const float voxel_size_z, const float range_min_x, - const float range_min_y, const float range_min_z, float * features, cudaStream_t stream) + const float range_min_y, const float range_min_z, float * features, + const std::size_t encoder_in_feature_size, cudaStream_t stream) { dim3 blocks(divup(max_voxel_size, WARPS_PER_BLOCK)); dim3 threads(WARPS_PER_BLOCK * MAX_POINT_IN_VOXEL_SIZE); generateFeatures_kernel<<>>( voxel_features, voxel_num_points, coords, num_voxels, voxel_size_x, voxel_size_y, voxel_size_z, - range_min_x, range_min_y, range_min_z, features); + range_min_x, range_min_y, range_min_z, features, encoder_in_feature_size); return cudaGetLastError(); } diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index d195f7870fc71..bd49d9e446f1f 100755 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -50,9 +50,8 @@ std::size_t VoxelGenerator::pointsToVoxels( pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_msg.header.stamp).seconds()); for (sensor_msgs::PointCloud2ConstIterator x_iter(pc_msg, "x"), y_iter(pc_msg, "y"), - z_iter(pc_msg, "z"), car_iter(pc_msg, "CAR"), ped_iter(pc_msg, "PEDESTRIAN"), - bic_iter(pc_msg, "BICYCLE"); - x_iter != x_iter.end(); ++x_iter, ++y_iter, ++z_iter, ++car_iter, ++ped_iter, ++bic_iter) { + z_iter(pc_msg, "z"), class_iter(pc_msg, "CLASS"); + x_iter != x_iter.end(); ++x_iter, ++y_iter, ++z_iter, ++class_iter) { point_past << *x_iter, *y_iter, *z_iter; point_current = affine_past2current * point_past; @@ -60,9 +59,9 @@ std::size_t VoxelGenerator::pointsToVoxels( point[1] = point_current.y(); point[2] = point_current.z(); point[3] = time_lag; - point[4] = *car_iter; - point[5] = *ped_iter; - point[6] = *bic_iter; + for (std::size_t i = 1; i <= config_.class_size_; i++) { + point[3 + i] = (*class_iter == i) ? 1 : 0; + } out_of_range = false; for (std::size_t di = 0; di < config_.point_dim_size_; di++) { 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 dc6c74c25a539..6a0bb29e9fb2d 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 @@ -139,6 +139,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Parameters bool enable_delay_compensation_; double prediction_time_horizon_; + double prediction_time_horizon_rate_for_validate_lane_length_; double prediction_sampling_time_interval_; double min_velocity_for_map_based_prediction_; double min_crosswalk_user_velocity_; 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 68f31593116f7..d3a22f6221855 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include namespace map_based_prediction @@ -207,6 +208,114 @@ double calcAbsYawDiffBetweenLaneletAndObject( return abs_norm_delta; } +/** + * @brief Get the Right LineSharing Lanelets object + * + * @param current_lanelet + * @param lanelet_map_ptr + * @return lanelet::ConstLanelets + */ +lanelet::ConstLanelets getRightLineSharingLanelets( + const lanelet::ConstLanelet & current_lanelet, const lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::ConstLanelets + output_lanelets; // create an empty container of type lanelet::ConstLanelets + + // step1: look for lane sharing current right bound + lanelet::Lanelets right_lane_candidates = + lanelet_map_ptr->laneletLayer.findUsages(current_lanelet.rightBound()); + for (auto & candidate : right_lane_candidates) { + // exclude self lanelet + if (candidate == current_lanelet) continue; + // if candidate has linestring as leftbound, assign it to output + if (candidate.leftBound() == current_lanelet.rightBound()) { + output_lanelets.push_back(candidate); + } + } + return output_lanelets; // return empty +} + +/** + * @brief Get the Left LineSharing Lanelets object + * + * @param current_lanelet + * @param lanelet_map_ptr + * @return lanelet::ConstLanelets + */ +lanelet::ConstLanelets getLeftLineSharingLanelets( + const lanelet::ConstLanelet & current_lanelet, const lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::ConstLanelets + output_lanelets; // create an empty container of type lanelet::ConstLanelets + + // step1: look for lane sharing current left bound + lanelet::Lanelets left_lane_candidates = + lanelet_map_ptr->laneletLayer.findUsages(current_lanelet.leftBound()); + for (auto & candidate : left_lane_candidates) { + // exclude self lanelet + if (candidate == current_lanelet) continue; + // if candidate has linestring as rightbound, assign it to output + if (candidate.rightBound() == current_lanelet.leftBound()) { + output_lanelets.push_back(candidate); + } + } + return output_lanelets; // return empty +} + +/** + * @brief Check if the lanelet is isolated in routing graph + * @param current_lanelet + * @param lanelet_map_ptr + */ +bool isIsolatedLanelet( + const lanelet::ConstLanelet & lanelet, lanelet::routing::RoutingGraphPtr & graph) +{ + const auto & following_lanelets = graph->following(lanelet); + const auto & left_lanelets = graph->lefts(lanelet); + const auto & right_lanelets = graph->rights(lanelet); + return left_lanelets.empty() && right_lanelets.empty() && following_lanelets.empty(); +} + +/** + * @brief Get the Possible Paths For Isolated Lanelet object + * @param lanelet + * @return lanelet::routing::LaneletPaths + */ +lanelet::routing::LaneletPaths getPossiblePathsForIsolatedLanelet( + const lanelet::ConstLanelet & lanelet) +{ + lanelet::ConstLanelets possible_lanelets; + possible_lanelets.push_back(lanelet); + lanelet::routing::LaneletPaths possible_paths; + // need to init path with constlanelets + lanelet::routing::LaneletPath possible_path(possible_lanelets); + possible_paths.push_back(possible_path); + return possible_paths; +} + +/** + * @brief validate isolated lanelet length has enough length for prediction + * @param lanelet + * @param object: object information for calc length threshold + * @param prediction_time: time horizon[s] for calc length threshold + * @return bool + */ +bool validateIsolatedLaneletLength( + const lanelet::ConstLanelet & lanelet, const TrackedObject & object, const double prediction_time) +{ + // get closest center line point to object + const auto & center_line = lanelet.centerline2d(); + const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; + const lanelet::BasicPoint2d obj_point(obj_pos.x, obj_pos.y); + // get end point of the center line + const auto & end_point = center_line.back(); + // calc approx distance between closest point and end point + const double approx_distance = lanelet::geometry::distance2d(obj_point, end_point); + const double min_length = + object.kinematics.twist_with_covariance.twist.linear.x * prediction_time; + return approx_distance > min_length; +} + lanelet::ConstLanelets getLanelets(const map_based_prediction::LaneletsData & data) { lanelet::ConstLanelets lanelets; @@ -522,6 +631,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ declare_parameter("lane_change_detection.num_continuous_state_transition"); } reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5); + /* prediction path will disabled when the estimated path length exceeds lanelet length. This + * parameter control the estimated path length = vx * th * (rate) */ + prediction_time_horizon_rate_for_validate_lane_length_ = + declare_parameter("prediction_time_horizon_rate_for_validate_lane_length", 0.8); path_generator_ = std::make_shared( prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); @@ -1119,25 +1232,69 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( const double search_dist = prediction_time_horizon_ * obj_vel + lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet); lanelet::routing::PossiblePathsParams possible_params{search_dist, {}, 0, false, true}; + const double validate_time_horizon = + prediction_time_horizon_ * prediction_time_horizon_rate_for_validate_lane_length_; + + // lambda function to get possible paths for isolated lanelet + // isolated is often caused by lanelet with no connection e.g. shoulder-lane + auto getPathsForNormalOrIsolatedLanelet = [&](const lanelet::ConstLanelet & lanelet) { + // if lanelet is not isolated, return normal possible paths + if (!isIsolatedLanelet(lanelet, routing_graph_ptr_)) { + return routing_graph_ptr_->possiblePaths(lanelet, possible_params); + } + // if lanelet is isolated, check if it has enough length + if (!validateIsolatedLaneletLength(lanelet, object, validate_time_horizon)) { + return lanelet::routing::LaneletPaths{}; + } else { + // if lanelet has enough length, return possible paths + return getPossiblePathsForIsolatedLanelet(lanelet); + } + }; + + // lambda function to extract left/right lanelets + auto getLeftOrRightLanelets = [&]( + const lanelet::ConstLanelet & lanelet, + const bool get_left) -> std::optional { + const auto opt = + get_left ? routing_graph_ptr_->left(lanelet) : routing_graph_ptr_->right(lanelet); + if (!!opt) { + return *opt; + } + const auto adjacent = get_left ? routing_graph_ptr_->adjacentLeft(lanelet) + : routing_graph_ptr_->adjacentRight(lanelet); + if (!!adjacent) { + return *adjacent; + } + // search for unconnected lanelet + const auto unconnected_lanelets = get_left + ? getLeftLineSharingLanelets(lanelet, lanelet_map_ptr_) + : getRightLineSharingLanelets(lanelet, lanelet_map_ptr_); + // just return first candidate of unconnected lanelet for now + if (!unconnected_lanelets.empty()) { + return unconnected_lanelets.front(); + } + // if no candidate lanelet found, return empty + return std::nullopt; + }; // Step1. Get the path // Step1.1 Get the left lanelet lanelet::routing::LaneletPaths left_paths; - auto opt_left = routing_graph_ptr_->left(current_lanelet_data.lanelet); - if (!!opt_left) { - left_paths = routing_graph_ptr_->possiblePaths(*opt_left, possible_params); + const auto left_lanelet = getLeftOrRightLanelets(current_lanelet_data.lanelet, true); + if (!!left_lanelet) { + left_paths = getPathsForNormalOrIsolatedLanelet(left_lanelet.value()); } // Step1.2 Get the right lanelet lanelet::routing::LaneletPaths right_paths; - auto opt_right = routing_graph_ptr_->right(current_lanelet_data.lanelet); - if (!!opt_right) { - right_paths = routing_graph_ptr_->possiblePaths(*opt_right, possible_params); + const auto right_lanelet = getLeftOrRightLanelets(current_lanelet_data.lanelet, false); + if (!!right_lanelet) { + right_paths = getPathsForNormalOrIsolatedLanelet(right_lanelet.value()); } // Step1.3 Get the centerline lanelet::routing::LaneletPaths center_paths = - routing_graph_ptr_->possiblePaths(current_lanelet_data.lanelet, possible_params); + getPathsForNormalOrIsolatedLanelet(current_lanelet_data.lanelet); // Skip calculations if all paths are empty if (left_paths.empty() && right_paths.empty() && center_paths.empty()) { diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 85f60e21cf528..845f9c38e269b 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -16,21 +16,29 @@ successive_num_to_entry_dynamic_avoidance_condition: 5 - min_obj_lat_offset_to_ego_path: 0.3 # [m] + min_obj_lat_offset_to_ego_path: 0.0 # [m] + max_obj_lat_offset_to_ego_path: 1.0 # [m] + + crossing_object: + min_object_vel: 1.0 + max_object_angle: 1.05 + + front_object: + max_object_angle: 0.785 drivable_area_generation: - lat_offset_from_obstacle: 0.8 # [m] + lat_offset_from_obstacle: 1.0 # [m] max_lat_offset_to_avoid: 0.5 # [m] # for same directional object overtaking_object: - max_time_to_collision: 3.0 # [s] - start_duration_to_avoid: 4.0 # [s] - end_duration_to_avoid: 8.0 # [s] + max_time_to_collision: 10.0 # [s] + start_duration_to_avoid: 2.0 # [s] + end_duration_to_avoid: 4.0 # [s] duration_to_hold_avoidance: 3.0 # [s] # for opposite directional object oncoming_object: - max_time_to_collision: 3.0 # [s] + max_time_to_collision: 15.0 # [s] start_duration_to_avoid: 12.0 # [s] end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 6d2b66ff025c1..d43e1d08050e7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -48,7 +48,13 @@ struct DynamicAvoidanceParameters bool avoid_pedestrian{false}; double min_obstacle_vel{0.0}; int successive_num_to_entry_dynamic_avoidance_condition{0}; + double min_obj_lat_offset_to_ego_path{0.0}; + double max_obj_lat_offset_to_ego_path{0.0}; + + double max_front_object_angle{0.0}; + double min_crossing_object_vel{0.0}; + double max_crossing_object_angle{0.0}; // drivable area generation double lat_offset_from_obstacle{0.0}; @@ -70,12 +76,15 @@ class DynamicAvoidanceModule : public SceneModuleInterface struct DynamicAvoidanceObject { DynamicAvoidanceObject( - const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel) + const PredictedObject & predicted_object, const double arg_vel, const double arg_lat_vel, + const bool arg_is_left, const double arg_time_to_collision) : uuid(tier4_autoware_utils::toHexString(predicted_object.object_id)), pose(predicted_object.kinematics.initial_pose_with_covariance.pose), + shape(predicted_object.shape), vel(arg_vel), lat_vel(arg_lat_vel), - shape(predicted_object.shape) + is_left(arg_is_left), + time_to_collision(arg_time_to_collision) { for (const auto & path : predicted_object.kinematics.predicted_paths) { predicted_paths.push_back(path); @@ -84,12 +93,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::string uuid; geometry_msgs::msg::Pose pose; + autoware_auto_perception_msgs::msg::Shape shape; double vel; double lat_vel; - autoware_auto_perception_msgs::msg::Shape shape; - std::vector predicted_paths{}; - bool is_left; + double time_to_collision; + std::vector predicted_paths{}; }; struct DynamicAvoidanceObjectCandidate { @@ -113,7 +122,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map > & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map); void updateModuleParams(const std::shared_ptr & parameters) { @@ -135,7 +144,20 @@ class DynamicAvoidanceModule : public SceneModuleInterface private: bool isLabelTargetObstacle(const uint8_t label) const; - std::vector calcTargetObjectsCandidate() const; + std::vector calcTargetObjectsCandidate(); + bool willObjectCutIn( + const std::vector & ego_path, const PredictedPath & predicted_path, + const double obj_tangent_vel) const; + bool willObjectCutOut( + const double obj_tangent_vel, const double obj_normal_vel, const bool is_left) const; + bool isObjectFarFromPath( + const PredictedObject & predicted_object, const double obj_dist_to_path) const; + double calcTimeToCollision( + const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, + const double obj_tangent_vel) const; + std::optional> calcCollisionSection( + const std::vector & ego_path, const PredictedPath & obj_path) const; + std::pair getAdjacentLanes( const double forward_distance, const double backward_distance) const; std::optional calcDynamicObstaclePolygon( @@ -176,7 +198,11 @@ class DynamicAvoidanceModule : public SceneModuleInterface } void update(const std::string & uuid, const double new_variable) { - variable_.emplace(uuid, new_variable); + if (variable_.count(uuid) != 0) { + variable_.at(uuid) = new_variable; + } else { + variable_.emplace(uuid, new_variable); + } } std::unordered_map variable_; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 11198a49360fb..444515f6cd398 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -33,38 +33,6 @@ namespace behavior_path_planner { namespace { -bool isCentroidWithinLanelets( - const geometry_msgs::msg::Point & obj_pos, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return false; - } - - lanelet::BasicPoint2d object_centroid(obj_pos.x, obj_pos.y); - - for (const auto & llt : target_lanelets) { - if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { - return true; - } - } - - return false; -} - -std::vector getObjectsInLanes( - const std::vector & objects, - const lanelet::ConstLanelets & target_lanes) -{ - std::vector target_objects; - for (const auto & object : objects) { - if (isCentroidWithinLanelets(object.pose.position, target_lanes)) { - target_objects.push_back(object); - } - } - - return target_objects; -} - geometry_msgs::msg::Point toGeometryPoint(const tier4_autoware_utils::Point2d & point) { geometry_msgs::msg::Point geom_obj_point; @@ -143,6 +111,76 @@ std::pair projectObstacleVelocityToTrajectory( return std::make_pair( obj_vel * std::cos(obj_yaw - path_yaw), obj_vel * std::sin(obj_yaw - path_yaw)); } + +double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +{ + if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + + throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); +} + +double calcDiffAngleAgainstPath( + const std::vector & path_points, + const geometry_msgs::msg::Pose & target_pose) +{ + const size_t nearest_idx = motion_utils::findNearestIndex(path_points, target_pose.position); + const double traj_yaw = tf2::getYaw(path_points.at(nearest_idx).point.pose.orientation); + + const double target_yaw = tf2::getYaw(target_pose.orientation); + + const double diff_yaw = tier4_autoware_utils::normalizeRadian(target_yaw - traj_yaw); + return diff_yaw; +} + +double calcDistanceToPath( + const std::vector & path_points, + const geometry_msgs::msg::Point & target_pos) +{ + const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); + if (target_idx == 0 || target_idx == path_points.size() - 1) { + const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); + const double angle_to_target_pos = tier4_autoware_utils::calcAzimuthAngle( + path_points.at(target_idx).point.pose.position, target_pos); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(angle_to_target_pos - target_yaw); + + if ( + (target_idx == 0 && (diff_yaw < -M_PI_2 || M_PI_2 < diff_yaw)) || + (target_idx == path_points.size() - 1 && (-M_PI_2 < diff_yaw && diff_yaw < M_PI_2))) { + return tier4_autoware_utils::calcDistance2d(path_points.at(target_idx), target_pos); + } + } + + return std::abs(motion_utils::calcLateralOffset(path_points, target_pos)); +} + +bool isLeft( + const std::vector & path_points, + const geometry_msgs::msg::Point & target_pos) +{ + const size_t target_idx = motion_utils::findNearestIndex(path_points, target_pos); + const double target_yaw = tf2::getYaw(path_points.at(target_idx).point.pose.orientation); + const double angle_to_target_pos = tier4_autoware_utils::calcAzimuthAngle( + path_points.at(target_idx).point.pose.position, target_pos); + const double diff_yaw = tier4_autoware_utils::normalizeRadian(angle_to_target_pos - target_yaw); + + if (0 < diff_yaw) { + return true; + } + return false; +} } // namespace DynamicAvoidanceModule::DynamicAvoidanceModule( @@ -187,7 +225,6 @@ void DynamicAvoidanceModule::updateData() { // calculate target objects candidate const auto target_objects_candidate = calcTargetObjectsCandidate(); - prev_target_objects_candidate_ = target_objects_candidate; // calculate target objects considering flickering suppress target_objects_.clear(); @@ -216,13 +253,10 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() info_marker_.markers.clear(); debug_marker_.markers.clear(); - // 1. get reference path from previous module const auto prev_module_path = getPreviousModuleOutput().path; - - // 2. get drivable lanes from previous module const auto drivable_lanes = getPreviousModuleOutput().drivable_area_info.drivable_lanes; - // 3. create obstacles to avoid (= extract from the drivable area) + // create obstacles to avoid (= extract from the drivable area) std::vector obstacles_for_drivable_area; prev_objects_min_bound_lat_offset_.resetCurrentUuids(); for (const auto & object : target_objects_) { @@ -241,7 +275,6 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() BehaviorModuleOutput output; output.path = prev_module_path; output.reference_path = getPreviousModuleOutput().reference_path; - // for new architecture output.drivable_area_info.drivable_lanes = drivable_lanes; output.drivable_area_info.obstacles = obstacles_for_drivable_area; output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; @@ -293,121 +326,199 @@ bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const } std::vector -DynamicAvoidanceModule::calcTargetObjectsCandidate() const +DynamicAvoidanceModule::calcTargetObjectsCandidate() { const auto prev_module_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - // 1. convert predicted objects to dynamic avoidance objects - std::vector input_objects; + // convert predicted objects to dynamic avoidance objects + std::vector output_objects_candidate; for (const auto & predicted_object : predicted_objects) { - // check label + const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id); + const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const double obj_vel = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto obj_path = *std::max_element( + predicted_object.kinematics.predicted_paths.begin(), + predicted_object.kinematics.predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + // 1. check label const bool is_label_target_obstacle = isLabelTargetObstacle(predicted_object.classification.front().label); if (!is_label_target_obstacle) { continue; } - const auto [tangent_vel, normal_vel] = + // 2. check if velocity is large enough + const auto [obj_tangent_vel, obj_normal_vel] = projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object); - // check if velocity is high enough - if (std::abs(tangent_vel) < parameters_->min_obstacle_vel) { + if (std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel) { continue; } - input_objects.push_back(DynamicAvoidanceObject(predicted_object, tangent_vel, normal_vel)); - } + // 3. check if object is not crossing ego's path + const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, obj_pose); + const bool is_obstacle_crossing_path = + parameters_->max_crossing_object_angle < std::abs(obj_angle) && + parameters_->max_crossing_object_angle < M_PI - std::abs(obj_angle); + const bool is_crossing_object_to_ignore = + parameters_->min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path; + if (is_crossing_object_to_ignore) { + continue; + } - // 2. calculate target lanes to filter obstacles - const auto [right_lanes, left_lanes] = getAdjacentLanes(100.0, 50.0); + // 4. check if object is not to be followed by ego + const double obj_dist_to_path = calcDistanceToPath(prev_module_path->points, obj_pose.position); + const bool is_object_on_ego_path = + obj_dist_to_path < + planner_data_->parameters.vehicle_width / 2.0 + parameters_->min_obj_lat_offset_to_ego_path; + if (is_object_on_ego_path && std::abs(obj_angle) < parameters_->max_front_object_angle) { + continue; + } - // 3. filter obstacles for dynamic avoidance - const auto objects_in_right_lanes = getObjectsInLanes(input_objects, right_lanes); - const auto objects_in_left_lanes = getObjectsInLanes(input_objects, left_lanes); + // 5. check if object lateral offset to ego's path is large enough + const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path); + if (is_object_far_from_path) { + continue; + } - // 4. check if object will not cut into the ego lane or cut out to the next lane, - // or close to the ego's path considering ego's lane change. - // NOTE: The oncoming object will be ignored. - constexpr double epsilon_path_lat_diff = 0.3; - std::vector output_objects_candidate; - for (const bool is_left : {true, false}) { - for (const auto & object : (is_left ? objects_in_left_lanes : objects_in_right_lanes)) { - const auto reliable_predicted_path = std::max_element( - object.predicted_paths.begin(), object.predicted_paths.end(), - [](const PredictedPath & a, const PredictedPath & b) { - return a.confidence < b.confidence; - }); - - // Ignore object that will cut into the ego lane - const bool will_object_cut_in = [&]() { - if (object.vel < 0) { - // Ignore oncoming object - return false; - } + // 6. calculate which side object exists against ego's path + const bool is_left = isLeft(prev_module_path->points, obj_pose.position); - for (const auto & predicted_path_point : reliable_predicted_path->path) { - const double paths_lat_diff = motion_utils::calcLateralOffset( - prev_module_path->points, predicted_path_point.position); - if (std::abs(paths_lat_diff) < epsilon_path_lat_diff) { - return true; - } - } - return false; - }(); - if (will_object_cut_in) { - continue; - } + // 6. check if object will not cut in or cut out + const bool will_object_cut_in = + willObjectCutIn(prev_module_path->points, obj_path, obj_tangent_vel); + const bool will_object_cut_out = willObjectCutOut(obj_tangent_vel, obj_normal_vel, is_left); + if (will_object_cut_in || will_object_cut_out) { + continue; + } - // Ignore object that will cut out to the next lane - const bool will_object_cut_out = [&]() { - if (object.vel < 0) { - // Ignore oncoming object - return false; - } + // 7. check if time to collision + const double time_to_collision = + calcTimeToCollision(prev_module_path->points, obj_pose, obj_tangent_vel); + + // 8. calculate alive counter for filtering objects + const auto prev_target_object_candidate = + DynamicAvoidanceObjectCandidate::getObjectFromUuid(prev_target_objects_candidate_, obj_uuid); + const int alive_counter = + prev_target_object_candidate + ? std::min( + parameters_->successive_num_to_entry_dynamic_avoidance_condition, + prev_target_object_candidate->alive_counter + 1) + : 0; + + const auto target_object = DynamicAvoidanceObject( + predicted_object, obj_tangent_vel, obj_normal_vel, is_left, time_to_collision); + const auto target_object_candidate = + DynamicAvoidanceObjectCandidate{target_object, alive_counter}; + output_objects_candidate.push_back(target_object_candidate); + } + + prev_target_objects_candidate_ = output_objects_candidate; + return output_objects_candidate; +} - constexpr double object_lat_vel_thresh = 0.3; - if (is_left) { - if (object_lat_vel_thresh < object.lat_vel) { - return true; - } - } else { - if (object.lat_vel < -object_lat_vel_thresh) { - return true; - } +[[maybe_unused]] std::optional> +DynamicAvoidanceModule::calcCollisionSection( + const std::vector & ego_path, const PredictedPath & obj_path) const +{ + const size_t ego_idx = planner_data_->findEgoIndex(ego_path); + const double ego_vel = getEgoSpeed(); + + std::optional collision_start_idx{std::nullopt}; + double lon_dist = 0.0; + for (size_t i = ego_idx; i < ego_path.size() - 1; ++i) { + lon_dist += tier4_autoware_utils::calcDistance2d(ego_path.at(i), ego_path.at(i + 1)); + const double elapsed_time = lon_dist / ego_vel; + + const auto future_ego_pose = ego_path.at(i); + const auto future_obj_pose = + object_recognition_utils::calcInterpolatedPose(obj_path, elapsed_time); + + if (future_obj_pose) { + const double dist_ego_to_obj = + tier4_autoware_utils::calcDistance2d(future_ego_pose, *future_obj_pose); + if (dist_ego_to_obj < 1.0) { + if (!collision_start_idx) { + collision_start_idx = i; } - return false; - }(); - if (will_object_cut_out) { continue; } - - // Ignore object that is close to the ego's path. - const double lat_offset_to_path = - motion_utils::calcLateralOffset(prev_module_path->points, object.pose.position); - if ( - std::abs(lat_offset_to_path) < planner_data_->parameters.vehicle_width / 2.0 + - parameters_->min_obj_lat_offset_to_ego_path) { + } else { + if (!collision_start_idx) { continue; } + } + + return std::make_pair(*collision_start_idx, i - 1); + } + + return std::make_pair(*collision_start_idx, ego_path.size() - 1); +} + +double DynamicAvoidanceModule::calcTimeToCollision( + const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, + const double obj_tangent_vel) const +{ + const double relative_velocity = getEgoSpeed() - obj_tangent_vel; + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(ego_path); + const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); + const double signed_lon_length = motion_utils::calcSignedArcLength( + ego_path, getEgoPosition(), ego_seg_idx, obj_pose.position, obj_seg_idx); + const double positive_relative_velocity = std::max(relative_velocity, 1.0); + return signed_lon_length / positive_relative_velocity; +} - // get previous object if it exists - const auto prev_target_object_candidate = DynamicAvoidanceObjectCandidate::getObjectFromUuid( - prev_target_objects_candidate_, object.uuid); - const int alive_counter = - prev_target_object_candidate - ? std::min( - parameters_->successive_num_to_entry_dynamic_avoidance_condition, - prev_target_object_candidate->alive_counter + 1) - : 0; - - auto target_object = object; - target_object.is_left = is_left; - output_objects_candidate.push_back( - DynamicAvoidanceObjectCandidate{target_object, alive_counter}); +bool DynamicAvoidanceModule::isObjectFarFromPath( + const PredictedObject & predicted_object, const double obj_dist_to_path) const +{ + const double obj_max_length = calcObstacleMaxLength(predicted_object.shape); + const double min_obj_dist_to_path = std::max( + 0.0, obj_dist_to_path - planner_data_->parameters.vehicle_width / 2.0 - obj_max_length); + + return parameters_->max_obj_lat_offset_to_ego_path < min_obj_dist_to_path; +} + +bool DynamicAvoidanceModule::willObjectCutIn( + const std::vector & ego_path, const PredictedPath & predicted_path, + const double obj_tangent_vel) const +{ + constexpr double epsilon_path_lat_diff = 0.3; + + // Ignore oncoming object + if (obj_tangent_vel < 0) { + return false; + } + + for (const auto & predicted_path_point : predicted_path.path) { + const double paths_lat_diff = + motion_utils::calcLateralOffset(ego_path, predicted_path_point.position); + if (std::abs(paths_lat_diff) < epsilon_path_lat_diff) { + return true; } } + return false; +} - return output_objects_candidate; +bool DynamicAvoidanceModule::willObjectCutOut( + const double obj_tangent_vel, const double obj_normal_vel, const bool is_left) const +{ + // Ignore oncoming object + if (obj_tangent_vel < 0) { + return false; + } + + constexpr double object_lat_vel_thresh = 0.3; + if (is_left) { + if (object_lat_vel_thresh < obj_normal_vel) { + return true; + } + } else { + if (obj_normal_vel < -object_lat_vel_thresh) { + return true; + } + } + return false; } std::pair DynamicAvoidanceModule::getAdjacentLanes( @@ -507,34 +618,20 @@ std::optional DynamicAvoidanceModule::calcDynam const auto [raw_min_obj_lon_offset, raw_max_obj_lon_offset] = getMinMaxValues(obj_lon_offset_vec); - - // calculate time to collision and apply it to drivable area extraction - const double relative_velocity = getEgoSpeed() - object.vel; - const double time_to_collision = [&]() { - const auto prev_module_path = getPreviousModuleOutput().path; - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(prev_module_path->points); - const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(prev_module_path->points, object.pose.position); - const double signed_lon_length = motion_utils::calcSignedArcLength( - prev_module_path->points, getEgoPosition(), ego_seg_idx, object.pose.position, obj_seg_idx); - const double positive_relative_velocity = std::max(relative_velocity, 1.0); - return signed_lon_length / positive_relative_velocity; - }(); - - if (time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { + if (object.time_to_collision < -parameters_->duration_to_hold_avoidance_overtaking_object) { return std::nullopt; } if (0 <= object.vel) { const double limited_time_to_collision = - std::min(parameters_->max_time_to_collision_overtaking_object, time_to_collision); + std::min(parameters_->max_time_to_collision_overtaking_object, object.time_to_collision); return std::make_pair( raw_min_obj_lon_offset + object.vel * limited_time_to_collision, raw_max_obj_lon_offset + object.vel * limited_time_to_collision); } const double limited_time_to_collision = - std::min(parameters_->max_time_to_collision_oncoming_object, time_to_collision); + std::min(parameters_->max_time_to_collision_oncoming_object, object.time_to_collision); return std::make_pair( raw_min_obj_lon_offset + object.vel * limited_time_to_collision, raw_max_obj_lon_offset); }(); diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index cfc57e480af85..eddb93094574d 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -42,8 +42,19 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( p.min_obstacle_vel = node->declare_parameter(ns + "min_obstacle_vel"); p.successive_num_to_entry_dynamic_avoidance_condition = node->declare_parameter(ns + "successive_num_to_entry_dynamic_avoidance_condition"); + p.min_obj_lat_offset_to_ego_path = node->declare_parameter(ns + "min_obj_lat_offset_to_ego_path"); + p.max_obj_lat_offset_to_ego_path = + node->declare_parameter(ns + "max_obj_lat_offset_to_ego_path"); + + p.max_front_object_angle = + node->declare_parameter(ns + "front_object.max_object_angle"); + + p.min_crossing_object_vel = + node->declare_parameter(ns + "crossing_object.min_object_vel"); + p.max_crossing_object_angle = + node->declare_parameter(ns + "crossing_object.max_object_angle"); } { // drivable_area_generation @@ -94,8 +105,19 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "successive_num_to_entry_dynamic_avoidance_condition", p->successive_num_to_entry_dynamic_avoidance_condition); + updateParam( parameters, ns + "min_obj_lat_offset_to_ego_path", p->min_obj_lat_offset_to_ego_path); + updateParam( + parameters, ns + "max_obj_lat_offset_to_ego_path", p->max_obj_lat_offset_to_ego_path); + + updateParam( + parameters, ns + "front_object.max_object_angle", p->max_front_object_angle); + + updateParam( + parameters, ns + "crossing_object.min_object_vel", p->min_crossing_object_vel); + updateParam( + parameters, ns + "crossing_object.max_object_angle", p->max_crossing_object_angle); } { // drivable_area_generation diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp index 3555d3b799983..b1c6ec61db845 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp @@ -92,11 +92,12 @@ class PathSampler : public rclcpp::Node sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const; // sub-functions of generateTrajectory + void copyZ( + const std::vector & from_traj, std::vector & to_traj); + void copyVelocity( + const std::vector & from_traj, std::vector & to_traj, + const geometry_msgs::msg::Pose & ego_pose); std::vector generatePath(const PlannerData & planner_data); - void applyInputVelocity( - std::vector & output_traj_points, - const std::vector & input_traj_points, - const geometry_msgs::msg::Pose & ego_pose) const; void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; void publishDebugMarker(const std::vector & traj_points) const; }; diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/path_sampler/src/node.cpp index 298bdf76a7c58..32c7cebc39b5f 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/path_sampler/src/node.cpp @@ -295,6 +295,68 @@ PlannerData PathSampler::createPlannerData(const Path & path) const return planner_data; } +void copyZ(const std::vector & from_traj, std::vector & to_traj) +{ + if (from_traj.empty() || to_traj.empty()) return; + to_traj.front().pose.position.z = from_traj.front().pose.position.z; + if (from_traj.size() < 2 || to_traj.size() < 2) return; + auto from = from_traj.begin() + 1; + auto s_from = tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + auto s_to = 0.0; + auto s_from_prev = 0.0; + for (auto to = to_traj.begin() + 1; to + 1 != to_traj.end(); ++to) { + s_to += tier4_autoware_utils::calcDistance2d(std::prev(to)->pose, to->pose); + for (; s_from < s_to && from + 1 != from_traj.end(); ++from) { + s_from_prev = s_from; + s_from += tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + } + const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); + to->pose.position.z = std::prev(from)->pose.position.z + + ratio * (from->pose.position.z - std::prev(from)->pose.position.z); + } + to_traj.back().pose.position.z = from->pose.position.z; +} + +void copyVelocity( + const std::vector & from_traj, std::vector & to_traj, + const geometry_msgs::msg::Pose & ego_pose) +{ + if (to_traj.empty() || from_traj.empty()) return; + + const auto closest_fn = [&](const auto & p1, const auto & p2) { + return tier4_autoware_utils::calcDistance2d(p1.pose, ego_pose) <= + tier4_autoware_utils::calcDistance2d(p2.pose, ego_pose); + }; + const auto first_from = std::min_element(from_traj.begin(), from_traj.end() - 1, closest_fn); + const auto first_to = std::min_element(to_traj.begin(), to_traj.end() - 1, closest_fn); + + auto to = to_traj.begin(); + for (; to != first_to; ++to) + to->longitudinal_velocity_mps = first_from->longitudinal_velocity_mps; + + auto from = first_from; + auto s_from = tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + auto s_to = 0.0; + auto s_from_prev = 0.0; + for (; to + 1 != to_traj.end(); ++to) { + s_to += tier4_autoware_utils::calcDistance2d(to->pose, std::next(to)->pose); + for (; s_from < s_to && from + 1 != from_traj.end(); ++from) { + s_from_prev = s_from; + s_from += tier4_autoware_utils::calcDistance2d(from->pose, std::next(from)->pose); + } + if ( + from->longitudinal_velocity_mps == 0.0 || std::prev(from)->longitudinal_velocity_mps == 0.0) { + to->longitudinal_velocity_mps = 0.0; + } else { + const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); + to->longitudinal_velocity_mps = + std::prev(from)->longitudinal_velocity_mps + + ratio * (from->longitudinal_velocity_mps - std::prev(from)->longitudinal_velocity_mps); + } + } + to_traj.back().longitudinal_velocity_mps = from->longitudinal_velocity_mps; +} + std::vector PathSampler::generateTrajectory(const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); @@ -303,7 +365,8 @@ std::vector PathSampler::generateTrajectory(const PlannerData & auto generated_traj_points = generatePath(planner_data); - applyInputVelocity(generated_traj_points, input_traj_points, planner_data.ego_pose); + copyVelocity(input_traj_points, generated_traj_points, planner_data.ego_pose); + copyZ(input_traj_points, generated_traj_points); publishDebugMarker(generated_traj_points); time_keeper_ptr_->toc(__func__, " "); @@ -415,57 +478,6 @@ std::vector PathSampler::generatePath(const PlannerData & plann return trajectory; } -void PathSampler::applyInputVelocity( - std::vector & output_traj_points, - const std::vector & input_traj_points, - const geometry_msgs::msg::Pose & ego_pose) const -{ - if (output_traj_points.empty()) return; - time_keeper_ptr_->tic(__func__); - - // crop forward for faster calculation - const auto forward_cropped_input_traj_points = [&]() { - const double generated_traj_length = motion_utils::calcArcLength(output_traj_points); - constexpr double margin_traj_length = 10.0; - - const size_t ego_seg_idx = - trajectory_utils::findEgoSegmentIndex(input_traj_points, ego_pose, ego_nearest_param_); - return motion_utils::cropForwardPoints( - input_traj_points, ego_pose.position, ego_seg_idx, - generated_traj_length + margin_traj_length); - }(); - - // update velocity - size_t input_traj_start_idx = 0; - for (size_t i = 0; i < output_traj_points.size(); i++) { - // crop backward for efficient calculation - const auto cropped_input_traj_points = std::vector{ - forward_cropped_input_traj_points.begin() + input_traj_start_idx, - forward_cropped_input_traj_points.end()}; - - const size_t nearest_seg_idx = trajectory_utils::findEgoSegmentIndex( - cropped_input_traj_points, output_traj_points.at(i).pose, ego_nearest_param_); - input_traj_start_idx = nearest_seg_idx; - - // calculate velocity with zero order hold - const double velocity = cropped_input_traj_points.at(nearest_seg_idx).longitudinal_velocity_mps; - output_traj_points.at(i).longitudinal_velocity_mps = velocity; - } - - // insert stop point explicitly - const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); - if (stop_idx) { - const auto input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; - const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( - output_traj_points, input_stop_pose, ego_nearest_param_); - - // calculate and insert stop pose on output trajectory - trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, stop_seg_idx); - } - - time_keeper_ptr_->toc(__func__, " "); -} - void PathSampler::publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const { time_keeper_ptr_->tic(__func__); @@ -485,7 +497,6 @@ void PathSampler::publishDebugMarker(const std::vector & traj_p // debug marker time_keeper_ptr_->tic("getDebugMarker"); - // const auto markers = getDebugMarker(debug_data_); visualization_msgs::msg::MarkerArray markers; if (debug_markers_pub_->get_subscription_count() > 0LU) { visualization_msgs::msg::Marker m;