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

dbplayer: fixed camera index suffix added when there is only a single stereo camera

Browse Source
This commit is contained in:
matlabbe
2025-12-03 21:43:29 -08:00
parent 001ed0d512
commit 64fa848dba
1 changed files with 27 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/src/nodelets/db_player.cpp
+27 -1
View File
@@ -417,11 +417,12 @@ bool DbPlayer::publishNextFrame()
stereo = true; stereo = true;
} }
int index = 0; int index = 0;
static bool firstTimeCamMsg = true;
for(const auto & cam: *models) { for(const auto & cam: *models) {
rtabmap::Transform localTransform = cam.localTransform(); rtabmap::Transform localTransform = cam.localTransform();
if(!localTransform.isNull()) { if(!localTransform.isNull()) {
geometry_msgs::msg::TransformStamped baseToCamera; geometry_msgs::msg::TransformStamped baseToCamera;
baseToCamera.child_frame_id = (stereo?index%2==0?"left_":"right_":"") + cameraFrameId_ + (models->size()>1?uNumber2Str(index/(stereo?2:1)):""); baseToCamera.child_frame_id = (stereo?index%2==0?"left_":"right_":"") + cameraFrameId_ + (((stereo && models->size()>2) || (!stereo && models->size()>1))?uNumber2Str(index/(stereo?2:1)):"");
baseToCamera.header.frame_id = frameId_; baseToCamera.header.frame_id = frameId_;
baseToCamera.header.stamp = time; baseToCamera.header.stamp = time;
if(cam.Tx() != 0) { if(cam.Tx() != 0) {
@@ -429,9 +430,13 @@ bool DbPlayer::publishNextFrame()
} }
rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform); rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform);
transforms.push_back(baseToCamera); transforms.push_back(baseToCamera);
if(firstTimeCamMsg) {
RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToCamera.header.frame_id.c_str(), baseToCamera.child_frame_id.c_str());
}
} }
++index; ++index;
} }
firstTimeCamMsg = firstTimeCamMsg && models->empty()?true:false;
if(!odom.pose().isNull()) if(!odom.pose().isNull())
{ {
@@ -441,6 +446,12 @@ bool DbPlayer::publishNextFrame()
odomToBase.header.stamp = time; odomToBase.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform); rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform);
transforms.push_back(odomToBase); transforms.push_back(odomToBase);
static bool firstTimeMsg = true;
if(firstTimeMsg) {
RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", odomToBase.header.frame_id.c_str(), odomToBase.child_frame_id.c_str());
}
firstTimeMsg = false;
} }
if(scanPub_.get() || scanCloudPub_.get()) if(scanPub_.get() || scanCloudPub_.get())
@@ -451,6 +462,11 @@ bool DbPlayer::publishNextFrame()
baseToLaserScan.header.stamp = time; baseToLaserScan.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform); rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
transforms.push_back(baseToLaserScan); transforms.push_back(baseToLaserScan);
static bool firstTimeMsg = true;
if(firstTimeMsg) {
RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToLaserScan.header.frame_id.c_str(), baseToLaserScan.child_frame_id.c_str());
}
firstTimeMsg = false;
} }
if(!odom.data().groundTruth().isNull()) { if(!odom.data().groundTruth().isNull()) {
@@ -460,6 +476,11 @@ bool DbPlayer::publishNextFrame()
worldToBase.header.stamp = time; worldToBase.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform); rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform);
transforms.push_back(worldToBase); transforms.push_back(worldToBase);
static bool firstTimeMsg = true;
if(firstTimeMsg) {
RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", worldToBase.header.frame_id.c_str(), worldToBase.child_frame_id.c_str());
}
firstTimeMsg = false;
} }
if(!odom.data().imu().empty()) { if(!odom.data().imu().empty()) {
@@ -469,6 +490,11 @@ bool DbPlayer::publishNextFrame()
baseToImu.header.stamp = time; baseToImu.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(odom.data().imu().localTransform(), baseToImu.transform); rtabmap_conversions::transformToGeometryMsg(odom.data().imu().localTransform(), baseToImu.transform);
transforms.push_back(baseToImu); transforms.push_back(baseToImu);
static bool firstTimeMsg = true;
if(firstTimeMsg) {
RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToImu.header.frame_id.c_str(), baseToImu.child_frame_id.c_str());
}
firstTimeMsg = false;
} }
tfBroadcaster_->sendTransform(transforms); tfBroadcaster_->sendTransform(transforms);
} }