cherry pick (SteveMacenski#377) (SteveMacenski#479)

techtilesg · Nov 8, 2023 · 7385ac6 · 7385ac6
1 parent 0001607
commit 7385ac6
Show file tree

Hide file tree

Showing 6 changed files with 15 additions and 7 deletions.
diff --git a/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp b/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp
@@ -113,6 +113,7 @@ class SlamToolbox
   std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_;
   ros::Duration transform_timeout_, tf_buffer_dur_, minimum_time_interval_;
   int throttle_scans_;
+  std_msgs::Header scan_header_;
 
   double resolution_;
   double position_covariance_scale_;

diff --git a/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp b/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp
@@ -69,6 +69,7 @@ void LifelongSlamToolbox::laserCallback(
   const sensor_msgs::LaserScan::ConstPtr& scan)
 /*****************************************************************************/
 {
+  scan_header_ = scan->header;
   // no odom info
   Pose2 pose;
   if(!pose_helper_->getOdomPose(pose, scan->header.stamp))

diff --git a/slam_toolbox/src/slam_toolbox_async.cpp b/slam_toolbox/src/slam_toolbox_async.cpp
@@ -35,6 +35,7 @@ void AsynchronousSlamToolbox::laserCallback(
   const sensor_msgs::LaserScan::ConstPtr& scan)
 /*****************************************************************************/
 {
+  scan_header_ = scan->header;
   // no odom info
   karto::Pose2 pose;
   if(!pose_helper_->getOdomPose(pose, scan->header.stamp))

diff --git a/slam_toolbox/src/slam_toolbox_common.cpp b/slam_toolbox/src/slam_toolbox_common.cpp
@@ -176,12 +176,15 @@ void SlamToolbox::publishTransformLoop(const double& transform_publish_period)
   {
     {
       boost::mutex::scoped_lock lock(map_to_odom_mutex_);
-      geometry_msgs::TransformStamped msg;
-      tf2::convert(map_to_odom_, msg.transform);
-      msg.child_frame_id = odom_frame_;
-      msg.header.frame_id = map_frame_;
-      msg.header.stamp = ros::Time::now() + transform_timeout_;
-      tfB_->sendTransform(msg);
+      if(scan_header_.stamp.toSec() > 0.0 && !scan_header_.frame_id.empty()) {
+        geometry_msgs::TransformStamped msg;
+        tf2::convert(map_to_odom_, msg.transform);
+        msg.child_frame_id = odom_frame_;
+        msg.header.frame_id = map_frame_;
+        //TODO: why transform_timeout_ is here?
+        msg.header.stamp = scan_header_.stamp + transform_timeout_;
+        tfB_->sendTransform(msg);
+      }
     }
     r.sleep();
   }
@@ -327,7 +330,7 @@ bool SlamToolbox::updateMap()
   vis_utils::toNavMap(occ_grid, map_.map);
 
   // publish map as current
-  map_.map.header.stamp = ros::Time::now();
+  map_.map.header.stamp = scan_header_.stamp;
   sst_.publish(map_.map);
   sstm_.publish(map_.map.info);
 

diff --git a/slam_toolbox/src/slam_toolbox_localization.cpp b/slam_toolbox/src/slam_toolbox_localization.cpp
@@ -104,6 +104,7 @@ void LocalizationSlamToolbox::laserCallback(
   const sensor_msgs::LaserScan::ConstPtr& scan)
 /*****************************************************************************/
 {
+  scan_header_ = scan->header;
   // no odom info
   Pose2 pose;
   if(!pose_helper_->getOdomPose(pose, scan->header.stamp))

diff --git a/slam_toolbox/src/slam_toolbox_sync.cpp b/slam_toolbox/src/slam_toolbox_sync.cpp
@@ -78,6 +78,7 @@ void SynchronousSlamToolbox::laserCallback(
   const sensor_msgs::LaserScan::ConstPtr& scan)
 /*****************************************************************************/
 {
+  scan_header_ = scan->header;
   // no odom info
   karto::Pose2 pose;
   if(!pose_helper_->getOdomPose(pose, scan->header.stamp))