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 }
ROSNodeThread::~ROSNodeThread
virtual ~ROSNodeThread()
Destructor.
Definition: node_thread.cpp:47
ROSNodeThread::ROSNodeThread
ROSNodeThread()
Constructor.
Definition: node_thread.cpp:39
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
fawkes::AspectProviderAspect
Definition: aspect_provider.h:41
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:56
fawkes::ROSAspectIniFin::set_rosnode
void set_rosnode(LockPtr< ros::NodeHandle > rosnode)
Set the ROS node handle to use for aspect initialization.
Definition: ros_inifin.cpp:84
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
fawkes::HostInfo::short_name
const char * short_name()
Get short hostname (up to first dot).
Definition: hostinfo.cpp:115
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes
fawkes::LockPtr::clear
void clear()
Set underlying instance to 0, decrementing reference count of existing instance appropriately.
Definition: lockptr.h:505
ROSNodeThread::init
virtual void init()
Initialize the thread.
Definition: node_thread.cpp:52
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
fawkes::HostInfo
Definition: hostinfo.h:31
fawkes::Thread
Definition: thread.h:45
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
ROSNodeThread::finalize
virtual void finalize()
Finalize the thread.
Definition: node_thread.cpp:99
ROSNodeThread::loop
virtual void loop()
Code to execute in the thread.
Definition: node_thread.cpp:112
fawkes::Exception
Definition: exception.h:41