Support Depth Mapping Streams #2757
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -19,6 +19,8 @@ | |
| #include <rclcpp/clock.hpp> | ||
| #include <fstream> | ||
| #include <image_publisher.h> | ||
| #include <sensor_msgs/msg/point_cloud2.hpp> | ||
| #include <sensor_msgs/point_cloud2_iterator.hpp> | ||
|
|
||
| // Header files for disabling intra-process comms for static broadcaster. | ||
| #include <rclcpp/publisher_options.hpp> | ||
|
|
@@ -520,29 +522,36 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) | |
| if (f.is<rs2::video_frame>()) | ||
| ROS_DEBUG_STREAM("frame: " << f.as<rs2::video_frame>().get_width() << " x " << f.as<rs2::video_frame>().get_height()); | ||
|
|
||
| if (f.is<rs2::labeled_points>()) | ||
|
Collaborator
There was a problem hiding this comment. better change this if statement to else if - do you agree? |
||
| { | ||
| publishLabeledPointCloud(f.as<rs2::labeled_points>(), t); | ||
| publishMetadata(f, t, OPTICAL_FRAME_ID(sip)); | ||
| } | ||
| else if (f.is<rs2::points>()) | ||
| { | ||
| publishPointCloud(f.as<rs2::points>(), t, frameset); | ||
| } | ||
| else | ||
| { | ||
| if (stream_type == RS2_STREAM_DEPTH) | ||
| { | ||
| if (sent_depth_frame) continue; | ||
| sent_depth_frame = true; | ||
| if (_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, | ||
| _image, | ||
| _info_publisher, | ||
| _image_publishers); | ||
| } | ||
| } | ||
| if (original_depth_frame && _align_depth_filter->is_enabled()) | ||
| { | ||
|
|
@@ -580,6 +589,16 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) | |
| _info_publisher, | ||
| _image_publishers); | ||
| } | ||
| else if (frame.is<rs2::labeled_points>()) | ||
| { | ||
| auto stream_type = frame.get_profile().stream_type(); | ||
| auto stream_index = frame.get_profile().stream_index(); | ||
| stream_index_pair sip{stream_type,stream_index}; | ||
| ROS_DEBUG("Single labeled point cloud frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", | ||
| rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame_time, t.nanoseconds()); | ||
| publishLabeledPointCloud(frame.as<rs2::labeled_points>(), t); | ||
| publishMetadata(frame, t, OPTICAL_FRAME_ID(sip)); | ||
| } | ||
| _synced_imu_publisher->Resume(); | ||
| } // frame_callback | ||
|
|
||
|
|
@@ -785,6 +804,59 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const rclcpp::Time& t, | |
| _pc_filter->Publish(pc, t, frameset, frame_id); | ||
| } | ||
|
|
||
| bool BaseRealSenseNode::shouldPublishCameraInfo(const stream_index_pair& sip) | ||
| { | ||
| const rs2_stream stream = sip.first; | ||
| return (stream != RS2_STREAM_SAFETY && stream != RS2_STREAM_OCCUPANCY && stream != RS2_STREAM_LABELED_POINT_CLOUD); | ||
| } | ||
|
|
||
| void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const rclcpp::Time& t){ | ||
| if(0 == _labeled_pointcloud_publisher->get_subscription_count()) | ||
| return; | ||
|
|
||
| ROS_DEBUG("Publishing Labeled Point Cloud Frame"); | ||
|
|
||
| // Create the PointCloud message | ||
| sensor_msgs::msg::PointCloud2::UniquePtr msg_pointcloud = std::make_unique<sensor_msgs::msg::PointCloud2>(); | ||
|
|
||
| // Define the fields of the PointCloud message | ||
| sensor_msgs::PointCloud2Modifier modifier(*msg_pointcloud); | ||
|
|
||
| modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::msg::PointField::FLOAT32, | ||
| "y", 1, sensor_msgs::msg::PointField::FLOAT32, | ||
| "z", 1, sensor_msgs::msg::PointField::FLOAT32, | ||
| "label", 1, sensor_msgs::msg::PointField::UINT8); | ||
| modifier.resize(pc.size()); | ||
|
|
||
| // Fill the PointCloud message with data | ||
| sensor_msgs::PointCloud2Iterator<float> iter_x(*msg_pointcloud, "x"); | ||
| sensor_msgs::PointCloud2Iterator<float> iter_y(*msg_pointcloud, "y"); | ||
| sensor_msgs::PointCloud2Iterator<float> iter_z(*msg_pointcloud, "z"); | ||
| sensor_msgs::PointCloud2Iterator<uint8_t> iter_label(*msg_pointcloud, "label"); | ||
| const rs2::vertex* vertex = pc.get_vertices(); | ||
| const uint8_t* label = pc.get_labels(); | ||
| msg_pointcloud->width = 320; | ||
|
Contributor
Author
There was a problem hiding this comment. @remibettan
Collaborator
There was a problem hiding this comment. The API size() should be used instead of width and height
Contributor
Author
There was a problem hiding this comment. but I used size to get the size of the whole PC.
Collaborator
There was a problem hiding this comment. Well, you may this. Because, as for point cloud, there is no meaning to height and width there, since vertices are passed, and not a frame
Contributor
Author
There was a problem hiding this comment. I understand we are getting unordered vertices, not an ordered ones.
Collaborator
There was a problem hiding this comment. This was a valid question :) |
||
| msg_pointcloud->height = 180; | ||
| msg_pointcloud->point_step = 3*sizeof(float) + sizeof(uint8_t); | ||
| msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step; | ||
| msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step); | ||
|
|
||
| for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++, label++) | ||
| { | ||
| *iter_x = vertex->x; | ||
| *iter_y = vertex->y; | ||
| *iter_z = vertex->z; | ||
| *iter_label = *label; | ||
| ++iter_x; ++iter_y; ++iter_z; ++iter_label; | ||
| } | ||
|
|
||
| msg_pointcloud->header.stamp = t; | ||
| msg_pointcloud->header.frame_id = OPTICAL_FRAME_ID(LABELED_POINT_CLOUD); | ||
|
|
||
| // Publish the PointCloud message | ||
| _labeled_pointcloud_publisher->publish(std::move(msg_pointcloud)); | ||
| } | ||
|
|
||
|
|
||
| Extrinsics BaseRealSenseNode::rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const | ||
| { | ||
|
|
@@ -861,50 +933,54 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t, | |
| image = fix_depth_scale(image, _depth_scaled_image[stream]); | ||
| } | ||
|
|
||
| // if stream is on, and is not a SC stream, publish cameraInfo | ||
| if(shouldPublishCameraInfo(stream) && info_publishers.find(stream) != info_publishers.end()) | ||
| { | ||
| auto& info_publisher = info_publishers.at(stream); | ||
| if(0 != info_publisher->get_subscription_count()) | ||
| { | ||
| auto& cam_info = _camera_info.at(stream); | ||
| if (cam_info.width != width) | ||
| { | ||
| updateStreamCalibData(f.get_profile().as<rs2::video_stream_profile>()); | ||
| } | ||
| cam_info.header.stamp = t; | ||
|
|
||
| info_publisher->publish(cam_info); | ||
| } | ||
| } | ||
|
|
||
| if (image_publishers.find(stream) != image_publishers.end()) | ||
| { | ||
| auto &image_publisher = image_publishers.at(stream); | ||
| if (0 != image_publisher->get_subscription_count()) | ||
| { | ||
| // 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()); | ||
|
|
||
| if (!img) | ||
| { | ||
| ROS_ERROR("sensor image message allocation failed, frame was dropped"); | ||
| return; | ||
| } | ||
|
|
||
| // 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); | ||
|
|
||
| // 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)); | ||
|
|
||
| ROS_DEBUG_STREAM(rs2_stream_to_string(f.get_profile().stream_type()) << " stream published, message address: " << std::hex << msg_address); | ||
| } | ||
| } | ||
| if (is_publishMetadata) | ||
| { | ||
|
|
||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Can you explain why you have a specific publisher for lpc and not for pc?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Because we can turn on both Depth point cloud and Labeled Point cloud at the same time
These are two different publishers