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 
26 using namespace fawkes;
27 
28 /** @class RosClockThread "clock_thread.h"
29  * Thread to publish clock to ROS.
30  * @author Tim Niemueller
31  */
32 
33 /** Constructor. */
34 RosClockThread::RosClockThread() : 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 void
64 RosClockThread::publish_clock()
65 {
66  rosgraph_msgs::Clock clock_msg;
67 
68  fawkes::Time now(clock);
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 
78 void
79 ros_clock_cb(const rosgraph_msgs::Clock::ConstPtr &msg)
80 {
81  ros::Time::setNow(msg->clock);
82 }
83 
84 void
86 {
87  time_wait_->mark_start();
88  publish_clock();
89  time_wait_->wait_systime();
90 }
fawkes::Thread::set_prepfin_conc_loop
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
Definition: thread.cpp:720
RosClockThread::RosClockThread
RosClockThread()
Constructor.
Definition: clock_thread.cpp:33
fawkes::ROSAspect::rosnode
LockPtr< ros::NodeHandle > rosnode
Definition: ros.h:46
fawkes::ClockAspect::clock
Clock * clock
Definition: clock.h:53
fawkes
fawkes::TimeWait::mark_start
void mark_start()
Mark start of loop.
Definition: wait.cpp:72
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:50
RosClockThread::finalize
virtual void finalize()
Finalize the thread.
Definition: clock_thread.cpp:55
fawkes::Time
Definition: time.h:96
fawkes::Thread
Definition: thread.h:44
fawkes::TimeWait
Definition: wait.h:36
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
fawkes::TimeWait::wait_systime
void wait_systime()
Wait until minimum loop time has been reached in real time.
Definition: wait.cpp:100
RosClockThread::init
virtual void init()
Initialize the thread.
Definition: clock_thread.cpp:39
RosClockThread::loop
virtual void loop()
Code to execute in the thread.
Definition: clock_thread.cpp:84