Skip to content

Support Depth Mapping Streams #2757

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Nir-Az merged 6 commits into from
Jun 1, 2023
+188 −84
Merged

Support Depth Mapping Streams #2757

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
5edd3d4
support occupancy and labeled point cloud - c1
SamerKhshiboun May 24, 2023
4429766
disable cameraInfo on SC streams
SamerKhshiboun May 29, 2023
7ac48da
Merge branch 'ros2-hkr' into hkr-occupancy
SamerKhshiboun May 29, 2023
b9b32f9
update publishFrame flow
SamerKhshiboun May 29, 2023
872016b
add if/else instead of continue
SamerKhshiboun Jun 1, 2023
8a1e5f9
fix frame name typo causing to null frame
SamerKhshiboun Jun 1, 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
Next Next commit
support occupancy and labeled point cloud - c1
  • Loading branch information
@SamerKhshiboun
SamerKhshiboun committed May 24, 2023
commit 5edd3d4c818253632b180eee142e42988ee30135
31 changes: 30 additions & 1 deletion realsense2_camera/include/base_realsense_node.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -171,6 +171,33 @@ namespace realsense2_camera
void eraseTransformMsgs(const stream_index_pair& sip, const rs2::stream_profile& profile);
void setup();


std::map<rs2_point_cloud_label, float3> _get_label_to_color3f()
{
static const float3 DARK_PURPLE_COL = { 0.2f, 0.1f, 0.2f };
static const float3 BROWN_COL = { 0.6f, 0.3f, 0.0f };
static const float3 GREEN_COL = { 0.0f, 1.0f, 0.0f };
static const float3 RED_COL = { 1.0f, 0.0f, 0.0f };
static const float3 TURQUOISE_COL = { 0.0f, 1.0f, 1.0f };
static const float3 BLUE_COL = { 0.0f, 0.0f, 1.0f };
static const float3 PINK_COL = { 1.0f, 0.75f, 0.8f };
static const float3 GREY_COL = { 0.5f, 0.5f, 0.5f };
std::map<rs2_point_cloud_label, float3> label_to_color3f;

label_to_color3f[RS2_POINT_CLOUD_LABEL_UNKNOWN] = DARK_PURPLE_COL;
label_to_color3f[RS2_POINT_CLOUD_LABEL_UNDEFINED] = DARK_PURPLE_COL;
label_to_color3f[RS2_POINT_CLOUD_LABEL_INVALID] = DARK_PURPLE_COL;
label_to_color3f[RS2_POINT_CLOUD_LABEL_GROUND] = BROWN_COL;
label_to_color3f[RS2_POINT_CLOUD_LABEL_NEAR_GROUND] = GREEN_COL;
label_to_color3f[RS2_POINT_CLOUD_LABEL_OBSTACLE] = RED_COL;
label_to_color3f[RS2_POINT_CLOUD_LABEL_OVERHEAD] = TURQUOISE_COL;
label_to_color3f[RS2_POINT_CLOUD_LABEL_ABOVE_CEILING_HEIGHT] = BLUE_COL;
label_to_color3f[RS2_POINT_CLOUD_LABEL_GAP] = PINK_COL;
label_to_color3f[RS2_POINT_CLOUD_LABEL_MASKED] = GREY_COL;

return label_to_color3f;
}

private:
class CimuData
{
Expand Down Expand Up @@ -208,6 +235,8 @@ namespace realsense2_camera
void startDynamicTf();
void publishDynamicTransforms();
void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset);
void publishLabeledPointCloud(rs2::labeled_points points, const rclcpp::Time& t, const stream_index_pair& stream);

Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const;

IMUInfo getImuInfo(const rs2::stream_profile& profile);
Expand Down Expand Up @@ -271,7 +300,7 @@ namespace realsense2_camera

bool _use_intra_process;
std::map<stream_index_pair, std::shared_ptr<image_publisher>> _image_publishers;

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr _labeled_pointcloud_publisher;
Copy link
Copy Markdown
Collaborator

@remibettan remibettan May 30, 2023

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?

Copy link
Copy Markdown
Contributor Author

@SamerKhshiboun SamerKhshiboun May 30, 2023

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

std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> _imu_publishers;
std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> _odom_publisher;
std::shared_ptr<SyncedImuPublisher> _synced_imu_publisher;
Expand Down
3 changes: 3 additions & 0 deletions realsense2_camera/launch/rs_launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,9 @@
{'name': 'enable_gyro', 'default': 'false', 'description': "'enable gyro stream'"},
{'name': 'enable_accel', 'default': 'false', 'description': "'enable accel stream'"},
{'name': 'enable_safety', 'default': 'false', 'description': "'enable safety stream'"},
{'name': 'enable_labeled_point_cloud', 'default': 'false', 'description': "'enable labeled point cloud stream'"},
{'name': 'enable_occupancy', 'default': 'false', 'description': "'enable occupancy stream'"},
{'name': 'depth_mapping_camera.profile', 'default': '0,0,0', 'description': "'depth mapping camera profile'"},
{'name': 'enable_pose', 'default': 'true', 'description': "'enable pose stream'"},
{'name': 'pose_fps', 'default': '200', 'description': "''"},
{'name': 'pointcloud.enable', 'default': 'false', 'description': ''},
Expand Down
65 changes: 65 additions & 0 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -520,6 +522,11 @@ 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>())
Copy link
Copy Markdown
Collaborator

