Skip to content

Commit

Permalink
return status when filling cv matrix and ros images
Browse files Browse the repository at this point in the history
  • Loading branch information
@SamerKhshiboun
SamerKhshiboun committed Aug 15, 2023
1 parent 4cc3d65 commit 6f40a4f
Show file tree
Hide file tree
Showing 2 changed files with 58 additions and 22 deletions.
4 changes: 2 additions & 2 deletions realsense2_camera/include/base_realsense_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,7 +208,7 @@ namespace realsense2_camera
IMUInfo getImuInfo(const rs2::stream_profile& profile);
void initializeFormatsMaps();

void fillMessageImage(
bool fillROSImageMsgAndReturnStatus(
const cv::Mat& cv_matrix_image,
const stream_index_pair& stream,
unsigned int width,
Expand All @@ -217,7 +217,7 @@ namespace realsense2_camera
const rclcpp::Time& t,
sensor_msgs::msg::Image* img_msg_ptr);

cv::Mat& getCVMatImage(
bool fillCVMatImageAndReturnStatus(
rs2::frame& frame,
std::map<stream_index_pair, cv::Mat>& images,
unsigned int width,
Expand Down
76 changes: 56 additions & 20 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -862,7 +862,7 @@ IMUInfo BaseRealSenseNode::getImuInfo(const rs2::stream_profile& profile)
return info;
}

void BaseRealSenseNode::fillMessageImage(
bool BaseRealSenseNode::fillROSImageMsgAndReturnStatus(
const cv::Mat& cv_matrix_image,
const stream_index_pair& stream,
unsigned int width,
Expand All @@ -871,9 +871,16 @@ void BaseRealSenseNode::fillMessageImage(
const rclcpp::Time& t,
sensor_msgs::msg::Image* img_msg_ptr)
{
if(_rs_format_to_ros_format.find(stream_format) == _rs_format_to_ros_format.end())
if (cv_matrix_image.empty())
{
ROS_ERROR_STREAM("Format " << rs2_format_to_string(stream_format) << " is not supported in ROS2 image messages");
ROS_ERROR_STREAM("cv::Mat is empty. Ignoring this frame.");
return false;
}
else if (_rs_format_to_ros_format.find(stream_format) == _rs_format_to_ros_format.end())
{
ROS_ERROR_STREAM("Format " << rs2_format_to_string(stream_format) << " is not supported in ROS2 image messages"
<< "Please try different format of this stream.");
return false;
}
// Convert the CV::Mat into a ROS image message (1 copy is done here)
cv_bridge::CvImage(std_msgs::msg::Header(), _rs_format_to_ros_format[stream_format], cv_matrix_image).toImageMsg(*img_msg_ptr);
Expand All @@ -885,9 +892,11 @@ void BaseRealSenseNode::fillMessageImage(
img_msg_ptr->width = width;
img_msg_ptr->is_bigendian = false;
img_msg_ptr->step = width * cv_matrix_image.elemSize();

return true;
}

cv::Mat& BaseRealSenseNode::getCVMatImage(
bool BaseRealSenseNode::fillCVMatImageAndReturnStatus(
rs2::frame& frame,
std::map<stream_index_pair, cv::Mat>& images,
unsigned int width,
Expand All @@ -899,7 +908,9 @@ cv::Mat& BaseRealSenseNode::getCVMatImage(

if (_rs_format_to_cv_format.find(stream_format) == _rs_format_to_cv_format.end())
{
ROS_ERROR_STREAM("Format " << rs2_format_to_string(stream_format) << " is not supported in realsense2_camera node");
ROS_ERROR_STREAM("Format " << rs2_format_to_string(stream_format) << " is not supported in realsense2_camera node."
<< "\nPlease try different format of this stream.");
return false;
}
// we try to reduce image creation as much we can, so we check if the same image structure
// was already created before, and we fill this image next with the frame data
Expand All @@ -916,8 +927,7 @@ cv::Mat& BaseRealSenseNode::getCVMatImage(
image = fix_depth_scale(image, _depth_scaled_image[stream]);
}

return image;

return true;
}

void BaseRealSenseNode::publishFrame(
Expand All @@ -932,14 +942,19 @@ void BaseRealSenseNode::publishFrame(
ROS_DEBUG("publishFrame(...)");
unsigned int width = 0;
unsigned int height = 0;
auto stream_format = f.get_profile().format();
auto stream_format = RS2_FORMAT_ANY;
if (f.is<rs2::video_frame>())
{
auto timage = f.as<rs2::video_frame>();
width = timage.get_width();
height = timage.get_height();
stream_format = timage.get_profile().format();
}
else
{
ROS_ERROR("f.is<rs2::video_frame>() check failed. Frame was dropped.");
return;
}

// Publish stream image
if (image_publishers.find(stream) != image_publishers.end())
Expand All @@ -950,33 +965,43 @@ void BaseRealSenseNode::publishFrame(
// if rgbd has subscribers we fetch the CV image here
if (_rgbd_publisher && 0 != _rgbd_publisher->get_subscription_count())
{
image_cv_matrix = getCVMatImage(f, images, width, height, stream);
if (fillCVMatImageAndReturnStatus(f, images, width, height, stream))
{
image_cv_matrix = images[stream];
}
}

// 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())
{
if(image_cv_matrix.empty())
if (image_cv_matrix.empty() && fillCVMatImageAndReturnStatus(f, images, width, height, stream))
{
image_cv_matrix = getCVMatImage(f, images, width, height, stream);
image_cv_matrix = images[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, stream_format, t, img_msg_ptr.get());
if (!img_msg_ptr)
{
ROS_ERROR("sensor image message allocation failed, frame was dropped");
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);
if (fillROSImageMsgAndReturnStatus(image_cv_matrix, stream, width, height, stream_format, t, img_msg_ptr.get()))
{

// 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);
}
else
{
ROS_ERROR("Could not fill ROS message. Frame was dropped.");
}
}
}

Expand Down Expand Up @@ -1043,8 +1068,19 @@ void BaseRealSenseNode::publishRGBD(

realsense2_camera_msgs::msg::RGBD::UniquePtr msg(new realsense2_camera_msgs::msg::RGBD());

fillMessageImage(rgb_cv_matrix, COLOR, rgb_width, rgb_height, color_format, t, &msg->rgb);
fillMessageImage(depth_cv_matrix, DEPTH, depth_width, depth_height, depth_format, t, &msg->depth);
bool rgb_message_filled = fillROSImageMsgAndReturnStatus(rgb_cv_matrix, COLOR, rgb_width, rgb_height, color_format, t, &msg->rgb);
if(!rgb_message_filled)
{
ROS_ERROR_STREAM("Failed to fill rgb message inside RGBD message");
return;
}

bool depth_messages_filled = fillROSImageMsgAndReturnStatus(depth_cv_matrix, DEPTH, depth_width, depth_height, depth_format, t, &msg->depth);
if(!depth_messages_filled)
{
ROS_ERROR_STREAM("Failed to fill depth message inside RGBD message");
return;
}

msg->header.frame_id = "camera_rgbd_optical_frame";
msg->header.stamp = t;
Expand Down

0 comments on commit 6f40a4f

Please sign in to comment.