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 
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"), &GazsimTimesourceThread::on_time_sync_msg, this);
49 
50  //Create Time Source
51  time_source_ = new GazsimTimesource(clock);
52 
53  //register timesource and make it default
54  clock->register_ext_timesource(time_source_, true);
55 }
56 
57 
59 {
60  //remove time source
61  clock->remove_ext_timesource(time_source_);
62  delete time_source_;
63 }
64 
65 
67 {
68  //nothing interesting
69 }
70 
71 void GazsimTimesourceThread::on_time_sync_msg(ConstSimTimePtr &msg)
72 {
73  // logger->log_info(name(), "Got Simulation Time");
74 
75  //provide time source with newest message
76  time_source_->on_time_sync_msg(msg);
77 }
GazsimTimesource(Clock *clock)
Constructor.
Fawkes library namespace.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Thread aspect to use blocked timing.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
void register_ext_timesource(TimeSource *ts, bool make_default=false)
Register an external time source.
Definition: clock.cpp:92
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:205
virtual void finalize()
Finalize the thread.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
void remove_ext_timesource(TimeSource *ts=0)
Remove external time source.
Definition: clock.cpp:108