Page 1 of 2

V-Rep should publish to the /clock topic

Posted: 03 Mar 2013, 12:53
by ayberkozgur
I'm using ROS nodes extensively in conjunction with V-Rep as well as real robots. For ROS nodes that depend on a Rate object to spin them, there should exist a /clock topic updated by the simulator (or the clock server that depends on system clock). For now, I am getting by via an auxiliary node that reads the /vrep/info topic and redirects its time info to the /clock topic; since their stream types are not compatible (/clock is of rosgraph_msgs/Clock type while /vrep/info is vrep_common/VRepInfo which contains additional information regarding the simulation), I cannot directly remap them. However, it would be great if V-Rep published directly to the /clock topic.

Re: V-Rep should publish to the /clock topic

Posted: 03 Mar 2013, 14:33
by ayberkozgur
For those who are interested, below is the extremely simple C++ code for the ROS node which performs the remapping from /vrep/info to /clock:

Code: Select all

#include <ros/ros.h>
#include <vrep_common/VrepInfo.h>
#include <rosgraph_msgs/Clock.h>

ros::Publisher clock_publisher;
ros::Subscriber vrep_clock_subscriber;

float sim_time = -1;

void updateJointStates(const vrep_common::VrepInfo::ConstPtr& new_time){
	if(sim_time != new_time->simulationTime.data){
		sim_time = new_time->simulationTime.data;
		rosgraph_msgs::Clock c;
		c.clock.sec = (int)sim_time;
		c.clock.nsec = (int)((sim_time - (int)sim_time)*1000000000);
		clock_publisher.publish(c);
	}
	else
		sim_time = new_time->simulationTime.data;
}

int main(int argc, char** argv){
	ros::init(argc,argv,"vrep_clock_remapper");
	ros::NodeHandle node_handle;
	vrep_clock_subscriber = node_handle.subscribe("/vrep/info",1,updateJointStates);
	clock_publisher = node_handle.advertise<rosgraph_msgs::Clock>("/clock", 1);
	ros::spin();
}
After launching this node, your other nodes will synchronize with the V-Rep simulation clock while using Rate::sleep().

Re: V-Rep should publish to the /clock topic

Posted: 26 Aug 2016, 17:47
by rick187
This topic is quite old, but this command also works

Code: Select all

rosrun topic_tools transform /vrep/info /clock rosgraph_msgs/Clock 'rospy.Time.from_sec(m.simulationTime.data)' --import rospy
and it can also be included in a launch file with:

Code: Select all

<node name="sim_time" pkg="topic_tools" type="transform" args="/vrep/info /clock rosgraph_msgs/Clock 'rospy.Time.from_sec(m.simulationTime.data)' --import rospy" />

Re: V-Rep should publish to the /clock topic

Posted: 30 Aug 2016, 14:41
by v01d
I also agree that vrep should publish simulated time. We also resort to an external node. Moreover, using the new ros interface, it is now necessary to manually publish time from vreps lua script. The problem is that vrep does not expose appropriate functions with sufficient resolution and simulated time thus advances in relatively large steps. Note that ros measures time in seconds and nanoseconds.

Again it would make much more sense to have vrep handle this, which makes sense a simulator.

Re: V-Rep should publish to the /clock topic

Posted: 31 Aug 2016, 08:24
by coppelia
I agree. We'll look into this. Thanks for the inputs!

Cheers

Re: V-Rep should publish to the /clock topic

Posted: 31 Aug 2016, 13:25
by fferri
Using the new RosInterface plugin, you can publish /clock like any other topic. First set up the publisher (e.g. in sim_childscriptcall_initialization):

Code: Select all

clockPub=simExtRosInterface_advertise('/clock','rosgraph_msgs/Clock')
then, whenever you want to publish the current simulation time (for example in sim_childscriptcall_actuation):

Code: Select all

simExtRosInterface_publish(clockPub,{clock=simGetSimulationTime()})
the frequency at which the clock is published depends on the simulation timestep T (it will be f=1/T Hz).
Since numbers are internally stored as double-precision floating point, resolution is not an issue here.

Re: V-Rep should publish to the /clock topic

Posted: 06 Jan 2017, 12:49
by makbut
Hello, I am using gmapping with vrep and it seems there should be a problem with the clock, so I want to make slam_gmapping node to subscribe to the clock topic. How can I do that?

Re: V-Rep should publish to the /clock topic

Posted: 06 Jan 2017, 13:53
by fferri
What have you tried? Which problem have you got?

Re: V-Rep should publish to the /clock topic

Posted: 08 Jan 2017, 18:31
by makbut
Well, I have the base_scan, base_link and odom frames broadcasted by vrep. Then the slam_gmapping node broadcasts the map frame. However, while the tree is well connected, the frame stamps on the frames broadcasted by vrep are having simulation time values and the map frame has ros time values. Thus when I execute the rosrun tf echo_frames base_link map, I get that the one frame is too old. I tried using the sim_time parameter set to true, but nothing changed.

Re: V-Rep should publish to the /clock topic

Posted: 09 Jan 2017, 11:56
by fferri
The parameter name is /use_sim_time, and it must be set to true before other nodes are initialized.

See http://wiki.ros.org/Clock for details.