i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 02:07:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added rgbd_relay node/nodelet (can be used exactly like a relay of topic_tools but with ability to uncompress or compress rgbd_image)

Browse Source
This commit is contained in:
matlabbe
2019-03-03 18:31:35 -05:00
parent 70dfe3b56d
commit 4646b99cbb
8 changed files with 366 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/rgbd_sync.cpp
+36 -15
View File
@@ -59,6 +59,7 @@ class RGBDSync : public nodelet::Nodelet
public:
RGBDSync() :
depthScale_(1.0),
compressedRate_(0),
warningThread_(0),
callbackCalled_(false),
approxSyncDepth_(0),
@@ -91,10 +92,12 @@ private:
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("depth_scale", depthScale_, depthScale_);
pnh.param("compressed_rate", compressedRate_, compressedRate_);
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_);
NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
rgbdImagePub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image", 1);
rgbdImageCompressedPub_ = nh.advertise<rtabmap_ros::RGBDImage>("rgbd_image/compressed", 1);
@@ -173,24 +176,39 @@ private:
if(rgbdImageCompressedPub_.getNumSubscribers())
{
rtabmap_ros::RGBDImage msgCompressed = msg;
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
msgCompressed.depthCompressed.header = imageDepthPtr->header;
if(depthScale_ != 1.0)
bool publishCompressed = true;
if (compressedRate_ > 0.0)
{
msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
if ( lastCompressedPublished_ + ros::Duration(1.0/compressedRate_) > ros::Time::now())
{
NODELET_DEBUG("throttle last update at %f skipping", lastCompressedPublished_.toSec());
publishCompressed = false;
}
}
else
{
msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
}
msgCompressed.depthCompressed.format = "png";
rgbdImageCompressedPub_.publish(msgCompressed);
if(publishCompressed)
{
lastCompressedPublished_ = ros::Time::now();
rtabmap_ros::RGBDImage msgCompressed = msg;
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
imagePtr->toCompressedImageMsg(msgCompressed.rgbCompressed, cv_bridge::JPG);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
msgCompressed.depthCompressed.header = imageDepthPtr->header;
if(depthScale_ != 1.0)
{
msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image*depthScale_, ".png");
}
else
{
msgCompressed.depthCompressed.data = rtabmap::compressImage(imageDepthPtr->image, ".png");
}
msgCompressed.depthCompressed.format = "png";
rgbdImageCompressedPub_.publish(msgCompressed);
}
}
if(rgbdImagePub_.getNumSubscribers())
@@ -226,9 +244,12 @@ private:
private:
double depthScale_;
double compressedRate_;
boost::thread * warningThread_;
bool callbackCalled_;
ros::Time lastCompressedPublished_;
ros::Publisher rgbdImagePub_;
ros::Publisher rgbdImageCompressedPub_;