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
34using 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
57void
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
86void
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
97void
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.