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
23#include <rosgraph_msgs/Clock.h>
24#include <utils/time/wait.h>
25
26using namespace fawkes;
27
28/** @class RosClockThread "clock_thread.h"
29 * Thread to publish clock to ROS.
30 * @author Tim Niemueller
31 */
32
33/** Constructor. */
34RosClockThread::RosClockThread() : Thread("ClockThread", Thread::OPMODE_CONTINUOUS)
35{
37}
38
39void
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
55void
57{
58 rosnode->deleteParam("/use_sim_time");
59 pub_.shutdown();
60 delete time_wait_;
61}
62
63void
64RosClockThread::publish_clock()
65{
66 rosgraph_msgs::Clock clock_msg;
67
69 clock_msg.clock.sec = now.get_sec();
70 clock_msg.clock.nsec = now.get_usec() * 1000;
71
72 pub_.publish(clock_msg);
73
74 if (set_local_)
75 ros::Time::setNow(clock_msg.clock);
76}
77
78void
79ros_clock_cb(const rosgraph_msgs::Clock::ConstPtr &msg)
80{
81 ros::Time::setNow(msg->clock);
82}
83
84void
86{
87 time_wait_->mark_start();
88 publish_clock();
89 time_wait_->wait_systime();
90}
RosClockThread()
Constructor.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
Definition: thread.cpp:716
Time wait utility.
Definition: wait.h:33
void mark_start()
Mark start of loop.
Definition: wait.cpp:68
void wait_systime()
Wait until minimum loop time has been reached in real time.
Definition: wait.cpp:96
A class for handling time.
Definition: time.h:93
Fawkes library namespace.