modeExecutor.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #include <modeExecutor.h>
19 #include <ros/ros.h>
20 
22 : _stopRequested(false), default_priority(0)
23 {
24  _colorO = colorO;
25  _colorO->signalColorSet()->connect(boost::bind(&ModeExecutor::onColorSetReceived, this, _1));
26 }
27 
29 {
30 }
31 
32 uint64_t ModeExecutor::execute(cob_light::LightMode requestedMode)
33 {
35  // check if mode was correctly created
36  if(mode)
37  return execute(mode);
38  else
39  return 0;
40 }
41 
43 {
44  uint64_t u_id;
45 
46  // check if modes allready executing
47  if(_mapActiveModes.size() > 0)
48  {
49  // check if mode with lower prio is running
50  if(_mapActiveModes.begin()->first < mode->getPriority())
51  {
52  ROS_DEBUG("Pause mode: %i with prio %i",
53  ModeFactory::type(_mapActiveModes.begin()->second.get()), _mapActiveModes.begin()->second->getPriority());
54  _mapActiveModes.begin()->second->pause();
55  }
56  else
57  {
58  std::map<int, boost::shared_ptr<Mode>, std::greater<int> >::iterator itr;
59  itr = _mapActiveModes.find(mode->getPriority());
60  if(itr != _mapActiveModes.end())
61  {
62  ROS_DEBUG("Stopping mode: %i with prio %i",
63  ModeFactory::type(itr->second.get()), itr->second->getPriority());
64  itr->second->stop();
65  _mapActiveModes.erase(itr);
66  }
67  }
68  }
69  mode->signalColorReady()->connect(boost::bind(&IColorO::setColor, _colorO, _1));
70  mode->signalColorsReady()->connect(boost::bind(&IColorO::setColorMulti, _colorO, _1));
71  mode->signalModeFinished()->connect(boost::bind(&ModeExecutor::onModeFinishedReceived, this, _1));
72  mode->setActualColor(_activeColor);
73  ROS_DEBUG("Attaching Mode %i with prio: %i freq: %f timeout: %f pulses: %i ",
74  ModeFactory::type(mode.get()), mode->getPriority(), mode->getFrequency(), mode->getTimeout(), mode->getPulses());
75  _mapActiveModes.insert(std::pair<int, boost::shared_ptr<Mode> >(mode->getPriority(), mode));
76 
77  if(!_mapActiveModes.begin()->second->isRunning())
78  {
79  ROS_DEBUG("Executing Mode %i with prio: %i freq: %f timeout: %f pulses: %i ",
80  ModeFactory::type(mode.get()), mode->getPriority(), mode->getFrequency(), mode->getTimeout(), mode->getPulses());
81  _mapActiveModes.begin()->second->start();
82  }
83  Mode* ptr = mode.get();
84  u_id = reinterpret_cast<uint64_t>( ptr );
85  return u_id;
86 }
87 
89 {
90  if(_mapActiveModes.size() > 0)
91  {
92  _mapActiveModes.begin()->second->pause();
93  }
94 }
95 
97 {
98  if(_mapActiveModes.size() > 0 && !_mapActiveModes.begin()->second->isRunning())
99  _mapActiveModes.begin()->second->start();
100 }
101 
103 {
104  if(_mapActiveModes.size() > 0)
105  {
106  std::map<int, boost::shared_ptr<Mode>, std::greater<int> >::iterator itr;
107  for(itr = _mapActiveModes.begin(); itr != _mapActiveModes.end(); itr++)
108  {
109  itr->second->stop();
110  }
111  _mapActiveModes.clear();
112  }
113 }
114 
115 bool ModeExecutor::stop(uint64_t uId)
116 {
117  bool ret = false;
118  if(_mapActiveModes.size() > 0)
119  {
120  std::map<int, boost::shared_ptr<Mode>, std::greater<int> >::iterator itr;
121  for(itr = _mapActiveModes.begin(); itr != _mapActiveModes.end(); itr++)
122  {
123  uint64_t uid = reinterpret_cast<uint64_t>(itr->second.get());
124  if(uid == uId)
125  {
126  ROS_DEBUG("Stopping mode: %i with prio %i",
127  ModeFactory::type(itr->second.get()), itr->second->getPriority());
128  itr->second->stop();
129  _mapActiveModes.erase(itr);
130 
131  if(_mapActiveModes.size() > 0)
132  {
133  if(!_mapActiveModes.begin()->second->isRunning())
134  {
135  ROS_DEBUG("Resume mode: %i with prio %i",
136  ModeFactory::type(_mapActiveModes.begin()->second.get()), _mapActiveModes.begin()->second->getPriority());
137  _mapActiveModes.begin()->second->start();
138  }
139  }
140  ret = true;
141  break;
142  }
143  }
144  }
145  return ret;
146 }
148 {
149  //check if finished mode is the current active
150  if(_mapActiveModes.begin()->first == prio)
151  {
152  //erase mode from map and exec mode with next lower prio
153  _mapActiveModes.erase(prio);
154  if(_mapActiveModes.size() > 0)
155  {
156  ROS_DEBUG("Resume mode: %i with prio %i",
157  ModeFactory::type(_mapActiveModes.begin()->second.get()), _mapActiveModes.begin()->second->getPriority());
158  _mapActiveModes.begin()->second->start();
159  }
160  }
161  //finished mode is not the current executing one (this should never happen)
162  else
163  {
164  ROS_WARN("Mode finished which should't be executed");
165  _mapActiveModes.erase(prio);
166  }
167 }
168 
170 {
171  _activeColor = color;
172 }
173 
175 {
176  if(_mapActiveModes.size() > 0)
177  return ModeFactory::type(_mapActiveModes.begin()->second.get());
178  else
179  return ModeFactory::type(NULL);
180 }
181 
183 {
184  if(_mapActiveModes.size()>0)
185  return _mapActiveModes.begin()->second->getPriority();
186  else
187  return default_priority;
188 }
189 
191 {
192  if(_mapActiveModes.size()>0)
193  return reinterpret_cast<uint64_t>(_mapActiveModes.begin()->second.get());
194  else
195  return 0;
196 }
197 
199 {
200  default_priority = priority;
201 }
void onColorSetReceived(color::rgba color)
boost::signals2::signal< void(color::rgba color)> * signalColorSet()
Definition: iColorO.h:38
Definition: mode.h:26
void setDefaultPriority(int priority)
int default_priority
Definition: modeExecutor.h:56
std::map< int, boost::shared_ptr< Mode >, std::greater< int > > _mapActiveModes
Definition: modeExecutor.h:52
#define ROS_WARN(...)
int getExecutingPriority()
static int type(Mode *mode)
ModeExecutor(IColorO *colorO)
int getExecutingMode()
IColorO * _colorO
Definition: modeExecutor.h:49
uint64_t execute(boost::shared_ptr< Mode > mode)
void onModeFinishedReceived(int prio)
virtual void setColorMulti(std::vector< color::rgba > &colors)=0
color::rgba _activeColor
Definition: modeExecutor.h:53
uint64_t getExecutingUId()
virtual void setColor(color::rgba color)=0
static boost::shared_ptr< Mode > create(cob_light::LightMode requestMode, IColorO *colorO)
Definition: modeFactory.cpp:38
#define ROS_DEBUG(...)


cob_light
Author(s): Benjamin Maidel
autogenerated on Wed Apr 7 2021 02:11:39