SMACC2
Loading...
Searching...
No Matches
smacc_state_base.hpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20
21#pragma once
28
29#define STATE_NAME (demangleSymbol(typeid(MostDerived).name()).c_str())
30
31namespace smacc2
32{
33using namespace smacc2::introspection;
34using namespace smacc2::default_events;
35
36template <
37 class MostDerived, class Context, class InnerInitial = mpl::list<>,
38 sc::history_mode historyMode = sc::has_deep_history>
39class SmaccState : public sc::simple_state<MostDerived, Context, InnerInitial, historyMode>,
40 public ISmaccState
41{
43
44public:
45 typedef Context TContext;
46 typedef typename Context::inner_context_type context_type;
47 typedef typename context_type::state_iterator state_iterator;
48
49 typedef InnerInitial LastDeepState;
50
52 InnerInitial * smacc_inner_type;
53
56 {
57 my_context(typename base_type::context_ptr_type pContext) : pContext_(pContext) {}
58
59 typename base_type::context_ptr_type pContext_;
60 };
61
62 SmaccState() = delete;
63
64 // Constructor that initializes the state ros node handle
66 {
67 static_assert(
68 std::is_base_of<ISmaccState, Context>::value ||
69 std::is_base_of<ISmaccStateMachine, Context>::value,
70 "The context class must be a SmaccState or a SmaccStateMachine");
71 static_assert(
72 !std::is_same<MostDerived, Context>::value,
73 "The context must be a different state or state machine "
74 "than the current state");
75
76 logger_.reset(new rclcpp::Logger(
77 rclcpp::get_logger(smacc2::utils::cleanShortTypeName(typeid(MostDerived)))));
78
79 RCLCPP_INFO(getLogger(), "[%s] creating state ", STATE_NAME);
80 this->set_context(ctx.pContext_);
81
82 node_ = this->getStateMachine().getNode();
83
84 this->stateInfo_ = getStateInfo();
85
86 // storing a reference to the parent state
87 auto & ps = this->template context<Context>();
88 parentState_ = dynamic_cast<ISmaccState *>(&ps);
89 finishStateThrown = false;
90 }
91
92 virtual ~SmaccState() {}
93
95 {
96 auto smInfo = this->getStateMachine().getStateMachineInfo();
97
98 auto key = typeid(MostDerived).name();
99 if (smInfo.states.count(key))
100 {
101 return smInfo.states[key].get();
102 }
103 else
104 {
105 return nullptr;
106 }
107 }
108
109 std::string getName() override { return getShortName(); }
110
111 std::string getFullName() { return demangleSymbol(typeid(MostDerived).name()); }
112
113 std::string getShortName() { return smacc2::utils::cleanShortTypeName(typeid(MostDerived)); }
114
116 {
117 // auto* ctx = dynamic_cast<ISmaccState*>(this->template context<Context *>());
118
119 return parentState_;
120 }
121
122 // this function is called by boot statechart before the destructor call
123 void exit()
124 {
125 auto * derivedThis = static_cast<MostDerived *>(this);
126 this->getStateMachine().notifyOnStateExitting(derivedThis);
127 {
128 std::lock_guard<std::recursive_mutex> lock(this->getStateMachine().getMutex());
129 this->getStateMachine().notifyOnStateExitting(derivedThis);
130 try
131 {
133 // static_cast<MostDerived *>(this)->onExit();
134 standardOnExit(*derivedThis);
136 }
137 catch (...)
138 {
139 }
140 this->getStateMachine().notifyOnStateExited(derivedThis);
141 }
142 }
143
144public:
145 // This method is static-polymorphic because of the curiously recurring template pattern. It
146 // calls to the most derived class onEntry method if declared on smacc state construction
148
149 // This method is static-polymorphic because of the curiously recurring template pattern. It
150 // calls to the most derived class onEntry method if declared on smacc state construction
151 void onEntry() {}
152
153 // this method is static-polimorphic because of the curiously recurreing pattern. It
154 // calls to the most derived class onExit method if declared on smacc state destruction
155 void onExit() {}
156
157 template <typename T>
158 bool getGlobalSMData(std::string name, T & ret)
159 {
160 return base_type::outermost_context().getGlobalSMData(name, ret);
161 }
162
163 // Store globally in this state machine. (By value parameter )
164 template <typename T>
165 void setGlobalSMData(std::string name, T value)
166 {
167 base_type::outermost_context().setGlobalSMData(name, value);
168 }
169
170 template <typename SmaccComponentType>
171 void requiresComponent(SmaccComponentType *& storage)
172 {
173 base_type::outermost_context().requiresComponent(storage);
174 }
175
176 virtual ISmaccStateMachine & getStateMachine() { return base_type::outermost_context(); }
177
178 template <typename TOrthogonal, typename TBehavior>
180 std::function<void(TBehavior & bh, MostDerived &)> initializationFunction)
181 {
182 configure_orthogonal_internal<TOrthogonal, TBehavior>(
183 [=](ISmaccState * state)
184 {
185 // auto bh = std::make_shared<TBehavior>(args...);
186 auto bh = state->configure<TOrthogonal, TBehavior>();
187 initializationFunction(*bh, *(static_cast<MostDerived *>(state)));
188 });
189 }
190
191 template <typename TOrthogonal, typename TBehavior>
193 std::function<void(TBehavior & bh)> initializationFunction)
194 {
195 configure_orthogonal_internal<TOrthogonal, TBehavior>(
196 [=](ISmaccState * state)
197 {
198 // auto bh = std::make_shared<TBehavior>(args...);
199 auto bh = state->configure<TOrthogonal, TBehavior>();
200 initializationFunction(*bh);
201 });
202 }
203
204 template <typename TOrthogonal, typename TBehavior, typename... Args>
205 static void configure_orthogonal(Args &&... args)
206 {
207 configure_orthogonal_internal<TOrthogonal, TBehavior>(
208 [=](ISmaccState * state)
209 {
210 // auto bh = std::make_shared<TBehavior>(args...);
211 state->configure<TOrthogonal, TBehavior>(args...);
212 });
213 }
214
215 template <
216 typename TStateReactor, typename TOutputEvent, typename TInputEventList, typename... TArgs>
217 static std::shared_ptr<smacc2::introspection::StateReactorHandler> static_createStateReactor(
218 TArgs... args)
219 {
220 auto srh = std::make_shared<smacc2::introspection::StateReactorHandler>(globalNh_);
221 auto srinfo = std::make_shared<SmaccStateReactorInfo>();
222
223 srinfo->stateReactorType = TypeInfo::getTypeInfoFromType<TStateReactor>();
224 srinfo->outputEventType = TypeInfo::getTypeInfoFromType<TOutputEvent>();
225
226 if (srinfo->outputEventType->templateParameters.size() == 2)
227 {
228 srinfo->objectTagType = srinfo->outputEventType->templateParameters[1];
229 }
230 else if (srinfo->outputEventType->templateParameters.size() == 1)
231 {
232 srinfo->objectTagType = TypeInfo::getTypeInfoFromType<state_reactors::EmptyObjectTag>();
233 }
234 else
235 {
236 assert(
237 false &&
238 "state reactor output events should have one or two parameters (SourceType, ObjectTag)");
239 }
240
241 // iterate statically on all event sources
242 using boost::mpl::_1;
243 using wrappedList = typename boost::mpl::transform<TInputEventList, _1>::type;
245 boost::mpl::for_each<wrappedList>(op);
246
247 srinfo->srh = srh;
248 srh->srInfo_ = srinfo;
249
250 const std::type_info * tindex = &(typeid(MostDerived)); // get identifier of the current state
251
252 if (!SmaccStateInfo::stateReactorsInfo.count(tindex))
254 std::vector<std::shared_ptr<SmaccStateReactorInfo>>();
255
256 srinfo->factoryFunction = [&, srh, args...](ISmaccState * state)
257 {
258 auto sr =
259 state->createStateReactor<TStateReactor, TOutputEvent, TInputEventList, TArgs...>(args...);
260 srh->configureStateReactor(sr);
261 sr->initialize(state);
262 return sr;
263 };
264
265 SmaccStateInfo::stateReactorsInfo[tindex].push_back(srinfo);
266
267 return srh;
268 }
269
270 template <typename TEventGenerator, typename... TUArgs>
271 static std::shared_ptr<smacc2::introspection::EventGeneratorHandler> static_createEventGenerator(
272 TUArgs... args)
273 {
274 auto egh = std::make_shared<smacc2::introspection::EventGeneratorHandler>();
275 auto eginfo = std::make_shared<SmaccEventGeneratorInfo>();
276 eginfo->eventGeneratorType = TypeInfo::getTypeInfoFromType<TEventGenerator>();
277
278 eginfo->egh = egh;
279 egh->egInfo_ = eginfo;
280
281 const std::type_info * tindex = &(typeid(MostDerived)); // get identifier of the current state
282
283 if (!SmaccStateInfo::eventGeneratorsInfo.count(tindex))
285 std::vector<std::shared_ptr<SmaccEventGeneratorInfo>>();
286
287 eginfo->factoryFunction = [&, egh, args...](ISmaccState * state)
288 {
289 auto eg = state->createEventGenerator<TEventGenerator>(args...);
290 egh->configureEventGenerator(eg);
291 eg->initialize(state);
292 eg->template onStateAllocation<MostDerived, TEventGenerator>();
293 return eg;
294 };
295
296 SmaccStateInfo::eventGeneratorsInfo[tindex].push_back(eginfo);
297 return egh;
298 }
299
300 template <typename TStateReactor, typename... TUArgs>
301 static std::shared_ptr<smacc2::introspection::StateReactorHandler> static_createStateReactor_aux(
302 TUArgs... args)
303 {
304 auto srh = std::make_shared<smacc2::introspection::StateReactorHandler>(globalNh_);
305 auto srinfo = std::make_shared<SmaccStateReactorInfo>();
306
307 srinfo->stateReactorType = TypeInfo::getTypeInfoFromType<TStateReactor>();
308 srinfo->srh = srh;
309 srh->srInfo_ = srinfo;
310
311 const std::type_info * tindex = &(typeid(MostDerived)); // get identifier of the current state
312
313 if (!SmaccStateInfo::stateReactorsInfo.count(tindex))
315 std::vector<std::shared_ptr<SmaccStateReactorInfo>>();
316
317 srinfo->factoryFunction = [&, srh, args...](ISmaccState * state)
318 {
319 auto sr = state->createStateReactor<TStateReactor>(args...);
320 srh->configureStateReactor(sr);
321 sr->initialize(state);
322 return sr;
323 };
324
325 SmaccStateInfo::stateReactorsInfo[tindex].push_back(srinfo);
326
327 return srh;
328 }
329
330 void checkWhileLoopConditionAndThrowEvent(bool (MostDerived::*conditionFn)())
331 {
332 auto * thisobject = static_cast<MostDerived *>(this);
333 auto condition = boost::bind(conditionFn, thisobject);
334 bool conditionResult = condition();
335
336 // RCLCPP_INFO("LOOP EVENT CONDITION: %d", conditionResult);
337 if (conditionResult)
338 {
339 this->postEvent<EvLoopContinue<MostDerived>>();
340 }
341 else
342 {
343 this->postEvent<EvLoopEnd<MostDerived>>();
344 }
345 RCLCPP_INFO(getLogger(), "[%s] POST THROW CONDITION", STATE_NAME);
346 }
347
348 void throwSequenceFinishedEvent() { this->postEvent<EvSequenceFinished<MostDerived>>(); }
349
351 // The following declarations should be private.
352 // They are only public because many compilers lack template friends.
354 // See base class for documentation
355 typedef typename base_type::outermost_context_base_type outermost_context_base_type;
356 typedef typename base_type::inner_context_ptr_type inner_context_ptr_type;
357 typedef typename base_type::context_ptr_type context_ptr_type;
358 typedef typename base_type::inner_initial_list inner_initial_list;
359
360 static void initial_deep_construct(outermost_context_base_type & outermostContextBase)
361 {
362 deep_construct(&outermostContextBase, outermostContextBase);
363 }
364
365 // See base class for documentation
366 static void deep_construct(
367 const context_ptr_type & pContext, outermost_context_base_type & outermostContextBase)
368 {
369 const inner_context_ptr_type pInnerContext(shallow_construct(pContext, outermostContextBase));
370 base_type::template deep_construct_inner<inner_initial_list>(
371 pInnerContext, outermostContextBase);
372 }
373
375 const context_ptr_type & pContext, outermost_context_base_type & outermostContextBase)
376 {
377 // allocating in memory
378 auto state = new MostDerived(
380 const inner_context_ptr_type pInnerContext(state);
381
383 state->entryStateInternal();
385
386 outermostContextBase.add(pInnerContext);
387 return pInnerContext;
388 }
389
390private:
391 template <typename TOrthogonal, typename TBehavior>
393 std::function<void(ISmaccState * state)> initializationFunction)
394 {
396 bhinfo.factoryFunction = initializationFunction;
397
398 bhinfo.behaviorType = &(typeid(TBehavior));
399 bhinfo.orthogonalType = &(typeid(TOrthogonal));
400
401 const std::type_info * tindex = &(typeid(MostDerived));
402 if (!SmaccStateInfo::staticBehaviorInfo.count(tindex))
403 SmaccStateInfo::staticBehaviorInfo[tindex] = std::vector<ClientBehaviorInfoEntry>();
404
405 SmaccStateInfo::staticBehaviorInfo[tindex].push_back(bhinfo);
406 RCLCPP_INFO_STREAM(
407 rclcpp::get_logger("static"), "[states walking] State "
409 << "client behavior count: "
410 << SmaccStateInfo::staticBehaviorInfo[tindex].size());
411 }
412
414 {
415 // finally we go to the derived state onEntry Function
416
417 RCLCPP_DEBUG(getLogger(), "[%s] State object created. Initializing...", STATE_NAME);
418 this->getStateMachine().notifyOnStateEntryStart(static_cast<MostDerived *>(this));
419
420 RCLCPP_DEBUG_STREAM(
421 getLogger(), "[" << smacc2::utils::cleanShortTypeName(typeid(MostDerived)).c_str()
422 << "] creating ros subnode");
423
424 // before dynamic runtimeConfigure, we execute the staticConfigure behavior configurations
425 {
426 RCLCPP_DEBUG(getLogger(), "[%s] -- STATIC STATE DESCRIPTION --", STATE_NAME);
427
428 for (const auto & clientBehavior : SmaccStateInfo::staticBehaviorInfo)
429 {
430 RCLCPP_DEBUG(
431 getLogger(), "[%s] client behavior info: %s", STATE_NAME,
432 demangleSymbol(clientBehavior.first->name()).c_str());
433 for (auto & cbinfo : clientBehavior.second)
434 {
435 RCLCPP_DEBUG(
436 getLogger(), "[%s] client behavior: %s", STATE_NAME,
437 demangleSymbol(cbinfo.behaviorType->name()).c_str());
438 }
439 }
440
441 const std::type_info * tindex = &(typeid(MostDerived));
442 auto & staticDefinedBehaviors = SmaccStateInfo::staticBehaviorInfo[tindex];
443 auto & staticDefinedStateReactors = SmaccStateInfo::stateReactorsInfo[tindex];
444 auto & staticDefinedEventGenerators = SmaccStateInfo::eventGeneratorsInfo[tindex];
445
446 for (auto & ortho : this->getStateMachine().getOrthogonals())
447 {
448 RCLCPP_INFO(
449 getLogger(), "[%s] Initializing orthogonal: %s", STATE_NAME,
450 demangleSymbol(typeid(*ortho.second).name()).c_str());
451 ortho.second->initState(this);
452 }
453
454 RCLCPP_DEBUG_STREAM(
455 getLogger(), "finding static client behaviors. State Database: "
456 << SmaccStateInfo::staticBehaviorInfo.size() << ". Current state "
457 << cleanShortTypeName(*tindex)
458 << " cbs: " << SmaccStateInfo::staticBehaviorInfo[tindex].size());
459 for (auto & bhinfo : staticDefinedBehaviors)
460 {
461 RCLCPP_DEBUG(
462 getLogger(), "[%s] Creating static client behavior: %s", STATE_NAME,
463 demangleSymbol(bhinfo.behaviorType->name()).c_str());
464 bhinfo.factoryFunction(this);
465 }
466
467 for (auto & sr : staticDefinedStateReactors)
468 {
469 RCLCPP_DEBUG(
470 getLogger(), "[%s] Creating static state reactor: %s", STATE_NAME,
471 sr->stateReactorType->getFullName().c_str());
472 sr->factoryFunction(this);
473 }
474
475 for (auto & eg : staticDefinedEventGenerators)
476 {
477 RCLCPP_DEBUG(
478 getLogger(), "[%s] Creating static event generator: %s", STATE_NAME,
479 eg->eventGeneratorType->getFullName().c_str());
480 eg->factoryFunction(this);
481 }
482
483 RCLCPP_DEBUG(getLogger(), "[%s] ---- END STATIC DESCRIPTION", STATE_NAME);
484 }
485
486 RCLCPP_DEBUG(getLogger(), "[%s] State runtime configuration", STATE_NAME);
487
488 auto * derivedthis = static_cast<MostDerived *>(this);
489
490 // second the orthogonals are internally configured
491 this->getStateMachine().notifyOnRuntimeConfigured(derivedthis);
492
494 // first we runtime configure the state, where we create client behaviors
495 static_cast<MostDerived *>(this)->runtimeConfigure();
497
499
500 RCLCPP_DEBUG(getLogger(), "[%s] State OnEntry", STATE_NAME);
501
502 static_cast<MostDerived *>(this)->onEntry();
503
504 // here orthogonals and client behaviors are entered OnEntry
505 this->getStateMachine().notifyOnStateEntryEnd(derivedthis);
506 }
507};
508} // namespace smacc2
rclcpp::Node::SharedPtr getNode()
void notifyOnStateExitting(StateType *state)
void notifyOnRuntimeConfigurationFinished(StateType *state)
const std::map< std::string, std::shared_ptr< smacc2::ISmaccOrthogonal > > & getOrthogonals() const
void notifyOnStateExited(StateType *state)
void notifyOnStateEntryEnd(StateType *state)
const SmaccStateMachineInfo & getStateMachineInfo()
void notifyOnRuntimeConfigured(StateType *state)
void notifyOnStateEntryStart(StateType *state)
std::shared_ptr< rclcpp::Logger > logger_
rclcpp::Logger getLogger()
Definition: smacc_state.hpp:36
std::shared_ptr< TBehavior > configure(Args &&... args)
rclcpp::Node::SharedPtr node_
ISmaccState * parentState_
const smacc2::introspection::SmaccStateInfo * stateInfo_
sc::simple_state< MostDerived, Context, InnerInitial, historyMode > base_type
static inner_context_ptr_type shallow_construct(const context_ptr_type &pContext, outermost_context_base_type &outermostContextBase)
static std::shared_ptr< smacc2::introspection::StateReactorHandler > static_createStateReactor(TArgs... args)
base_type::context_ptr_type context_ptr_type
static void initial_deep_construct(outermost_context_base_type &outermostContextBase)
InnerInitial * smacc_inner_type
base_type::inner_context_ptr_type inner_context_ptr_type
void checkWhileLoopConditionAndThrowEvent(bool(MostDerived::*conditionFn)())
static void configure_orthogonal_internal(std::function< void(ISmaccState *state)> initializationFunction)
base_type::inner_initial_list inner_initial_list
static std::shared_ptr< smacc2::introspection::EventGeneratorHandler > static_createEventGenerator(TUArgs... args)
virtual ISmaccStateMachine & getStateMachine()
static std::shared_ptr< smacc2::introspection::StateReactorHandler > static_createStateReactor_aux(TUArgs... args)
const smacc2::introspection::SmaccStateInfo * getStateInfo()
bool getGlobalSMData(std::string name, T &ret)
std::string getFullName()
static void deep_construct(const context_ptr_type &pContext, outermost_context_base_type &outermostContextBase)
base_type::outermost_context_base_type outermost_context_base_type
InnerInitial LastDeepState
virtual ISmaccState * getParentState()
SmaccState(my_context ctx)
context_type::state_iterator state_iterator
std::string getName() override
void setGlobalSMData(std::string name, T value)
Context::inner_context_type context_type
static void configure_orthogonal_runtime(std::function< void(TBehavior &bh)> initializationFunction)
void requiresComponent(SmaccComponentType *&storage)
std::string getShortName()
static void configure_orthogonal_runtime(std::function< void(TBehavior &bh, MostDerived &)> initializationFunction)
static void configure_orthogonal(Args &&... args)
static std::map< const std::type_info *, std::vector< std::shared_ptr< SmaccStateReactorInfo > > > stateReactorsInfo
static std::map< const std::type_info *, std::vector< std::shared_ptr< SmaccEventGeneratorInfo > > > eventGeneratorsInfo
static std::map< const std::type_info *, std::vector< ClientBehaviorInfoEntry > > staticBehaviorInfo
std::map< std::string, std::shared_ptr< SmaccStateInfo > > states
rclcpp::Node::SharedPtr globalNh_
Definition: reflection.cpp:28
std::string demangleSymbol()
std::string cleanShortTypeName()
Definition: common.hpp:61
void standardOnExit(TState &st, std::true_type)
#define STATE_NAME
void TRACEPOINT(spinOnce)
my_context(typename base_type::context_ptr_type pContext)
base_type::context_ptr_type pContext_
std::function< void(smacc2::ISmaccState *)> factoryFunction
smacc2_state_onExit_end
smacc2_state_onRuntimeConfigure_start
smacc2_state_onEntry_end
smacc2_state_onRuntimeConfigure_end
smacc2_state_onEntry_start
smacc2_state_onExit_start