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

Adding rtabmap_msgs/RGBDImages output topic for odometry

Browse Source
This commit is contained in:
matlabbe
2025-08-12 17:41:22 -07:00
parent ad785e2327
commit 0243f58a8b
6 changed files with 246 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/include/rtabmap_odom/OdometryROS.h
+5 -1
View File
@@ -63,7 +63,7 @@ public:
OdometryROS(bool stereoParams, bool visParams, bool icpParams);
virtual ~OdometryROS();
void processData(rtabmap::SensorData & data, const std_msgs::Header & header);
void processData(rtabmap::SensorData & data, const std_msgs::Header & header, const std::vector<std_msgs::Header> & multiCamHeaders = std::vector<std_msgs::Header>());
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetToPose(rtabmap_msgs::ResetPose::Request&, rtabmap_msgs::ResetPose::Response&);
@@ -127,6 +127,9 @@ private:
ros::Publisher odomLocalScanMap_;
ros::Publisher odomLastFrame_;
ros::Publisher odomRgbdImagePub_;
ros::Publisher odomRgbdImageCompressedPub_;
ros::Publisher odomRgbdImagesPub_;
ros::Publisher odomRgbdImagesCompressedPub_;
ros::Publisher odomSensorDataPub_;
ros::Publisher odomSensorDataFeaturesPub_;
ros::Publisher odomSensorDataCompressedPub_;
@@ -148,6 +151,7 @@ private:
USemaphore dataReady_;
rtabmap::SensorData dataToProcess_;
std_msgs::Header dataHeaderToProcess_;
std::vector<std_msgs::Header> dataMultiCamHeadersToProcess_;
bool bufferedDataToProcess_;
bool paused_;
rtabmap_odom/src/OdometryROS.cpp
+49 -10
View File
@@ -108,6 +108,9 @@ void OdometryROS::onInit()
odomLocalScanMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_scan_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
odomRgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("odom_rgbd_image", 1);
odomRgbdImageCompressedPub_ = nh.advertise<rtabmap_msgs::RGBDImage>("odom_rgbd_image/compressed", 1);
odomRgbdImagesPub_ = nh.advertise<rtabmap_msgs::RGBDImages>("odom_rgbd_images", 1);
odomRgbdImagesCompressedPub_ = nh.advertise<rtabmap_msgs::RGBDImages>("odom_rgbd_images/compressed", 1);
odomSensorDataPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/raw", 1);
odomSensorDataFeaturesPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/features", 1);
odomSensorDataCompressedPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/compressed", 1);
@@ -457,7 +460,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
}
}
void OdometryROS::processData(SensorData & data, const std_msgs::Header & header)
void OdometryROS::processData(SensorData & data, const std_msgs::Header & header, const std::vector<std_msgs::Header> & multiCamHeaders)
{
//NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec());
if(dataMutex_.lockTry() == 0)
@@ -468,6 +471,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
}
dataToProcess_ = data;
dataHeaderToProcess_ = header;
dataMultiCamHeadersToProcess_ = multiCamHeaders;
bufferedDataToProcess_ = false;
dataReady_.release();
dataMutex_.unlock();
@@ -551,12 +555,14 @@ void OdometryROS::mainLoop()
(previousClockTime_ - clockNow).toSec());
SensorData dataCpy = dataToProcess_;
std_msgs::Header headerCpy = dataHeaderToProcess_;
std::vector<std_msgs::Header> multiCamHeadersCpy = dataMultiCamHeadersToProcess_;
ros::Time previousCpy = previousClockTime_;
this->reset(odometry_->getPose());
if(previousCpy > headerCpy.stamp) {
// new frame is using new clock, process it now
dataToProcess_ = dataCpy;
dataHeaderToProcess_ = headerCpy;
dataMultiCamHeadersToProcess_ = multiCamHeadersCpy;
dataReady_.release();
NODELET_WARN("Odometry: Restarting with frame: %f (clock previous=%f, new=%f)",
headerCpy.stamp.toSec(), previousCpy.toSec(), clockNow.toSec());
@@ -1037,18 +1043,50 @@ void OdometryROS::mainLoop()
postProcessData(data, header);
if(!data.imageRaw().empty() && odomRgbdImagePub_.getNumSubscribers())
{
if(!header.frame_id.empty())
if(!data.imageRaw().empty()) {
if(odomRgbdImagePub_.getNumSubscribers() || odomRgbdImageCompressedPub_.getNumSubscribers())
{
rtabmap_msgs::RGBDImage msg;
rtabmap_conversions::rgbdImageToROS(data, msg, header.frame_id);
msg.header = header; // use corresponding time stamp to image
odomRgbdImagePub_.publish(msg);
if(data.cameraModels().size()<=1 && data.stereoCameraModels().size()<=1)
{
if(odomRgbdImagePub_.getNumSubscribers()) {
rtabmap_msgs::RGBDImage msg;
rtabmap_conversions::rgbdImageToROS(data, msg, header, false);
odomRgbdImagePub_.publish(msg);
}
if(odomRgbdImageCompressedPub_.getNumSubscribers())
{
rtabmap_msgs::RGBDImage msg;
rtabmap_conversions::rgbdImageToROS(data, msg, header, true, compressionImgFormat_);
odomRgbdImageCompressedPub_.publish(msg);
}
}
else
{
ROS_WARN("Cannot convert SensorData for %s topic because it has more than one camera (%ld). "
"Subscribe to %s topic instead to get all cameras.",
odomRgbdImagePub_.getTopic().c_str(),
std::max(data.cameraModels().size(), data.stereoCameraModels().size()),
odomRgbdImagesPub_.getTopic().c_str());
}
}
else
if(!dataMultiCamHeadersToProcess_.empty())
{
ROS_WARN("Sensor frame not set, cannot convert SensorData to RGBDImage");
if(odomRgbdImagesPub_.getNumSubscribers()) {
rtabmap_msgs::RGBDImages msg;
rtabmap_conversions::rgbdImagesToROS(data, msg, dataMultiCamHeadersToProcess_, false);
msg.header = header;
odomRgbdImagesPub_.publish(msg);
}
if(odomRgbdImagesCompressedPub_.getNumSubscribers())
{
rtabmap_msgs::RGBDImages msg;
rtabmap_conversions::rgbdImagesToROS(data, msg, dataMultiCamHeadersToProcess_, true, compressionImgFormat_);
msg.header = header;
odomRgbdImagesCompressedPub_.publish(msg);
}
}
}
@@ -1197,6 +1235,7 @@ void OdometryROS::reset(const Transform & pose)
imuProcessed_ = false;
dataToProcess_ = SensorData();
dataHeaderToProcess_ = std_msgs::Header();
dataMultiCamHeadersToProcess_.clear();
bufferedDataToProcess_ = false;
imuMutex_.lock();
imus_.clear();
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+6 -3
View File
@@ -443,6 +443,7 @@ private:
cv::Mat rgb;
cv::Mat depth;
std::vector<rtabmap::CameraModel> cameraModels;
std::vector<std_msgs::Header> mutiCamHeaders;
for(unsigned int i=0; i<rgbImages.size(); ++i)
{
if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
@@ -539,7 +540,7 @@ private:
}
if(depth.empty())
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
depth = cv::Mat::zeros(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
}
if(ptrImage->image.type() == rgb.type())
@@ -554,7 +555,8 @@ private:
if(ptrDepth->image.type() == depth.type())
{
ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
//if(i < 4)
ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
}
else
{
@@ -563,6 +565,7 @@ private:
}
cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform));
mutiCamHeaders.push_back(rgbImages[i]->header);
}
rtabmap::SensorData data(
@@ -575,7 +578,7 @@ private:
std_msgs::Header header;
header.stamp = higherStamp;
header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
this->processData(data, header);
this->processData(data, header, mutiCamHeaders);
}
void callback(
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+7 -1
View File
@@ -432,6 +432,7 @@ private:
cv::Mat left;
cv::Mat right;
std::vector<rtabmap::StereoCameraModel> cameraModels;
std::vector<std_msgs::Header> mutiCamHeaders;
for(unsigned int i=0; i<leftImages.size(); ++i)
{
if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
@@ -555,6 +556,8 @@ private:
rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
std::cout << i << " " << stereoModel << std::endl;
if( stereoModel.baseline() == 0 &&
alreadyRectified &&
!rightCameraInfos[i].header.frame_id.empty() &&
@@ -587,6 +590,8 @@ private:
stereoTransform.x(),
stereoModel.localTransform(),
stereoModel.left().imageSize());
std::cout << i << " B " << stereoModel << std::endl;
}
}
@@ -662,6 +667,7 @@ private:
}
cameraModels.push_back(stereoModel);
mutiCamHeaders.push_back(leftImages[i]->header);
}
else
{
@@ -681,7 +687,7 @@ private:
std_msgs::Header header;
header.stamp = higherStamp;
header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
this->processData(data, header);
this->processData(data, header, mutiCamHeaders);
}
void callback(