From 04ccba632646f2277e2d841cc184509bcb5478e2 Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Mon, 10 Jun 2019 12:07:30 -0700 Subject: [PATCH] Use rclcpp::Time for stamping transforms (#20) * Use rclcpp::Time for stamping transforms The motivation is that right now, `robot_state_publisher` doesn't respect `use_sim_time` at all. * Use std::chrono constructor to represent 0.5 sec --- include/robot_state_publisher/robot_state_publisher.h | 1 + src/robot_state_publisher.cpp | 11 +++++------ 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/robot_state_publisher/robot_state_publisher.h b/include/robot_state_publisher/robot_state_publisher.h index 7653a1c..fb5302f 100644 --- a/include/robot_state_publisher/robot_state_publisher.h +++ b/include/robot_state_publisher/robot_state_publisher.h @@ -99,6 +99,7 @@ class RobotStatePublisher std_msgs::msg::String model_xml_; bool description_published_; rclcpp::Publisher::SharedPtr description_pub_; + rclcpp::Clock::SharedPtr clock_; }; } // namespace robot_state_publisher diff --git a/src/robot_state_publisher.cpp b/src/robot_state_publisher.cpp index 6317e64..2f6339b 100644 --- a/src/robot_state_publisher.cpp +++ b/src/robot_state_publisher.cpp @@ -76,7 +76,8 @@ RobotStatePublisher::RobotStatePublisher( : model_(model), tf_broadcaster_(node_handle), static_tf_broadcaster_(node_handle), - description_published_(false) + description_published_(false), + clock_(node_handle->get_clock()) { // walk the tree and add segments to segments_ addChildren(tree.getRootSegment()); @@ -160,13 +161,11 @@ void RobotStatePublisher::publishFixedTransforms(const std::string & tf_prefix, // loop over all fixed segments for (auto seg = segments_fixed_.begin(); seg != segments_fixed_.end(); seg++) { geometry_msgs::msg::TransformStamped tf_transform = kdlToTransform(seg->second.segment.pose(0)); - std::chrono::nanoseconds now = std::chrono::high_resolution_clock::now().time_since_epoch(); + rclcpp::Time now = clock_->now(); if (!use_tf_static) { - now += std::chrono::milliseconds(500); // 0.5sec in NS + now = now + rclcpp::Duration(std::chrono::milliseconds(500)); // 0.5sec in NS } - tf_transform.header.stamp.sec = - static_cast(now.count() / 1000000000); - tf_transform.header.stamp.nanosec = now.count() % 1000000000; + tf_transform.header.stamp = now; tf_transform.header.frame_id = tf_prefix + "/" + seg->second.root; tf_transform.child_frame_id = tf_prefix + "/" + seg->second.tip;