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

Plumbing env_sensor topic to rtabmap

Browse Source
This commit is contained in:
matlabbe
2025-11-11 23:56:08 +00:00
parent 515eb50d83
commit 2f258bdae0
7 changed files with 102 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/src/DbPlayerNode.cpp
+24
View File
@@ -219,6 +219,7 @@ int main(int argc, char** argv)
ros::Publisher scanCloudPub;
ros::Publisher globalPosePub;
ros::Publisher gpsFixPub;
ros::Publisher envSensorPub;
ros::Publisher clockPub;
tf2_ros::TransformBroadcaster tfBroadcaster;
@@ -387,6 +388,15 @@ int main(int argc, char** argv)
}
}
if(!odom.data().envSensors().empty())
{
if(envSensorPub.getTopic().empty())
{
envSensorPub = nh.advertise<rtabmap_msgs::EnvSensor>("env_sensor", 1);
ROS_INFO("EnvSensor will be published.");
}
}
// publish transforms first
if(publishTf)
{
@@ -486,6 +496,20 @@ int main(int argc, char** argv)
gpsFixPub.publish(msg);
}
if(!odom.data().envSensors().empty())
{
for(rtabmap::EnvSensors::const_iterator iter=odom.data().envSensors().begin(); iter!=odom.data().envSensors().end(); ++iter)
{
rtabmap_msgs::EnvSensor msg;
rtabmap_conversions::envSensorToROS(iter->second, msg);
msg.header.frame_id = frameId;
if(iter->second.stamp() == 0.0) {
msg.header.stamp = ros::Time(data.stamp());
}
envSensorPub.publish(msg);
}
}
if(type >= 0)
{
if(rgbCamInfoPub.getNumSubscribers() && type == 0)