Fawkes API  Fawkes Development Version
gazsim_timesource_thread.cpp
1 /***************************************************************************
2  * gazsim_timesource_plugin.cpp - Plugin sets the fawkes time
3  * to the simulation time
4  *
5  * Created: Sat Sep 21 20:56:29 2013
6  * Copyright 2013 Frederik Zwilling
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "gazsim_timesource_thread.h"
24 
25 using namespace fawkes;
26 
27 /** @class GazsimTimesourceThread "clips_thread.h"
28  * Plugin provides the simulation time from gazebo
29  * @author Frederik Zwilling
30  */
31 
32 GazsimTimesourceThread::GazsimTimesourceThread()
33 : Thread("GazsimTimesourceThread", Thread::OPMODE_WAITFORWAKEUP),
34  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
35 {
36 }
37 
38 GazsimTimesourceThread::~GazsimTimesourceThread()
39 {
40 }
41 
42 void
44 {
45  logger->log_info(name(), "GazsimTimesource initializing");
46 
47  //Create Subscriber
48  time_sync_sub_ = gazebo_world_node->Subscribe(config->get_string("/gazsim/topics/time"),
49  &GazsimTimesourceThread::on_time_sync_msg,
50  this);
51 
52  //Create Time Source
53  time_source_ = new GazsimTimesource(clock);
54 
55  //register timesource and make it default
56  clock->register_ext_timesource(time_source_, true);
57 }
58 
59 void
61 {
62  //remove time source
63  clock->remove_ext_timesource(time_source_);
64  delete time_source_;
65 }
66 
67 void
69 {
70  //nothing interesting
71 }
72 
73 void
74 GazsimTimesourceThread::on_time_sync_msg(ConstSimTimePtr &msg)
75 {
76  // logger->log_info(name(), "Got Simulation Time");
77 
78  //provide time source with newest message
79  time_source_->on_time_sync_msg(msg);
80 }
fawkes::GazsimTimesource
Definition: gazsim_timesource_source.h:39
GazsimTimesourceThread::finalize
virtual void finalize()
Finalize the thread.
Definition: gazsim_timesource_thread.cpp:59
fawkes::Clock::remove_ext_timesource
void remove_ext_timesource(TimeSource *ts=0)
Remove external time source.
Definition: clock.cpp:108
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
GazsimTimesourceThread::init
virtual void init()
Initialize the thread.
Definition: gazsim_timesource_thread.cpp:42
fawkes
fawkes::Clock::register_ext_timesource
void register_ext_timesource(TimeSource *ts, bool make_default=false)
Register an external time source.
Definition: clock.cpp:93
fawkes::Thread
Definition: thread.h:44
GazsimTimesourceThread::loop
virtual void loop()
Code to execute in the thread.
Definition: gazsim_timesource_thread.cpp:67
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::MultiLogger::log_info
virtual void log_info(const char *component, const char *format,...)
Definition: multi.cpp:199