sim_time.cpp
Go to the documentation of this file.
1 /***************************************************************************/
37 #include "ros/ros.h"
38 #include "rosgraph_msgs/Clock.h"
39 
40 #include <sys/time.h>
41 
42 #define SIM_TIME_INCREMENT_US 10000
43 
44 /*
45  * This node publishes increments of 1ms in time to the /clock topic. It does so
46  * at a rate determined by sim_speedup (simulation speedup factor), which should
47  * be passed
48  * in as a private parameter.
49  */
50 
51 int main(int argc, char** argv)
52 {
53  ros::init(argc, argv, "sim_time_source");
54  ros::NodeHandle sim_time_node;
55 
56  // support integral multiples of wallclock time for simulation speedup
57  int sim_speedup; // integral factor by which to speed up simulation
58  ros::NodeHandle node_priv("~");
59  node_priv.param<int>("sim_speedup", sim_speedup, 1);
60 
61  // get the current time & populate sim_time with it
62  struct timeval start;
63  int rv = gettimeofday(&start, NULL);
64  usleep(1000);
65  struct timeval now;
66  rv = gettimeofday(&now, NULL);
67  if (0 != rv)
68  {
69  ROS_ERROR("Invalid return from gettimeofday: %d", rv);
70  return -1;
71  }
72 
73  rosgraph_msgs::Clock sim_time;
74  sim_time.clock.sec = now.tv_sec - start.tv_sec;
75  sim_time.clock.nsec = now.tv_usec * 1000;
76  ros::Publisher sim_time_pub = sim_time_node.advertise<rosgraph_msgs::Clock>("clock", 1);
77 
78  ROS_INFO("Starting simulation time publisher at time: %d.%d", sim_time.clock.sec, sim_time.clock.nsec);
79 
80  while (ros::ok())
81  {
82  sim_time_pub.publish(sim_time);
83 
84  sim_time.clock.nsec = sim_time.clock.nsec + SIM_TIME_INCREMENT_US * 1000;
85  while (sim_time.clock.nsec > 1000000000)
86  {
87  sim_time.clock.nsec -= 1000000000;
88  ++sim_time.clock.sec;
89  }
90 
91  usleep(SIM_TIME_INCREMENT_US / sim_speedup);
92  ros::spinOnce();
93  }
94 }
#define SIM_TIME_INCREMENT_US
Definition: sim_time.cpp:42
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spinOnce()
int main(int argc, char **argv)
Definition: sim_time.cpp:51
#define ROS_ERROR(...)


pid
Author(s): Andy Zelenak , Paul Bouchier
autogenerated on Mon Apr 15 2019 02:41:32