Skip to content

Commit d694b34

Browse files
SamerKhshibounSamerKhshiboun
committed
update publishFrame flow
1 parent 7ac48da commit d694b34

4 files changed

Lines changed: 69 additions & 54 deletions

File tree

‎realsense2_camera/include/base_realsense_node.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -209,7 +209,7 @@ namespace realsense2_camera
209209
void publishDynamicTransforms();
210210
void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset);
211211
void publishLabeledPointCloud(rs2::labeled_points points, const rclcpp::Time& t);
212-
212+
bool isSCStream(const stream_index_pair& sip);
213213
Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const;
214214

215215
IMUInfo getImuInfo(const rs2::stream_profile& profile);

‎realsense2_camera/include/ros_utils.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,9 @@ namespace realsense2_camera
3737
const stream_index_pair GYRO{RS2_STREAM_GYRO, 0};
3838
const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0};
3939
const stream_index_pair POSE{RS2_STREAM_POSE, 0};
40+
const stream_index_pair SAFETY{RS2_STREAM_SAFETY, 0};
41+
const stream_index_pair LABELED_POINT_CLOUD{RS2_STREAM_LABELED_POINT_CLOUD, 0};
42+
const stream_index_pair OCCUPANCY{RS2_STREAM_OCCUPANCY, 0};
4043

4144
bool isValidCharInName(char c);
4245

‎realsense2_camera/src/base_realsense_node.cpp‎

Lines changed: 57 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -525,6 +525,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
525525
if (f.is<rs2::labeled_points>())
526526
{
527527
publishLabeledPointCloud(frame.as<rs2::labeled_points>(), t);
528+
publishMetadata(f, t, OPTICAL_FRAME_ID(sip));
528529
continue;
529530
}
530531
if (f.is<rs2::points>())
@@ -595,6 +596,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
595596
ROS_DEBUG("Single labeled point cloud frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu",
596597
rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame_time, t.nanoseconds());
597598
publishLabeledPointCloud(frame.as<rs2::labeled_points>(), t);
599+
publishMetadata(frame, t, OPTICAL_FRAME_ID(sip));
598600
}
599601
_synced_imu_publisher->Resume();
600602
} // frame_callback
@@ -801,10 +803,18 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const rclcpp::Time& t,
801803
_pc_filter->Publish(pc, t, frameset, frame_id);
802804
}
803805

806+
bool BaseRealSenseNode::isSCStream(const stream_index_pair& sip)
807+
{
808+
const rs2_stream stream = sip.first;
809+
return (stream == RS2_STREAM_SAFETY || stream == RS2_STREAM_OCCUPANCY || stream == RS2_STREAM_LABELED_POINT_CLOUD);
810+
}
811+
804812
void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const rclcpp::Time& t){
805813
if(0 == _labeled_pointcloud_publisher->get_subscription_count())
806814
return;
807815

816+
ROS_DEBUG("Publishing Labeled Point Cloud Frame");
817+
808818
// Create the PointCloud message
809819
sensor_msgs::msg::PointCloud2::UniquePtr msg_pointcloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
810820

@@ -824,6 +834,9 @@ void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const r
824834
sensor_msgs::PointCloud2Iterator<float> iter_label(*msg_pointcloud, "label");
825835
const rs2::vertex* vertex = pc.get_vertices();
826836
const uint8_t* label = pc.get_labels();
837+
msg_pointcloud->width = 320;
838+
msg_pointcloud->height = 180;
839+
msg_pointcloud->point_step = 3*sizeof(float) + sizeof(uint8_t);
827840
msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step;
828841
msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step);
829842

@@ -835,8 +848,9 @@ void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const r
835848
*iter_label = *label;
836849
++iter_x; ++iter_y; ++iter_z; ++iter_label;
837850
}
851+
838852
msg_pointcloud->header.stamp = t;
839-
msg_pointcloud->header.frame_id = "camera_link";
853+
msg_pointcloud->header.frame_id = OPTICAL_FRAME_ID(LABELED_POINT_CLOUD);
840854

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

921-
if (info_publishers.find(stream) == info_publishers.end() ||
922-
image_publishers.find(stream) == image_publishers.end())
923-
{
924-
// Stream is already disabled.
925-
return;
926-
}
927-
auto& info_publisher = info_publishers.at(stream);
928-
auto& image_publisher = image_publishers.at(stream);
929-
if(0 != info_publisher->get_subscription_count() ||
930-
0 != image_publisher->get_subscription_count())
935+
bool is_sc_stream = isSCStream(stream);
936+
937+
// if stream is on, and is not a SC stream, publish camereaInfo
938+
if(!is_sc_stream && info_publishers.find(stream) != info_publishers.end())
931939
{
932-
auto& cam_info = _camera_info.at(stream);
933-
if (cam_info.width != width)
940+
auto& info_publisher = info_publishers.at(stream);
941+
if(0 != info_publisher->get_subscription_count())
934942
{
935-
updateStreamCalibData(f.get_profile().as<rs2::video_stream_profile>());
936-
}
937-
cam_info.header.stamp = t;
943+
auto& cam_info = _camera_info.at(stream);
944+
if (cam_info.width != width)
945+
{
946+
updateStreamCalibData(f.get_profile().as<rs2::video_stream_profile>());
947+
}
948+
cam_info.header.stamp = t;
938949

939-
// don't publish camera info messages for SC streams
940-
auto stream_type = f.get_profile().stream_type();
941-
if(stream_type != RS2_STREAM_SAFETY &&
942-
stream_type != RS2_STREAM_OCCUPANCY &&
943-
stream_type != RS2_STREAM_LABELED_POINT_CLOUD)
944-
{
945950
info_publisher->publish(cam_info);
946951
}
952+
}
947953

