modeExecutor.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_light
00013  * Description: Switch robots led color by sending data to
00014  * the led-µC over serial connection.
00015  *                                                              
00016  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00017  *                      
00018  * Author: Benjamin Maidel, email:benjamin.maidel@ipa.fraunhofer.de
00019  * Supervised by: Benjamin Maidel, email:benjamin.maidel@ipa.fraunhofer.de
00020  *
00021  * Date of creation: August 2012
00022  * ToDo:
00023  *
00024  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00025  *
00026  * Redistribution and use in source and binary forms, with or without
00027  * modification, are permitted provided that the following conditions are met:
00028  *
00029  *     * Redistributions of source code must retain the above copyright
00030  *       notice, this list of conditions and the following disclaimer.
00031  *     * Redistributions in binary form must reproduce the above copyright
00032  *       notice, this list of conditions and the following disclaimer in the
00033  *       documentation and/or other materials provided with the distribution.
00034  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00035  *       Engineering and Automation (IPA) nor the names of its
00036  *       contributors may be used to endorse or promote products derived from
00037  *       this software without specific prior written permission.
00038  *
00039  * This program is free software: you can redistribute it and/or modify
00040  * it under the terms of the GNU Lesser General Public License LGPL as 
00041  * published by the Free Software Foundation, either version 3 of the 
00042  * License, or (at your option) any later version.
00043  * 
00044  * This program is distributed in the hope that it will be useful,
00045  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00046  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00047  * GNU Lesser General Public License LGPL for more details.
00048  * 
00049  * You should have received a copy of the GNU Lesser General Public 
00050  * License LGPL along with this program. 
00051  * If not, see <http://www.gnu.org/licenses/>.
00052  *
00053  ****************************************************************/
00054 
00055 #include <modeExecutor.h>
00056 #include <ros/ros.h>
00057 
00058 ModeExecutor::ModeExecutor(IColorO* colorO)
00059 : _stopRequested(false), default_priority(0)
00060 {
00061         _colorO = colorO;
00062         _activeMode = NULL;
00063 }
00064 
00065 ModeExecutor::~ModeExecutor()
00066 {
00067         
00068 }
00069 
00070 void ModeExecutor::execute(cob_light::LightMode requestedMode)
00071 {
00072         Mode* mode = ModeFactory::create(requestedMode);
00073         // check if mode was correctly created
00074         if(mode != NULL)
00075                 execute(mode);
00076 }
00077 
00078 void ModeExecutor::execute(Mode* mode)
00079 {
00080         // check if a mode is already executed
00081         if(_activeMode != NULL)
00082         {
00083                 // check if priority from requested mode is higher or the same
00084                 if(_activeMode->getPriority() <= mode->getPriority())
00085                 {
00086                         stop();
00087                         _activeMode = mode;
00088                         _activeMode->signalColorReady()->connect(boost::bind(&IColorO::setColor, _colorO, _1));
00089                         _thread_ptr.reset(new boost::thread(boost::lambda::bind(&ModeExecutor::run, this)));
00090                         ROS_INFO("Executing new mode: %s",_activeMode->getName().c_str() );
00091                         ROS_DEBUG("Executing Mode %i with prio: %i freq: %f timeout: %f pulses: %i ",
00092                                 ModeFactory::type(mode), mode->getPriority(), mode->getFrequency(), mode->getTimeout(), mode->getPulses());
00093                 }
00094                 else
00095                         ROS_DEBUG("Mode with higher priority is allready executing");
00096         }
00097         else
00098         {
00099                 _activeMode = mode;
00100                 _activeMode->signalColorReady()->connect(boost::bind(&IColorO::setColor, _colorO, _1));
00101                 _thread_ptr.reset(new boost::thread(boost::lambda::bind(&ModeExecutor::run, this)));
00102                 ROS_INFO("Executing new mode: %s",_activeMode->getName().c_str() );
00103                 ROS_DEBUG("Executing Mode %i with prio: %i freq: %f timeout: %f pulses: %i ",
00104                                 ModeFactory::type(mode), mode->getPriority(), mode->getFrequency(), mode->getTimeout(), mode->getPulses());
00105         }
00106 
00107 }
00108 
00109 void ModeExecutor::run()
00110 {
00111         ros::Rate r(10);
00112         if(_activeMode->getFrequency() != 0.0)
00113                 r = ros::Rate(_activeMode->getFrequency());
00114         else
00115                 _activeMode->setFrequency(10);
00116 
00117         ros::Time timeStart = ros::Time::now();
00118 
00119         while(!isStopRequested() && !ros::isShuttingDown())
00120         {
00121                 _activeMode->execute();
00122 
00123                 if((_activeMode->getPulses() != 0) && 
00124                         (_activeMode->getPulses() <= _activeMode->pulsed()))
00125                         break;
00126 
00127                 if(_activeMode->getTimeout() != 0)
00128                 {
00129                         ros::Duration timePassed = ros::Time::now() - timeStart;
00130                         if(timePassed.toSec() >= _activeMode->getTimeout())
00131                                 break;
00132                 }
00133                 r.sleep();
00134         }
00135         ROS_INFO("Mode %s finished",_activeMode->getName().c_str());
00136         delete _activeMode;
00137         _activeMode = NULL;
00138 }
00139 
00140 void ModeExecutor::stop()
00141 {
00142         _mutex.lock();
00143         _stopRequested = true;
00144         _mutex.unlock();
00145 
00146         if (_thread_ptr)
00147                 _thread_ptr->join();
00148 
00149         _mutex.lock();
00150         _stopRequested = false;
00151         _mutex.unlock();
00152 
00153 }
00154 
00155 bool ModeExecutor::isStopRequested()
00156 {
00157         bool ret;
00158         _mutex.lock();
00159         ret =_stopRequested;
00160         _mutex.unlock();
00161         return ret;
00162 }
00163 
00164 int ModeExecutor::getExecutingMode()
00165 {
00166         return ModeFactory::type(_activeMode);
00167 }
00168 
00169 int ModeExecutor::getExecutingPriority()
00170 {
00171         if(_activeMode != NULL)
00172                 return _activeMode->getPriority();
00173         else
00174                 return default_priority;
00175 }
00176 
00177 void ModeExecutor::setDefaultPriority(int priority)
00178 {
00179         default_priority = priority;
00180 }


cob_light
Author(s): Benjamin Maidel
autogenerated on Sun Oct 5 2014 23:09:07