From 8a208e34010b985a9bd4c8d6e735037b89c1767a Mon Sep 17 00:00:00 2001 From: Christian Eichmann Date: Mon, 24 Apr 2023 16:34:42 +0200 Subject: [PATCH] Publication of calibrations, RGBInfo, TOFInfo and CameraInfo Moved ifm3d buffer to ROS message convertions to buffer_conversions.hpp Added Intrinsics, InverseIntrinsics, RGBInfo and TOFInfo messages. Added Intrinsics, InverseIntrinsics, RGBInfo and TOFInfo publication. Automatically publish CameraInfo if INTRINSIC_CALIB buffer is received. --- CMakeLists.txt | 4 + config/camera_default_parameters.yaml | 8 +- include/ifm3d_ros2/buffer_conversions.hpp | 446 ++++++++++++++++++++++ include/ifm3d_ros2/buffer_id_utils.hpp | 20 +- include/ifm3d_ros2/camera_node.hpp | 28 ++ msg/Intrinsics.msg | 4 + msg/InverseIntrinsics.msg | 4 + msg/RGBInfo.msg | 9 + msg/TOFInfo.msg | 19 + src/lib/camera_node.cpp | 286 +++++--------- 10 files changed, 632 insertions(+), 196 deletions(-) create mode 100644 include/ifm3d_ros2/buffer_conversions.hpp create mode 100644 msg/Intrinsics.msg create mode 100644 msg/InverseIntrinsics.msg create mode 100644 msg/RGBInfo.msg create mode 100644 msg/TOFInfo.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index bb83e59..10b6320 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,6 +35,10 @@ ament_auto_find_build_dependencies(REQUIRED ${IFM3D_ROS2_DEPS}) rosidl_generate_interfaces(${PROJECT_NAME} "msg/Extrinsics.msg" + "msg/Intrinsics.msg" + "msg/InverseIntrinsics.msg" + "msg/RGBInfo.msg" + "msg/TOFInfo.msg" "srv/Dump.srv" "srv/Config.srv" "srv/Softoff.srv" diff --git a/config/camera_default_parameters.yaml b/config/camera_default_parameters.yaml index 9aabcc1..7f09a43 100644 --- a/config/camera_default_parameters.yaml +++ b/config/camera_default_parameters.yaml @@ -5,12 +5,16 @@ ros__parameters: buffer_id_list: - AMPLITUDE_IMAGE - - NORM_AMPLITUDE_IMAGE - CONFIDENCE_IMAGE + - EXTRINSIC_CALIB + - INTRINSIC_CALIB + - INVERSE_INTRINSIC_CALIBRATION - JPEG_IMAGE + - NORM_AMPLITUDE_IMAGE - RADIAL_DISTANCE_IMAGE + - RGB_INFO + - TOF_INFO - XYZ - - EXTRINSIC_CALIB ip: 192.168.0.69 pcic_port: 50010 tf: diff --git a/include/ifm3d_ros2/buffer_conversions.hpp b/include/ifm3d_ros2/buffer_conversions.hpp new file mode 100644 index 0000000..fc75bda --- /dev/null +++ b/include/ifm3d_ros2/buffer_conversions.hpp @@ -0,0 +1,446 @@ +// -*- c++ -*- +/* + * SPDX-License-Identifier: Apache-2.0 + * Copyright (C) 2019 ifm electronic, gmbh + */ + +#ifndef IFM3D_ROS2_BUFFER_CONVERSIONS_HPP_ +#define IFM3D_ROS2_BUFFER_CONVERSIONS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ifm3d_ros2 +{ +sensor_msgs::msg::Image ifm3d_to_ros_image(ifm3d::Buffer& image, // Need non-const image because image.begin(), + // image.end() don't have const overloads. + const std_msgs::msg::Header& header, const rclcpp::Logger& logger) +{ + static constexpr auto max_pixel_format = static_cast(ifm3d::pixel_format::FORMAT_32F3); + static constexpr auto image_format_info = [] { + auto image_format_info = std::array{}; + + { + using namespace ifm3d; + using namespace sensor_msgs::image_encodings; + image_format_info[static_cast(pixel_format::FORMAT_8U)] = TYPE_8UC1; + image_format_info[static_cast(pixel_format::FORMAT_8S)] = TYPE_8SC1; + image_format_info[static_cast(pixel_format::FORMAT_16U)] = TYPE_16UC1; + image_format_info[static_cast(pixel_format::FORMAT_16S)] = TYPE_16SC1; + image_format_info[static_cast(pixel_format::FORMAT_32U)] = "32UC1"; + image_format_info[static_cast(pixel_format::FORMAT_32S)] = TYPE_32SC1; + image_format_info[static_cast(pixel_format::FORMAT_32F)] = TYPE_32FC1; + image_format_info[static_cast(pixel_format::FORMAT_64U)] = "64UC1"; + image_format_info[static_cast(pixel_format::FORMAT_64F)] = TYPE_64FC1; + image_format_info[static_cast(pixel_format::FORMAT_16U2)] = TYPE_16UC2; + image_format_info[static_cast(pixel_format::FORMAT_32F3)] = TYPE_32FC3; + } + + return image_format_info; + }(); + + const auto format = static_cast(image.dataFormat()); + + sensor_msgs::msg::Image result{}; + result.header = header; + result.height = image.height(); + result.width = image.width(); + result.is_bigendian = 0; + + if (image.begin() == image.end()) + { + return result; + } + + if (format >= max_pixel_format) + { + RCLCPP_ERROR(logger, "Pixel format out of range (%ld >= %ld)", format, max_pixel_format); + return result; + } + + result.encoding = image_format_info.at(format); + result.step = result.width * sensor_msgs::image_encodings::bitDepth(image_format_info.at(format)) / 8; + result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), result.step * result.height)); + + if (result.encoding.empty()) + { + RCLCPP_WARN(logger, "Can't handle encoding %ld (32U == %ld, 64U == %ld)", format, + static_cast(ifm3d::pixel_format::FORMAT_32U), + static_cast(ifm3d::pixel_format::FORMAT_64U)); + result.encoding = sensor_msgs::image_encodings::TYPE_8UC1; + } + + return result; +} + +sensor_msgs::msg::Image ifm3d_to_ros_image(ifm3d::Buffer&& image, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + return ifm3d_to_ros_image(image, header, logger); +} + +sensor_msgs::msg::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Buffer& image, // Need non-const image because + // image.begin(), image.end() + // don't have const overloads. + const std_msgs::msg::Header& header, + const std::string& format, // "jpeg" or "png" + const rclcpp::Logger& logger) +{ + sensor_msgs::msg::CompressedImage result{}; + result.header = header; + result.format = format; + + if (const auto dataFormat = image.dataFormat(); + dataFormat != ifm3d::pixel_format::FORMAT_8S && dataFormat != ifm3d::pixel_format::FORMAT_8U) + { + RCLCPP_ERROR(logger, "Invalid data format for %s data (%ld)", format.c_str(), static_cast(dataFormat)); + return result; + } + + result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), image.width() * image.height())); + return result; +} + +sensor_msgs::msg::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Buffer&& image, + const std_msgs::msg::Header& header, + const std::string& format, const rclcpp::Logger& logger) +{ + return ifm3d_to_ros_compressed_image(image, header, format, logger); +} + +sensor_msgs::msg::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Buffer& image, // Need non-const image because image.begin(), + // image.end() don't have const overloads. + const std_msgs::msg::Header& header, const rclcpp::Logger& logger) +{ + sensor_msgs::msg::PointCloud2 result{}; + result.header = header; + result.height = image.height(); + result.width = image.width(); + result.is_bigendian = false; + + if (image.begin() == image.end()) + { + return result; + } + + if (image.dataFormat() != ifm3d::pixel_format::FORMAT_32F3 && image.dataFormat() != ifm3d::pixel_format::FORMAT_32F) + { + RCLCPP_ERROR(logger, "Unsupported pixel format %ld for point cloud", static_cast(image.dataFormat())); + return result; + } + + sensor_msgs::msg::PointField x_field{}; + x_field.name = "x"; + x_field.offset = 0; + x_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + x_field.count = 1; + + sensor_msgs::msg::PointField y_field{}; + y_field.name = "y"; + y_field.offset = 4; + y_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + y_field.count = 1; + + sensor_msgs::msg::PointField z_field{}; + z_field.name = "z"; + z_field.offset = 8; + z_field.datatype = sensor_msgs::msg::PointField::FLOAT32; + z_field.count = 1; + + result.fields = { + x_field, + y_field, + z_field, + }; + + result.point_step = result.fields.size() * sizeof(float); + result.row_step = result.point_step * result.width; + result.is_dense = true; + result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), result.row_step * result.height)); + + return result; +} + +sensor_msgs::msg::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Buffer&& image, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + return ifm3d_to_ros_cloud(image, header, logger); +} + +ifm3d_ros2::msg::Extrinsics ifm3d_to_extrinsics(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + ifm3d_ros2::msg::Extrinsics extrinsics_msg; + extrinsics_msg.header = header; + try + { + extrinsics_msg.tx = buffer.at(0); + extrinsics_msg.ty = buffer.at(1); + extrinsics_msg.tz = buffer.at(2); + extrinsics_msg.rot_x = buffer.at(3); + extrinsics_msg.rot_y = buffer.at(4); + extrinsics_msg.rot_z = buffer.at(5); + } + catch (const std::out_of_range& ex) + { + RCLCPP_WARN(logger, "Out-of-range error fetching extrinsics"); + } + return extrinsics_msg; +} + +ifm3d_ros2::msg::Extrinsics ifm3d_to_extrinsics(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + return ifm3d_to_extrinsics(buffer, header, logger); +} + +sensor_msgs::msg::CameraInfo ifm3d_to_camera_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const uint32_t height, const uint32_t width, + const rclcpp::Logger& logger) +{ + ifm3d::calibration::IntrinsicCalibration intrinsic; + intrinsic.Read(buffer.ptr(0)); + + sensor_msgs::msg::CameraInfo camera_info_msg; + camera_info_msg.header = header; + + try + { + camera_info_msg.height = height; + camera_info_msg.width = width; + camera_info_msg.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; + + // Read data from buffer + const float fx = intrinsic.model_parameters[0]; + const float fy = intrinsic.model_parameters[1]; + const float mx = intrinsic.model_parameters[2]; + const float my = intrinsic.model_parameters[3]; + const float alpha = intrinsic.model_parameters[4]; + const float k1 = intrinsic.model_parameters[5]; + const float k2 = intrinsic.model_parameters[6]; + const float k3 = intrinsic.model_parameters[7]; + const float k4 = intrinsic.model_parameters[8]; + // next in buffer is k5 for bouguet or theta_max for fisheye model, both not needed here + + const float ix = width - 1; + const float iy = height - 1; + const float cy = (iy + 0.5 - my) / fy; + const float cx = (ix + 0.5 - mx) / fx - alpha * cy; + const float r2 = cx * cx + cy * cy; + const float h = 2 * cx * cy; + const float tx = k3 * h + k4 * (r2 + 2 * cx * cx); + const float ty = k3 * (r2 + 2 * cy * cy) + k4 * h; + + // Distortion parameters + camera_info_msg.d.resize(5); + camera_info_msg.d[0] = k1; + camera_info_msg.d[1] = k2; + camera_info_msg.d[2] = tx; // TODO t1 == tx ? + camera_info_msg.d[3] = ty; // TODO t2 == ty ? + camera_info_msg.d[4] = k3; + + // Intrinsic camera matrix + camera_info_msg.k[0] = fx; + camera_info_msg.k[4] = fy; + camera_info_msg.k[2] = cx; + camera_info_msg.k[5] = cy; + camera_info_msg.k[8] = 1.0; // fixed to 1.0 + + // Projection matrix + camera_info_msg.p[0] = fx; + camera_info_msg.p[5] = fy; + camera_info_msg.p[2] = cx; + camera_info_msg.p[6] = cy; + camera_info_msg.p[10] = 1.0; // fixed to 1.0 + + RCLCPP_DEBUG_ONCE(logger, + "Intrinsics:\nfx=%f \nfy=%f \nmx=%f \nmy=%f \nalpha=%f \nk1=%f \nk2=%f \nk3=%f \nk4=%f " + "\nCalculated:\nix=%f \niy=%f \ncx=%f \ncy=%f \nr2=%f \nh=%f \ntx=%f \nty=%f", + fx, fy, mx, my, alpha, k1, k2, k3, k4, ix, iy, cx, cy, r2, h, tx, ty); + } + catch (const std::out_of_range& ex) + { + RCLCPP_WARN(logger, "Out-of-range error fetching intrinsics"); + } + + return camera_info_msg; +} + +sensor_msgs::msg::CameraInfo ifm3d_to_camera_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const uint32_t height, const uint32_t width, + const rclcpp::Logger& logger) + +{ + return ifm3d_to_camera_info(buffer, header, height, width, logger); +} + +ifm3d_ros2::msg::Intrinsics ifm3d_to_intrinsics(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + ifm3d_ros2::msg::Intrinsics intrinsics_msg; + intrinsics_msg.header = header; + + ifm3d::calibration::IntrinsicCalibration intrinsics; + + try + { + intrinsics.Read(buffer.ptr(0)); + intrinsics_msg.model_id = intrinsics.model_id; + intrinsics_msg.model_parameters = intrinsics.model_parameters; + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read intrinsics."); + } + + return intrinsics_msg; +} + +ifm3d_ros2::msg::Intrinsics ifm3d_to_intrinsics(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) + +{ + return ifm3d_to_intrinsics(buffer, header, logger); +} + +ifm3d_ros2::msg::InverseIntrinsics ifm3d_to_inverse_intrinsics(ifm3d::Buffer& buffer, + const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + ifm3d_ros2::msg::InverseIntrinsics inverse_intrinsics_msg; + inverse_intrinsics_msg.header = header; + + ifm3d::calibration::InverseIntrinsicCalibration inverse_intrinsics; + + try + { + inverse_intrinsics.Read(buffer.ptr(0)); + inverse_intrinsics_msg.model_id = inverse_intrinsics.model_id; + inverse_intrinsics_msg.model_parameters = inverse_intrinsics.model_parameters; + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read inverse intrinsics."); + } + + return inverse_intrinsics_msg; +} + +ifm3d_ros2::msg::InverseIntrinsics ifm3d_to_inverse_intrinsics(ifm3d::Buffer&& buffer, + const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) + +{ + return ifm3d_to_inverse_intrinsics(buffer, header, logger); +} + +ifm3d_ros2::msg::RGBInfo ifm3d_to_rgb_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + ifm3d_ros2::msg::RGBInfo rgb_info_msg; + rgb_info_msg.header = header; + + try + { + auto rgb_info = ifm3d::RGBInfoV1::Deserialize(buffer); + + rgb_info_msg.version = rgb_info.version; + rgb_info_msg.frame_counter = rgb_info.frame_counter; + rgb_info_msg.timestamp_ns = rgb_info.timestamp_ns; + rgb_info_msg.exposure_time = rgb_info.exposure_time; + + rgb_info_msg.extrinsics.header = header; + rgb_info_msg.extrinsics.tx = rgb_info.extrinsic_optic_to_user.trans_x; + rgb_info_msg.extrinsics.ty = rgb_info.extrinsic_optic_to_user.trans_y; + rgb_info_msg.extrinsics.tz = rgb_info.extrinsic_optic_to_user.trans_z; + rgb_info_msg.extrinsics.rot_x = rgb_info.extrinsic_optic_to_user.rot_x; + rgb_info_msg.extrinsics.rot_y = rgb_info.extrinsic_optic_to_user.rot_y; + rgb_info_msg.extrinsics.rot_z = rgb_info.extrinsic_optic_to_user.rot_z; + + rgb_info_msg.intrinsics.header = header; + rgb_info_msg.intrinsics.model_id = rgb_info.intrinsic_calibration.model_id; + rgb_info_msg.intrinsics.model_parameters = rgb_info.intrinsic_calibration.model_parameters; + + rgb_info_msg.inverse_intrinsics.header = header; + rgb_info_msg.inverse_intrinsics.model_id = rgb_info.inverse_intrinsic_calibration.model_id; + rgb_info_msg.inverse_intrinsics.model_parameters = rgb_info.inverse_intrinsic_calibration.model_parameters; + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read rgb info."); + } + + return rgb_info_msg; +} + +ifm3d_ros2::msg::RGBInfo ifm3d_to_rgb_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) + +{ + return ifm3d_to_rgb_info(buffer, header, logger); +} + +ifm3d_ros2::msg::TOFInfo ifm3d_to_tof_info(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) +{ + ifm3d_ros2::msg::TOFInfo tof_info_msg; + tof_info_msg.header = header; + + try + { + auto tof_info = ifm3d::TOFInfoV4::Deserialize(buffer); + tof_info_msg.measurement_block_index = tof_info.measurement_block_index; + tof_info_msg.measurement_range_min = tof_info.measurement_range_min; + tof_info_msg.measurement_range_max = tof_info.measurement_range_max; + tof_info_msg.version = tof_info.version; + tof_info_msg.distance_resolution = tof_info.distance_resolution; + tof_info_msg.amplitude_resolution = tof_info.amplitude_resolution; + tof_info_msg.amp_normalization_factors = tof_info.amp_normalization_factors; + tof_info_msg.exposure_timestamps_ns = tof_info.exposure_timestamps_ns; + tof_info_msg.exposure_times_s = tof_info.exposure_times_s; + tof_info_msg.illu_temperature = tof_info.illu_temperature; + tof_info_msg.mode = std::string(std::begin(tof_info.mode), std::end(tof_info.mode)); + tof_info_msg.imager = std::string(std::begin(tof_info.imager), std::end(tof_info.imager)); + + tof_info_msg.extrinsics.header = header; + tof_info_msg.extrinsics.tx = tof_info.extrinsic_optic_to_user.trans_x; + tof_info_msg.extrinsics.ty = tof_info.extrinsic_optic_to_user.trans_y; + tof_info_msg.extrinsics.tz = tof_info.extrinsic_optic_to_user.trans_z; + tof_info_msg.extrinsics.rot_x = tof_info.extrinsic_optic_to_user.rot_x; + tof_info_msg.extrinsics.rot_y = tof_info.extrinsic_optic_to_user.rot_y; + tof_info_msg.extrinsics.rot_z = tof_info.extrinsic_optic_to_user.rot_z; + + tof_info_msg.intrinsics.header = header; + tof_info_msg.intrinsics.model_id = tof_info.intrinsic_calibration.model_id; + tof_info_msg.intrinsics.model_parameters = tof_info.intrinsic_calibration.model_parameters; + + tof_info_msg.inverse_intrinsics.header = header; + tof_info_msg.inverse_intrinsics.model_id = tof_info.inverse_intrinsic_calibration.model_id; + tof_info_msg.inverse_intrinsics.model_parameters = tof_info.inverse_intrinsic_calibration.model_parameters; + } + catch (...) + { + RCLCPP_ERROR(logger, "Failed to read tof info."); + } + + return tof_info_msg; +} + +ifm3d_ros2::msg::TOFInfo ifm3d_to_tof_info(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, + const rclcpp::Logger& logger) + +{ + return ifm3d_to_tof_info(buffer, header, logger); +} + +} // namespace ifm3d_ros2 + +#endif \ No newline at end of file diff --git a/include/ifm3d_ros2/buffer_id_utils.hpp b/include/ifm3d_ros2/buffer_id_utils.hpp index 24d5cf1..bc2704c 100644 --- a/include/ifm3d_ros2/buffer_id_utils.hpp +++ b/include/ifm3d_ros2/buffer_id_utils.hpp @@ -25,10 +25,14 @@ enum data_stream_type */ enum message_type { - raw_image, + intrinsics, compressed_image, - pointcloud, extrinsics, + inverse_intrinsics, + pointcloud, + raw_image, + rgb_info, + tof_info, not_implemented, }; @@ -98,7 +102,9 @@ std::multimap data_stream_type_map = { { ifm3d::buffer_id::EXTRINSIC_CALIB, data_stream_type::tof_3d }, { ifm3d::buffer_id::EXTRINSIC_CALIB, data_stream_type::rgb_2d }, // TODO assuming ExCal for both { ifm3d::buffer_id::INTRINSIC_CALIB, data_stream_type::tof_3d }, + { ifm3d::buffer_id::INTRINSIC_CALIB, data_stream_type::rgb_2d }, { ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION, data_stream_type::tof_3d }, + { ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION, data_stream_type::rgb_2d }, { ifm3d::buffer_id::TOF_INFO, data_stream_type::tof_3d }, { ifm3d::buffer_id::RGB_INFO, data_stream_type::rgb_2d }, { ifm3d::buffer_id::JSON_MODEL, data_stream_type::tof_3d }, @@ -136,10 +142,10 @@ std::map message_type_map = { { ifm3d::buffer_id::DIAGNOSTIC, message_type::not_implemented }, { ifm3d::buffer_id::JSON_DIAGNOSTIC, message_type::not_implemented }, { ifm3d::buffer_id::EXTRINSIC_CALIB, message_type::extrinsics }, - { ifm3d::buffer_id::INTRINSIC_CALIB, message_type::not_implemented }, - { ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION, message_type::not_implemented }, - { ifm3d::buffer_id::TOF_INFO, message_type::not_implemented }, - { ifm3d::buffer_id::RGB_INFO, message_type::not_implemented }, + { ifm3d::buffer_id::INTRINSIC_CALIB, message_type::intrinsics }, + { ifm3d::buffer_id::INVERSE_INTRINSIC_CALIBRATION, message_type::inverse_intrinsics }, + { ifm3d::buffer_id::TOF_INFO, message_type::tof_info }, + { ifm3d::buffer_id::RGB_INFO, message_type::rgb_info }, { ifm3d::buffer_id::JSON_MODEL, message_type::not_implemented }, { ifm3d::buffer_id::ALGO_DEBUG, message_type::not_implemented }, { ifm3d::buffer_id::O3R_ODS_OCCUPANCY_GRID, message_type::not_implemented }, @@ -152,7 +158,7 @@ std::map message_type_map = { /** * @brief mapping buffer_ids to topic names * - * To allow for easily readable topic names, usage of common ROS terms (like camera_info) + * To allow for easily readable topic names, usage of common ROS terms * and backwards compatibility. * It is not declared const because the operator[] of std::map would not be available. */ diff --git a/include/ifm3d_ros2/camera_node.hpp b/include/ifm3d_ros2/camera_node.hpp index 865e245..f9f0251 100644 --- a/include/ifm3d_ros2/camera_node.hpp +++ b/include/ifm3d_ros2/camera_node.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -28,6 +29,9 @@ #include #include +#include +#include +#include #include #include #include @@ -50,6 +54,21 @@ using PCLPublisher = std::shared_ptr>; +using CameraInfoMsg = sensor_msgs::msg::CameraInfo; +using CameraInfoPublisher = std::shared_ptr>; + +using IntrinsicsMsg = ifm3d_ros2::msg::Intrinsics; +using IntrinsicsPublisher = std::shared_ptr>; + +using InverseIntrinsicsMsg = ifm3d_ros2::msg::InverseIntrinsics; +using InverseIntrinsicsPublisher = std::shared_ptr>; + +using TOFInfoMsg = ifm3d_ros2::msg::TOFInfo; +using TOFInfoPublisher = std::shared_ptr>; + +using RGBInfoMsg = ifm3d_ros2::msg::RGBInfo; +using RGBInfoPublisher = std::shared_ptr>; + using DumpRequest = std::shared_ptr; using DumpResponse = std::shared_ptr; using DumpService = ifm3d_ros2::srv::Dump; @@ -285,6 +304,10 @@ class IFM3D_ROS2_PUBLIC CameraNode : public rclcpp_lifecycle::LifecycleNode std::vector tf_optical_link_transform_{}; std::uint16_t xmlrpc_port_{}; + // Values read from incomming image buffers + uint32_t width_; + uint32_t height_; + /// Subscription to parameter changes std::shared_ptr param_subscriber_; /// Callbacks need to be stored to work properly; using a map with parameter name as key @@ -308,6 +331,11 @@ class IFM3D_ROS2_PUBLIC CameraNode : public rclcpp_lifecycle::LifecycleNode std::map compressed_image_publishers_; std::map pcl_publishers_; std::map extrinsics_publishers_; + std::map camera_info_publishers_; + std::map rgb_info_publishers_; + std::map tof_info_publishers_; + std::map intrinsics_publishers_; + std::map inverse_intrinsics_publishers_; }; // end: class CameraNode diff --git a/msg/Intrinsics.msg b/msg/Intrinsics.msg new file mode 100644 index 0000000..964385e --- /dev/null +++ b/msg/Intrinsics.msg @@ -0,0 +1,4 @@ +std_msgs/Header header + +uint32 model_id +float32[32] model_parameters \ No newline at end of file diff --git a/msg/InverseIntrinsics.msg b/msg/InverseIntrinsics.msg new file mode 100644 index 0000000..964385e --- /dev/null +++ b/msg/InverseIntrinsics.msg @@ -0,0 +1,4 @@ +std_msgs/Header header + +uint32 model_id +float32[32] model_parameters \ No newline at end of file diff --git a/msg/RGBInfo.msg b/msg/RGBInfo.msg new file mode 100644 index 0000000..560acaf --- /dev/null +++ b/msg/RGBInfo.msg @@ -0,0 +1,9 @@ +std_msgs/Header header + +uint32 version +uint32 frame_counter +uint64 timestamp_ns +float32 exposure_time +ifm3d_ros2/Extrinsics extrinsics +ifm3d_ros2/Intrinsics intrinsics +ifm3d_ros2/InverseIntrinsics inverse_intrinsics \ No newline at end of file diff --git a/msg/TOFInfo.msg b/msg/TOFInfo.msg new file mode 100644 index 0000000..f7483b0 --- /dev/null +++ b/msg/TOFInfo.msg @@ -0,0 +1,19 @@ +std_msgs/Header header + +uint32 measurement_block_index +float32 measurement_range_min +float32 measurement_range_max +uint32 version +float32 distance_resolution +float32 amplitude_resolution +float32[3] amp_normalization_factors + +ifm3d_ros2/Extrinsics extrinsics +ifm3d_ros2/Intrinsics intrinsics +ifm3d_ros2/InverseIntrinsics inverse_intrinsics + +uint64[3] exposure_timestamps_ns +uint32[3] exposure_times_s +float32 illu_temperature +string<=32 mode +string<=32 imager \ No newline at end of file diff --git a/src/lib/camera_node.cpp b/src/lib/camera_node.cpp index 8c5d8cf..a60ff2e 100644 --- a/src/lib/camera_node.cpp +++ b/src/lib/camera_node.cpp @@ -20,193 +20,12 @@ #include #include +#include #include #include #include -sensor_msgs::msg::Image ifm3d_to_ros_image(ifm3d::Buffer& image, // Need non-const image because image.begin(), - // image.end() don't have const overloads. - const std_msgs::msg::Header& header, const rclcpp::Logger& logger) -{ - static constexpr auto max_pixel_format = static_cast(ifm3d::pixel_format::FORMAT_32F3); - static constexpr auto image_format_info = [] { - auto image_format_info = std::array{}; - - { - using namespace ifm3d; - using namespace sensor_msgs::image_encodings; - image_format_info[static_cast(pixel_format::FORMAT_8U)] = TYPE_8UC1; - image_format_info[static_cast(pixel_format::FORMAT_8S)] = TYPE_8SC1; - image_format_info[static_cast(pixel_format::FORMAT_16U)] = TYPE_16UC1; - image_format_info[static_cast(pixel_format::FORMAT_16S)] = TYPE_16SC1; - image_format_info[static_cast(pixel_format::FORMAT_32U)] = "32UC1"; - image_format_info[static_cast(pixel_format::FORMAT_32S)] = TYPE_32SC1; - image_format_info[static_cast(pixel_format::FORMAT_32F)] = TYPE_32FC1; - image_format_info[static_cast(pixel_format::FORMAT_64U)] = "64UC1"; - image_format_info[static_cast(pixel_format::FORMAT_64F)] = TYPE_64FC1; - image_format_info[static_cast(pixel_format::FORMAT_16U2)] = TYPE_16UC2; - image_format_info[static_cast(pixel_format::FORMAT_32F3)] = TYPE_32FC3; - } - - return image_format_info; - }(); - - const auto format = static_cast(image.dataFormat()); - - sensor_msgs::msg::Image result{}; - result.header = header; - result.height = image.height(); - result.width = image.width(); - result.is_bigendian = 0; - - if (image.begin() == image.end()) - { - return result; - } - - if (format >= max_pixel_format) - { - RCLCPP_ERROR(logger, "Pixel format out of range (%ld >= %ld)", format, max_pixel_format); - return result; - } - - result.encoding = image_format_info.at(format); - result.step = result.width * sensor_msgs::image_encodings::bitDepth(image_format_info.at(format)) / 8; - result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), result.step * result.height)); - - if (result.encoding.empty()) - { - RCLCPP_WARN(logger, "Can't handle encoding %ld (32U == %ld, 64U == %ld)", format, - static_cast(ifm3d::pixel_format::FORMAT_32U), - static_cast(ifm3d::pixel_format::FORMAT_64U)); - result.encoding = sensor_msgs::image_encodings::TYPE_8UC1; - } - - return result; -} - -sensor_msgs::msg::Image ifm3d_to_ros_image(ifm3d::Buffer&& image, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - return ifm3d_to_ros_image(image, header, logger); -} - -sensor_msgs::msg::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Buffer& image, // Need non-const image because - // image.begin(), image.end() - // don't have const overloads. - const std_msgs::msg::Header& header, - const std::string& format, // "jpeg" or "png" - const rclcpp::Logger& logger) -{ - sensor_msgs::msg::CompressedImage result{}; - result.header = header; - result.format = format; - - if (const auto dataFormat = image.dataFormat(); - dataFormat != ifm3d::pixel_format::FORMAT_8S && dataFormat != ifm3d::pixel_format::FORMAT_8U) - { - RCLCPP_ERROR(logger, "Invalid data format for %s data (%ld)", format.c_str(), static_cast(dataFormat)); - return result; - } - - result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), image.width() * image.height())); - return result; -} - -sensor_msgs::msg::CompressedImage ifm3d_to_ros_compressed_image(ifm3d::Buffer&& image, - const std_msgs::msg::Header& header, - const std::string& format, const rclcpp::Logger& logger) -{ - return ifm3d_to_ros_compressed_image(image, header, format, logger); -} - -sensor_msgs::msg::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Buffer& image, // Need non-const image because image.begin(), - // image.end() don't have const overloads. - const std_msgs::msg::Header& header, const rclcpp::Logger& logger) -{ - sensor_msgs::msg::PointCloud2 result{}; - result.header = header; - result.height = image.height(); - result.width = image.width(); - result.is_bigendian = false; - - if (image.begin() == image.end()) - { - return result; - } - - if (image.dataFormat() != ifm3d::pixel_format::FORMAT_32F3 && image.dataFormat() != ifm3d::pixel_format::FORMAT_32F) - { - RCLCPP_ERROR(logger, "Unsupported pixel format %ld for point cloud", static_cast(image.dataFormat())); - return result; - } - - sensor_msgs::msg::PointField x_field{}; - x_field.name = "x"; - x_field.offset = 0; - x_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - x_field.count = 1; - - sensor_msgs::msg::PointField y_field{}; - y_field.name = "y"; - y_field.offset = 4; - y_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - y_field.count = 1; - - sensor_msgs::msg::PointField z_field{}; - z_field.name = "z"; - z_field.offset = 8; - z_field.datatype = sensor_msgs::msg::PointField::FLOAT32; - z_field.count = 1; - - result.fields = { - x_field, - y_field, - z_field, - }; - - result.point_step = result.fields.size() * sizeof(float); - result.row_step = result.point_step * result.width; - result.is_dense = true; - result.data.insert(result.data.end(), image.ptr<>(0), std::next(image.ptr<>(0), result.row_step * result.height)); - - return result; -} - -sensor_msgs::msg::PointCloud2 ifm3d_to_ros_cloud(ifm3d::Buffer&& image, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - return ifm3d_to_ros_cloud(image, header, logger); -} - -ifm3d_ros2::msg::Extrinsics ifm3d_to_extrinsics(ifm3d::Buffer& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - ifm3d_ros2::msg::Extrinsics extrinsics_msg; - extrinsics_msg.header = header; - try - { - extrinsics_msg.tx = buffer.at(0); - extrinsics_msg.ty = buffer.at(1); - extrinsics_msg.tz = buffer.at(2); - extrinsics_msg.rot_x = buffer.at(3); - extrinsics_msg.rot_y = buffer.at(4); - extrinsics_msg.rot_z = buffer.at(5); - } - catch (const std::out_of_range& ex) - { - RCLCPP_WARN(logger, "Out-of-range error fetching extrinsics"); - } - return extrinsics_msg; -} - -ifm3d_ros2::msg::Extrinsics ifm3d_to_extrinsics(ifm3d::Buffer&& buffer, const std_msgs::msg::Header& header, - const rclcpp::Logger& logger) -{ - return ifm3d_to_extrinsics(buffer, header, logger); -} - using json = ifm3d::json; using namespace std::chrono_literals; @@ -223,7 +42,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions& opts) : CameraNode::CameraNode } CameraNode::CameraNode(const std::string& node_name, const rclcpp::NodeOptions& opts) - : rclcpp_lifecycle::LifecycleNode(node_name, "", opts), logger_(this->get_logger()) + : rclcpp_lifecycle::LifecycleNode(node_name, "", opts), logger_(this->get_logger()), width_(0), height_(0) { // unbuffered I/O to stdout (so we can see our log messages) std::setvbuf(stdout, nullptr, _IONBF, BUFSIZ); @@ -704,6 +523,19 @@ void CameraNode::initialize_publishers() case buffer_id_utils::message_type::extrinsics: extrinsics_publishers_[id] = this->create_publisher(topic_name, qos); break; + case buffer_id_utils::message_type::intrinsics: + intrinsics_publishers_[id] = this->create_publisher(topic_name, qos); + camera_info_publishers_[id] = this->create_publisher("~/camera_info", qos); + break; + case buffer_id_utils::message_type::rgb_info: + rgb_info_publishers_[id] = this->create_publisher(topic_name, qos); + break; + case buffer_id_utils::message_type::tof_info: + tof_info_publishers_[id] = this->create_publisher(topic_name, qos); + break; + case buffer_id_utils::message_type::inverse_intrinsics: + inverse_intrinsics_publishers_[id] = this->create_publisher(topic_name, qos); + break; default: std::string id_string; convert(id, id_string); @@ -747,6 +579,26 @@ void CameraNode::activate_publishers() { publisher->on_activate(); } + for (auto& [id, publisher] : intrinsics_publishers_) + { + publisher->on_activate(); + } + for (auto& [id, publisher] : camera_info_publishers_) + { + publisher->on_activate(); + } + for (auto& [id, publisher] : tof_info_publishers_) + { + publisher->on_activate(); + } + for (auto& [id, publisher] : rgb_info_publishers_) + { + publisher->on_activate(); + } + for (auto& [id, publisher] : inverse_intrinsics_publishers_) + { + publisher->on_activate(); + } }; void CameraNode::deactivate_publishers() @@ -767,6 +619,26 @@ void CameraNode::deactivate_publishers() { publisher->on_deactivate(); } + for (auto& [id, publisher] : intrinsics_publishers_) + { + publisher->on_deactivate(); + } + for (auto& [id, publisher] : camera_info_publishers_) + { + publisher->on_deactivate(); + } + for (auto& [id, publisher] : tof_info_publishers_) + { + publisher->on_deactivate(); + } + for (auto& [id, publisher] : rgb_info_publishers_) + { + publisher->on_deactivate(); + } + for (auto& [id, publisher] : inverse_intrinsics_publishers_) + { + publisher->on_deactivate(); + } }; void CameraNode::Config(const std::shared_ptr /*unused*/, ConfigRequest req, ConfigResponse resp) @@ -994,32 +866,72 @@ void CameraNode::frame_callback(ifm3d::Frame::Ptr frame) { case buffer_id_utils::message_type::raw_image: { auto buffer = frame->GetBuffer(id); - ImageMsg raw_image_msg = ifm3d_to_ros_image(frame->GetBuffer(id), optical_header, logger_); + ImageMsg raw_image_msg = ifm3d_ros2::ifm3d_to_ros_image(frame->GetBuffer(id), optical_header, logger_); image_publishers_[id]->publish(raw_image_msg); + width_ = raw_image_msg.width; + height_ = raw_image_msg.width; } break; case buffer_id_utils::message_type::compressed_image: { auto buffer = frame->GetBuffer(id); CompressedImageMsg compressed_image_msg = - ifm3d_to_ros_compressed_image(frame->GetBuffer(id), optical_header, "jpeg", logger_); + ifm3d_ros2::ifm3d_to_ros_compressed_image(frame->GetBuffer(id), optical_header, "jpeg", logger_); compressed_image_publishers_[id]->publish(compressed_image_msg); } break; case buffer_id_utils::message_type::pointcloud: { auto buffer = frame->GetBuffer(id); - PCLMsg pointcloud_msg = ifm3d_to_ros_cloud(frame->GetBuffer(id), cloud_header, logger_); + PCLMsg pointcloud_msg = ifm3d_ros2::ifm3d_to_ros_cloud(frame->GetBuffer(id), cloud_header, logger_); pcl_publishers_[id]->publish(pointcloud_msg); } break; case buffer_id_utils::message_type::extrinsics: { auto buffer = frame->GetBuffer(id); - ExtrinsicsMsg extrinsics_msg = ifm3d_to_extrinsics(buffer, optical_header, logger_); + ExtrinsicsMsg extrinsics_msg = ifm3d_ros2::ifm3d_to_extrinsics(buffer, optical_header, logger_); extrinsics_publishers_[id]->publish(extrinsics_msg); // Set/Update mounting link to cloud link transform publish_cloud_link_transform_if_changed(extrinsics_msg); } break; + case buffer_id_utils::intrinsics: { + auto buffer = frame->GetBuffer(id); + IntrinsicsMsg intrinsics_msg = ifm3d_ros2::ifm3d_to_intrinsics(buffer, optical_header, logger_); + intrinsics_publishers_[id]->publish(intrinsics_msg); + + // Also publish CameraInfo from Intrinsics + if (width_ == 0 || height_ == 0) + { + RCLCPP_WARN_THROTTLE(logger_, clk, 5000, "Needs at least one raw image buffer to parse CameraInfo!"); + } + else + { + CameraInfoMsg camera_info_msg = + ifm3d_ros2::ifm3d_to_camera_info(buffer, optical_header, height_, width_, logger_); + RCLCPP_INFO_ONCE(logger_, "Parsing CameraInfo successfull."); + camera_info_publishers_[id]->publish(camera_info_msg); + } + } + break; + case buffer_id_utils::message_type::inverse_intrinsics: { + auto buffer = frame->GetBuffer(id); + InverseIntrinsicsMsg inverse_intrinsics_msg = + ifm3d_ros2::ifm3d_to_inverse_intrinsics(buffer, optical_header, logger_); + inverse_intrinsics_publishers_[id]->publish(inverse_intrinsics_msg); + } + break; + case buffer_id_utils::message_type::tof_info: { + auto buffer = frame->GetBuffer(id); + TOFInfoMsg tof_info_msg = ifm3d_ros2::ifm3d_to_tof_info(buffer, optical_header, logger_); + tof_info_publishers_[id]->publish(tof_info_msg); + } + break; + case buffer_id_utils::message_type::rgb_info: { + auto buffer = frame->GetBuffer(id); + RGBInfoMsg rgb_info_msg = ifm3d_ros2::ifm3d_to_rgb_info(buffer, optical_header, logger_); + rgb_info_publishers_[id]->publish(rgb_info_msg); + } + break; default: RCLCPP_ERROR_THROTTLE(logger_, clk, 5000, "Unknown message type for buffer_id %s. Can not publish.", id_string.c_str());