From 0288102cc179b91046eb48e78a9337ec1a89614f Mon Sep 17 00:00:00 2001 From: Janne Date: Wed, 2 Feb 2022 18:04:15 +0200 Subject: [PATCH] backport #377 foxy --- include/slam_toolbox/slam_toolbox_common.hpp | 1 + src/experimental/slam_toolbox_lifelong.cpp | 2 ++ src/slam_toolbox_async.cpp | 2 ++ src/slam_toolbox_common.cpp | 10 +++++----- src/slam_toolbox_localization.cpp | 2 ++ src/slam_toolbox_sync.cpp | 2 ++ 6 files changed, 14 insertions(+), 5 deletions(-) diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 2c51ce78..9274ba54 100644 --- 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 index 38caaffe..a56dc59c 100644 --- 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 index 8a200c33..3ef1fa84 100644 --- 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 index 5e7c6845..c9ab7a52 100644 --- 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(map_.map))); sstm_->publish( diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index f8b00e81..e8dfbf6b 100644 --- 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 index b6be9086..b2fcd135 100644 --- 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)) {