Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * node_thread.cpp - ROS node handle providing Thread 00004 * 00005 * Created: Thu May 05 18:37:08 2011 00006 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "node_thread.h" 00024 00025 #include <ros/ros.h> 00026 00027 using namespace fawkes; 00028 00029 /** @class ROSNodeThread "node_thread.h" 00030 * ROS node handle thread. 00031 * This thread maintains a ROS node which can be used by other 00032 * threads and is provided via the ROSAspect. 00033 * 00034 * @author Tim Niemueller 00035 */ 00036 00037 /** Constructor. */ 00038 ROSNodeThread::ROSNodeThread() 00039 : Thread("ROSNodeThread", Thread::OPMODE_WAITFORWAKEUP), 00040 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_POST_LOOP), 00041 AspectProviderAspect("ROSAspect", &__ros_aspect_inifin) 00042 { 00043 } 00044 00045 00046 /** Destructor. */ 00047 ROSNodeThread::~ROSNodeThread() 00048 { 00049 } 00050 00051 00052 void 00053 ROSNodeThread::init() 00054 { 00055 if (! ros::isInitialized()) { 00056 int argc = 1; 00057 const char *argv[] = {"fawkes"}; 00058 ros::init(argc, (char **)argv, "fawkes", 00059 (uint32_t)ros::init_options::NoSigintHandler); 00060 } else { 00061 logger->log_warn(name(), "ROS node already initialized"); 00062 } 00063 00064 if (ros::isStarted()) { 00065 logger->log_warn(name(), "ROS node already *started*"); 00066 } 00067 00068 __rosnode = new ros::NodeHandle(); 00069 00070 __ros_aspect_inifin.set_rosnode(__rosnode); 00071 } 00072 00073 00074 void 00075 ROSNodeThread::finalize() 00076 { 00077 __rosnode->shutdown(); 00078 00079 __rosnode.clear(); 00080 __ros_aspect_inifin.set_rosnode(__rosnode); 00081 } 00082 00083 00084 void 00085 ROSNodeThread::loop() 00086 { 00087 ros::spinOnce(); 00088 }