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

pointcloud_to_depthimage: republish camera_info in output image namespace

Browse Source
This commit is contained in:
matlabbe
2022-03-06 17:59:43 -05:00
parent ec9d32bebd
commit 0325a87bca
1 changed files with 12 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/pointcloud_to_depthimage.cpp
+12 -7
View File
@@ -123,7 +123,8 @@ private:
depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm
depthImage32Pub_ = it.advertise("image", 1); // 32 bits float in meters depthImage32Pub_ = it.advertise("image", 1); // 32 bits float in meters
pointCloudTransformedPub_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("cloud")+"_transformed", 1); pointCloudTransformedPub_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("cloud")+"_transformed", 1);
cameraInfoPub_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("camera_info")+"_depth", 1); cameraInfo16Pub_ = nh.advertise<sensor_msgs::CameraInfo>(nh.resolveName("image_raw")+"/camera_info", 1);
cameraInfo32Pub_ = nh.advertise<sensor_msgs::CameraInfo>(nh.resolveName("image")+"/camera_info", 1);
if(approx) if(approx)
{ {
@@ -242,6 +243,10 @@ private:
{ {
depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
depthImage32Pub_.publish(depthImage.toImageMsg()); depthImage32Pub_.publish(depthImage.toImageMsg());
if(cameraInfo32Pub_.getNumSubscribers())
{
cameraInfo32Pub_.publish(cameraInfoMsgOut);
}
} }
if(depthImage16Pub_.getNumSubscribers()) if(depthImage16Pub_.getNumSubscribers())
@@ -249,6 +254,10 @@ private:
depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1; depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image); depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
depthImage16Pub_.publish(depthImage.toImageMsg()); depthImage16Pub_.publish(depthImage.toImageMsg());
if(cameraInfo16Pub_.getNumSubscribers())
{
cameraInfo16Pub_.publish(cameraInfoMsgOut);
}
} }
if( cloudStamp != pointCloud2Msg->header.stamp.toSec() || if( cloudStamp != pointCloud2Msg->header.stamp.toSec() ||
@@ -261,18 +270,14 @@ private:
cloudStamp, pointCloud2Msg->header.stamp.toSec(), cloudStamp, pointCloud2Msg->header.stamp.toSec(),
infoStamp, cameraInfoMsg->header.stamp.toSec()); infoStamp, cameraInfoMsg->header.stamp.toSec());
} }
if(cameraInfoPub_.getNumSubscribers())
{
rtabmap_ros::cameraModelToROS(model, cameraInfoMsgOut);
}
} }
} }
private: private:
image_transport::Publisher depthImage16Pub_; image_transport::Publisher depthImage16Pub_;
image_transport::Publisher depthImage32Pub_; image_transport::Publisher depthImage32Pub_;
ros::Publisher cameraInfoPub_; ros::Publisher cameraInfo16Pub_;
ros::Publisher cameraInfo32Pub_;
ros::Publisher pointCloudTransformedPub_; ros::Publisher pointCloudTransformedPub_;
message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSub_; message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_; message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;