From e421b263865b490643d3a85b5eb9e17d6fe98a92 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Wed, 7 Sep 2022 11:50:10 +0900 Subject: [PATCH] feat(behavior_path_planner): add new turn signal algorithm (#1964) * clean code Signed-off-by: yutaka * clean format Signed-off-by: yutaka * udpate Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * add test Signed-off-by: yutaka * add test * add test Signed-off-by: yutaka * fix(avoidance): use new turn signal algorithm Signed-off-by: satoshi-ota * fix(avoidance): fix desired_start_point position Signed-off-by: satoshi-ota * update Signed-off-by: yutaka * change policy Signed-off-by: yutaka * feat(behavior_path_planner): update pull_over for new blinker logic Signed-off-by: kosuke55 * feat(behavior_path_planner): update pull_out for new blinker logic * tmp: install flask via pip Signed-off-by: Takayuki Murooka * feat(lane_change): added lane change point * fix start_point and non backward driving turn signal Signed-off-by: kosuke55 * get 3 second during constructing lane change path Signed-off-by: Muhammad Zulfaqar Azmi * fix pull over desired start point Signed-off-by: kosuke55 * update Signed-off-by: yutaka * delete Signed-off-by: yutaka * Update Readme * Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe * fix format Signed-off-by: yutaka Signed-off-by: yutaka Signed-off-by: satoshi-ota Signed-off-by: kosuke55 Signed-off-by: Takayuki Murooka Signed-off-by: Muhammad Zulfaqar Azmi Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: satoshi-ota Co-authored-by: kosuke55 Co-authored-by: Takayuki Murooka Co-authored-by: Muhammad Zulfaqar Azmi Co-authored-by: Fumiya Watanabe Signed-off-by: tomoya.kimura --- .../src/pointpainting_fusion/node.cpp | 5 ++++- .../include/lidar_centerpoint/centerpoint_config.hpp | 8 +++++++- .../lidar_centerpoint/launch/lidar_centerpoint.launch.xml | 3 +++ .../lib/postprocess/postprocess_kernel.cu | 7 ++++--- perception/lidar_centerpoint/src/node.cpp | 5 ++++- 5 files changed, 22 insertions(+), 6 deletions(-) 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 870028bce8161..4b1a351739fc5 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -33,6 +33,8 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt static_cast(this->declare_parameter("score_threshold", 0.4)); const float circle_nms_dist_threshold = static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); + const float yaw_norm_threshold = + static_cast(this->declare_parameter("yaw_norm_threshold", 0.5)); // densification param const std::string densification_world_frame_id = this->declare_parameter("densification_world_frame_id", "map"); @@ -64,7 +66,8 @@ PointpaintingFusionNode::PointpaintingFusionNode(const rclcpp::NodeOptions & opt densification_world_frame_id, densification_num_past_frames); centerpoint::CenterPointConfig config( class_names_.size(), point_feature_size, max_voxel_size, pointcloud_range, voxel_size, - downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold); + downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, + yaw_norm_threshold); // create detector detector_ptr_ = std::make_unique( diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp index 373e661650e16..152702a5bd359 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_config.hpp @@ -27,7 +27,8 @@ class CenterPointConfig const std::size_t class_size, const float point_feature_size, const std::size_t max_voxel_size, const std::vector & point_cloud_range, const std::vector & voxel_size, const std::size_t downsample_factor, const std::size_t encoder_in_feature_size, - const float score_threshold, const float circle_nms_dist_threshold) + const float score_threshold, const float circle_nms_dist_threshold, + const float yaw_norm_threshold) { class_size_ = class_size; point_feature_size_ = point_feature_size; @@ -56,6 +57,10 @@ class CenterPointConfig circle_nms_dist_threshold_ = circle_nms_dist_threshold; } + if (yaw_norm_threshold > 0 && yaw_norm_threshold < 1) { + yaw_norm_threshold_ = yaw_norm_threshold; + } + grid_size_x_ = static_cast((range_max_x_ - range_min_x_) / voxel_size_x_); grid_size_y_ = static_cast((range_max_y_ - range_min_y_) / voxel_size_y_); grid_size_z_ = static_cast((range_max_z_ - range_min_z_) / voxel_size_z_); @@ -97,6 +102,7 @@ class CenterPointConfig // post-process params float score_threshold_{0.4f}; float circle_nms_dist_threshold_{1.5f}; + float yaw_norm_threshold_{0.5f}; // calculated params std::size_t grid_size_x_ = (range_max_x_ - range_min_x_) / voxel_size_x_; diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 53fb01221ecc2..839300dbcf376 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -6,11 +6,14 @@ + + + diff --git a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu index 3015374f0f72d..03fcae205634c 100644 --- a/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/postprocess/postprocess_kernel.cu @@ -54,7 +54,7 @@ __global__ void generateBoxes3D_kernel( const float * out_rot, const float * out_vel, const float voxel_size_x, const float voxel_size_y, const float range_min_x, const float range_min_y, const std::size_t down_grid_size_x, const std::size_t down_grid_size_y, const std::size_t downsample_factor, const int class_size, - Box3D * det_boxes3d) + const float yaw_norm_threshold, Box3D * det_boxes3d) { // generate boxes3d from the outputs of the network. // shape of out_*: (N, DOWN_GRID_SIZE_Y, DOWN_GRID_SIZE_X) @@ -88,11 +88,12 @@ __global__ void generateBoxes3D_kernel( const float h = out_dim[down_grid_size * 2 + idx]; const float yaw_sin = out_rot[down_grid_size * 0 + idx]; const float yaw_cos = out_rot[down_grid_size * 1 + idx]; + const float yaw_norm = sqrtf(yaw_sin * yaw_sin + yaw_cos * yaw_cos); const float vel_x = out_vel[down_grid_size * 0 + idx]; const float vel_y = out_vel[down_grid_size * 1 + idx]; det_boxes3d[idx].label = label; - det_boxes3d[idx].score = max_score; + det_boxes3d[idx].score = yaw_norm >= yaw_norm_threshold ? max_score : 0.f; det_boxes3d[idx].x = x; det_boxes3d[idx].y = y; det_boxes3d[idx].z = z; @@ -123,7 +124,7 @@ cudaError_t PostProcessCUDA::generateDetectedBoxes3D_launch( out_heatmap, out_offset, out_z, out_dim, out_rot, out_vel, config_.voxel_size_x_, config_.voxel_size_y_, config_.range_min_x_, config_.range_min_y_, config_.down_grid_size_x_, config_.down_grid_size_y_, config_.downsample_factor_, config_.class_size_, - thrust::raw_pointer_cast(boxes3d_d_.data())); + config_.yaw_norm_threshold_, thrust::raw_pointer_cast(boxes3d_d_.data())); // suppress by socre const auto num_det_boxes3d = thrust::count_if( diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index c733171d78c99..bda666e1c74c7 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -39,6 +39,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti static_cast(this->declare_parameter("score_threshold", 0.4)); const float circle_nms_dist_threshold = static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); + const float yaw_norm_threshold = + static_cast(this->declare_parameter("yaw_norm_threshold", 0.5)); const std::string densification_world_frame_id = this->declare_parameter("densification_world_frame_id", "map"); const int densification_num_past_frames = @@ -80,7 +82,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti } CenterPointConfig config( class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, - downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold); + downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, + yaw_norm_threshold); detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config);