Skip to content

Commit

Permalink
update publishFrame flow
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamerKhshiboun
SamerKhshiboun committed May 30, 2023
1 parent 7ac48da commit d3c09cb
Show file tree
Hide file tree
Showing 4 changed files with 113 additions and 96 deletions.
2 changes: 1 addition & 1 deletion realsense2_camera/include/base_realsense_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -209,7 +209,7 @@ namespace realsense2_camera
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);

bool shouldPublishCameraInfo(const stream_index_pair& sip);
Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const;

IMUInfo getImuInfo(const rs2::stream_profile& profile);
Expand Down
3 changes: 3 additions & 0 deletions realsense2_camera/include/ros_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,9 @@ namespace realsense2_camera
const stream_index_pair GYRO{RS2_STREAM_GYRO, 0};
const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0};
const stream_index_pair POSE{RS2_STREAM_POSE, 0};
const stream_index_pair SAFETY{RS2_STREAM_SAFETY, 0};
const stream_index_pair LABELED_POINT_CLOUD{RS2_STREAM_LABELED_POINT_CLOUD, 0};
const stream_index_pair OCCUPANCY{RS2_STREAM_OCCUPANCY, 0};

bool isValidCharInName(char c);

Expand Down
100 changes: 55 additions & 45 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -525,6 +525,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
if (f.is<rs2::labeled_points>())
{
publishLabeledPointCloud(frame.as<rs2::labeled_points>(), t);
publishMetadata(f, t, OPTICAL_FRAME_ID(sip));
continue;
}
if (f.is<rs2::points>())
Expand Down Expand Up @@ -595,6 +596,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
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
Expand Down Expand Up @@ -801,10 +803,18 @@ 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>();

Expand All @@ -824,6 +834,9 @@ void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const r
sensor_msgs::PointCloud2Iterator<float> iter_label(*msg_pointcloud, "label");
const rs2::vertex* vertex = pc.get_vertices();
const uint8_t* label = pc.get_labels();
msg_pointcloud->width = 320;
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);

Expand All @@ -835,8 +848,9 @@ void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const r
*iter_label = *label;
++iter_x; ++iter_y; ++iter_z; ++iter_label;
}

msg_pointcloud->header.stamp = t;
msg_pointcloud->header.frame_id = "camera_link";
msg_pointcloud->header.frame_id = OPTICAL_FRAME_ID(LABELED_POINT_CLOUD);

// Publish the PointCloud message
_labeled_pointcloud_publisher->publish(std::move(msg_pointcloud));
Expand Down Expand Up @@ -918,58 +932,54 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t,
image = fix_depth_scale(image, _depth_scaled_image[stream]);
}

