Erratic tf behavior due to use_sim_time parameter changed from within stage_ros

[jrgnicho]

stage_ros/src/stageros.cpp

Line 326 in c3abc19

n_.setParam("/use_sim_time", true);

I just took notice that the stage_ros node sets the /use_sim_time parameter to true which causes tf lookups to fail presumably due to clock time mismatches. The error message I get is as follows:

Exception thrown:Lookup would require extrapolation into the past.  Requested time 14.600000000 but the earliest data is at time 1518479321.574429988, when looking up transform from frame [link_a] to frame [odom]

In addition to that, the robot model display in rviz reports tf failures and you see the ghost white links collapsed at the origin.

This issue is inconsistent as there are times when I launch and everything works fine but when it doesn't I restart once or twice (including the roscore) and the issue goes away. My guess is that during launch, some nodes fetch the /use_sim_time parameter before it is set by stage_ros and assign it a false value. By the time that stage_ros sets the parameter to true several nodes are already configured to operate with the real clock time.

I fixed this behavior by adding a <param name=/use_sim_time value=true/> entry to my launch file and this way there's no longer a race condition to set the parameter. I couldn't find any documentation that states this but I think changing a critical parameter from within a node is not the best course of action.

[CodeFinder2]

I agree. Guess the best option would be to not set it within stage_ros but rather test for it and warn (or even terminate) if it's not set. ;-)

EDIT: For instance, something like this:

  if (!nh_default.hasParam("/use_sim_time") ||
      !nh_default.param<bool>("/use_sim_time", false)) {
    ROS_FATAL("'/use_sim_time' parameter not set or false. Set this parameter on the ROS parameter "
              "server before starting any node, exiting.");
    ros::shutdown();
    return;
  }

[jrgnicho]

I think that printing a warning should suffice.

[gavanderhoorn]

A warning should be ok, but I would suggest a repeating (but rate limited) warning. A single one at startup is easily missed (in the many loglines flooding a terminal) and users would still be trying to understand why TF is acting up.

[jrgnicho]

Printing a warning periodically might get bothersome if you intended to run without setting that parameter to true.
I don't know of any cases where stage ros could be use in that mode but if there are I rather warn once and document this in the wiki.

[CodeFinder2]

Why would one use Stage w/o simulation time?

[jrgnicho]

I'm not familiar enough with stage to know if it can work without sim time but if it must absolutely run off of simulated time then perhaps it'd be best to terminate and print the appropriate error message instructing to set use sim time parameter.

[gavanderhoorn]

I think the point is that stage is a simulator, so stage itself doesn't work with sim time, but any ROS application communicating with it should probably always use sim time. The time published by stage (here).

[jrgnicho]

@gavanderhoorn thanks for clarifying, I think your suggestion above to periodically print warnings makes the most sense.