21 #include "clock_thread.h" 22 #include <rosgraph_msgs/Clock.h> 23 #include <utils/time/wait.h> 43 pub_ =
rosnode->advertise<rosgraph_msgs::Clock>(
"clock", 1);
44 rosnode->setParam(
"/use_sim_time",
true);
46 set_local_ = ros::Time::isSystemTime();
49 ros::Time::setNow(ros::Time::now());
58 rosnode->deleteParam(
"/use_sim_time");
65 RosClockThread::publish_clock()
67 rosgraph_msgs::Clock clock_msg;
70 clock_msg.clock.sec = now.
get_sec();
71 clock_msg.clock.nsec = now.
get_usec() * 1000;
73 pub_.publish(clock_msg);
75 if (set_local_) ros::Time::setNow(clock_msg.clock);
79 ros_clock_cb(
const rosgraph_msgs::Clock::ConstPtr& msg)
81 ros::Time::setNow(msg->clock);
RosClockThread()
Constructor.
virtual void finalize()
Finalize the thread.
Fawkes library namespace.
A class for handling time.
Thread class encapsulation of pthreads.
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
void wait_systime()
Wait until minimum loop time has been reached in real time.
Clock * clock
By means of this member access to the clock is given.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
long get_sec() const
Get seconds.
void mark_start()
Mark start of loop.
long get_usec() const
Get microseconds.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
Configuration * config
This is the Configuration member used to access the configuration.