From c844a6c6d9c73b8d74d9d7e1a1dd9eb5c44b8f7b Mon Sep 17 00:00:00 2001 From: uhobeike Date: Mon, 13 Nov 2023 21:51:57 +0900 Subject: [PATCH 1/2] Add param --- config/emcl2.param.yaml | 1 + config/emcl2_quick_start.param.yaml | 1 + 2 files changed, 2 insertions(+) diff --git a/config/emcl2.param.yaml b/config/emcl2.param.yaml index 15ccf47..314eb0d 100644 --- a/config/emcl2.param.yaml +++ b/config/emcl2.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: odom_freq: 20 + transform_tolerance: 0.2 global_frame_id: "map" odom_frame_id: "odom" base_frame_id: "base_link" diff --git a/config/emcl2_quick_start.param.yaml b/config/emcl2_quick_start.param.yaml index df00e13..6c784ab 100644 --- a/config/emcl2_quick_start.param.yaml +++ b/config/emcl2_quick_start.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: odom_freq: 20 + transform_tolerance: 0.2 global_frame_id: "map" odom_frame_id: "odom" base_frame_id: "base_link" From a2f0b40f39a2e085443f108f3fdc2e15efaf586f Mon Sep 17 00:00:00 2001 From: uhobeike Date: Mon, 13 Nov 2023 21:52:16 +0900 Subject: [PATCH 2/2] Feature param transform_tolerance --- include/emcl2/emcl2_node.h | 1 + src/emcl2_node.cpp | 7 +++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/include/emcl2/emcl2_node.h b/include/emcl2/emcl2_node.h index 7c4c959..08bd1c8 100644 --- a/include/emcl2/emcl2_node.h +++ b/include/emcl2/emcl2_node.h @@ -75,6 +75,7 @@ class EMcl2Node : public rclcpp::Node bool scan_receive_; bool map_receive_; double init_x_, init_y_, init_t_; + double transform_tolerance_; void publishPose( double x, double y, double t, double x_dev, double y_dev, double t_dev, double xy_cov, diff --git a/src/emcl2_node.cpp b/src/emcl2_node.cpp index 6fdbc5c..06e8656 100644 --- a/src/emcl2_node.cpp +++ b/src/emcl2_node.cpp @@ -73,6 +73,9 @@ void EMcl2Node::initCommunication(void) this->declare_parameter("odom_freq", 20); this->get_parameter("odom_freq", odom_freq_); + + this->declare_parameter("transform_tolerance", 0.2); + this->get_parameter("transform_tolerance", transform_tolerance_); } void EMcl2Node::initTF(void) @@ -310,11 +313,11 @@ void EMcl2Node::publishOdomFrame(double x, double y, double t) } tf2::convert(odom_to_map.pose, latest_tf_); auto stamp = tf2_ros::fromMsg(scan_time_stamp_); - tf2::TimePoint transform_tolerance_ = stamp + tf2::durationFromSec(0.2); + tf2::TimePoint transform_expiration = stamp + tf2::durationFromSec(transform_tolerance_); geometry_msgs::msg::TransformStamped tmp_tf_stamped; tmp_tf_stamped.header.frame_id = global_frame_id_; - tmp_tf_stamped.header.stamp = tf2_ros::toMsg(transform_tolerance_); + tmp_tf_stamped.header.stamp = tf2_ros::toMsg(transform_expiration); tmp_tf_stamped.child_frame_id = odom_frame_id_; tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);