00001 #include <ros/ros.h> 00002 #include <vrep_common/VrepInfo.h> 00003 #include <rosgraph_msgs/Clock.h> 00004 00005 #include <string> 00006 00007 constexpr unsigned int SIM_STARTED_MASK = 1; 00008 ros::Publisher clock_publisher; 00009 ros::Subscriber vrep_clock_subscriber; 00010 00011 float sim_time = -1; 00012 00013 void updateClock(const vrep_common::VrepInfo::ConstPtr& new_time) { 00014 /* 00015 simulatorState.data is bitwise encoded state 00016 bit0 set: simulation not stopped 00017 bit1 set: simulation paused 00018 bit2 set: real-time switch on 00019 bit3-bit5: the edit mode type (0=no edit mode, 1=triangle, 2=vertex, 3=edge, 4=path, 5=UI) 00020 */ 00021 if (!(new_time->simulatorState.data & SIM_STARTED_MASK)) { 00022 return; 00023 } 00024 00025 sim_time = new_time->simulationTime.data; 00026 rosgraph_msgs::Clock c; 00027 /* convert float time to sec a nano sec */ 00028 c.clock.sec = static_cast<uint32_t>(sim_time); 00029 c.clock.nsec = static_cast<uint32_t>((sim_time - c.clock.sec) * 1000000000); 00030 00031 clock_publisher.publish(c); 00032 } 00033 00034 int main(int argc, char **argv) { 00035 ros::init(argc, argv, "vrepClockServer"); 00036 ros::NodeHandle nh; 00037 vrep_clock_subscriber = nh.subscribe("/vrep/info", 1, updateClock); 00038 clock_publisher = nh.advertise<rosgraph_msgs::Clock>("/clock", 1); 00039 ros::spin(); 00040 }