Skip to content
This repository was archived by the owner on Jul 26, 2024. It is now read-only.

Enable Node composition and pointcloud ipc #269

Open
wants to merge 3 commits into
base: humble
Choose a base branch
from
Open

Enable Node composition and pointcloud ipc #269

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9ff5a99
enable composition and IPC
Jun 14, 2023
48e4b2b
fixes
Jun 14, 2023
c4b87a7
.
Jun 14, 2023
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
42 changes: 40 additions & 2 deletions CMakeLists.txt
View file
Open in desktop
Copy link
Author

@tonynajjar tonynajjar Jun 14, 2023

Choose a reason for hiding this comment

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

I'm really not sure how to write the Cmakelist such that we can use it both as a Component and as a regular Node

Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -46,16 +47,40 @@ 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
image_transport
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
Expand Down Expand Up @@ -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 ##
#############
Expand All @@ -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
Expand Down
12 changes: 6 additions & 6 deletions include/azure_kinect_ros_driver/k4a_ros_device.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@
class K4AROSDevice : public rclcpp::Node
{
public:
K4AROSDevice();
explicit K4AROSDevice(const rclcpp::NodeOptions & options);

~K4AROSDevice();

Expand All @@ -54,10 +54,10 @@ class K4AROSDevice : public rclcpp::Node

k4a_result_t getDepthFrame(const k4a::capture& capture, std::shared_ptr<sensor_msgs::msg::Image>& depth_frame, bool rectified);

k4a_result_t getPointCloud(const k4a::capture& capture, std::shared_ptr<sensor_msgs::msg::PointCloud2>& 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<sensor_msgs::msg::PointCloud2>& point_cloud);
k4a_result_t getRgbPointCloudInDepthFrame(const k4a::capture& capture, std::shared_ptr<sensor_msgs::msg::PointCloud2>& 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<sensor_msgs::msg::Imu>& imu_frame);

Expand All @@ -81,9 +81,9 @@ class K4AROSDevice : public rclcpp::Node
k4a_result_t renderDepthToROS(std::shared_ptr<sensor_msgs::msg::Image>& depth_image, k4a::image& k4a_depth_frame);
k4a_result_t renderIrToROS(std::shared_ptr<sensor_msgs::msg::Image>& ir_image, k4a::image& k4a_ir_frame);

k4a_result_t fillPointCloud(const k4a::image& pointcloud_image, std::shared_ptr<sensor_msgs::msg::PointCloud2>& 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<sensor_msgs::msg::PointCloud2>& point_cloud);
sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud);

void framePublisherThread();
#if defined(K4A_BODY_TRACKING)
Expand Down
2 changes: 1 addition & 1 deletion src/k4a_ros_bridge_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ int main(int argc, char** argv)


// Setup the K4A device
std::shared_ptr<K4AROSDevice> device(new K4AROSDevice);
std::shared_ptr<K4AROSDevice> device(new K4AROSDevice(node->get_node_options()));

k4a_result_t result = device->startCameras();

Expand Down
27 changes: 16 additions & 11 deletions src/k4a_ros_device.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -269,7 +269,10 @@ K4AROSDevice::K4AROSDevice()
imu_orientation_publisher_ = this->create_publisher<Imu>("imu", 200);

if (params_.point_cloud || params_.rgb_point_cloud) {
pointcloud_publisher_ = this->create_publisher<PointCloud2>("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<PointCloud2>("points2", custom_qos, options);
}

#if defined(K4A_BODY_TRACKING)
Expand Down Expand Up @@ -554,7 +557,7 @@ k4a_result_t K4AROSDevice::renderBGRA32ToROS(std::shared_ptr<sensor_msgs::msg::I
}

k4a_result_t K4AROSDevice::getRgbPointCloudInDepthFrame(const k4a::capture& capture,
std::shared_ptr<sensor_msgs::msg::PointCloud2>& point_cloud)
sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud)
{
const k4a::image k4a_depth_frame = capture.get_depth_image();
if (!k4a_depth_frame)
Expand Down Expand Up @@ -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<sensor_msgs::msg::PointCloud2>& point_cloud)
sensor_msgs::msg::PointCloud2::UniquePtr& point_cloud)
{
k4a::image k4a_depth_frame = capture.get_depth_image();
if (!k4a_depth_frame)
Expand Down Expand Up @@ -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<sensor_msgs::msg::PointCloud2>& 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();

Expand All @@ -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<sensor_msgs::msg::PointCloud2>& 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();
Expand Down Expand Up @@ -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<sensor_msgs::msg::PointCloud2>& 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();
Expand Down Expand Up @@ -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<PointCloud2>();

if (params_.depth_enabled)
{
Expand Down Expand Up @@ -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();
}
}
Expand Down Expand Up @@ -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)