backport SteveMacenski#377 foxy

Jannkar · Feb 2, 2022 · 0288102 · 0288102
1 parent e2f0a71
commit 0288102
Show file tree

Hide file tree

Showing 6 changed files with 14 additions and 5 deletions.
diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp
@@ -138,6 +138,7 @@ class SlamToolbox : public rclcpp::Node
   // Storage for ROS parameters
   std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_;
   rclcpp::Duration transform_timeout_, minimum_time_interval_;
+  rclcpp::Time scan_timestamped;
   int throttle_scans_;
 
   double resolution_;

diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp
@@ -83,6 +83,8 @@ void LifelongSlamToolbox::laserCallback(
   sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
 /*****************************************************************************/
 {
+  // store scan timestamped
+  scan_timestamped = scan->header.stamp;
   // no odom info
   Pose2 pose;
   if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) {

diff --git a/src/slam_toolbox_async.cpp b/src/slam_toolbox_async.cpp
@@ -35,6 +35,8 @@ void AsynchronousSlamToolbox::laserCallback(
   sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
 /*****************************************************************************/
 {
+  // store scan timestamped
+  scan_timestamped = scan->header.stamp;
   // no odom info
   Pose2 pose;
   if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) {

diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp
@@ -39,7 +39,7 @@ SlamToolbox::SlamToolbox(rclcpp::NodeOptions options)
   processor_type_(PROCESS),
   first_measurement_(true),
   process_near_pose_(nullptr),
-  transform_timeout_(rclcpp::Duration(0.5 * 1000000000)),
+  transform_timeout_(rclcpp::Duration::from_seconds(0.5)),
   minimum_time_interval_(0.)
 /*****************************************************************************/
 {
@@ -150,9 +150,9 @@ void SlamToolbox::setParams()
 
   double tmp_val = 0.5;
   tmp_val = this->declare_parameter("transform_timeout", tmp_val);
-  transform_timeout_ = rclcpp::Duration(tmp_val * 1000000000);
+  transform_timeout_ = rclcpp::Duration::from_seconds(tmp_val);
   tmp_val = this->declare_parameter("minimum_time_interval", tmp_val);
-  minimum_time_interval_ = rclcpp::Duration(tmp_val * 1000000000);
+  minimum_time_interval_ = rclcpp::Duration::from_seconds(tmp_val);
 
   bool debug = false;
   debug = this->declare_parameter("debug_logging", debug);
@@ -230,7 +230,7 @@ void SlamToolbox::publishTransformLoop(
       msg.transform = tf2::toMsg(map_to_odom_);
       msg.child_frame_id = odom_frame_;
       msg.header.frame_id = map_frame_;
-      msg.header.stamp = this->now() + transform_timeout_;
+      msg.header.stamp = scan_timestamped + transform_timeout_;
       tfB_->sendTransform(msg);
     }
     r.sleep();
@@ -371,7 +371,7 @@ bool SlamToolbox::updateMap()
   vis_utils::toNavMap(occ_grid, map_.map);
 
   // publish map as current
-  map_.map.header.stamp = this->now();
+  map_.map.header.stamp = scan_timestamped;
   sst_->publish(
     std::move(std::make_unique<nav_msgs::msg::OccupancyGrid>(map_.map)));
   sstm_->publish(

diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp
@@ -118,6 +118,8 @@ void LocalizationSlamToolbox::laserCallback(
   sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
 /*****************************************************************************/
 {
+  // store scan timestamped
+  scan_timestamped = scan->header.stamp;
   // no odom info
   Pose2 pose;
   if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) {

diff --git a/src/slam_toolbox_sync.cpp b/src/slam_toolbox_sync.cpp
@@ -65,6 +65,8 @@ void SynchronousSlamToolbox::laserCallback(
   sensor_msgs::msg::LaserScan::ConstSharedPtr scan)
 /*****************************************************************************/
 {
+  // store scan timestamped
+  scan_timestamped = scan->header.stamp;
   // no odom info
   Pose2 pose;
   if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) {