Fawkes API  Fawkes Development Version
clock_thread.cpp
1 /***************************************************************************
2  * clock_thread.h - Thread to publish clock to ROS
3  *
4  * Created: Sun Jul 12 16:14:41 2015
5  * Copyright 2015 Tim Niemueller
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "clock_thread.h"
22 #include <rosgraph_msgs/Clock.h>
23 #include <utils/time/wait.h>
24 
25 using namespace fawkes;
26 
27 /** @class RosClockThread "clock_thread.h"
28  * Thread to publish clock to ROS.
29  * @author Tim Niemueller
30  */
31 
32 /** Constructor. */
34  : Thread("ClockThread", Thread::OPMODE_CONTINUOUS)
35 {
37 }
38 
39 void
41 {
42  cfg_freq_ = config->get_uint("/ros/clock/frequency");
43  pub_ = rosnode->advertise<rosgraph_msgs::Clock>("clock", 1);
44  rosnode->setParam("/use_sim_time", true);
45 
46  set_local_ = ros::Time::isSystemTime();
47  if (set_local_) {
48  // enable sim time
49  ros::Time::setNow(ros::Time::now());
50  }
51 
52  time_wait_ = new TimeWait(clock, 1000000l / cfg_freq_);
53 }
54 
55 void
57 {
58  rosnode->deleteParam("/use_sim_time");
59  pub_.shutdown();
60  delete time_wait_;
61 }
62 
63 
64 void
65 RosClockThread::publish_clock()
66 {
67  rosgraph_msgs::Clock clock_msg;
68 
69  fawkes::Time now(clock);
70  clock_msg.clock.sec = now.get_sec();
71  clock_msg.clock.nsec = now.get_usec() * 1000;
72 
73  pub_.publish(clock_msg);
74 
75  if (set_local_) ros::Time::setNow(clock_msg.clock);
76 }
77 
78 void
79 ros_clock_cb(const rosgraph_msgs::Clock::ConstPtr& msg)
80 {
81  ros::Time::setNow(msg->clock);
82 }
83 
84 
85 void
87 {
88  time_wait_->mark_start();
89  publish_clock();
90  time_wait_->wait_systime();
91 }
RosClockThread()
Constructor.
virtual void finalize()
Finalize the thread.
Fawkes library namespace.
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
Definition: thread.cpp:727
void wait_systime()
Wait until minimum loop time has been reached in real time.
Definition: wait.cpp:100
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
long get_sec() const
Get seconds.
Definition: time.h:110
void mark_start()
Mark start of loop.
Definition: wait.cpp:70
long get_usec() const
Get microseconds.
Definition: time.h:112
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:48
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.
Definition: configurable.h:44
Time wait utility.
Definition: wait.h:32