Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(autoware_lidar_centerpoint): added the cuda_blackboard to centerpoint #9453

Open
wants to merge 14 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@

#include <memory>
#include <string>
#include <vector>

namespace autoware::image_projection_based_fusion
{
Expand All @@ -35,6 +36,11 @@ class PointPaintingTRT : public autoware::lidar_centerpoint::CenterPointTRT
const autoware::lidar_centerpoint::DensificationParam & densification_param,
const autoware::lidar_centerpoint::CenterPointConfig & config);

bool detect(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
std::vector<autoware::lidar_centerpoint::Box3D> & det_boxes3d,
bool & is_num_pillars_within_range);

protected:
bool preprocess(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,42 @@ PointPaintingTRT::PointPaintingTRT(
std::make_unique<image_projection_based_fusion::VoxelGenerator>(densification_param, config_);
}

bool PointPaintingTRT::detect(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
std::vector<autoware::lidar_centerpoint::Box3D> & det_boxes3d, bool & is_num_pillars_within_range)
{
is_num_pillars_within_range = true;

CHECK_CUDA_ERROR(cudaMemsetAsync(
encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_));
CHECK_CUDA_ERROR(
cudaMemsetAsync(spatial_features_d_.get(), 0, spatial_features_size_ * sizeof(float), stream_));
if (!preprocess(input_pointcloud_msg, tf_buffer)) {
RCLCPP_WARN_STREAM(
rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect.");
return false;
}
inference();
postProcess(det_boxes3d);

// Check the actual number of pillars after inference to avoid unnecessary synchronization.
unsigned int num_pillars = 0;
CHECK_CUDA_ERROR(
cudaMemcpy(&num_pillars, num_voxels_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost));

if (num_pillars >= config_.max_voxel_size_) {
rclcpp::Clock clock{RCL_ROS_TIME};
RCLCPP_WARN_THROTTLE(
rclcpp::get_logger("lidar_centerpoint"), clock, 1000,
"The actual number of pillars (%u) exceeds its maximum value (%zu). "
"Please considering increasing it since it may limit the detection performance.",
num_pillars, config_.max_voxel_size_);
is_num_pillars_within_range = false;
}

return true;
}

bool PointPaintingTRT::preprocess(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,12 @@
#include "autoware/lidar_centerpoint/cuda_utils.hpp"
#include "autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp"
#include "autoware/lidar_centerpoint/preprocess/voxel_generator.hpp"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"

#include <autoware/tensorrt_common/tensorrt_common.hpp>
#include <cuda_blackboard/cuda_pointcloud2.hpp>

#include "sensor_msgs/msg/point_cloud2.hpp"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <memory>
#include <string>
Expand All @@ -45,15 +45,17 @@ class CenterPointTRT
virtual ~CenterPointTRT();

bool detect(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
std::vector<Box3D> & det_boxes3d, bool & is_num_pillars_within_range);
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you use pointer as an argument, please remove dereference in node.cpp for detect call.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry, which de-reference?

void LidarCenterPointNode::pointCloudCallback(
  const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg)
...
bool is_success = detector_ptr_->detect(
    input_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range);

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You addressed it in 3893f19

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ahh sorry, had not pushed the branch 🙏

const tf2_ros::Buffer & tf_buffer, std::vector<Box3D> & det_boxes3d,
bool & is_num_pillars_within_range);

protected:
void initPtr();
void initTrt(const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param);

virtual bool preprocess(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer);
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
const tf2_ros::Buffer & tf_buffer);

void inference();

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,10 @@
#include <autoware_utils/ros/diagnostics_interface.hpp>
#include <autoware_utils/ros/published_time_publisher.hpp>
#include <autoware_utils/system/stop_watch.hpp>
#include <cuda_blackboard/cuda_adaptation.hpp>
#include <cuda_blackboard/cuda_blackboard_subscriber.hpp>
#include <cuda_blackboard/cuda_pointcloud2.hpp>
#include <cuda_blackboard/negotiated_types.hpp>
#include <rclcpp/rclcpp.hpp>

#include <autoware_perception_msgs/msg/detected_object_kinematics.hpp>
Expand All @@ -44,12 +48,14 @@ class LidarCenterPointNode : public rclcpp::Node
explicit LidarCenterPointNode(const rclcpp::NodeOptions & node_options);

private:
void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg);
void pointCloudCallback(
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg);

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_{tf_buffer_};

rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
std::unique_ptr<cuda_blackboard::CudaBlackboardSubscriber<cuda_blackboard::CudaPointCloud2>>
pointcloud_sub_;
rclcpp::Publisher<autoware_perception_msgs::msg::DetectedObjects>::SharedPtr objects_pub_;

std::vector<std::string> class_names_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,19 +15,21 @@
#ifndef AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_
#define AUTOWARE__LIDAR_CENTERPOINT__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_

#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include <cuda_blackboard/cuda_pointcloud2.hpp>

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <list>
#include <string>
#include <utility>
#ifdef ROS_DISTRO_GALACTIC
#include "tf2_sensor_msgs/tf2_sensor_msgs.h"
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#else
#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
#endif

#include "autoware/lidar_centerpoint/cuda_utils.hpp"
#include <memory>

namespace autoware::lidar_centerpoint
{
Expand All @@ -50,10 +52,7 @@ class DensificationParam

struct PointCloudWithTransform
{
cuda::unique_ptr<float[]> points_d{nullptr};
std_msgs::msg::Header header;
std::size_t num_points{0};
std::size_t point_step{0};
std::shared_ptr<const cuda_blackboard::CudaPointCloud2> input_pointcloud_msg_ptr;
Eigen::Affine3f affine_past2world;
};

Expand All @@ -63,8 +62,8 @@ class PointCloudDensification
explicit PointCloudDensification(const DensificationParam & param);

bool enqueuePointCloud(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
cudaStream_t stream);
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
const tf2_ros::Buffer & tf_buffer);

double getCurrentTimestamp() const { return current_timestamp_; }
Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; }
Expand All @@ -80,7 +79,8 @@ class PointCloudDensification

private:
void enqueue(
const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine, cudaStream_t stream);
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
const Eigen::Affine3f & affine);
void dequeue();

DensificationParam param_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
#include "autoware/lidar_centerpoint/centerpoint_config.hpp"
#include "autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp"

#include "sensor_msgs/msg/point_cloud2.hpp"
#include <cuda_blackboard/cuda_pointcloud2.hpp>

#include <memory>
#include <vector>
Expand All @@ -35,8 +35,8 @@ class VoxelGeneratorTemplate
virtual std::size_t generateSweepPoints(float * d_points, cudaStream_t stream) = 0;

bool enqueuePointCloud(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
cudaStream_t stream);
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
const tf2_ros::Buffer & tf_buffer);

protected:
std::unique_ptr<PointCloudDensification> pd_ptr_{nullptr};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
<load_composable_node target="$(var pointcloud_container_name)">
<composable_node pkg="autoware_lidar_centerpoint" plugin="autoware::lidar_centerpoint::LidarCenterPointNode" name="lidar_centerpoint">
<remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
<remap from="~/input/pointcloud/cuda" to="$(var input/pointcloud)/cuda"/>
<remap from="~/output/objects" to="$(var output/objects)"/>
<param from="$(var model_param_path)" allow_substs="true"/>
<param from="$(var ml_package_param_path)" allow_substs="true"/>
Expand All @@ -30,6 +31,7 @@
<group unless="$(var use_pointcloud_container)">
<node pkg="autoware_lidar_centerpoint" exec="autoware_lidar_centerpoint_node" name="lidar_centerpoint" output="screen">
<remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
<remap from="~/input/pointcloud/cuda" to="$(var input/pointcloud)/cuda"/>
<remap from="~/output/objects" to="$(var output/objects)"/>
<param from="$(var model_param_path)" allow_substs="true"/>
<param from="$(var ml_package_param_path)" allow_substs="true"/>
Expand Down
12 changes: 7 additions & 5 deletions perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -170,8 +170,9 @@ void CenterPointTRT::initTrt(
}

