diff --git a/CMakeLists.txt b/CMakeLists.txt index 8b991ec3..4eec876a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) @@ -46,7 +47,15 @@ add_executable(${PROJECT_NAME}_node src/k4a_calibration_transform_data.cpp ) -ament_target_dependencies(${PROJECT_NAME}_node rclcpp +add_library(${PROJECT_NAME}_library SHARED + src/k4a_ros_bridge_node.cpp + src/k4a_ros_device.cpp + src/k4a_ros_device_params.cpp + src/k4a_calibration_transform_data.cpp + ) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp std_msgs sensor_msgs visualization_msgs @@ -54,8 +63,24 @@ ament_target_dependencies(${PROJECT_NAME}_node rclcpp tf2 tf2_ros tf2_geometry_msgs + rclcpp_components cv_bridge - angles) + angles + ) + +ament_target_dependencies(${PROJECT_NAME}_library + rclcpp + std_msgs + sensor_msgs + visualization_msgs + image_transport + tf2 + tf2_ros + tf2_geometry_msgs + rclcpp_components + cv_bridge + angles +) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -105,6 +130,12 @@ target_link_libraries(${PROJECT_NAME}_node ${K4A_LIBS} ) +target_link_libraries(${PROJECT_NAME}_library + ${K4A_LIBS} +) + +rclcpp_components_register_nodes(${PROJECT_NAME}_library "K4AROSDevice") + ############# ## Install ## ############# @@ -120,6 +151,13 @@ install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME} ) +install(TARGETS ${PROJECT_NAME}_library + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + DESTINATION lib/${PROJECT_NAME} +) + ## Mark other files for installation (e.g. launch and bag files, etc.) install( DIRECTORY diff --git a/include/azure_kinect_ros_driver/k4a_ros_device.h b/include/azure_kinect_ros_driver/k4a_ros_device.h index 2c2de2f4..b01a15ec 100644 --- a/include/azure_kinect_ros_driver/k4a_ros_device.h +++ b/include/azure_kinect_ros_driver/k4a_ros_device.h @@ -37,7 +37,7 @@ class K4AROSDevice : public rclcpp::Node { public: - K4AROSDevice(); + explicit K4AROSDevice(const rclcpp::NodeOptions & options); ~K4AROSDevice(); @@ -54,10 +54,10 @@ class K4AROSDevice : public rclcpp::Node k4a_result_t getDepthFrame(const k4a::capture& capture, std::shared_ptr& depth_frame, bool rectified); - k4a_result_t getPointCloud(const k4a::capture& capture, std::shared_ptr& point_cloud); + k4a_result_t getPointCloud(const k4a::capture& capture, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud); - k4a_result_t getRgbPointCloudInRgbFrame(const k4a::capture& capture, std::shared_ptr& point_cloud); - k4a_result_t getRgbPointCloudInDepthFrame(const k4a::capture& capture, std::shared_ptr& point_cloud); + k4a_result_t getRgbPointCloudInRgbFrame(const k4a::capture& capture, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud); + k4a_result_t getRgbPointCloudInDepthFrame(const k4a::capture& capture, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud); k4a_result_t getImuFrame(const k4a_imu_sample_t& capture, std::shared_ptr& imu_frame); @@ -81,9 +81,9 @@ class K4AROSDevice : public rclcpp::Node k4a_result_t renderDepthToROS(std::shared_ptr& depth_image, k4a::image& k4a_depth_frame); k4a_result_t renderIrToROS(std::shared_ptr& ir_image, k4a::image& k4a_ir_frame); - k4a_result_t fillPointCloud(const k4a::image& pointcloud_image, std::shared_ptr& point_cloud); + k4a_result_t fillPointCloud(const k4a::image& pointcloud_image, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud); k4a_result_t fillColorPointCloud(const k4a::image& pointcloud_image, const k4a::image& color_image, - std::shared_ptr& point_cloud); + sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud); void framePublisherThread(); #if defined(K4A_BODY_TRACKING) diff --git a/src/k4a_ros_bridge_node.cpp b/src/k4a_ros_bridge_node.cpp index 63419e3d..cee9ee06 100644 --- a/src/k4a_ros_bridge_node.cpp +++ b/src/k4a_ros_bridge_node.cpp @@ -23,7 +23,7 @@ int main(int argc, char** argv) // Setup the K4A device - std::shared_ptr device(new K4AROSDevice); + std::shared_ptr device(new K4AROSDevice(node->get_node_options())); k4a_result_t result = device->startCameras(); diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp index b17884fd..6f794139 100644 --- a/src/k4a_ros_device.cpp +++ b/src/k4a_ros_device.cpp @@ -34,8 +34,8 @@ using namespace std; using namespace visualization_msgs::msg; #endif -K4AROSDevice::K4AROSDevice() - : Node("k4a_ros_device_node"), +K4AROSDevice::K4AROSDevice(const rclcpp::NodeOptions & options) +: Node("k4a_ros_device_node", options), k4a_device_(nullptr), k4a_playback_handle_(nullptr), // clang-format off @@ -269,7 +269,10 @@ K4AROSDevice::K4AROSDevice() imu_orientation_publisher_ = this->create_publisher("imu", 200); if (params_.point_cloud || params_.rgb_point_cloud) { - pointcloud_publisher_ = this->create_publisher("points2", 1); + rclcpp::QoS custom_qos(KeepLast(1), rmw_qos_profile_sensor_data); + rclcpp::PublisherOptions options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + pointcloud_publisher_ = this->create_publisher("points2", custom_qos, options); } #if defined(K4A_BODY_TRACKING) @@ -554,7 +557,7 @@ k4a_result_t K4AROSDevice::renderBGRA32ToROS(std::shared_ptr& point_cloud) + sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud) { const k4a::image k4a_depth_frame = capture.get_depth_image(); if (!k4a_depth_frame) @@ -586,7 +589,7 @@ k4a_result_t K4AROSDevice::getRgbPointCloudInDepthFrame(const k4a::capture& capt } k4a_result_t K4AROSDevice::getRgbPointCloudInRgbFrame(const k4a::capture& capture, - std::shared_ptr& point_cloud) + sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud) { k4a::image k4a_depth_frame = capture.get_depth_image(); if (!k4a_depth_frame) @@ -616,7 +619,7 @@ k4a_result_t K4AROSDevice::getRgbPointCloudInRgbFrame(const k4a::capture& captur return fillColorPointCloud(calibration_data_.point_cloud_image_, k4a_bgra_frame, point_cloud); } -k4a_result_t K4AROSDevice::getPointCloud(const k4a::capture& capture, std::shared_ptr& point_cloud) +k4a_result_t K4AROSDevice::getPointCloud(const k4a::capture& capture, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud) { k4a::image k4a_depth_frame = capture.get_depth_image(); @@ -637,7 +640,7 @@ k4a_result_t K4AROSDevice::getPointCloud(const k4a::capture& capture, std::share } k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_image, const k4a::image& color_image, - std::shared_ptr& point_cloud) + sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud) { point_cloud->height = pointcloud_image.get_height_pixels(); point_cloud->width = pointcloud_image.get_width_pixels(); @@ -695,7 +698,7 @@ k4a_result_t K4AROSDevice::fillColorPointCloud(const k4a::image& pointcloud_imag return K4A_RESULT_SUCCEEDED; } -k4a_result_t K4AROSDevice::fillPointCloud(const k4a::image& pointcloud_image, std::shared_ptr& point_cloud) +k4a_result_t K4AROSDevice::fillPointCloud(const k4a::image& pointcloud_image, sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud) { point_cloud->height = pointcloud_image.get_height_pixels(); point_cloud->width = pointcloud_image.get_width_pixels(); @@ -929,7 +932,7 @@ void K4AROSDevice::framePublisherThread() Image::SharedPtr depth_raw_frame(new Image); Image::SharedPtr depth_rect_frame(new Image); Image::SharedPtr ir_raw_frame(new Image); - PointCloud2::SharedPtr point_cloud(new PointCloud2); + PointCloud2::UniquePtr point_cloud = std::make_unique(); if (params_.depth_enabled) { @@ -1169,11 +1172,10 @@ void K4AROSDevice::framePublisherThread() if (params_.point_cloud || params_.rgb_point_cloud) { - pointcloud_publisher_->publish(*point_cloud); + pointcloud_publisher_->publish(std::move(point_cloud)); } } - rclcpp::spin_some(shared_from_this()); loop_rate.sleep(); } } @@ -1472,3 +1474,6 @@ void K4AROSDevice::updateTimestampOffset(const std::chrono::microseconds& k4a_de std::floor(alpha * (device_to_realtime - device_to_realtime_offset_).count()))); } } + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(K4AROSDevice)