if (info_publishers.find(stream) == info_publishers.end() ||
image_publishers.find(stream) == image_publishers.end())
{
// Stream is already disabled.
return;
}
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())
// if stream is on, and is not a SC stream, publish cameraInfo
if(shouldPublishCameraInfo(stream) && info_publishers.find(stream) != info_publishers.end())
{
auto& cam_info = _camera_info.at(stream);
if (cam_info.width != width)
auto& info_publisher = info_publishers.at(stream);
if(0 != info_publisher->get_subscription_count())
{
updateStreamCalibData(f.get_profile().as<rs2::video_stream_profile>());
}
cam_info.header.stamp = t;
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;

// don't publish camera info messages for SC streams
auto stream_type = f.get_profile().stream_type();
if(stream_type != RS2_STREAM_SAFETY &&
stream_type != RS2_STREAM_OCCUPANCY &&
stream_type != RS2_STREAM_LABELED_POINT_CLOUD)
{
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());

if (!img)
if (image_publishers.find(stream) != image_publishers.end())
{
auto &image_publisher = image_publishers.at(stream);
if (0 != image_publisher->get_subscription_count())
{
ROS_ERROR("sensor image message allocation failed, frame was dropped");
return;
}
// 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());

// 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 (!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)
{
Expand Down
104 changes: 54 additions & 50 deletions realsense2_camera/src/rs_node_setup.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,6 +189,11 @@ void BaseRealSenseNode::stopPublishers(const std::vector<stream_profile>& profil
_info_publisher.erase(sip);
_depth_aligned_image_publishers.erase(sip);
_depth_aligned_info_publisher.erase(sip);

if(_labeled_pointcloud_publisher)
{
_labeled_pointcloud_publisher.reset();
}
}
else if (profile.is<rs2::motion_stream_profile>())
{
Expand Down Expand Up @@ -216,68 +221,68 @@ 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);

// 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));

}
else if (profile.is<rs2::video_stream_profile>())
if (profile.is<rs2::video_stream_profile>())
{
std::stringstream image_raw, camera_info;
bool rectified_image = false;
if (sensor.rs2::sensor::is<rs2::depth_sensor>())
rectified_image = true;

image_raw << stream_name << "/image_" << ((rectified_image)?"rect_":"") << "raw";
camera_info << stream_name << "/camera_info";

// We can use 2 types of publishers:
// Native RCL publisher that support intra-process zero-copy comunication
// image-transport package publisher that adds a commpressed image topic if package is found installed
if (_use_intra_process)
{
_image_publishers[sip] = std::make_shared<image_rcl_publisher>(_node, image_raw.str(), qos);
}
else
{
_image_publishers[sip] = std::make_shared<image_transport_publisher>(_node, image_raw.str(), qos);
ROS_DEBUG_STREAM("image transport publisher was created for topic" << image_raw.str());
}

auto stream_type = profile.stream_type();
if(stream_type != RS2_STREAM_SAFETY &&
stream_type != RS2_STREAM_OCCUPANCY &&
stream_type != RS2_STREAM_LABELED_POINT_CLOUD)
{
_info_publisher[sip] = _node.create_publisher<sensor_msgs::msg::CameraInfo>(camera_info.str(),
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));
}

if (_align_depth_filter->is_enabled() && (sip != DEPTH) && sip.second < 2)
if (profile.stream_type() != RS2_STREAM_LABELED_POINT_CLOUD)
{
std::stringstream aligned_image_raw, aligned_camera_info;
aligned_image_raw << "aligned_depth_to_" << stream_name << "/image_raw";
aligned_camera_info << "aligned_depth_to_" << stream_name << "/camera_info";
std::stringstream image_raw, camera_info;
bool rectified_image = false;
if (sensor.rs2::sensor::is<rs2::depth_sensor>())
rectified_image = true;

std::string aligned_stream_name = "aligned_depth_to_" + stream_name;
image_raw << stream_name << "/image_" << ((rectified_image)?"rect_":"") << "raw";
camera_info << stream_name << "/camera_info";

// We can use 2 types of publishers:
// Native RCL publisher that support intra-process zero-copy comunication
// image-transport package publisher that add's a commpressed image topic if the package is installed
// image-transport package publisher that adds a commpressed image topic if package is found installed
if (_use_intra_process)
{
_depth_aligned_image_publishers[sip] = std::make_shared<image_rcl_publisher>(_node, aligned_image_raw.str(), qos);
_image_publishers[sip] = std::make_shared<image_rcl_publisher>(_node, image_raw.str(), qos);
}
else
{
_depth_aligned_image_publishers[sip] = std::make_shared<image_transport_publisher>(_node, aligned_image_raw.str(), qos);
_image_publishers[sip] = std::make_shared<image_transport_publisher>(_node, image_raw.str(), qos);
ROS_DEBUG_STREAM("image transport publisher was created for topic" << image_raw.str());
}
_depth_aligned_info_publisher[sip] = _node.create_publisher<sensor_msgs::msg::CameraInfo>(aligned_camera_info.str(),

// create cameraInfo publishers only for non-SC streams
if(shouldPublishCameraInfo(sip))
{
_info_publisher[sip] = _node.create_publisher<sensor_msgs::msg::CameraInfo>(camera_info.str(),
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));
}

if (_align_depth_filter->is_enabled() && (sip != DEPTH) && sip.second < 2)
{
std::stringstream aligned_image_raw, aligned_camera_info;
aligned_image_raw << "aligned_depth_to_" << stream_name << "/image_raw";
aligned_camera_info << "aligned_depth_to_" << stream_name << "/camera_info";

std::string aligned_stream_name = "aligned_depth_to_" + stream_name;

// We can use 2 types of publishers:
// Native RCL publisher that support intra-process zero-copy comunication
// image-transport package publisher that add's a commpressed image topic if the package is installed
if (_use_intra_process)
{
_depth_aligned_image_publishers[sip] = std::make_shared<image_rcl_publisher>(_node, aligned_image_raw.str(), qos);
}
else
{
_depth_aligned_image_publishers[sip] = std::make_shared<image_transport_publisher>(_node, aligned_image_raw.str(), qos);
ROS_DEBUG_STREAM("image transport publisher was created for topic" << image_raw.str());
}
_depth_aligned_info_publisher[sip] = _node.create_publisher<sensor_msgs::msg::CameraInfo>(aligned_camera_info.str(),
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));
}
}
else {

// special handling for labeled point cloud stream, since it a topic of PointCloud messages
// and not a normal image publisher
_labeled_pointcloud_publisher = _node.create_publisher<sensor_msgs::msg::PointCloud2>("labeled_point_cloud/points",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos),qos));
}
}
else if (profile.is<rs2::motion_stream_profile>())
Expand All @@ -304,8 +309,7 @@ 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) &&
profile.stream_type() != RS2_STREAM_LABELED_POINT_CLOUD)
if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile))
{

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

0 comments on commit d3c09cb

Please sign in to comment.