bool CenterPointTRT::detect(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
std::vector<Box3D> & det_boxes3d, bool & is_num_pillars_within_range)
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
const tf2_ros::Buffer & tf_buffer, std::vector<Box3D> & det_boxes3d,
bool & is_num_pillars_within_range)
{
is_num_pillars_within_range = true;

Expand All @@ -180,7 +181,7 @@ bool CenterPointTRT::detect(
CHECK_CUDA_ERROR(
cudaMemsetAsync(spatial_features_d_.get(), 0, spatial_features_size_ * sizeof(float), stream_));

if (!preprocess(input_pointcloud_msg, tf_buffer)) {
if (!preprocess(input_pointcloud_msg_ptr, tf_buffer)) {
RCLCPP_WARN(rclcpp::get_logger("lidar_centerpoint"), "Fail to preprocess and skip to detect.");
return false;
}
Expand Down Expand Up @@ -208,9 +209,10 @@ bool CenterPointTRT::detect(
}

bool CenterPointTRT::preprocess(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer)
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & input_pointcloud_msg_ptr,
const tf2_ros::Buffer & tf_buffer)
{
bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer, stream_);
bool is_success = vg_ptr_->enqueuePointCloud(input_pointcloud_msg_ptr, tf_buffer);
if (!is_success) {
return false;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,8 @@ All Rights Reserved 2019-2020.
#include "autoware/lidar_centerpoint/cuda_utils.hpp"
#include "autoware/lidar_centerpoint/postprocess/circle_nms_kernel.hpp"
#include "autoware/lidar_centerpoint/utils.hpp"
#include "thrust/host_vector.h"

#include <thrust/host_vector.h>

namespace
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,10 @@

#include "autoware/lidar_centerpoint/postprocess/circle_nms_kernel.hpp"
#include "autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp"
#include "thrust/count.h"
#include "thrust/device_vector.h"
#include "thrust/sort.h"

#include <thrust/count.h>
#include <thrust/device_vector.h>
#include <thrust/sort.h>

namespace
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,17 +14,20 @@

#include "autoware/lidar_centerpoint/preprocess/pointcloud_densification.hpp"

#include "pcl_conversions/pcl_conversions.h"
#include "pcl_ros/transforms.hpp"
#include <pcl_ros/transforms.hpp>

#include "boost/optional.hpp"
#include <boost/optional.hpp>

#include <pcl_conversions/pcl_conversions.h>

#include <memory>
#include <string>
#include <utility>

#ifdef ROS_DISTRO_GALACTIC
#include "tf2_eigen/tf2_eigen.h"
#include <tf2_eigen/tf2_eigen.h>
#else
#include "tf2_eigen/tf2_eigen.hpp"
#include <tf2_eigen/tf2_eigen.hpp>
#endif

namespace
Expand Down Expand Up @@ -60,10 +63,10 @@ PointCloudDensification::PointCloudDensification(const DensificationParam & para
}

bool PointCloudDensification::enqueuePointCloud(
const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
cudaStream_t stream)
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & pointcloud_msg_ptr,
const tf2_ros::Buffer & tf_buffer)
{
const auto header = pointcloud_msg.header;
const auto header = pointcloud_msg_ptr->header;

if (param_.pointcloud_cache_size() > 1) {
auto transform_world2current =
Expand All @@ -73,9 +76,9 @@ bool PointCloudDensification::enqueuePointCloud(
}
auto affine_world2current = transformToEigen(transform_world2current.get());

enqueue(pointcloud_msg, affine_world2current, stream);
enqueue(pointcloud_msg_ptr, affine_world2current);
} else {
enqueue(pointcloud_msg, Eigen::Affine3f::Identity(), stream);
enqueue(pointcloud_msg_ptr, Eigen::Affine3f::Identity());
}

dequeue();
Expand All @@ -84,24 +87,13 @@ bool PointCloudDensification::enqueuePointCloud(
}

void PointCloudDensification::enqueue(
const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current,
cudaStream_t stream)
const std::shared_ptr<const cuda_blackboard::CudaPointCloud2> & msg_ptr,
const Eigen::Affine3f & affine_world2current)
{
affine_world2current_ = affine_world2current;
current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds();

assert(sizeof(uint8_t) * msg.width * msg.height * msg.point_step % sizeof(float) == 0);
auto points_d = cuda::make_unique<float[]>(
sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(float));
CHECK_CUDA_ERROR(cudaMemcpyAsync(
points_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step,
cudaMemcpyHostToDevice, stream));

PointCloudWithTransform pointcloud = {
std::move(points_d), msg.header, msg.width * msg.height, msg.point_step,
affine_world2current.inverse()};

pointcloud_cache_.push_front(std::move(pointcloud));
current_timestamp_ = rclcpp::Time(msg_ptr->header.stamp).seconds();
PointCloudWithTransform pointcloud = {msg_ptr, affine_world2current.inverse()};
pointcloud_cache_.push_front(pointcloud);
}

void PointCloudDensification::dequeue()
Expand Down
Loading
Loading