diff --git a/README.md b/README.md index b68e7a948e..ec670e6614 100644 --- a/README.md +++ b/README.md @@ -32,6 +32,7 @@ * [TF from coordinate A to coordinate B](#tfs) * [Extrinsics from sensor A to sensor B](#extrinsics) * [Topics](#topics) + * [RGBD Topic](#rgbd) * [Metadata Topic](#metadata) * [Post-Processing Filters](#filters) * [Available Services](#services) @@ -416,6 +417,28 @@ Enabling stream adds matching topics. For instance, enabling the gyro and accel
+

+ RGBD Topic +

+ +RGBD new topic, publishing [RGB + Depth] in the same message (see RGBD.msg for reference). For now, works only with depth aligned to color images, as color and depth images are synchronized by frame time tag. + +These boolean paramters should be true to enable rgbd messages: + +- `enable_rgbd`: new paramter, to enable/disable rgbd topic, changeable during runtime +- `align_depth.enable`: align depth images to rgb images +- `enable_sync`: let librealsense sync between frames, and get the frameset with color and depth images combined +- `enable_color` + `enable_depth`: enable both color and depth sensors + +The current QoS of the topic itself, is the same as Depth and Color streams (SYSTEM_DEFAULT) + +Example: +``` +ros2 launch realsense2_camera rs_launch.py enable_rgbd:=true enable_sync:=true align_depth.enable:=true enable_color:=true enable_depth:=true +``` + +
+

Metadata topic

diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 2622adc160..d6f0a1b97c 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -30,6 +30,7 @@ #include "realsense2_camera_msgs/msg/imu_info.hpp" #include "realsense2_camera_msgs/msg/extrinsics.hpp" #include "realsense2_camera_msgs/msg/metadata.hpp" +#include "realsense2_camera_msgs/msg/rgbd.hpp" #include "realsense2_camera_msgs/srv/device_info.hpp" #include #include @@ -57,6 +58,7 @@ using realsense2_camera_msgs::msg::Extrinsics; using realsense2_camera_msgs::msg::IMUInfo; +using realsense2_camera_msgs::msg::RGBD; #define FRAME_ID(sip) (static_cast(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_frame")).str() #define IMU_FRAME_ID (static_cast(std::ostringstream() << _camera_name << "_imu_frame")).str() @@ -206,7 +208,25 @@ namespace realsense2_camera IMUInfo getImuInfo(const rs2::stream_profile& profile); - void publishFrame(rs2::frame f, + void fillMessageImage( + const cv::Mat& cv_matrix_image, + const stream_index_pair& stream, + unsigned int width, + unsigned int height, + unsigned int bpp, + const rclcpp::Time& t, + sensor_msgs::msg::Image* img_msg_ptr); + + cv::Mat& getCVMatImage( + rs2::frame& frame, + std::map& images, + unsigned int width, + unsigned int height, + unsigned int bpp, + const stream_index_pair& stream); + + void publishFrame( + rs2::frame f, const rclcpp::Time& t, const stream_index_pair& stream, std::map& images, @@ -214,6 +234,8 @@ namespace realsense2_camera const std::map>& image_publishers, const bool is_publishMetadata = true); + void publishRGBD(const cv::Mat& rgb_cv_matrix, const cv::Mat& depth_cv_matrix, const rclcpp::Time& t); + void publishMetadata(rs2::frame f, const rclcpp::Time& header_time, const std::string& frame_id); sensor_msgs::msg::Imu CreateUnitedMessage(const CimuData accel_data, const CimuData gyro_data); @@ -236,6 +258,7 @@ namespace realsense2_camera void updateSensors(); void publishServices(); void startPublishers(const std::vector& profiles, const RosSensor& sensor); + void startRGBDPublisherIfNeeded(); void stopPublishers(const std::vector& profiles); rs2::device _dev; @@ -275,6 +298,7 @@ namespace realsense2_camera std::map::SharedPtr> _metadata_publishers; std::map::SharedPtr> _imu_info_publishers; std::map::SharedPtr> _extrinsics_publishers; + rclcpp::Publisher::SharedPtr _rgbd_publisher; std::map _images; std::map _encoding; @@ -284,6 +308,9 @@ namespace realsense2_camera rclcpp::Time _ros_time_base; bool _sync_frames; + bool _enable_rgbd; + bool _is_color_enabled; + bool _is_depth_enabled; bool _pointcloud; bool _publish_odom_tf; imu_sync_method _imu_sync_method; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 723e97d988..cd53468e0f 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -89,6 +89,7 @@ namespace realsense2_camera const bool ALLOW_NO_TEXTURE_POINTS = false; const bool ORDERED_PC = false; const bool SYNC_FRAMES = false; + const bool ENABLE_RGBD = false; const bool PUBLISH_TF = true; const double TF_PUBLISH_RATE = 0; // Static transform diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 2f93d4de32..dacc9263cb 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -48,15 +48,16 @@ {'name': 'enable_confidence', 'default': 'true', 'description': 'enable confidence'}, {'name': 'gyro_fps', 'default': '0', 'description': "''"}, {'name': 'accel_fps', 'default': '0', 'description': "''"}, - {'name': 'enable_gyro', 'default': 'false', 'description': "''"}, - {'name': 'enable_accel', 'default': 'false', 'description': "''"}, - {'name': 'enable_pose', 'default': 'true', 'description': "''"}, + {'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"}, + {'name': 'enable_accel', 'default': 'false', 'description': "'enable accel stream'"}, + {'name': 'enable_pose', 'default': 'true', 'description': "'enable pose stream'"}, {'name': 'pose_fps', 'default': '200', 'description': "''"}, - {'name': 'pointcloud.enable', 'default': 'false', 'description': ''}, + {'name': 'pointcloud.enable', 'default': 'false', 'description': ''}, {'name': 'pointcloud.stream_filter', 'default': '2', 'description': 'texture stream for pointcloud'}, {'name': 'pointcloud.stream_index_filter','default': '0', 'description': 'texture stream index for pointcloud'}, - {'name': 'enable_sync', 'default': 'false', 'description': "''"}, - {'name': 'align_depth.enable', 'default': 'false', 'description': "''"}, + {'name': 'enable_sync', 'default': 'false', 'description': "'enable sync mode'"}, + {'name': 'enable_rgbd', 'default': 'false', 'description': "'enable rgbd topic'"}, + {'name': 'align_depth.enable', 'default': 'false', 'description': "'enable align depth filter'"}, {'name': 'colorizer.enable', 'default': 'false', 'description': "''"}, {'name': 'clip_distance', 'default': '-2.', 'description': "''"}, {'name': 'linear_accel_cov', 'default': '0.01', 'description': "''"}, diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index bddaa1a097..ae6b50d33c 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -109,6 +109,9 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node, _is_initialized_time_base(false), _camera_time_base(0), _sync_frames(SYNC_FRAMES), + _enable_rgbd(ENABLE_RGBD), + _is_color_enabled(false), + _is_depth_enabled(false), _pointcloud(false), _publish_odom_tf(false), _imu_sync_method(imu_sync_method::NONE), @@ -523,19 +526,21 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) if (f.is()) { publishPointCloud(f.as(), t, frameset); - continue; } - if (stream_type == RS2_STREAM_DEPTH) + else { - if (sent_depth_frame) continue; - sent_depth_frame = true; - if (original_color_frame && _align_depth_filter->is_enabled()) + if (stream_type == RS2_STREAM_DEPTH) { - publishFrame(f, t, COLOR, _depth_aligned_image, _depth_aligned_info_publisher, _depth_aligned_image_publishers, false); - continue; + if (sent_depth_frame) continue; + sent_depth_frame = true; + if (original_color_frame && _align_depth_filter->is_enabled()) + { + publishFrame(f, t, COLOR, _depth_aligned_image, _depth_aligned_info_publisher, _depth_aligned_image_publishers, false); + continue; + } } + publishFrame(f, t, sip, _images, _info_publishers, _image_publishers); } - publishFrame(f, t, sip, _images, _info_publishers, _image_publishers); } if (original_depth_frame && _align_depth_filter->is_enabled()) { @@ -544,8 +549,14 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) frame_to_send = _colorizer_filter->Process(original_depth_frame); else frame_to_send = original_depth_frame; - publishFrame(frame_to_send, t, DEPTH, _images, _info_publishers, _image_publishers); + + // Publish RGBD only if rgbd enabled and both depth and color frames exist. + // On this line we already know original_depth_frame is valid. + if(_enable_rgbd && original_color_frame) + { + publishRGBD(_images[COLOR], _depth_aligned_image[COLOR], t); + } } } else if (frame.is()) @@ -814,13 +825,60 @@ IMUInfo BaseRealSenseNode::getImuInfo(const rs2::stream_profile& profile) return info; } +void BaseRealSenseNode::fillMessageImage( + const cv::Mat& cv_matrix_image, + const stream_index_pair& stream, + unsigned int width, + unsigned int height, + unsigned int bpp, + const rclcpp::Time& t, + sensor_msgs::msg::Image* img_msg_ptr) +{ + // Convert the CV::Mat into a ROS image message (1 copy is done here) + cv_bridge::CvImage(std_msgs::msg::Header(), _encoding.at(bpp), cv_matrix_image).toImageMsg(*img_msg_ptr); + + // Convert OpenCV Mat to ROS Image + img_msg_ptr->header.frame_id = OPTICAL_FRAME_ID(stream); + img_msg_ptr->header.stamp = t; + img_msg_ptr->height = height; + img_msg_ptr->width = width; + img_msg_ptr->is_bigendian = false; + img_msg_ptr->step = width * bpp; +} + +cv::Mat& BaseRealSenseNode::getCVMatImage( + rs2::frame& frame, + std::map& images, + unsigned int width, + unsigned int height, + unsigned int bpp, + const stream_index_pair& stream) +{ + auto& image = images[stream]; + if (image.size() != cv::Size(width, height) || image.depth() != _image_formats[bpp]) + { + image.create(height, width, _image_formats[bpp]); + } + + image.data = (uint8_t*)frame.get_data(); -void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t, - const stream_index_pair& stream, - std::map& images, - const std::map::SharedPtr>& info_publishers, - const std::map>& image_publishers, - const bool is_publishMetadata) + if (frame.is()) + { + image = fix_depth_scale(image, _depth_scaled_image[stream]); + } + + return image; + +} + +void BaseRealSenseNode::publishFrame( + rs2::frame f, + const rclcpp::Time& t, + const stream_index_pair& stream, + std::map& images, + const std::map::SharedPtr>& info_publishers, + const std::map>& image_publishers, + const bool is_publishMetadata) { ROS_DEBUG("publishFrame(...)"); unsigned int width = 0; @@ -833,70 +891,124 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t, height = timage.get_height(); bpp = timage.get_bytes_per_pixel(); } - auto& image = images[stream]; - - if (image.size() != cv::Size(width, height) || image.depth() != _image_formats[bpp]) - { - image.create(height, width, _image_formats[bpp]); - } - image.data = (uint8_t*)f.get_data(); - if (f.is()) + // Publish stream image + if (image_publishers.find(stream) != image_publishers.end()) { - image = fix_depth_scale(image, _depth_scaled_image[stream]); - } + auto &image_publisher = image_publishers.at(stream); + cv::Mat image_cv_matrix; - if (info_publishers.find(stream) == info_publishers.end() || - image_publishers.find(stream) == image_publishers.end()) + // if rgbd has subscribers we fetch the CV image here + if (_rgbd_publisher && 0 != _rgbd_publisher->get_subscription_count()) { - // Stream is already disabled. - return; + image_cv_matrix = getCVMatImage(f, images, width, height, bpp, stream); } - auto& info_publisher = info_publishers.at(stream); - auto& image_publisher = image_publishers.at(stream); - if(0 != info_publisher->get_subscription_count() || - 0 != image_publisher->get_subscription_count()) - { - auto& cam_info = _camera_info.at(stream); - if (cam_info.width != width) + + // if depth/color has subscribers, ask first if rgbd already fetched + // the images from the frame. if not, fetch the relevant color/depth image. + if (0 != image_publisher->get_subscription_count()) { - updateStreamCalibData(f.get_profile().as()); + if(image_cv_matrix.empty()) + { + image_cv_matrix = getCVMatImage(f, images, width, height, bpp, stream); + } + + // Prepare image topic to be published + // We use UniquePtr for allow intra-process publish when subscribers of that type are available + sensor_msgs::msg::Image::UniquePtr img_msg_ptr(new sensor_msgs::msg::Image()); + fillMessageImage(image_cv_matrix, stream, width, height, bpp, t, img_msg_ptr.get()); + if (!img_msg_ptr) + { + ROS_ERROR("sensor image message allocation failed, frame was dropped"); + return; + } + + // Transfer the unique pointer ownership to the RMW + sensor_msgs::msg::Image *msg_address = img_msg_ptr.get(); + image_publisher->publish(std::move(img_msg_ptr)); + + ROS_DEBUG_STREAM(rs2_stream_to_string(f.get_profile().stream_type()) << " stream published, message address: " << std::hex << msg_address); } - cam_info.header.stamp = t; - info_publisher->publish(cam_info); + } - // Prepare image topic to be published - // We use UniquePtr for allow intra-process publish when subscribers of that type are available - sensor_msgs::msg::Image::UniquePtr img(new sensor_msgs::msg::Image()); + // Publish stream camera info + if(info_publishers.find(stream) != info_publishers.end()) + { + auto& info_publisher = info_publishers.at(stream); - if (!img) + // If rgbd has subscribers, get the camera info of color/detph sensors from _camera_info map. + // We need this camera info to fill the rgbd msg, regardless if there subscribers to depth/color camera info. + // We are not publishing this cam_info here, but will be published by rgbd publisher. + if (_rgbd_publisher && 0 != _rgbd_publisher->get_subscription_count()) { - ROS_ERROR("sensor image message allocation failed, frame was dropped"); - return; - } + auto& cam_info = _camera_info.at(stream); - // Convert the CV::Mat into a ROS image message (1 copy is done here) - cv_bridge::CvImage(std_msgs::msg::Header(), _encoding.at(bpp), image).toImageMsg(*img); + // Fix the camera info if needed, usually only in the first time + // when we init this object in the _camera_info map + if (cam_info.width != width) + { + updateStreamCalibData(f.get_profile().as()); + } + cam_info.header.stamp = t; + } - // Convert OpenCV Mat to ROS Image - img->header.frame_id = OPTICAL_FRAME_ID(stream); - img->header.stamp = t; - img->height = height; - img->width = width; - img->is_bigendian = false; - img->step = width * bpp; - // Transfer the unique pointer ownership to the RMW - sensor_msgs::msg::Image* msg_address = img.get(); - image_publisher->publish(std::move(img)); + // If depth/color camera info has subscribers get camera info from _camera_info map, + // and publish this msg. + if(0 != info_publisher->get_subscription_count()) + { + auto& cam_info = _camera_info.at(stream); - ROS_DEBUG_STREAM(rs2_stream_to_string(f.get_profile().stream_type()) << " stream published, message address: " << std::hex << msg_address); + // Fix the camera info if needed, usually only in the first time + // when we init this object in the _camera_info map + if (cam_info.width != width) + { + updateStreamCalibData(f.get_profile().as()); + } + cam_info.header.stamp = t; + info_publisher->publish(cam_info); + } } + + // Publish stream metadata if (is_publishMetadata) { publishMetadata(f, t, OPTICAL_FRAME_ID(stream)); } } + +void BaseRealSenseNode::publishRGBD(const cv::Mat& rgb_cv_matrix, const cv::Mat& depth_cv_matrix, const rclcpp::Time& t) +{ + if (_rgbd_publisher && 0 != _rgbd_publisher->get_subscription_count()) + { + ROS_DEBUG_STREAM("Publishing RGBD message"); + unsigned int rgb_width = rgb_cv_matrix.size().width; + unsigned int rgb_height = rgb_cv_matrix.size().height; + unsigned int rgb_bpp = rgb_cv_matrix.elemSize(); + unsigned int depth_width = depth_cv_matrix.size().width; + unsigned int depth_height = depth_cv_matrix.size().height; + unsigned int depth_bpp = depth_cv_matrix.elemSize(); + + realsense2_camera_msgs::msg::RGBD::UniquePtr msg(new realsense2_camera_msgs::msg::RGBD()); + + fillMessageImage(rgb_cv_matrix, COLOR, rgb_width, rgb_height, rgb_bpp, t, &msg->rgb); + fillMessageImage(depth_cv_matrix, DEPTH, depth_width, depth_height, depth_bpp, t, &msg->depth); + + msg->header.frame_id = "camera_rgbd_optical_frame"; + msg->header.stamp = t; + + auto rgb_camera_info = _camera_info.at(COLOR); + msg->rgb_camera_info = rgb_camera_info; + + auto depth_camera_info = _camera_info.at(DEPTH); + msg->depth_camera_info = depth_camera_info; + + realsense2_camera_msgs::msg::RGBD *msg_address = msg.get(); + _rgbd_publisher->publish(std::move(msg)); + ROS_DEBUG_STREAM("rgbd stream published, message address: " << std::hex << msg_address); + } +} + void BaseRealSenseNode::publishMetadata(rs2::frame f, const rclcpp::Time& header_time, const std::string& frame_id) { stream_index_pair stream = {f.get_profile().stream_type(), f.get_profile().stream_index()}; diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index 3a2ef1c77c..16965f27cf 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -46,6 +46,17 @@ void BaseRealSenseNode::getParameters() _parameters->setParamT(param_name, _sync_frames); _parameters_names.push_back(param_name); + param_name = std::string("enable_rgbd"); + _parameters->setParamT(param_name, _enable_rgbd, [this](const rclcpp::Parameter& ) + { + { + std::lock_guard lock_guard(_profile_changes_mutex); + _is_profile_changed = true; + } + _cv_mpc.notify_one(); + }); + _parameters_names.push_back(param_name); + param_name = std::string("json_file_path"); _json_file_path = _parameters->setParam(param_name, ""); _parameters_names.push_back(param_name); diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 57d7420489..fcdb673e34 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense2_camera/src/rs_node_setup.cpp @@ -160,8 +160,8 @@ void BaseRealSenseNode::setAvailableSensors() } else { - ROS_ERROR_STREAM("Module Name \"" << module_name << "\" does not define a callback."); - throw("Error: Module not supported"); + ROS_WARN_STREAM("Module Name \"" << module_name << "\" does not define a callback."); + continue; } _available_ros_sensors.push_back(std::move(rosSensor)); } @@ -216,6 +216,10 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi if (profile.is()) { + if(profile.stream_type() == RS2_STREAM_COLOR) + _is_color_enabled = true; + else if (profile.stream_type() == RS2_STREAM_DEPTH) + _is_depth_enabled = true; std::stringstream image_raw, camera_info; bool rectified_image = false; if (sensor.rs2::sensor::is()) @@ -303,6 +307,25 @@ void BaseRealSenseNode::startPublishers(const std::vector& profi } } +void BaseRealSenseNode::startRGBDPublisherIfNeeded() +{ + _rgbd_publisher.reset(); + if(_enable_rgbd && !_rgbd_publisher) + { + if (_sync_frames && _is_color_enabled && _is_depth_enabled && _align_depth_filter->is_enabled()) + { + rmw_qos_profile_t qos = _use_intra_process ? qos_string_to_qos(DEFAULT_QOS) : qos_string_to_qos(IMAGE_QOS); + + _rgbd_publisher = _node.create_publisher("rgbd", + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos)); + } + else { + ROS_ERROR("In order to get rgbd topic enabled, "\ + "you should enable: color stream, depth stream, sync_mode and align_depth"); + } + } +} + void BaseRealSenseNode::updateSensors() { std::lock_guard lock_guard(_update_sensor_mutex); @@ -364,6 +387,7 @@ void BaseRealSenseNode::updateSensors() std::lock_guard lock_guard(_publish_tf_mutex); publishStaticTransforms(); } + startRGBDPublisherIfNeeded(); } catch(const std::exception& ex) { diff --git a/realsense2_camera/src/tfs.cpp b/realsense2_camera/src/tfs.cpp index 4780eb278f..3e82666c9f 100644 --- a/realsense2_camera/src/tfs.cpp +++ b/realsense2_camera/src/tfs.cpp @@ -53,7 +53,7 @@ void BaseRealSenseNode::append_static_tf_msg(const rclcpp::Time& t, msg.header.frame_id = from; msg.child_frame_id = to; - // Convert x,y,z (taken from camera extrinsics) + // Convert translation vector (x,y,z) (taken from camera extrinsics) // from optical cooridnates to ros coordinates msg.transform.translation.x = trans.z; msg.transform.translation.y = -trans.x; @@ -124,12 +124,18 @@ void BaseRealSenseNode::calcAndAppendTransformMsgs(const rs2::stream_profile& pr { // Transform base to stream stream_index_pair sip(profile.stream_type(), profile.stream_index()); + + // rotation quaternion from ROS CS to Optical CS tf2::Quaternion quaternion_optical; - quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2); - float3 zero_trans{0, 0, 0}; + quaternion_optical.setRPY(-M_PI / 2, 0, -M_PI/2); // R,P,Y rotations over the original axes + + // zero rotation quaternion, used for IMU tf2::Quaternion zero_rot_quaternions; zero_rot_quaternions.setRPY(0, 0, 0); + // zero translation, used for moving from ROS frame to its correspending Optical frame + float3 zero_trans{0, 0, 0}; + rclcpp::Time transform_ts_ = _node.now(); // extrinsic from A to B is the position of A relative to B @@ -166,14 +172,28 @@ void BaseRealSenseNode::calcAndAppendTransformMsgs(const rs2::stream_profile& pr // publish normal extrinsics e.g. /camera/extrinsics/depth_to_color publishExtrinsicsTopic(sip, normal_ex); - // publish static TF + // Representing Rotations with Quaternions + // see https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Using_quaternions_as_rotations for reference + + // Q defines a quaternion rotation from to + // for example, rotation from depth to infra2 auto Q = rotationMatrixToQuaternion(tf_ex.rotation); - Q = quaternion_optical * Q * quaternion_optical.inverse(); + float3 trans{tf_ex.translation[0], tf_ex.translation[1], tf_ex.translation[2]}; - append_static_tf_msg(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip)); + // Rotation order is important (start from left to right): + // 1. quaternion_optical.inverse() [ROS -> Optical] + // 2. Q [Optical -> Optical] (usually no rotation, but might be very small rotations between sensors, like from Depth to Color) + // 3. quaternion_optical [Optical -> ROS] + // We do all these products since we want to finish in ROS CS, while Q is a rotation from optical to optical, + // and cant be used directly in ROS TF without this combination + Q = quaternion_optical * Q * quaternion_optical.inverse(); - // Transform stream frame to stream optical frame and publish it + // The translation vector is in the Optical CS, and we convert it to ROS CS inside append_static_tf_msg + append_static_tf_msg(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip)); + + // Transform stream frame from ROS CS to optical CS and publish it + // We are using zero translation vector here, since no translation between frame and optical_frame, but only rotation append_static_tf_msg(transform_ts_, zero_trans, quaternion_optical, FRAME_ID(sip), OPTICAL_FRAME_ID(sip)); if (profile.is() && diff --git a/realsense2_camera_msgs/CMakeLists.txt b/realsense2_camera_msgs/CMakeLists.txt index a1203e88c7..831cab9243 100644 --- a/realsense2_camera_msgs/CMakeLists.txt +++ b/realsense2_camera_msgs/CMakeLists.txt @@ -28,16 +28,18 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) set(msg_files "msg/IMUInfo.msg" "msg/Extrinsics.msg" "msg/Metadata.msg" + "msg/RGBD.msg" ) rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} "srv/DeviceInfo.srv" - DEPENDENCIES builtin_interfaces std_msgs + DEPENDENCIES builtin_interfaces std_msgs sensor_msgs ADD_LINTER_TESTS ) diff --git a/realsense2_camera_msgs/msg/RGBD.msg b/realsense2_camera_msgs/msg/RGBD.msg new file mode 100644 index 0000000000..faa44ce80a --- /dev/null +++ b/realsense2_camera_msgs/msg/RGBD.msg @@ -0,0 +1,6 @@ +# RGBD Message +std_msgs/Header header +sensor_msgs/CameraInfo rgb_camera_info +sensor_msgs/CameraInfo depth_camera_info +sensor_msgs/Image rgb +sensor_msgs/Image depth diff --git a/realsense2_camera_msgs/package.xml b/realsense2_camera_msgs/package.xml index 8c527be1fe..a9a3daa9e3 100644 --- a/realsense2_camera_msgs/package.xml +++ b/realsense2_camera_msgs/package.xml @@ -16,10 +16,12 @@ rosidl_default_generators builtin_interfaces std_msgs + sensor_msgs rosidl_default_runtime builtin_interfaces std_msgs + sensor_msgs ament_lint_common