diff --git a/build_depends.repos b/build_depends.repos index 2313a9be487a6..6c4fe0703b796 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -58,3 +58,7 @@ repositories: type: git url: https://github.com/autowarefoundation/ament_cmake.git version: feat/faster_ament_libraries_deduplicate + universe/external/bevdet_vendor: + type: git + url: https://github.com/autowarefoundation/bevdet_vendor.git + version: bevdet_vendor-ros2 diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 912a7f3e48f41..45d6f0b5823aa 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -221,6 +221,28 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_bev_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_bev_detector.launch.xml new file mode 100644 index 0000000000000..a8b1f3f63985d --- /dev/null +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_bev_detector.launch.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/autoware_tensorrt_bevdet/CMakeLists.txt b/perception/autoware_tensorrt_bevdet/CMakeLists.txt new file mode 100644 index 0000000000000..dbdacec3f41ea --- /dev/null +++ b/perception/autoware_tensorrt_bevdet/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.17) +project(autoware_tensorrt_bevdet) + +add_compile_options(-std=c++17) + +find_package(bevdet_vendor) +if(NOT ${bevdet_vendor_FOUND}) + message(WARNING "The bevdet_vendor package is not found. Please check its dependencies.") + return() +endif() + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(autoware_perception_msgs REQUIRED) + +ament_auto_add_library(${PROJECT_NAME}_component SHARED + src/bevdet_node.cpp + src/ros_utils.cpp +) + +include_directories( + include + ${EIGEN3_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${cv_bridge_INCLUDE_DIRS} + ${tf2_geometry_msgs_INCLUDE_DIRS} + ${autoware_perception_msgs_INCLUDE_DIRS} + ${bevdet_vendor_INCLUDE_DIRS} +) + +target_link_libraries(${PROJECT_NAME}_component + rclcpp::rclcpp + ${bevdet_vendor_TARGETS} + ${OpenCV_LIBS} +) + +rclcpp_components_register_node(${PROJECT_NAME}_component + PLUGIN "autoware::tensorrt_bevdet::TRTBEVDetNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/perception/autoware_tensorrt_bevdet/README.md b/perception/autoware_tensorrt_bevdet/README.md new file mode 100644 index 0000000000000..b2ce948dab0ab --- /dev/null +++ b/perception/autoware_tensorrt_bevdet/README.md @@ -0,0 +1,77 @@ +# tensorrt_bevdet + +## Purpose + +The core algorithm, named `BEVDet`, it unifies multi-view images into the perspective of BEV for 3D object detection task. + +## Inner-workings / Algorithms + +### Cite + + + +- Junjie Huang, Guan Huang, "BEVPoolv2: A Cutting-edge Implementation of BEVDet Toward Deployment", [[ref](https://arxiv.org/pdf/2211.17111)] +- [bevdet_vendor](https://github.com/autowarefoundation/bevdet_vendor) package are copied from the [original codes](https://github.com/LCH1238/bevdet-tensorrt-cpp/tree/one) (The TensorRT, C++ implementation by LCH1238) and modified. +- This package is ported version toward Autoware from [bevdet_vendor](https://github.com/autowarefoundation/bevdet_vendor). + +## Inputs / Outputs + +### Inputs + +| Name | Type | Description | +| ---------------------------------- | ------------------------------ | ----------------------------------- | +| `~/input/topic_img_fl` | `sensor_msgs::msg::Image` | input front_left camera image | +| `~/input/topic_img_f` | `sensor_msgs::msg::Image` | input front camera image | +| `~/input/topic_img_fr` | `sensor_msgs::msg::Image` | input front_right camera image | +| `~/input/topic_img_bl` | `sensor_msgs::msg::Image` | input back_left camera image | +| `~/input/topic_img_b` | `sensor_msgs::msg::Image` | input back camera image | +| `~/input/topic_img_br` | `sensor_msgs::msg::Image` | input back_right camera image | +| `~/input/topic_img_fl/camera_info` | `sensor_msgs::msg::CameraInfo` | input front_left camera parameters | +| `~/input/topic_img_f/camera_info` | `sensor_msgs::msg::CameraInfo` | input front camera parameters | +| `~/input/topic_img_fr/camera_info` | `sensor_msgs::msg::CameraInfo` | input front_right camera parameters | +| `~/input/topic_img_bl/camera_info` | `sensor_msgs::msg::CameraInfo` | input back_left camera parameters | +| `~/input/topic_img_b/camera_info` | `sensor_msgs::msg::CameraInfo` | input back camera parameters | +| `~/input/topic_img_br/camera_info` | `sensor_msgs::msg::CameraInfo` | input back_right camera parameters | + +### Outputs + +| Name | Type | Description | +| ---------------- | ------------------------------------------------ | ---------------- | +| `~/output/boxes` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects | + +## How to Use Tensorrt BEVDet Node + +1. launch `tensorrt_bevdet_node` + + ```bash + + ros2 launch autoware_tensorrt_bevdet tensorrt_bevdet_node.launch.xml + ``` + +2. play ros2 bag of nuScenes data + + Please refer to open source repository [ros2_dataset_bridge](https://github.com/Owen-Liuyuxuan/ros2_dataset_bridge) to publish the ROS 2 topics for NuScenes dataset. + +## Trained Models + +You can download the onnx format of trained models by clicking on the links below. + +- BEVDet: [bevdet_one_lt_d.onnx](https://drive.google.com/file/d/1eMGJfdCVlDPBphBTjMcnIh3wdW7Q7WZB/view?usp=sharing) + +The `BEVDet` model was trained in `NuScenes` dataset for 20 epochs. + +## Limitation + +The model is trained on open-source dataset `NuScenes` and has poor generalization on its own dataset, If you want to use this model to infer your data, you need to retrain it. + +## Training BEVDet Model + +If you want to train model using the [TIER IV's internal database(~2600 key frames)](https://drive.google.com/file/d/1UaarK88HZu09sf7Ix-bEVl9zGNGFwTVL/view?usp=sharing), please refer to the following repositories:[BEVDet adapted to TIER IV dataset](https://github.com/cyn-liu/BEVDet/tree/train_export). + +## References/External links + +[1] + +[2] + +[3] diff --git a/perception/autoware_tensorrt_bevdet/config/bevdet.param.yaml b/perception/autoware_tensorrt_bevdet/config/bevdet.param.yaml new file mode 100644 index 0000000000000..bbaef399687ad --- /dev/null +++ b/perception/autoware_tensorrt_bevdet/config/bevdet.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + # weight files + onnx_path: "$(var model_path)/$(var model_name).onnx" + engine_path: "$(var model_path)/$(var model_name).engine" + data_params: + CAM_NUM: 6 + cams: ["CAM_FRONT_LEFT", "CAM_FRONT", "CAM_FRONT_RIGHT", "CAM_BACK_LEFT", "CAM_BACK", "CAM_BACK_RIGHT"] + post_process_params: + # post-process params + score_threshold: 0.2 + class_names: ["car", "truck", "construction_vehicle", "bus", "trailer", "barrier", "motorcycle", "bicycle", "pedestrian", "traffic_cone"] diff --git a/perception/autoware_tensorrt_bevdet/config/bevdet_r50_4dlongterm_depth.yaml b/perception/autoware_tensorrt_bevdet/config/bevdet_r50_4dlongterm_depth.yaml new file mode 100644 index 0000000000000..8f0e4ac785a97 --- /dev/null +++ b/perception/autoware_tensorrt_bevdet/config/bevdet_r50_4dlongterm_depth.yaml @@ -0,0 +1,63 @@ +bev_range: [-51.2, -51.2, -5.0, 51.2, 51.2, 3.0] +mean: [123.675, 116.28, 103.53] +std: [58.395, 57.12, 57.375] +use_depth: true +use_adj: true +adj_num: 8 +transform_size: 6 +cam_params_size: 27 +sampling: nearest # nearest # or bicubic +data_config: + Ncams: 6 + cams: [CAM_FRONT_LEFT, CAM_FRONT, CAM_FRONT_RIGHT, CAM_BACK_LEFT, CAM_BACK, CAM_BACK_RIGHT] + resize_radio: 0.44 + crop: [140, 0] + flip: true + input_size: [256, 704] + resize: [-0.06, 0.11] + resize_test: 0.0 + rot: [-5.4, 5.4] + src_size: [900, 1600] +grid_config: + depth: [1.0, 60.0, 0.5] + x: [-51.2, 51.2, 0.8] + y: [-51.2, 51.2, 0.8] + z: [-5, 3, 8] +model: + bevpool_channels: 80 + coder: + code_size: 9 + max_num: 500 + post_center_range: [-61.2, -61.2, -10.0, 61.2, 61.2, 10.0] + score_threshold: 0.1 + common_head: + channels: [2, 1, 3, 2, 2] + names: [reg, height, dim, rot, vel] + down_sample: 16 + tasks: + - class_names: + [ + car, + truck, + construction_vehicle, + bus, + trailer, + barrier, + motorcycle, + bicycle, + pedestrian, + traffic_cone, + ] + num_class: 10 +test_cfg: + max_per_img: 500 + max_pool_nms: false + min_radius: [4, 12, 10, 1, 0.85, 0.175] + nms_rescale_factor: + - [1.0, 0.7, 0.7, 0.4, 0.55, 1.1, 1.0, 1.0, 1.5, 3.5] + nms_thr: [0.2] + nms_type: [rotate] + post_center_limit_range: [-61.2, -61.2, -10.0, 61.2, 61.2, 10.0] + post_max_size: 500 + pre_max_size: 1000 + score_threshold: 0.1 diff --git a/perception/autoware_tensorrt_bevdet/include/autoware/tensorrt_bevdet/bevdet_node.hpp b/perception/autoware_tensorrt_bevdet/include/autoware/tensorrt_bevdet/bevdet_node.hpp new file mode 100644 index 0000000000000..5d0ab4d5ac150 --- /dev/null +++ b/perception/autoware_tensorrt_bevdet/include/autoware/tensorrt_bevdet/bevdet_node.hpp @@ -0,0 +1,195 @@ +// Copyright 2024 AutoCore, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// cspell:ignore BEVDET, thre, TRTBEV, bevdet, caminfo, intrin +#ifndef AUTOWARE__TENSORRT_BEVDET__BEVDET_NODE_HPP_ +#define AUTOWARE__TENSORRT_BEVDET__BEVDET_NODE_HPP_ + +#include "autoware/tensorrt_bevdet/ros_utils.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_bevdet +{ +class TRTBEVDetNode : public rclcpp::Node +{ + /** + * @class TRTBEVDetNode + * @brief This class represents a node for performing object detection using TensorRT on BEV + * (Bird's Eye View) images. + */ +private: + size_t img_N_; ///< Number of images + uint32_t img_w_; ///< Width of the images + uint32_t img_h_; ///< Height of the images + + std::string model_config_; ///< Path to the model configuration file + + std::string onnx_file_; ///< Path to the ONNX file + std::string engine_file_; ///< Path to the TensorRT engine file + + std::vector imgs_name_; ///< Names of the images + std::vector class_names_; ///< Names of the object classes + + camsData sampleData_; ///< Sample data for camera parameters + std::shared_ptr bevdet_; ///< Object for performing object detection + + uchar * imgs_dev_ = nullptr; ///< Device pointer for storing the images + float score_thre_; ///< Score threshold for object detection + bool has_twist_ = true; /// wether set twist for objects + + rclcpp::Publisher::SharedPtr + pub_boxes_; ///< Publisher for publishing the detected objects + + // Subscribers of camera info for each camera, no need to synchronize + rclcpp::Subscription::SharedPtr + sub_f_caminfo_; ///< Subscriber for front camera info + rclcpp::Subscription::SharedPtr + sub_fl_caminfo_; ///< Subscriber for front-left camera info + rclcpp::Subscription::SharedPtr + sub_fr_caminfo_; ///< Subscriber for front-right camera info + rclcpp::Subscription::SharedPtr + sub_b_caminfo_; ///< Subscriber for back camera info + rclcpp::Subscription::SharedPtr + sub_bl_caminfo_; ///< Subscriber for back-left camera info + rclcpp::Subscription::SharedPtr + sub_br_caminfo_; ///< Subscriber for back-right camera info + + std::vector + caminfo_received_; ///< Flag indicating if camera info has been received for each camera + bool camera_info_received_flag_ = + false; ///< Flag indicating if camera info has been received for all cameras + bool initialized_ = false; /// Flag indicating if img_w_ and img_h_ has been initialized + + // tf listener + std::shared_ptr tf_listener_{ + nullptr}; ///< TF listener for transforming coordinates + std::unique_ptr tf_buffer_; ///< Buffer for storing TF transforms + + // Camera parameters; + std::vector cams_intrin_; ///< Intrinsic camera parameters for each camera + std::vector> + cams2ego_rot_; ///< Rotation from camera frame to ego frame for each camera + std::vector + cams2ego_trans_; ///< Translation from camera frame to ego frame for each camera + + // Subscribers of images for each camera, synchronized + message_filters::Subscriber + sub_f_img_; ///< Subscriber for front camera image + message_filters::Subscriber + sub_fl_img_; ///< Subscriber for front-left camera image + message_filters::Subscriber + sub_fr_img_; ///< Subscriber for front-right camera image + message_filters::Subscriber + sub_b_img_; ///< Subscriber for back camera image + message_filters::Subscriber + sub_bl_img_; ///< Subscriber for back-left camera image + message_filters::Subscriber + sub_br_img_; ///< Subscriber for back-right camera image + + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::Image, + sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::Image> + MySyncPolicy; + + typedef message_filters::Synchronizer Sync; + std::shared_ptr sync_; ///< Synchronizer for synchronizing image callbacks + + // Timer for checking initialization + rclcpp::TimerBase::SharedPtr timer_; ///< Timer for checking initialization + + /** + * @brief Starts the subscription to camera info topics for each camera. + */ + void startCameraInfoSubscription(); + + /** + * @brief Checks if the node has been initialized properly. + */ + void checkInitialization(); + + /** + * @brief Initializes the object detection model. + */ + void initModel(); + + /** + * @brief Starts the subscription to image topics for each camera. + */ + void startImageSubscription(); + +public: + /** + * @brief Constructor for TRTBEVDetNode. + * @param options The options for the node. + */ + explicit TRTBEVDetNode(const rclcpp::NodeOptions & options); + + /** + * @brief Destructor for TRTBEVDetNode. + */ + ~TRTBEVDetNode(); + + /** + * @brief Callback function for synchronized image messages. + * @param msg_fl_img The front-left camera image message. + * @param msg_f_img The front camera image message. + * @param msg_fr_img The front-right camera image message. + * @param msg_bl_img The back-left camera image message. + * @param msg_b_img The back camera image message. + * @param msg_br_img The back-right camera image message. + */ + void callback( + const sensor_msgs::msg::Image::ConstSharedPtr & msg_fl_img, + const sensor_msgs::msg::Image::ConstSharedPtr & msg_f_img, + const sensor_msgs::msg::Image::ConstSharedPtr & msg_fr_img, + const sensor_msgs::msg::Image::ConstSharedPtr & msg_bl_img, + const sensor_msgs::msg::Image::ConstSharedPtr & msg_b_img, + const sensor_msgs::msg::Image::ConstSharedPtr & msg_br_img); + + /** + * @brief Callback function for camera info messages. This function also reads from TF to get the + * transformation from the camera frame to the ego frame. + * @param idx The index of the camera. + * @param msg The camera info message. + */ + void cameraInfoCallback(int idx, const sensor_msgs::msg::CameraInfo::SharedPtr msg); +}; +} // namespace tensorrt_bevdet +} // namespace autoware +#endif // AUTOWARE__TENSORRT_BEVDET__BEVDET_NODE_HPP_ diff --git a/perception/autoware_tensorrt_bevdet/include/autoware/tensorrt_bevdet/ros_utils.hpp b/perception/autoware_tensorrt_bevdet/include/autoware/tensorrt_bevdet/ros_utils.hpp new file mode 100644 index 0000000000000..8ee6491d38605 --- /dev/null +++ b/perception/autoware_tensorrt_bevdet/include/autoware/tensorrt_bevdet/ros_utils.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 AutoCore, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// cspell:ignore BEVDET, bevdet + +#ifndef AUTOWARE__TENSORRT_BEVDET__ROS_UTILS_HPP_ +#define AUTOWARE__TENSORRT_BEVDET__ROS_UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +namespace autoware::tensorrt_bevdet +{ +uint8_t getSemanticType(const std::string & class_name); + +void box3DToDetectedObjects( + const std::vector & boxes, autoware_perception_msgs::msg::DetectedObjects & objects, + const std::vector & class_names, float score_thre, const bool has_twist); + +// Get the rotation and translation from a geometry_msgs::msg::TransformStamped +void getTransform( + const geometry_msgs::msg::TransformStamped & transform, Eigen::Quaternion & rot, + Eigen::Translation3f & translation); + +// Get the camera intrinsics from a sensor_msgs::msg::CameraInfo +void getCameraIntrinsics( + const sensor_msgs::msg::CameraInfo::SharedPtr msg, Eigen::Matrix3f & intrinsics); + +// Convert images from OpenCV's cv:: Mat format to a specific format +void imageTransport(std::vector imgs, uchar * out_imgs, size_t width, size_t height); + +} // namespace autoware::tensorrt_bevdet +#endif // AUTOWARE__TENSORRT_BEVDET__ROS_UTILS_HPP_ diff --git a/perception/autoware_tensorrt_bevdet/launch/tensorrt_bevdet.launch.xml b/perception/autoware_tensorrt_bevdet/launch/tensorrt_bevdet.launch.xml new file mode 100644 index 0000000000000..bbd585833f3ea --- /dev/null +++ b/perception/autoware_tensorrt_bevdet/launch/tensorrt_bevdet.launch.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/autoware_tensorrt_bevdet/package.xml b/perception/autoware_tensorrt_bevdet/package.xml new file mode 100644 index 0000000000000..b10af49221a5c --- /dev/null +++ b/perception/autoware_tensorrt_bevdet/package.xml @@ -0,0 +1,31 @@ + + + autoware_tensorrt_bevdet + 0.0.1 + This package is ported version toward Autoware from bevdet_vendor + + Cynthia + Cynthia + Yuxuan Liu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_perception_msgs + bevdet_vendor + cv_bridge + geometry_msgs + libopencv-dev + rclcpp + rclcpp_components + sensor_msgs + tf2_geometry_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/autoware_tensorrt_bevdet/src/bevdet_node.cpp b/perception/autoware_tensorrt_bevdet/src/bevdet_node.cpp new file mode 100644 index 0000000000000..afdbe6b212450 --- /dev/null +++ b/perception/autoware_tensorrt_bevdet/src/bevdet_node.cpp @@ -0,0 +1,222 @@ +// Copyright 2024 AutoCore, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// cspell:ignore BEVDET, thre, TRTBEV, bevdet, caminfo, intrin, Ncams, bevfeat, dlongterm + +#include "autoware/tensorrt_bevdet/bevdet_node.hpp" + +namespace autoware +{ +namespace tensorrt_bevdet +{ +TRTBEVDetNode::TRTBEVDetNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("tensorrt_bevdet_node", node_options) +{ // Only start camera info subscription and tf listener at the beginning + img_N_ = this->declare_parameter("data_params.CAM_NUM", 6); // camera num 6 + + caminfo_received_ = std::vector(img_N_, false); + cams_intrin_ = std::vector(img_N_); + cams2ego_rot_ = std::vector>(img_N_); + cams2ego_trans_ = std::vector(img_N_); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + startCameraInfoSubscription(); + + // Wait for camera info and tf transform initialization + timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), std::bind(&TRTBEVDetNode::checkInitialization, this)); +} + +void TRTBEVDetNode::initModel() +{ + score_thre_ = this->declare_parameter("post_process_params.score_threshold", 0.2); + + model_config_ = this->declare_parameter("model_config", "bevdet_r50_4dlongterm_depth.yaml"); + + onnx_file_ = this->declare_parameter("onnx_path", "bevdet_one_lt_d.onnx"); + engine_file_ = this->declare_parameter("engine_path", "bevdet_one_lt_d.engine"); + + imgs_name_ = this->declare_parameter>("data_params.cams"); + class_names_ = + this->declare_parameter>("post_process_params.class_names"); + + RCLCPP_INFO_STREAM(this->get_logger(), "Successful load config!"); + + sampleData_.param = camParams(cams_intrin_, cams2ego_rot_, cams2ego_trans_); + + RCLCPP_INFO_STREAM(this->get_logger(), "Successful load image params!"); + + bevdet_ = std::make_shared( + model_config_, img_N_, sampleData_.param.cams_intrin, sampleData_.param.cams2ego_rot, + sampleData_.param.cams2ego_trans, onnx_file_, engine_file_); + + RCLCPP_INFO_STREAM(this->get_logger(), "Successful create bevdet!"); + + CHECK_CUDA(cudaMalloc( + reinterpret_cast(&imgs_dev_), img_N_ * 3 * img_w_ * img_h_ * sizeof(uchar))); + + pub_boxes_ = this->create_publisher( + "~/output/boxes", rclcpp::QoS{1}); +} + +void TRTBEVDetNode::checkInitialization() +{ + if (camera_info_received_flag_) { + RCLCPP_INFO_STREAM( + this->get_logger(), "Camera Info and TF Transform Initialization completed!"); + initModel(); + startImageSubscription(); + timer_->cancel(); + timer_.reset(); + } else { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Waiting for Camera Info and TF Transform Initialization..."); + } +} + +void TRTBEVDetNode::startImageSubscription() +{ + using std::placeholders::_1; + using std::placeholders::_2; + using std::placeholders::_3; + using std::placeholders::_4; + using std::placeholders::_5; + using std::placeholders::_6; + + sub_f_img_.subscribe(this, "~/input/topic_img_f", rclcpp::QoS{1}.get_rmw_qos_profile()); + sub_b_img_.subscribe(this, "~/input/topic_img_b", rclcpp::QoS{1}.get_rmw_qos_profile()); + + sub_fl_img_.subscribe(this, "~/input/topic_img_fl", rclcpp::QoS{1}.get_rmw_qos_profile()); + sub_fr_img_.subscribe(this, "~/input/topic_img_fr", rclcpp::QoS{1}.get_rmw_qos_profile()); + + sub_bl_img_.subscribe(this, "~/input/topic_img_bl", rclcpp::QoS{1}.get_rmw_qos_profile()); + sub_br_img_.subscribe(this, "~/input/topic_img_br", rclcpp::QoS{1}.get_rmw_qos_profile()); + + sync_ = std::make_shared( + MySyncPolicy(10), sub_fl_img_, sub_f_img_, sub_fr_img_, sub_bl_img_, sub_b_img_, sub_br_img_); + + sync_->registerCallback(std::bind(&TRTBEVDetNode::callback, this, _1, _2, _3, _4, _5, _6)); +} + +void TRTBEVDetNode::startCameraInfoSubscription() +{ + // cams: ["CAM_FRONT_LEFT", "CAM_FRONT", "CAM_FRONT_RIGHT", "CAM_BACK_LEFT", "CAM_BACK", + // "CAM_BACK_RIGHT"] + sub_fl_caminfo_ = this->create_subscription( + "~/input/topic_img_fl/camera_info", rclcpp::QoS{1}, + [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { cameraInfoCallback(0, msg); }); + + sub_f_caminfo_ = this->create_subscription( + "~/input/topic_img_f/camera_info", rclcpp::QoS{1}, + [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { cameraInfoCallback(1, msg); }); + + sub_fr_caminfo_ = this->create_subscription( + "~/input/topic_img_fr/camera_info", rclcpp::QoS{1}, + [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { cameraInfoCallback(2, msg); }); + + sub_bl_caminfo_ = this->create_subscription( + "~/input/topic_img_bl/camera_info", rclcpp::QoS{1}, + [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { cameraInfoCallback(3, msg); }); + + sub_b_caminfo_ = this->create_subscription( + "~/input/topic_img_b/camera_info", rclcpp::QoS{1}, + [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { cameraInfoCallback(4, msg); }); + + sub_br_caminfo_ = this->create_subscription( + "~/input/topic_img_br/camera_info", rclcpp::QoS{1}, + [this](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { cameraInfoCallback(5, msg); }); +} + +void TRTBEVDetNode::callback( + const sensor_msgs::msg::Image::ConstSharedPtr & msg_fl_img, + const sensor_msgs::msg::Image::ConstSharedPtr & msg_f_img, + const sensor_msgs::msg::Image::ConstSharedPtr & msg_fr_img, + const sensor_msgs::msg::Image::ConstSharedPtr & msg_bl_img, + const sensor_msgs::msg::Image::ConstSharedPtr & msg_b_img, + const sensor_msgs::msg::Image::ConstSharedPtr & msg_br_img) +{ + cv::Mat img_fl, img_f, img_fr, img_bl, img_b, img_br; + std::vector imgs; + img_fl = cv_bridge::toCvShare(msg_fl_img, "bgr8")->image; + img_f = cv_bridge::toCvShare(msg_f_img, "bgr8")->image; + img_fr = cv_bridge::toCvShare(msg_fr_img, "bgr8")->image; + img_bl = cv_bridge::toCvShare(msg_bl_img, "bgr8")->image; + img_b = cv_bridge::toCvShare(msg_b_img, "bgr8")->image; + img_br = cv_bridge::toCvShare(msg_br_img, "bgr8")->image; + + imgs.emplace_back(img_fl); + imgs.emplace_back(img_f); + imgs.emplace_back(img_fr); + imgs.emplace_back(img_bl); + imgs.emplace_back(img_b); + imgs.emplace_back(img_br); + + imageTransport(imgs, imgs_dev_, img_w_, img_h_); + + // uchar *imgs_dev + sampleData_.imgs_dev = imgs_dev_; + + std::vector ego_boxes; + ego_boxes.clear(); + float time = 0.f; + + bevdet_->DoInfer(sampleData_, ego_boxes, time); + + autoware_perception_msgs::msg::DetectedObjects bevdet_objects; + bevdet_objects.header.frame_id = "base_link"; + bevdet_objects.header.stamp = msg_f_img->header.stamp; + + box3DToDetectedObjects(ego_boxes, bevdet_objects, class_names_, score_thre_, has_twist_); + + pub_boxes_->publish(bevdet_objects); +} + +void TRTBEVDetNode::cameraInfoCallback(int idx, const sensor_msgs::msg::CameraInfo::SharedPtr msg) +{ + if (caminfo_received_[idx]) + return; // already received; not expected to modify because of we init the model only once + + if (!initialized_) { // get image width and height + img_w_ = msg->width; + img_h_ = msg->height; + initialized_ = true; + } + Eigen::Matrix3f intrinsics; + getCameraIntrinsics(msg, intrinsics); + cams_intrin_[idx] = intrinsics; + + Eigen::Quaternion rot; + Eigen::Translation3f translation; + getTransform( + tf_buffer_->lookupTransform("base_link", msg->header.frame_id, rclcpp::Time(0)), rot, + translation); + cams2ego_rot_[idx] = rot; + cams2ego_trans_[idx] = translation; + + caminfo_received_[idx] = true; + camera_info_received_flag_ = + std::all_of(caminfo_received_.begin(), caminfo_received_.end(), [](bool i) { return i; }); +} + +TRTBEVDetNode::~TRTBEVDetNode() +{ + delete imgs_dev_; +} +} // namespace tensorrt_bevdet +} // namespace autoware +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::tensorrt_bevdet::TRTBEVDetNode) diff --git a/perception/autoware_tensorrt_bevdet/src/ros_utils.cpp b/perception/autoware_tensorrt_bevdet/src/ros_utils.cpp new file mode 100644 index 0000000000000..78b678d4c83e3 --- /dev/null +++ b/perception/autoware_tensorrt_bevdet/src/ros_utils.cpp @@ -0,0 +1,137 @@ +// Copyright 2024 AutoCore, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// cspell:ignore bevdet, RGBHWC, BGRCHW + +#include "autoware/tensorrt_bevdet/ros_utils.hpp" + +#include +#include +#include + +#include + +namespace autoware::tensorrt_bevdet +{ +using Label = autoware_perception_msgs::msg::ObjectClassification; + +uint8_t getSemanticType(const std::string & class_name) +{ + if (class_name == "car") { + return Label::CAR; + } else if (class_name == "truck") { + return Label::TRUCK; + } else if (class_name == "bus") { + return Label::BUS; + } else if (class_name == "trailer") { + return Label::TRAILER; + } else if (class_name == "bicycle") { + return Label::BICYCLE; + } else if (class_name == "motorcycle") { + return Label::MOTORCYCLE; + } else if (class_name == "pedestrian") { + return Label::PEDESTRIAN; + } else { + return Label::UNKNOWN; + } +} + +void box3DToDetectedObjects( + const std::vector & boxes, autoware_perception_msgs::msg::DetectedObjects & bevdet_objects, + const std::vector & class_names, float score_thre, const bool has_twist = true) +{ + for (auto b : boxes) { + if (b.score < score_thre) continue; + autoware_perception_msgs::msg::DetectedObject obj; + + Eigen::Vector3f center(b.x, b.y, b.z + b.h / 2.); + + obj.existence_probability = b.score; + // classification + autoware_perception_msgs::msg::ObjectClassification classification; + classification.probability = 1.0f; + if (b.label >= 0 && static_cast(b.label) < class_names.size()) { + classification.label = getSemanticType(class_names[b.label]); + } else { + classification.label = Label::UNKNOWN; + } + obj.classification.emplace_back(classification); + + // pose and shape + geometry_msgs::msg::Point p; + p.x = center.x(); + p.y = center.y(); + p.z = center.z(); + obj.kinematics.pose_with_covariance.pose.position = p; + + tf2::Quaternion q; + q.setRPY(0, 0, b.r); + obj.kinematics.pose_with_covariance.pose.orientation = tf2::toMsg(q); + + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + + geometry_msgs::msg::Vector3 v; + v.x = b.l; + v.y = b.w; + v.z = b.h; + obj.shape.dimensions = v; + if (has_twist) { + float vel_x = b.vx; + float vel_y = b.vy; + geometry_msgs::msg::Twist twist; + twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2)); + twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - b.r); + obj.kinematics.twist_with_covariance.twist = twist; + obj.kinematics.has_twist = has_twist; + } + + bevdet_objects.objects.emplace_back(obj); + } +} + +void getTransform( + const geometry_msgs::msg::TransformStamped & transform, Eigen::Quaternion & rot, + Eigen::Translation3f & translation) +{ + rot = Eigen::Quaternion( + transform.transform.rotation.w, transform.transform.rotation.x, transform.transform.rotation.y, + transform.transform.rotation.z); + translation = Eigen::Translation3f( + transform.transform.translation.x, transform.transform.translation.y, + transform.transform.translation.z); +} + +void getCameraIntrinsics( + const sensor_msgs::msg::CameraInfo::SharedPtr msg, Eigen::Matrix3f & intrinsics) +{ + intrinsics << msg->k[0], msg->k[1], msg->k[2], msg->k[3], msg->k[4], msg->k[5], msg->k[6], + msg->k[7], msg->k[8]; +} + +void imageTransport(std::vector imgs, uchar * out_imgs, size_t width, size_t height) +{ + uchar * temp = new uchar[width * height * 3]; + uchar * temp_gpu = nullptr; + CHECK_CUDA(cudaMalloc(&temp_gpu, width * height * 3)); + + for (size_t i = 0; i < imgs.size(); i++) { + cv::cvtColor(imgs[i], imgs[i], cv::COLOR_BGR2RGB); + CHECK_CUDA(cudaMemcpy(temp_gpu, imgs[i].data, width * height * 3, cudaMemcpyHostToDevice)); + convert_RGBHWC_to_BGRCHW(temp_gpu, out_imgs + i * width * height * 3, 3, height, width); + } + delete[] temp; + CHECK_CUDA(cudaFree(temp_gpu)); +} + +} // namespace autoware::tensorrt_bevdet