Fawkes API  Fawkes Development Version
node_thread.cpp
1 
2 /***************************************************************************
3  * node_thread.cpp - Gazebo node handle providing Thread
4  *
5  * Created: Fri Aug 24 11:04:04 2012
6  * Author Bastian Klingen, Frederik Zwilling (2013)
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 "node_thread.h"
24 
25 // from Gazebo
26 #include <gazebo/transport/TransportIface.hh>
27 #include <gazebo/transport/TransportTypes.hh>
28 #include <gazebo/transport/Node.hh>
29 #include <gazebo/gazebo_config.h>
30 #include <google/protobuf/message.h>
31 #include <gazebo/msgs/msgs.hh>
32 
33 
34 using namespace fawkes;
35 
36 /** @class GazeboNodeThread "node_thread.h"
37  * Gazebo node handle thread.
38  * This thread maintains a Gazebo node which can be used by other
39  * threads and is provided via the GazeboAspect.
40  *
41  * @author Bastian Klingen, Frederik Zwilling
42  */
43 
44 /** Constructor. */
46  : Thread("GazeboNodeThread", Thread::OPMODE_WAITFORWAKEUP),
47  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_POST_LOOP),
48  AspectProviderAspect(&__gazebo_aspect_inifin)
49 {
50 }
51 
52 
53 /** Destructor. */
55 {
56 }
57 
58 
59 void
61 {
62  //read config values
63  robot_channel = config->get_string("/gazsim/world-name")
64  + "/" + config->get_string("/gazsim/robot-name");
65 
66  world_name = config->get_string("/gazsim/world-name");
67 
68  if(gazebo::transport::is_stopped()) {
69  gazebo::transport::init();
70  gazebo::transport::run();
71  }
72  else {
73  logger->log_warn(name(), "Gazebo already running ");
74  }
75 
76  //Initialize Communication nodes:
77  //the common one for the robot
78  gazebo::transport::NodePtr node(new gazebo::transport::Node());
79  __gazebonode = node;
80  //initialize node (this node only communicates with nodes that were initialized with the same string)
81  __gazebonode->Init(robot_channel.c_str());
82  __gazebo_aspect_inifin.set_gazebonode(__gazebonode);
83 
84  //and the node for world change messages
85  __gazebo_world_node = gazebo::transport::NodePtr(new gazebo::transport::Node());
86  __gazebo_world_node->Init(world_name.c_str());
87  __gazebo_aspect_inifin.set_gazebo_world_node(__gazebo_world_node);
88 }
89 
90 
91 void
93 {
94  __gazebonode->Fini();
95  __gazebonode.reset();
96  __gazebo_aspect_inifin.set_gazebonode(__gazebonode);
97  __gazebo_world_node->Fini();
98  __gazebo_world_node.reset();
99  __gazebo_aspect_inifin.set_gazebonode(__gazebo_world_node);
100 }
101 
102 
103 void
105 {
106 }
Fawkes library namespace.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Thread aspect to use blocked timing.
virtual void init()
Initialize the thread.
Definition: node_thread.cpp:60
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void loop()
Code to execute in the thread.
Thread aspect provide a new aspect.
GazeboNodeThread()
Constructor.
Definition: node_thread.cpp:45
virtual ~GazeboNodeThread()
Destructor.
Definition: node_thread.cpp:54
void set_gazebonode(gazebo::transport::NodePtr gazebonode)
Set the Gazebo node handle to use for aspect initialization.
virtual void finalize()
Finalize the thread.
Definition: node_thread.cpp:92
void set_gazebo_world_node(gazebo::transport::NodePtr gazebo_world_node)
Set the Gazebo node handle to use for aspect initialization.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.