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 
28 using 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 
51 void
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 
98 void
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 
111 void
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.