From d941f99d12f22e6fcf98c01bb3072b1f390a2807 Mon Sep 17 00:00:00 2001 From: SirawatS Date: Thu, 8 Apr 2021 16:05:57 +0700 Subject: [PATCH 1/5] fixed timestamped ouput not match input --- include/slam_toolbox/slam_toolbox_common.hpp | 1 + include/slam_toolbox/visualization_utils.hpp | 2 +- src/slam_toolbox_common.cpp | 10 +++++----- src/slam_toolbox_sync.cpp | 3 +++ 4 files changed, 10 insertions(+), 6 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/include/slam_toolbox/visualization_utils.hpp b/include/slam_toolbox/visualization_utils.hpp index dd0ada90..3d2dfce1 100644 --- a/include/slam_toolbox/visualization_utils.hpp +++ b/include/slam_toolbox/visualization_utils.hpp @@ -50,7 +50,7 @@ inline visualization_msgs::msg::Marker toMarker( marker.color.b = 0.0; marker.color.a = 1.; marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration::from_nanoseconds(0); + marker.lifetime = rclcpp::Duration(0.); return marker; } diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 05adeb7a..d58929bc 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -40,7 +40,7 @@ SlamToolbox::SlamToolbox(rclcpp::NodeOptions options) processor_type_(PROCESS), first_measurement_(true), process_near_pose_(nullptr), - transform_timeout_(rclcpp::Duration::from_nanoseconds(0.5 * 1000000000)), + transform_timeout_(rclcpp::Duration::from_seconds(0.5 )), minimum_time_interval_(std::chrono::nanoseconds(0)) /*****************************************************************************/ { @@ -151,9 +151,9 @@ void SlamToolbox::setParams() double tmp_val = 0.5; tmp_val = this->declare_parameter("transform_timeout", tmp_val); - transform_timeout_ = rclcpp::Duration::from_nanoseconds(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::from_nanoseconds(tmp_val * 1000000000); + minimum_time_interval_ = rclcpp::Duration::from_seconds(tmp_val); bool debug = false; debug = this->declare_parameter("debug_logging", debug); @@ -231,7 +231,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_; // 8 Apr.21 | remove this->now tfB_->sendTransform(msg); } r.sleep(); @@ -372,7 +372,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_sync.cpp b/src/slam_toolbox_sync.cpp index 97800b4e..a4044ae4 100644 --- a/src/slam_toolbox_sync.cpp +++ b/src/slam_toolbox_sync.cpp @@ -65,6 +65,9 @@ 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)) { From 0458f6d68780c49efd417e3d3fbbcc7b368de52b Mon Sep 17 00:00:00 2001 From: SirawatS Date: Thu, 8 Apr 2021 17:02:03 +0700 Subject: [PATCH 2/5] fixed timestamped ouput not match input --- src/experimental/slam_toolbox_lifelong.cpp | 3 +++ src/slam_toolbox_async.cpp | 3 +++ src/slam_toolbox_localization.cpp | 3 +++ 3 files changed, 9 insertions(+) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 38caaffe..7a1de853 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -83,6 +83,9 @@ 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..c882f213 100644 --- a/src/slam_toolbox_async.cpp +++ b/src/slam_toolbox_async.cpp @@ -35,6 +35,9 @@ 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_localization.cpp b/src/slam_toolbox_localization.cpp index 38047263..7f8868ab 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -117,6 +117,9 @@ 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)) { From 199eef167f060904e2e43a06dafc2bbc274ab3d4 Mon Sep 17 00:00:00 2001 From: SirawatS Date: Wed, 14 Apr 2021 13:50:17 +0700 Subject: [PATCH 3/5] Return back to from_nanoseconds --- include/slam_toolbox/visualization_utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/slam_toolbox/visualization_utils.hpp b/include/slam_toolbox/visualization_utils.hpp index 3d2dfce1..dd0ada90 100644 --- a/include/slam_toolbox/visualization_utils.hpp +++ b/include/slam_toolbox/visualization_utils.hpp @@ -50,7 +50,7 @@ inline visualization_msgs::msg::Marker toMarker( marker.color.b = 0.0; marker.color.a = 1.; marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration(0.); + marker.lifetime = rclcpp::Duration::from_nanoseconds(0); return marker; } From 8b704c97b0e0bdfe5d17961374ea940c805cb969 Mon Sep 17 00:00:00 2001 From: SirawatS Date: Wed, 14 Apr 2021 13:51:02 +0700 Subject: [PATCH 4/5] No need for the comment, git history will have this. --- src/slam_toolbox_common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index d58929bc..1720e97f 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -231,7 +231,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 = scan_timestamped + transform_timeout_; // 8 Apr.21 | remove this->now + msg.header.stamp = scan_timestamped + transform_timeout_; tfB_->sendTransform(msg); } r.sleep(); From b16275ea9f80bc59dc5bc364962b9b80d618c63b Mon Sep 17 00:00:00 2001 From: SirawatS Date: Fri, 16 Apr 2021 17:28:12 +0700 Subject: [PATCH 5/5] cleanup lint violations by ament_cpplint --- src/experimental/slam_toolbox_lifelong.cpp | 1 - src/slam_toolbox_async.cpp | 1 - src/slam_toolbox_common.cpp | 4 ++-- src/slam_toolbox_localization.cpp | 1 - src/slam_toolbox_sync.cpp | 1 - 5 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/experimental/slam_toolbox_lifelong.cpp b/src/experimental/slam_toolbox_lifelong.cpp index 7a1de853..a56dc59c 100644 --- a/src/experimental/slam_toolbox_lifelong.cpp +++ b/src/experimental/slam_toolbox_lifelong.cpp @@ -85,7 +85,6 @@ void LifelongSlamToolbox::laserCallback( { // 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 c882f213..3ef1fa84 100644 --- a/src/slam_toolbox_async.cpp +++ b/src/slam_toolbox_async.cpp @@ -37,7 +37,6 @@ void AsynchronousSlamToolbox::laserCallback( { // 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 1720e97f..5848720d 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -40,7 +40,7 @@ SlamToolbox::SlamToolbox(rclcpp::NodeOptions options) processor_type_(PROCESS), first_measurement_(true), process_near_pose_(nullptr), - transform_timeout_(rclcpp::Duration::from_seconds(0.5 )), + transform_timeout_(rclcpp::Duration::from_seconds(0.5)), minimum_time_interval_(std::chrono::nanoseconds(0)) /*****************************************************************************/ { @@ -231,7 +231,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 = scan_timestamped + transform_timeout_; + msg.header.stamp = scan_timestamped + transform_timeout_; tfB_->sendTransform(msg); } r.sleep(); diff --git a/src/slam_toolbox_localization.cpp b/src/slam_toolbox_localization.cpp index 08fa7155..749c1b24 100644 --- a/src/slam_toolbox_localization.cpp +++ b/src/slam_toolbox_localization.cpp @@ -120,7 +120,6 @@ void LocalizationSlamToolbox::laserCallback( { // 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 a4044ae4..43f5bd18 100644 --- a/src/slam_toolbox_sync.cpp +++ b/src/slam_toolbox_sync.cpp @@ -67,7 +67,6 @@ void SynchronousSlamToolbox::laserCallback( { // store scan timestamped scan_timestamped = scan->header.stamp; - // no odom info Pose2 pose; if (!pose_helper_->getOdomPose(pose, scan->header.stamp)) {