948-
// Prepare image topic to be published
949-
// We use UniquePtr for allow intra-process publish when subscribers of that type are available
950-
sensor_msgs::msg::Image::UniquePtr img(new sensor_msgs::msg::Image());
951-
952-
if (!img)
954+
if (image_publishers.find(stream) != image_publishers.end())
955+
{
956+
auto &image_publisher = image_publishers.at(stream);
957+
if (0 != image_publisher->get_subscription_count())
953958
{
954-
ROS_ERROR("sensor image message allocation failed, frame was dropped");
955-
return;
956-
}
959+
// Prepare image topic to be published
960+
// We use UniquePtr for allow intra-process publish when subscribers of that type are available
961+
sensor_msgs::msg::Image::UniquePtr img(new sensor_msgs::msg::Image());
957962

958-
// Convert the CV::Mat into a ROS image message (1 copy is done here)
959-
cv_bridge::CvImage(std_msgs::msg::Header(), _encoding.at(bpp), image).toImageMsg(*img);
960-
961-
// Convert OpenCV Mat to ROS Image
962-
img->header.frame_id = OPTICAL_FRAME_ID(stream);
963-
img->header.stamp = t;
964-
img->height = height;
965-
img->width = width;
966-
img->is_bigendian = false;
967-
img->step = width * bpp;
968-
// Transfer the unique pointer ownership to the RMW
969-
sensor_msgs::msg::Image* msg_address = img.get();
970-
image_publisher->publish(std::move(img));
971-
972-
ROS_DEBUG_STREAM(rs2_stream_to_string(f.get_profile().stream_type()) << " stream published, message address: " << std::hex << msg_address);
963+
if (!img)
964+
{
965+
ROS_ERROR("sensor image message allocation failed, frame was dropped");
966+
return;
967+
}
968+
969+
// Convert the CV::Mat into a ROS image message (1 copy is done here)
970+
cv_bridge::CvImage(std_msgs::msg::Header(), _encoding.at(bpp), image).toImageMsg(*img);
971+
972+
// Convert OpenCV Mat to ROS Image
973+
img->header.frame_id = OPTICAL_FRAME_ID(stream);
974+
img->header.stamp = t;
975+
img->height = height;
976+
img->width = width;
977+
img->is_bigendian = false;
978+
img->step = width * bpp;
979+
// Transfer the unique pointer ownership to the RMW
980+
sensor_msgs::msg::Image *msg_address = img.get();
981+
image_publisher->publish(std::move(img));
982+
983+
ROS_DEBUG_STREAM(rs2_stream_to_string(f.get_profile().stream_type()) << " stream published, message address: " << std::hex << msg_address);
984+
}
973985
}
974986
if (is_publishMetadata)
975987
{

‎realsense2_camera/src/rs_node_setup.cpp‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -189,6 +189,11 @@ void BaseRealSenseNode::stopPublishers(const std::vector<stream_profile>& profil
189189
_info_publisher.erase(sip);
190190
_depth_aligned_image_publishers.erase(sip);
191191
_depth_aligned_info_publisher.erase(sip);
192+
193+
if(_labeled_pointcloud_publisher)
194+
{
195+
_labeled_pointcloud_publisher.reset();
196+
}
192197
}
193198
else if (profile.is<rs2::motion_stream_profile>())
194199
{
@@ -219,10 +224,8 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
219224
// special handling for labeled point cloud stream
220225
if (profile.is<rs2::video_stream_profile>() && profile.stream_type() == RS2_STREAM_LABELED_POINT_CLOUD)
221226
{
222-
std::stringstream camera_info(stream_name + "/camera_info");
223227
_labeled_pointcloud_publisher = _node.create_publisher<sensor_msgs::msg::PointCloud2>("labeled_point_cloud/points",
224228
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos),qos));
225-
226229
}
227230
else if (profile.is<rs2::video_stream_profile>())
228231
{
@@ -247,10 +250,8 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
247250
ROS_DEBUG_STREAM("image transport publisher was created for topic" << image_raw.str());
248251
}
249252

250-
auto stream_type = profile.stream_type();
251-
if(stream_type != RS2_STREAM_SAFETY &&
252-
stream_type != RS2_STREAM_OCCUPANCY &&
253-
stream_type != RS2_STREAM_LABELED_POINT_CLOUD)
253+
// create cameraInfo publishers only for non-SC streams
254+
if(!isSCStream(sip))
254255
{
255256
_info_publisher[sip] = _node.create_publisher<sensor_msgs::msg::CameraInfo>(camera_info.str(),
256257
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));
@@ -304,8 +305,7 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
304305
_metadata_publishers[sip] = _node.create_publisher<realsense2_camera_msgs::msg::Metadata>(topic_metadata,
305306
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));
306307

307-
if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile) &&
308-
profile.stream_type() != RS2_STREAM_LABELED_POINT_CLOUD)
308+
if (!((rs2::stream_profile)profile==(rs2::stream_profile)_base_profile))
309309
{
310310

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

0 commit comments

Comments
 (0)