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 #include <gazebo/gazebo_config.h>
26 #include <google/protobuf/message.h>
27 
28 #include <gazebo/gazebo_client.hh>
29 #include <gazebo/msgs/msgs.hh>
30 #include <gazebo/transport/Node.hh>
31 #include <gazebo/transport/TransportIface.hh>
32 #include <gazebo/transport/TransportTypes.hh>
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 /** Destructor. */
54 {
55 }
56 
57 void
59 {
60  const std::string robot_channel =
61  config->get_string("/gazsim/world-name") + "/" + config->get_string("/gazsim/robot-name");
62 
63  logger->log_info(name(), "Robot channel: %s", robot_channel.c_str());
64  const std::string world_name = config->get_string("/gazsim/world-name");
65 
66  if (!gazebo::client::setup()) {
67  throw Exception("Failed to initialize Gazebo client");
68  }
69 
70  // Initialize Communication nodes:
71  // Global world node
72  gazebo_world_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
73  logger->log_info(name(), "Initializing world node to namespace '%s'", world_name.c_str());
74  gazebo_world_node_->Init(world_name.c_str());
75  gazebo_aspect_inifin_.set_gazebo_world_node(gazebo_world_node_);
76  // Robot-specific node
77  // This node only communicates with nodes that were initialized with the same string.
78  gazebonode_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
79  logger->log_info(name(), "Initializing node to namespace '%s'", robot_channel.c_str());
80  gazebonode_->Init(robot_channel);
81  // For some reason, we need to advertise a topic to be connected to the master properly.
82  status_publisher_ = gazebonode_->Advertise<gazebo::msgs::Time>("~/heartbeat");
83  gazebo_aspect_inifin_.set_gazebonode(gazebonode_);
84 }
85 
86 void
88 {
89  gazebonode_->Fini();
90  gazebonode_.reset();
91  gazebo_aspect_inifin_.set_gazebonode(gazebonode_);
92  gazebo_world_node_->Fini();
93  gazebo_world_node_.reset();
94  gazebo_aspect_inifin_.set_gazebonode(gazebo_world_node_);
95 }
96 
97 void
99 {
100  gazebo::msgs::Time time;
101  status_publisher_->Publish(time);
102 }
GazeboNodeThread()
Constructor.
Definition: node_thread.cpp:45
virtual void loop()
Code to execute in the thread.
Definition: node_thread.cpp:98
virtual void init()
Initialize the thread.
Definition: node_thread.cpp:58
virtual ~GazeboNodeThread()
Destructor.
Definition: node_thread.cpp:53
virtual void finalize()
Finalize the thread.
Definition: node_thread.cpp:87
Thread aspect provide a new aspect.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void set_gazebonode(gazebo::transport::NodePtr gazebonode)
Set the Gazebo node handle to use for aspect initialization.
void set_gazebo_world_node(gazebo::transport::NodePtr gazebo_world_node)
Set the Gazebo node handle to use for aspect initialization.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.