@remibettan remibettan May 31, 2023

Choose a reason for hiding this comment

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

better change this if statement to else if - do you agree?

Copy link
Copy Markdown
Contributor Author

@SamerKhshiboun SamerKhshiboun Jun 1, 2023

Choose a reason for hiding this comment

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

done

{
publishLabeledPointCloud(frame.as<rs2::labeled_points>(), t, sip);
continue;
}
if (f.is<rs2::points>())
{
publishPointCloud(f.as<rs2::points>(), t, frameset);
Expand Down Expand Up @@ -580,6 +587,15 @@ 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, sip);
}
_synced_imu_publisher->Resume();
} // frame_callback

Expand Down Expand Up @@ -785,6 +801,55 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const rclcpp::Time& t,
_pc_filter->Publish(pc, t, frameset, frame_id);
}

void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const rclcpp::Time& t, const stream_index_pair& stream)
{
auto& info_publisher = _info_publisher.at(stream);
if(0 == info_publisher->get_subscription_count() && 0 == _labeled_pointcloud_publisher->get_subscription_count())
return;

auto mp = _get_label_to_color3f();

// 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::FLOAT32);
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<float> iter_color(*msg_pointcloud, "label");
const rs2::vertex* vertex = pc.get_vertices();
const uint8_t* label = pc.get_labels();
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++)
{
bool valid_pixel(vertex->z > 0);
if (valid_pixel)
{
*iter_x = vertex->x;
*iter_y = vertex->y;
*iter_z = vertex->z;
*iter_color = mp[static_cast<rs2_point_cloud_label>(*label)].x + 256.0*mp[static_cast<rs2_point_cloud_label>(*label)].y + 256.0*256.0*mp[static_cast<rs2_point_cloud_label>(*label)].z;
++iter_x; ++iter_y; ++iter_z; ++iter_color;
}
}
msg_pointcloud->header.stamp = t;
msg_pointcloud->header.frame_id = "camera_link";

// Publish the PointCloud message
_labeled_pointcloud_publisher->publish(std::move(msg_pointcloud));
}


Extrinsics BaseRealSenseNode::rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const
{
Expand Down
18 changes: 15 additions & 3 deletions realsense2_camera/src/rs_node_setup.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,8 @@ void BaseRealSenseNode::setAvailableSensors()
if (sensor.is<rs2::depth_sensor>() ||
sensor.is<rs2::color_sensor>() ||
sensor.is<rs2::fisheye_sensor>() ||
sensor.is<rs2::safety_sensor>())
sensor.is<rs2::safety_sensor>() ||
sensor.is<rs2::depth_mapping_sensor>())
{
ROS_DEBUG_STREAM("Set " << module_name << " as VideoSensor.");
rosSensor = std::make_unique<RosSensor>(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is<playback>());
Expand Down Expand Up @@ -215,7 +216,17 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
rmw_qos_profile_t qos = sensor.getQOS(sip);
rmw_qos_profile_t info_qos = sensor.getInfoQOS(sip);

if (profile.is<rs2::video_stream_profile>())
// special handling for labeled point cloud stream
if (profile.is<rs2::video_stream_profile>() && profile.stream_type() == RS2_STREAM_LABELED_POINT_CLOUD)
{
std::stringstream camera_info(stream_name + "/camera_info");
_labeled_pointcloud_publisher = _node.create_publisher<sensor_msgs::msg::PointCloud2>("labeled_point_cloud/points",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos),qos));
Copy link
Copy Markdown
Collaborator

@remibettan remibettan May 30, 2023

Choose a reason for hiding this comment

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

Please explain what is QoS?

Copy link
Copy Markdown
Contributor Author

@SamerKhshiboun SamerKhshiboun May 30, 2023

Choose a reason for hiding this comment

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

Quality Of Service.

Each publisher should define quality of service, regarding its Durability, Reliability, Depth, History, Queue Size, etc..
Reference: https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html

Read both parts:
1- https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-policies
2- https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-profiles

_info_publisher[sip] = _node.create_publisher<sensor_msgs::msg::CameraInfo>(camera_info.str(),
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));

}
else if (profile.is<rs2::video_stream_profile>())
Copy link
Copy Markdown
Collaborator

@remibettan remibettan May 30, 2023

Choose a reason for hiding this comment

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

Since the video_stream_profile happens most of the time, it should be the first condition in the code.
Please pass it back to the if condition, and move the lpc condition to some else if after it (even maybe last one)

Copy link
Copy Markdown
Contributor Author

@SamerKhshiboun SamerKhshiboun May 30, 2023

Choose a reason for hiding this comment

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

Changed the flow.
Combined it to one "if (profile.isrs2::video_stream_profile()) "
then asked about Labeled PC stream inside it.

{
std::stringstream image_raw, camera_info;
bool rectified_image = false;
Expand Down Expand Up @@ -289,7 +300,8 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
_metadata_publishers[sip] = _node.create_publisher<realsense2_camera_msgs::msg::Metadata>(topic_metadata,
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));

if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile))
if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile) &&
profile.stream_type() != RS2_STREAM_LABELED_POINT_CLOUD)
{

// intra-process do not support latched QoS, so we need to disable intra-process for this topic
Expand Down