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