fix: Hotfix time update leading to false positive reset detections

ros::Time::setNow directly sets the time value in a blocking manner which
intra-node ros::Time::now calls access. But we still need the clock publisher to
inform other nodes about the current ros time.
The publisher needs more time to set the value for intra-node calls and is
non-blocking. Using both setNow and the publisher caused an issue on high CPU
load, where multiple simulation steps were performed while the published message
had still not caused the callback that set the ros time 'externally'. Once it
had set the time, the now set ros time was outdated and plugins like
mujoco_ros_control that keep track of the last ros time they run could
misinterpret this as a reset and clear buffers.
Adding a while condition to yield the process until the time is correctly set
for intra-process calls to ros::Time::now simulates a blocking time update and
fixes such issues.
But I don't like this, because on my machine under heavy load this slows the
step frequency down by an additional factor of 10. We should look into solving
this more elegantly.

mujoco_ros/src/mujoco_sim.cpp

@@ -449,10 +449,16 @@ void publishSimTime(mjtNum time)
 	if (!use_sim_time_) {
 		return;
 	}
-	ros::Time::setNow(ros::Time(time));
-	rosgraph_msgs::Clock ros_time;
-	ros_time.clock.fromSec(time);
+	// This is the fastes option for intra-node time updates
+	// however, together with non-blocking publish it breaks stuff
+	// ros::Time::setNow(ros::Time(time));
+	rosgraph_msgs::ClockPtr ros_time(new rosgraph_msgs::Clock);
+	ros_time->clock.fromSec(time);
 	pub_clock_.publish(ros_time);
+	// Current workaround to simulate a blocking time update
+	while (ros::Time::now() < ros::Time(time)) {
+		std::this_thread::yield();
+	}
 }
 
 void controlCallback(const mjModel * /*model*/, mjData *data)