双摆仿真中Reset Simulation服务致Gazebo时钟与TF时间戳不同步求助

Hey there, let's work through this frustrating timestamp mismatch issue you're hitting with your double pendulum setup. When you reset the simulation, Gazebo's clock rolls back to 0, but your /tf messages are stuck on the last timestamp before the reset—totally breaks sync, and it's extra confusing because TurtleBot works fine. Let's break down the likely causes and fixes:

1. Make Sure Your TF Node Uses Simulation Time #

First, double-check that your TF publisher node is configured to use Gazebo's simulation clock instead of system time. This is probably why TurtleBot works: its launch files almost certainly set this parameter.

• Verify the setting with this command:

  rosparam get /<your_tf_node_name>/use_sim_time
  

• If it returns false, update your launch file to add:

  <param name="use_sim_time" value="true" />
  

  Or set it dynamically mid-session:

  rosparam set /<your_tf_node_name>/use_sim_time true
  

  This ensures your node uses the /clock topic (not your computer's clock) for all timestamps.

2. Add a Reset Trigger to Your TF Node #

The core issue is likely that your TF publisher isn't aware when Gazebo resets. It doesn't know to reset its internal time tracking. You can fix this by listening for either the reset service or the clock rollback:

Example C++ Code Snippet #

Add this logic to your TF publisher to detect and react to resets:

#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <tf2_ros/transform_broadcaster.h>

ros::Time last_clock;
bool need_reset = false;

void clockCallback(const ros::Time::ConstPtr& clock_msg) {
    // Detect when clock rolls back from non-zero to near-zero
    if (last_clock.toSec() > 1.0 && clock_msg->toSec() < 0.1) {
        need_reset = true;
    }
    last_clock = *clock_msg;
}

bool resetCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) {
    need_reset = true;
    return true;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "double_pendulum_tf_pub");
    ros::NodeHandle nh;

    ros::Subscriber clock_sub = nh.subscribe("/clock", 10, clockCallback);
    ros::ServiceServer reset_srv = nh.advertiseService("/gazebo/reset_simulation", resetCallback);
    tf2_ros::TransformBroadcaster tf_broadcaster;

    while (ros::ok()) {
        if (need_reset) {
            // Reset any internal time-dependent variables here
            need_reset = false;
            // Wait a tiny bit to let Gazebo's clock stabilize
            ros::Duration(0.1).sleep();
        }

        // Publish TF with current simulation time
        geometry_msgs::TransformStamped tf_msg;
        tf_msg.header.stamp = ros::Time::now(); // Uses /clock since use_sim_time=true
        tf_msg.header.frame_id = "world";
        tf_msg.child_frame_id = "pendulum_link_1";
        // Fill in your transform data here...
        tf_broadcaster.sendTransform(tf_msg);

        ros::spinOnce();
        ros::Rate(100).sleep();
    }
    return 0;
}

Python Equivalent #

For Python nodes, the logic is similar:

import rospy
from std_srvs.srv import Empty, EmptyResponse
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped

last_clock = rospy.Time(0)
need_reset = False

def clock_callback(clock_msg):
    global last_clock, need_reset
    if last_clock.to_sec() > 1.0 and clock_msg.to_sec() < 0.1:
        need_reset = True
    last_clock = clock_msg

def reset_callback(req):
    global need_reset
    need_reset = True
    return EmptyResponse()

if __name__ == '__main__':
    rospy.init_node('double_pendulum_tf_pub')
    rospy.Subscriber('/clock', rospy.Time, clock_callback)
    rospy.Service('/gazebo/reset_simulation', Empty, reset_callback)
    tf_broadcaster = TransformBroadcaster()

    rate = rospy.Rate(100)
    while not rospy.is_shutdown():
        if need_reset:
            # Reset internal state here
            need_reset = False
            rospy.sleep(0.1)
        
        tf_msg = TransformStamped()
        tf_msg.header.stamp = rospy.Time.now()
        tf_msg.header.frame_id = "world"
        tf_msg.child_frame_id = "pendulum_link_1"
        # Populate transform data...
        tf_broadcaster.sendTransform(tf_msg)

        rate.sleep()

3. Clear the TF Cache After Reset #

TF maintains a cache of recent transforms. After resetting, old timestamps might still linger in the cache, causing issues with listeners. Add a cache clear when detecting a reset:

• In C++: Call tf2_ros::Buffer::clear() on your TF buffer (if you're using one for listening).
• In Python: Call tf2_ros.Buffer().clear().

4. Verify the Fix #

After making these changes:

1. Reset the simulation
2. Run rostopic echo /clock to confirm it's back to 0
3. Run rostopic echo /tf and check that new messages have timestamps near 0 (matching the simulation clock)

This should align your TF timestamps with Gazebo's clock post-reset, just like it works for TurtleBot.

内容的提问来源于stack exchange，提问作者Alain Saad