SMACC2
smacc_state_machine_info.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
22
23#include <map>
24#include <memory>
25#include <string>
26#include <vector>
27
28#include <smacc2/common.hpp>
30
31// smacc_msgs
32#include <smacc2_msgs/msg/smacc_event_generator.hpp>
33#include <smacc2_msgs/msg/smacc_orthogonal.hpp>
34#include <smacc2_msgs/msg/smacc_state.hpp>
35#include <smacc2_msgs/msg/smacc_state_reactor.hpp>
36#include <smacc2_msgs/msg/smacc_transition.hpp>
37
38namespace smacc2
39{
40namespace introspection
41{
42extern rclcpp::Node::SharedPtr globalNh_;
43
44class SmaccStateMachineInfo : public std::enable_shared_from_this<SmaccStateMachineInfo>
45{
46public:
47 explicit SmaccStateMachineInfo(rclcpp::Node::SharedPtr nh) : nh_(nh) {}
48
49 std::map<std::string, std::shared_ptr<SmaccStateInfo>> states;
50
51 std::vector<smacc2_msgs::msg::SmaccState> stateMsgs;
52
53 template <typename InitialStateType>
55
56 template <typename StateType>
57 std::shared_ptr<SmaccStateInfo> createState(std::shared_ptr<SmaccStateInfo> parentState);
58
59 template <typename StateType>
61 {
62 auto typeNameStr = typeid(StateType).name();
63
64 return states.count(typeNameStr) > 0;
65 }
66
67 template <typename StateType>
68 std::shared_ptr<SmaccStateInfo> getState()
69 {
70 if (this->containsState<StateType>())
71 {
72 return states[typeid(StateType).name()];
73 }
74 return nullptr;
75 }
76
77 inline rclcpp::Node::SharedPtr getNode() { return nh_; }
78
79 inline rclcpp::Logger getLogger() { return nh_->get_logger(); }
80
81 template <typename StateType>
82 void addState(std::shared_ptr<StateType> & state);
84
85private:
86 rclcpp::Node::SharedPtr nh_;
87};
88
89//---------------------------------------------
91{
92 std::shared_ptr<SmaccStateInfo> & parentState_;
93 explicit AddSubState(std::shared_ptr<SmaccStateInfo> & parentState) : parentState_(parentState) {}
94
95 template <typename T>
96 void operator()(T);
97};
98
99//---------------------------------------------
101{
102 std::shared_ptr<SmaccStateInfo> & currentState_;
103
104 explicit AddTransition(std::shared_ptr<SmaccStateInfo> & currentState)
105 : currentState_(currentState)
106 {
107 }
108
109 template <
110 template <typename, typename, typename> typename TTransition, typename TevSource,
111 template <typename> typename EvType, typename Tag, typename DestinyState>
112 void operator()(TTransition<EvType<TevSource>, DestinyState, Tag>);
113
114 template <
115 template <typename, typename> typename TTransition, typename TevSource,
116 template <typename> typename EvType, typename DestinyState>
117 void operator()(TTransition<EvType<TevSource>, DestinyState>);
118
119 template <typename T>
121};
122
123//---------------------------------------------
124template <typename InitialStateType>
126{
127 static void walkStates(std::shared_ptr<SmaccStateInfo> & currentState, bool rootInitialNode);
128};
129
130//---------------------------------------------
131template <typename T>
133{
134 using type_t = typename T::type;
135 //auto childState = this->parentState_->createChildState<type_t>()
137}
138
139//--------------------------------------------
140template <typename T>
141typename disable_if<boost::mpl::is_sequence<T>>::type processSubState(
142 std::shared_ptr<SmaccStateInfo> & parentState)
143{
144 WalkStatesExecutor<T>::walkStates(parentState, false);
145}
146
147//---------------------------------------------
148template <typename T>
149typename enable_if<boost::mpl::is_sequence<T>>::type processSubState(
150 std::shared_ptr<SmaccStateInfo> & parentState)
151{
152 using boost::mpl::_1;
153 using wrappedList = typename boost::mpl::transform<T, add_type_wrapper<_1>>::type;
154 boost::mpl::for_each<wrappedList>(AddSubState(parentState));
155}
156
157//--------------------------------------------
158/*Iterate on All Transitions*/
159template <typename T>
160typename enable_if<boost::mpl::is_sequence<T>>::type processTransitions(
161 std::shared_ptr<SmaccStateInfo> & sourceState)
162{
163 RCLCPP_INFO(
164 globalNh_->get_logger(), "State %s Walker has transition list",
165 sourceState->fullStateName.c_str());
166 using boost::mpl::_1;
167 using wrappedList = typename boost::mpl::transform<T, add_type_wrapper<_1>>::type;
168 boost::mpl::for_each<wrappedList>(AddTransition(sourceState));
169}
170
171template <typename Ev, typename Dst, typename Tag>
173 smacc2::Transition<Ev, boost::statechart::deep_history<Dst>, Tag> *,
174 std::shared_ptr<SmaccStateInfo> & sourceState)
175{
176 auto transitionTypeInfo = TypeInfo::getTypeInfoFromType<
178 smacc2::Transition<Ev, Dst, Tag> * mock = nullptr;
179 processTransitionAux(mock, sourceState, true, transitionTypeInfo);
180}
181
182template <typename Ev, typename Dst, typename Tag>
184 smacc2::Transition<Ev, Dst, Tag> * t, std::shared_ptr<SmaccStateInfo> & sourceState)
185{
186 auto transitionTypeInfo = TypeInfo::getTypeInfoFromType<smacc2::Transition<Ev, Dst, Tag>>();
187 RCLCPP_INFO(
188 globalNh_->get_logger(), "State %s Walker transition: %s", sourceState->toShortName().c_str(),
189 demangleSymbol(typeid(Ev).name()).c_str());
190 processTransitionAux(t, sourceState, false, transitionTypeInfo);
191}
192
193template <typename Ev, typename Dst, typename Tag>
195 smacc2::Transition<Ev, Dst, Tag> *, std::shared_ptr<SmaccStateInfo> & sourceState, bool history,
196 TypeInfo::Ptr & transitionTypeInfo)
197{
198 RCLCPP_INFO(
199 globalNh_->get_logger(), "State %s Walker transition: %s", sourceState->toShortName().c_str(),
200 demangleSymbol(typeid(Ev).name()).c_str());
201 std::string transitionTag;
202 std::string transitionType;
203
204 if (typeid(Tag) != typeid(default_transition_name))
205 {
206 transitionTag = demangleSymbol<Tag>();
207 transitionType = getTransitionType<Tag>();
208 RCLCPP_DEBUG_STREAM(globalNh_->get_logger(), "TRANSITION TYPE:" << transitionType);
209 }
210 else
211 {
212 transitionTag = "";
213 automaticTransitionTag<Ev>(transitionTag);
214 automaticTransitionType<Ev>(transitionType);
215 }
216
217 RCLCPP_INFO_STREAM(globalNh_->get_logger(), "Transition tag: " << transitionTag);
218
219 if (!sourceState->stateMachine_->containsState<Dst>())
220 {
221 auto realparentState = sourceState->stateMachine_->getState<typename Dst::TContext>();
222 auto siblingnode = sourceState->stateMachine_->createState<Dst>(realparentState);
223
224 // auto siblingnode = sourceState->stateMachine_->createState<Dst>(sourceState->parentState_);
225 WalkStatesExecutor<Dst>::walkStates(siblingnode, true);
226 sourceState->declareTransition<Ev>(
227 siblingnode, transitionTag, transitionType, history, transitionTypeInfo);
228 }
229 else
230 {
231 // auto realparentState = sourceState->stateMachine_->getState<typename Dst::TContext>();
232 // auto siblingnode = sourceState->stateMachine_->createState<Dst>(realparentState);
233
234 auto siblingnode = sourceState->stateMachine_->getState<Dst>();
235 sourceState->declareTransition<Ev>(
236 siblingnode, transitionTag, transitionType, history, transitionTypeInfo);
237 }
238}
239
240//---------------------------------------------
241template <typename EvType>
243 std::shared_ptr<SmaccStateInfo> & dstState, std::string transitionTag, std::string transitionType,
244 bool history, TypeInfo::Ptr transitionTypeInfo)
245{
246 auto evtype = demangledTypeName<EvType>();
247
248 SmaccTransitionInfo transitionInfo;
249 transitionInfo.index = transitions_.size();
250 transitionInfo.sourceState = shared_from_this();
251 transitionInfo.destinyState = dstState;
252 transitionInfo.transitionTypeInfo = transitionTypeInfo;
253
254 if (transitionTag != "")
255 transitionInfo.transitionTag = transitionTag;
256 else
257 transitionInfo.transitionTag = "Transition_" + std::to_string(transitionInfo.index);
258
259 transitionInfo.transitionType = transitionType;
260 transitionInfo.historyNode = history;
261
262 transitionInfo.eventInfo =
263 std::make_shared<SmaccEventInfo>(transitionTypeInfo->templateParameters.front());
264
265 EventLabel<EvType>(transitionInfo.eventInfo->label);
266 // RCLCPP_DEBUG_STREAM(getLogger(), "LABEL: " << transitionInfo.eventInfo->label);
267
268 transitions_.push_back(transitionInfo);
269}
270//---------------------------------------------
271
272//------------------ staticConfigure -----------------------------
273
274// SFINAE test
275template <typename T>
277{
278private:
279 typedef char YesType[1];
280 typedef char NoType[2];
281
282 template <typename C>
283 static YesType & test(decltype(&C::staticConfigure));
284 template <typename C>
285 static NoType & test(...);
286
287public:
288 enum
289 {
290 value = sizeof(test<T>(0)) == sizeof(YesType)
291 };
292};
293
294// template <typename TevSource, template <typename> typename EvType>
295// void SmaccStateInfo::declareTransition(std::shared_ptr<SmaccStateInfo> &dstState, std::string transitionTag, std::string transitionType, bool history)
296// {
297// auto evtype = demangledTypeName<EvType<TevSource>>();
298
299// SmaccTransitionInfo transitionInfo;
300// transitionInfo.index = transitions_.size();
301// transitionInfo.sourceState = shared_from_this();
302// transitionInfo.destinyState = dstState;
303
304// if (transitionTag != "")
305// transitionInfo.transitionTag = transitionTag;
306// else
307// transitionInfo.transitionTag = "Transition_" + std::to_string(transitionInfo.index);
308
309// transitionInfo.transitionType = transitionType;
310
311// transitionInfo.eventInfo = std::make_shared<SmaccEventInfo>(TypeInfo::getTypeInfoFromString(demangleSymbol(typeid(EvType<TevSource>).name())));
312
313// EventLabel<EvType<TevSource>>(transitionInfo.eventInfo->label);
314// RCLCPP_ERROR_STREAM(getLogger(),"LABEL: " << transitionInfo.eventInfo->label);
315
316// transitions_.push_back(transitionInfo);
317// }
318
319//---------------------------------------------
320template <typename Ev, typename Dst>
322 statechart::transition<Ev, Dst> *, std::shared_ptr<SmaccStateInfo> & sourceState)
323{
324 // RCLCPP_INFO_STREAM(getLogger(),"GOTCHA");
325}
326
327template <typename Ev>
329 statechart::custom_reaction<Ev> *, std::shared_ptr<SmaccStateInfo> & sourceState)
330{
331 // RCLCPP_INFO_STREAM(getLogger(),"GOTCHA");
332}
333
334//---------------------------------------------
335// only reached if it is a leaf transition in the mpl::list
336
337template <typename T>
338typename disable_if<boost::mpl::is_sequence<T>>::type processTransitions(
339 std::shared_ptr<SmaccStateInfo> & sourceState)
340{
341 // RCLCPP_INFO_STREAM(getLogger(),"state transition from: " << sourceState->demangledStateName <<
342 // " of type: " << demangledTypeName<T>());
343 T * dummy = nullptr;
344 processTransition(dummy, sourceState);
345}
346
347template <typename T>
348typename std::enable_if<HasOnDefinition<T>::value, void>::type CallOnDefinition()
349{
350 /* something when T has toString ... */
351 RCLCPP_INFO_STREAM(
352 globalNh_->get_logger(), "EXECUTING ONDEFINITION: " << demangleSymbol(typeid(T).name()));
353 T::staticConfigure();
354}
355
356template <typename T>
357typename std::enable_if<!HasOnDefinition<T>::value, void>::type CallOnDefinition()
358{
359 RCLCPP_INFO_STREAM(
360 globalNh_->get_logger(),
361 "static OnDefinition: dont exist for " << demangleSymbol(typeid(T).name()));
362 /* something when T has toString ... */
363}
364
365/*
366// only reached if it is a leaf transition in the mpl::list
367template <template <typename,typename,typename> typename TTransition, typename TevSource,
368template <typename> typename EvType, typename Tag, typename DestinyState >
369typename disable_if<boost::mpl::is_sequence<TTransition<EvType<TevSource>,DestinyState, Tag>>>::type
370processTransitions(std::shared_ptr<SmaccStateInfo> &sourceState)
371{
372 RCLCPP_INFO(getLogger(),"DETECTED COMPLEX TRANSITION **************");
373 // RCLCPP_INFO_STREAM(getLogger(),"state transition from: " << sourceState->demangledStateName
374 << " of type: " << demangledTypeName<T>());
375 TTransition<EvType<TevSource>,DestinyState, Tag> *dummy;
376 processTransition(dummy, sourceState);
377}
378
379template <template <typename,typename> typename TTransition, typename TevSource,
380template <typename> typename EvType, typename DestinyState >
381typename disable_if<boost::mpl::is_sequence<TTransition<EvType<TevSource>,DestinyState>>>::type
382processTransitions(std::shared_ptr<SmaccStateInfo> &sourceState)
383{
384 RCLCPP_INFO(getLogger(),"DETECTED COMPLEX TRANSITION **************");
385 // RCLCPP_INFO_STREAM(getLogger(),"state transition from: " << sourceState->demangledStateName
386 << " of type: " << demangledTypeName<T>());
387 TTransition<EvType<TevSource>,DestinyState> *dummy;
388 processTransition(dummy, sourceState);
389}
390*/
391
392//--------------------------------------------
393
394template <
395 template <typename, typename, typename> typename TTransition, typename TevSource,
396 template <typename> typename EvType, typename Tag, typename DestinyState>
397void AddTransition::operator()(TTransition<EvType<TevSource>, DestinyState, Tag>)
398{
399 processTransitions<TTransition<EvType<TevSource>, DestinyState, Tag>>(currentState_);
400}
401
402//--------------------------------------------
403
404template <
405 template <typename, typename> typename TTransition, typename TevSource,
406 template <typename> typename EvType, typename DestinyState>
407void AddTransition::operator()(TTransition<EvType<TevSource>, DestinyState>)
408{
409 processTransitions<TTransition<EvType<TevSource>, DestinyState>>(currentState_);
410}
411
412template <typename TTrans>
414{
415 using type_t = typename TTrans::type;
416 processTransitions<type_t>(currentState_);
417}
418
419/*
420void CallOnDefinition(...)
421{
422
423}*/
424
425//-----------------------------------------------------------------------------------
426template <typename InitialStateType>
428 std::shared_ptr<SmaccStateInfo> & parentState, bool rootInitialNode)
429{
430 //rclcpp::Duration(1).sleep();
431 auto currentname = demangledTypeName<InitialStateType>();
432
433 std::shared_ptr<SmaccStateInfo> targetState;
434
435 if (!rootInitialNode)
436 {
437 if (parentState->stateMachine_->containsState<InitialStateType>())
438 {
439 // it already exist: break;
440 return;
441 }
442
443 targetState = parentState->createChildState<InitialStateType>();
444 }
445 else
446 {
447 targetState = parentState;
448 }
449
450 CallOnDefinition<InitialStateType>();
451
452 typedef
453 typename std::remove_pointer<decltype(InitialStateType::smacc_inner_type)>::type InnerType;
454 processSubState<InnerType>(targetState);
455
456 // -------------------- REACTIONS --------------------
457 typedef typename InitialStateType::reactions reactions;
458 // RCLCPP_INFO_STREAM(getLogger(),"state machine initial state reactions: "
459 // << demangledTypeName<reactions>());
460
461 processTransitions<reactions>(targetState);
462}
463
464//------------------------------------------------
465
466template <typename InitialStateType>
468{
469 auto initialState = this->createState<InitialStateType>(nullptr);
471}
472
473template <typename StateType>
474std::shared_ptr<SmaccStateInfo> SmaccStateMachineInfo::createState(
475 std::shared_ptr<SmaccStateInfo> parent)
476{
477 auto thisptr = this->shared_from_this();
478 auto * statetid = &(typeid(StateType));
479
480 auto demangledName = demangledTypeName<StateType>();
481 RCLCPP_INFO_STREAM(getLogger(), "Creating State Info: " << demangledName);
482
483 auto state = std::make_shared<SmaccStateInfo>(statetid, parent, thisptr);
484 state->demangledStateName = demangledName;
485 state->fullStateName = typeid(StateType).name();
486 state->stateIndex_ = states.size();
487
488 if (parent != nullptr)
489 {
490 parent->children_.push_back(state);
491 }
492
493 this->addState(state);
494
495 return state;
496}
497
498template <typename StateType>
499void SmaccStateMachineInfo::addState(std::shared_ptr<StateType> & state)
500{
501 states[state->fullStateName] = state;
502}
503
504template <typename StateType>
505std::shared_ptr<SmaccStateInfo> SmaccStateInfo::createChildState()
506{
507 auto realparentState = this->stateMachine_->getState<typename StateType::TContext>();
508
509 auto childState = this->stateMachine_->createState<StateType>(realparentState);
510
511 RCLCPP_WARN_STREAM(
512 getLogger(), "Real parent state> " << demangleSymbol<typename StateType::TContext>());
513
514 /*auto contextInfo = TypeInfo::getTypeInfoFromType<InitialStateType>();
515 auto parentState2= getState<InitialStateType::TContext>();
516 parentState2->createChildState<InitialStateType>();*/
517
518 // this->stateMachine_->addState(childState);
519 // stateMachineInfo.addState(stateMachineInfo)
520 // stateNames.push_back(currentname);
521 // RCLCPP_INFO(getLogger(),"------------");
522 // RCLCPP_INFO_STREAM(getLogger(),"** STATE state: "<< this->demangledStateName);
523
524 return childState;
525}
526} // namespace introspection
527} // namespace smacc2
static YesType & test(decltype(&C::staticConfigure))
std::shared_ptr< SmaccStateInfo > createChildState()
void declareTransition(std::shared_ptr< SmaccStateInfo > &dstState, std::string transitionTag, std::string transitionType, bool history, TypeInfo::Ptr transitionTypeInfo)
std::vector< SmaccTransitionInfo > transitions_
std::shared_ptr< SmaccStateMachineInfo > stateMachine_
void addState(std::shared_ptr< StateType > &state)
std::shared_ptr< SmaccStateInfo > createState(std::shared_ptr< SmaccStateInfo > parentState)
std::map< std::string, std::shared_ptr< SmaccStateInfo > > states
std::vector< smacc2_msgs::msg::SmaccState > stateMsgs
static TypeInfo::Ptr getTypeInfoFromType()
std::shared_ptr< TypeInfo > Ptr
disable_if< boost::mpl::is_sequence< T > >::type processSubState(std::shared_ptr< SmaccStateInfo > &parentState)
rclcpp::Node::SharedPtr globalNh_
Definition: reflection.cpp:28
void processTransition(smacc2::Transition< Ev, boost::statechart::deep_history< Dst >, Tag > *, std::shared_ptr< SmaccStateInfo > &sourceState)
enable_if< boost::mpl::is_sequence< T > >::type processTransitions(std::shared_ptr< SmaccStateInfo > &sourceState)
void processTransitionAux(smacc2::Transition< Ev, Dst, Tag > *, std::shared_ptr< SmaccStateInfo > &sourceState, bool history, TypeInfo::Ptr &transitionTypeInfo)
std::enable_if< HasOnDefinition< T >::value, void >::type CallOnDefinition()
std::string demangleSymbol(const std::string &name)
std::shared_ptr< SmaccStateInfo > & parentState_
AddSubState(std::shared_ptr< SmaccStateInfo > &parentState)
AddTransition(std::shared_ptr< SmaccStateInfo > &currentState)
void operator()(TTransition< EvType< TevSource >, DestinyState, Tag >)
std::shared_ptr< SmaccStateInfo > & currentState_
std::shared_ptr< const SmaccStateInfo > destinyState
std::shared_ptr< SmaccEventInfo > eventInfo
std::shared_ptr< const SmaccStateInfo > sourceState
smacc2::introspection::TypeInfo::Ptr transitionTypeInfo
static void walkStates(std::shared_ptr< SmaccStateInfo > &currentState, bool rootInitialNode)