Fawkes API Fawkes Development Version
node_thread.cpp
1
2/***************************************************************************
3 * node_thread.cpp - ROS node handle providing Thread
4 *
5 * Created: Thu May 05 18:37:08 2011
6 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de]
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 <ros/ros.h>
26#include <utils/system/hostinfo.h>
27
28using namespace fawkes;
29
30/** @class ROSNodeThread "node_thread.h"
31 * ROS node handle thread.
32 * This thread maintains a ROS node which can be used by other
33 * threads and is provided via the ROSAspect.
34 *
35 * @author Tim Niemueller
36 */
37
38/** Constructor. */
40: Thread("ROSNodeThread", Thread::OPMODE_WAITFORWAKEUP),
41 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_POST_LOOP),
42 AspectProviderAspect(&ros_aspect_inifin_)
43{
44}
45
46/** Destructor. */
48{
49}
50
51void
53{
54 cfg_async_spinning_ = false;
55 try {
56 cfg_async_spinning_ = config->get_bool("/ros/async-spinning");
57 } catch (Exception &e) {
58 } // ignored, use default
59
60 cfg_async_num_threads_ = 4;
61 try {
62 cfg_async_num_threads_ = config->get_uint("/ros/async-num-threads");
63 } catch (Exception &e) {
64 } // ignored, use default
65
66 if (!ros::isInitialized()) {
67 int argc = 1;
68 const char *argv[] = {"fawkes"};
69 std::string node_name = "fawkes";
70 try {
71 node_name = config->get_string("/ros/node-name");
72 } catch (Exception &e) {
73 } // ignored, use default
74 if (node_name == "$HOSTNAME") {
75 HostInfo hinfo;
76 node_name = hinfo.short_name();
77 }
78
79 ros::init(argc, (char **)argv, node_name, (uint32_t)ros::init_options::NoSigintHandler);
80 } else {
81 logger->log_warn(name(), "ROS node already initialized");
82 }
83
84 if (ros::isStarted()) {
85 logger->log_warn(name(), "ROS node already *started*");
86 }
87
88 rosnode_ = new ros::NodeHandle();
89
90 ros_aspect_inifin_.set_rosnode(rosnode_);
91
92 if (cfg_async_spinning_) {
93 async_spinner_ = new ros::AsyncSpinner(cfg_async_num_threads_);
94 async_spinner_->start();
95 }
96}
97
98void
100{
101 if (cfg_async_spinning_) {
102 async_spinner_->stop();
103 delete async_spinner_;
104 }
105 rosnode_->shutdown();
106
107 rosnode_.clear();
108 ros_aspect_inifin_.set_rosnode(rosnode_);
109}
110
111void
113{
114 if (!cfg_async_spinning_) {
115 ros::spinOnce();
116 }
117}
virtual void init()
Initialize the thread.
Definition: node_thread.cpp:52
virtual void loop()
Code to execute in the thread.
ROSNodeThread()
Constructor.
Definition: node_thread.cpp:39
virtual void finalize()
Finalize the thread.
Definition: node_thread.cpp:99
virtual ~ROSNodeThread()
Destructor.
Definition: node_thread.cpp:47
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 unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
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
Host information.
Definition: hostinfo.h:32
const char * short_name()
Get short hostname (up to first dot).
Definition: hostinfo.cpp:109
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: lockptr.h:499
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
void set_rosnode(LockPtr< ros::NodeHandle > rosnode)
Set the ROS node handle to use for aspect initialization.
Definition: ros_inifin.cpp:78
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.