From d3d6c7c83a0a2ae1d701b52b5e9f0a1478d49306 Mon Sep 17 00:00:00 2001 From: CNR Date: Wed, 9 Nov 2022 00:21:13 +0100 Subject: [PATCH] fixes the following bug: while the playback of a bagfile is stopped and use_sime_time is true the /tf topics timestamps switch from simulated time to system time causing eventually lots of error messages (rviz2 and so on) --- src/robot_state_publisher.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/robot_state_publisher.cpp b/src/robot_state_publisher.cpp index 4863c94..94fcec7 100644 --- a/src/robot_state_publisher.cpp +++ b/src/robot_state_publisher.cpp @@ -345,7 +345,12 @@ void RobotStatePublisher::callbackJointState( } } - publishTransforms(joint_positions, state->header.stamp); + rclcpp::Parameter sim_p = this->get_parameter("use_sim_time"); + if (sim_p.as_bool()) { + publishTransforms(joint_positions, now); + } else { + publishTransforms(joint_positions, state->header.stamp); + } // store publish time in joint map for (size_t i = 0; i < state->name.size(); i++) {