SMACC2
Loading...
Searching...
No Matches
Public Member Functions | Private Member Functions | Private Attributes | List of all members
smacc2::SignalDetector Class Reference

#include <smacc_signal_detector.hpp>

Collaboration diagram for smacc2::SignalDetector:
Collaboration graph

Public Member Functions

 SignalDetector (SmaccFifoScheduler *scheduler, ExecutionModel executionModel=ExecutionModel::SINGLE_THREAD_SPINNER)
 
void initialize (ISmaccStateMachine *stateMachine)
 
void setProcessorHandle (SmaccFifoScheduler::processor_handle processorHandle)
 
void runThread ()
 
void join ()
 
void stop ()
 
void pollingLoop ()
 
template<typename EventType >
void postEvent (EventType *ev)
 
rclcpp::Node::SharedPtr getNode ()
 
rclcpp::Logger getLogger ()
 
void notifyStateConfigured (ISmaccState *currentState)
 
void notifyStateExited (ISmaccState *currentState)
 

Private Member Functions

void pollOnce ()
 
void findUpdatableClientsAndComponents ()
 
void findUpdatableStateElements (ISmaccState *currentState)
 

Private Attributes

ISmaccStateMachinesmaccStateMachine_
 
std::vector< ISmaccUpdatable * > updatableClients_
 
std::vector< std::vector< ISmaccUpdatable * > > updatableStateElements_
 
std::atomic< int64_t > lastState_
 
double loop_rate_hz
 
std::atomic< boolend_
 
std::atomic< boolinitialized_
 
rclcpp::Publisher< smacc2_msgs::msg::SmaccStatus >::SharedPtr statusPub_
 
SmaccFifoSchedulerscheduler_
 
SmaccFifoScheduler::processor_handle processorHandle_
 
boost::thread signalDetectorThread_
 
ExecutionModel executionModel_
 

Detailed Description

Definition at line 37 of file smacc_signal_detector.hpp.

Constructor & Destructor Documentation

◆ SignalDetector()

smacc2::SignalDetector::SignalDetector ( SmaccFifoScheduler scheduler,
ExecutionModel  executionModel = ExecutionModel::SINGLE_THREAD_SPINNER 
)

SignalDetector()

Definition at line 44 of file signal_detector.cpp.

45{
46 scheduler_ = scheduler;
47 loop_rate_hz = 20.0;
48 end_ = false;
49 initialized_ = false;
50 executionModel_ = executionModel;
51}
SmaccFifoScheduler * scheduler_
std::atomic< bool > initialized_

References end_, executionModel_, initialized_, loop_rate_hz, and scheduler_.

Member Function Documentation

◆ findUpdatableClientsAndComponents()

void smacc2::SignalDetector::findUpdatableClientsAndComponents ( )
private

findUpdatableClientsAndComponents()

Definition at line 75 of file signal_detector.cpp.

76{
77 this->updatableClients_.clear();
78 for (auto pair : this->smaccStateMachine_->getOrthogonals())
79 {
80 auto & orthogonal = pair.second;
81 auto & clients = orthogonal->getClients();
82
83 for (auto & client : clients)
84 {
85 // updatable client components
86 auto updatableClient = dynamic_cast<ISmaccUpdatable *>(client.get());
87
88 if (updatableClient != nullptr)
89 {
90 RCLCPP_DEBUG_STREAM(
91 getLogger(), "Adding updatable client: " << demangleType(typeid(updatableClient)));
92 this->updatableClients_.push_back(updatableClient);
93 }
94
95 // updatable client components
96 std::vector<std::shared_ptr<ISmaccComponent>> components;
97 client->getComponents(components);
98 for (auto & componententry : components)
99 {
100 auto updatableComponent = dynamic_cast<ISmaccUpdatable *>(componententry.get());
101 if (updatableComponent != nullptr)
102 {
103 RCLCPP_DEBUG_STREAM(
104 getLogger(),
105 "Adding updatable component: " << demangleType(typeid(*updatableComponent)));
106 this->updatableClients_.push_back(updatableComponent);
107 }
108 }
109 }
110 }
111}
std::vector< ISmaccUpdatable * > updatableClients_
ISmaccStateMachine * smaccStateMachine_
std::string demangleType(const std::type_info *tinfo)

References smacc2::introspection::demangleType(), getLogger(), smacc2::ISmaccStateMachine::getOrthogonals(), smaccStateMachine_, and updatableClients_.

Referenced by initialize(), and pollOnce().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ findUpdatableStateElements()

void smacc2::SignalDetector::findUpdatableStateElements ( ISmaccState currentState)
private

findUpdatableClientBehaviors()

Definition at line 118 of file signal_detector.cpp.

119{
120 updatableStateElements_.push_back(std::vector<ISmaccUpdatable *>());
121
122 auto & updatableElements = updatableStateElements_.back();
123
124 for (auto pair : this->smaccStateMachine_->getOrthogonals())
125 {
126 auto & orthogonal = pair.second;
127 auto & behaviors = orthogonal->getClientBehaviors().back();
128
129 for (auto & currentBehavior : behaviors)
130 {
131 ISmaccUpdatable * updatableClientBehavior =
132 dynamic_cast<ISmaccUpdatable *>(currentBehavior.get());
133
134 if (updatableClientBehavior != nullptr)
135 {
136 RCLCPP_DEBUG_STREAM(
137 getLogger(),
138 "Adding updatable behavior: " << demangleType(typeid(updatableClientBehavior)));
139 updatableElements.push_back(updatableClientBehavior);
140 }
141 }
142 }
143
144 auto updatableState = dynamic_cast<ISmaccUpdatable *>(currentState);
145 if (updatableState != nullptr)
146 {
147 updatableElements.push_back(updatableState);
148 }
149
150 auto statereactors = currentState->getStateReactors();
151 for (auto & sr : statereactors)
152 {
153 ISmaccUpdatable * updatableStateReactor = dynamic_cast<ISmaccUpdatable *>(sr.get());
154 if (updatableStateReactor != nullptr)
155 {
156 RCLCPP_DEBUG_STREAM(
157 getLogger(),
158 "Adding updatable stateReactorr: " << demangleType(typeid(updatableStateReactor)));
159 updatableElements.push_back(updatableStateReactor);
160 }
161 }
162
163 auto eventgenerators = currentState->getEventGenerators();
164 for (auto & eg : eventgenerators)
165 {
166 ISmaccUpdatable * updatableEventGenerator = dynamic_cast<ISmaccUpdatable *>(eg.get());
167 if (updatableEventGenerator != nullptr)
168 {
169 RCLCPP_DEBUG_STREAM(
170 getLogger(),
171 "Adding updatable eventGenerator: " << demangleType(typeid(updatableEventGenerator)));
172 updatableElements.push_back(updatableEventGenerator);
173 }
174 }
175}
std::vector< std::vector< ISmaccUpdatable * > > updatableStateElements_

References smacc2::introspection::demangleType(), smacc2::ISmaccState::getEventGenerators(), getLogger(), smacc2::ISmaccStateMachine::getOrthogonals(), smacc2::ISmaccState::getStateReactors(), smaccStateMachine_, and updatableStateElements_.

Referenced by notifyStateConfigured().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getLogger()

rclcpp::Logger smacc2::SignalDetector::getLogger ( )
inline

Definition at line 66 of file smacc_signal_detector.hpp.

66{ return getNode()->get_logger(); }
rclcpp::Node::SharedPtr getNode()

References getNode().

Referenced by findUpdatableClientsAndComponents(), findUpdatableStateElements(), pollingLoop(), and pollOnce().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getNode()

rclcpp::Node::SharedPtr smacc2::SignalDetector::getNode ( )

Definition at line 53 of file signal_detector.cpp.

53{ return this->smaccStateMachine_->getNode(); }
rclcpp::Node::SharedPtr getNode()

References smacc2::ISmaccStateMachine::getNode(), and smaccStateMachine_.

Referenced by getLogger(), initialize(), pollingLoop(), and pollOnce().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ initialize()

void smacc2::SignalDetector::initialize ( ISmaccStateMachine stateMachine)

initialize()

Definition at line 60 of file signal_detector.cpp.

61{
62 smaccStateMachine_ = stateMachine;
63 lastState_ = std::numeric_limits<int64_t>::quiet_NaN();
65 this->getNode()->declare_parameter("signal_detector_loop_freq", this->loop_rate_hz);
66
67 initialized_ = true;
68}
std::atomic< int64_t > lastState_

References findUpdatableClientsAndComponents(), getNode(), initialized_, lastState_, loop_rate_hz, and smaccStateMachine_.

Referenced by smacc2::ISmaccStateMachine::ISmaccStateMachine().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ join()

void smacc2::SignalDetector::join ( )

join()

Definition at line 212 of file signal_detector.cpp.

212{ signalDetectorThread_.join(); }

References signalDetectorThread_.

◆ notifyStateConfigured()

void smacc2::SignalDetector::notifyStateConfigured ( ISmaccState currentState)

Definition at line 177 of file signal_detector.cpp.

178{
179 this->findUpdatableStateElements(currentState);
180}
void findUpdatableStateElements(ISmaccState *currentState)

References findUpdatableStateElements().

Referenced by smacc2::ISmaccStateMachine::notifyOnRuntimeConfigurationFinished().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ notifyStateExited()

void smacc2::SignalDetector::notifyStateExited ( ISmaccState currentState)

Definition at line 182 of file signal_detector.cpp.

183{
184 this->updatableStateElements_.pop_back();
185}

References updatableStateElements_.

Referenced by smacc2::ISmaccStateMachine::notifyOnStateExited().

Here is the caller graph for this function:

◆ pollingLoop()

void smacc2::SignalDetector::pollingLoop ( )

pollingLoop()

Definition at line 313 of file signal_detector.cpp.

314{
315 // rclcpp::Node::SharedPtr nh("~"); // use node name as root of the parameter server
316 rclcpp::Node::SharedPtr _;
317 rclcpp::Rate r0(20);
318
319 while (!initialized_)
320 {
321 r0.sleep();
322 }
323
324 auto nh = getNode();
325
326 if (!nh->get_parameter("signal_detector_loop_freq", this->loop_rate_hz))
327 {
328 RCLCPP_WARN(
329 getLogger(),
330 "Signal detector frequency (ros param signal_detector_loop_freq) was not set, using default "
331 "frequency: "
332 "%lf",
333 this->loop_rate_hz);
334 }
335 else
336 {
337 RCLCPP_WARN(
338 getLogger(), "Signal detector frequency (ros param signal_detector_loop_freq): %lf",
339 this->loop_rate_hz);
340 }
341
342 nh->set_parameter(rclcpp::Parameter("signal_detector_loop_freq", this->loop_rate_hz));
343
344 RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] loop rate hz:" << loop_rate_hz);
345
347 {
348 RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] running in single threaded mode");
349
350 rclcpp::Rate r(loop_rate_hz);
351 while (rclcpp::ok() && !end_)
352 {
353 RCLCPP_INFO_STREAM_THROTTLE(
354 getLogger(), *getNode()->get_clock(), 10000, "[SignalDetector] heartbeat");
355 pollOnce();
356 rclcpp::spin_some(nh);
357 r.sleep();
358 }
359 }
360 else
361 {
362 RCLCPP_INFO_STREAM(getLogger(), "[SignalDetector] running in multi threaded mode");
363
364 rclcpp::executors::MultiThreadedExecutor executor;
365 executor.add_node(nh);
366 executor.spin();
367 }
368}

References end_, executionModel_, getLogger(), getNode(), initialized_, loop_rate_hz, pollOnce(), and smacc2::SINGLE_THREAD_SPINNER.

Referenced by smacc2::run(), smacc2::run_async(), and runThread().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ pollOnce()

void smacc2::SignalDetector::pollOnce ( )
private

poll()

Definition at line 226 of file signal_detector.cpp.

227{
228 // precondition: smaccStateMachine_ != nullptr
229
230 //TRACEPOINT( spinOnce);
231 TRACEPOINT(spinOnce);
232
233 std::lock_guard<std::recursive_mutex> lock(smaccStateMachine_->m_mutex_);
234 try
235 {
236 //smaccStateMachine_->lockStateMachine("update behaviors");
237
239 RCLCPP_DEBUG_STREAM(getLogger(), "updatable clients: " << this->updatableClients_.size());
240
241 if (this->updatableClients_.size())
242 {
243 auto node = getNode();
244 for (auto * updatableClient : this->updatableClients_)
245 {
246 auto updatableElementName = demangleType(typeid(*updatableClient)).c_str();
247 try
248 {
249 RCLCPP_DEBUG_STREAM(
250 node->get_logger(),
251 "[PollOnce] update client call: " << demangleType(typeid(*updatableClient)));
252
253 TRACEPOINT(smacc2_state_update_start, updatableElementName);
254 updatableClient->executeUpdate(smaccStateMachine_->getNode());
255 TRACEPOINT(smacc2_state_update_start, updatableElementName);
256 }
257 catch (const std::exception & e)
258 {
259 RCLCPP_ERROR_STREAM(
260 node->get_logger(),
261 "Error in updatable elemnent " << updatableElementName << ": " << e.what() << '\n');
262 }
263 }
264 }
265
266 // STATE UPDATABLE ELEMENTS
267 if (
270 this->smaccStateMachine_->stateMachineCurrentAction !=
272 this->smaccStateMachine_->stateMachineCurrentAction !=
274 this->smaccStateMachine_->stateMachineCurrentAction !=
276 {
277 RCLCPP_DEBUG_STREAM(
278 getLogger(), "updatable states: " << this->updatableStateElements_.size());
279
280 for (auto stateElement : this->updatableStateElements_)
281 {
282 for (auto * udpatableStateElement : stateElement)
283 {
284 std::string updatableElementName = demangleType(typeid(*udpatableStateElement));
285 auto updatableElementNameCstr = updatableElementName.c_str();
286
287 RCLCPP_DEBUG_STREAM(
288 getLogger(), "pollOnce update client behavior call: "
289 << demangleType(typeid(*udpatableStateElement)));
290 TRACEPOINT(smacc2_state_update_start, updatableElementNameCstr);
291
292 udpatableStateElement->executeUpdate(smaccStateMachine_->getNode());
293 TRACEPOINT(smacc2_state_update_start, updatableElementNameCstr);
294 }
295 }
296 }
297 }
298 catch (std::exception & ex)
299 {
300 RCLCPP_ERROR(getLogger(), "Exception during Signal Detector update loop. %s", ex.what());
301 }
302
303 auto nh = this->getNode();
304 rclcpp::spin_some(nh);
305 //smaccStateMachine_->unlockStateMachine("update behaviors");
306}
StateMachineInternalAction stateMachineCurrentAction
void TRACEPOINT(spinOnce)
smacc2_state_update_start

References smacc2::introspection::demangleType(), findUpdatableClientsAndComponents(), getLogger(), getNode(), smacc2::ISmaccStateMachine::getNode(), smacc2::ISmaccStateMachine::m_mutex_, smacc2_state_update_start, smaccStateMachine_, smacc2::STATE_CONFIGURING, smacc2::STATE_ENTERING, smacc2::STATE_EXITING, smacc2::ISmaccStateMachine::stateMachineCurrentAction, TRACEPOINT(), smacc2::TRANSITIONING, updatableClients_, and updatableStateElements_.

Referenced by pollingLoop().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ postEvent()

template<typename EventType >
void smacc2::SignalDetector::postEvent ( EventType ev)
inline

Definition at line 59 of file smacc_signal_detector.hpp.

60 {
61 boost::intrusive_ptr<EventType> weakPtrEvent = ev;
62 this->scheduler_->queue_event(processorHandle_, weakPtrEvent);
63 }
SmaccFifoScheduler::processor_handle processorHandle_

References processorHandle_, and scheduler_.

Referenced by smacc2::ISmaccStateMachine::postEvent().

Here is the caller graph for this function:

◆ runThread()

void smacc2::SignalDetector::runThread ( )

runThread()

Definition at line 202 of file signal_detector.cpp.

203{
204 signalDetectorThread_ = boost::thread(boost::bind(&SignalDetector::pollingLoop, this));
205}

References pollingLoop(), and signalDetectorThread_.

Here is the call graph for this function:

◆ setProcessorHandle()

void smacc2::SignalDetector::setProcessorHandle ( SmaccFifoScheduler::processor_handle  processorHandle)

setProcessorHandle()

Definition at line 192 of file signal_detector.cpp.

193{
194 processorHandle_ = processorHandle;
195}

References processorHandle_.

Referenced by smacc2::run().

Here is the caller graph for this function:

◆ stop()

void smacc2::SignalDetector::stop ( )

stop()

Definition at line 219 of file signal_detector.cpp.

219{ end_ = true; }

References end_.

Member Data Documentation

◆ end_

std::atomic<bool> smacc2::SignalDetector::end_
private

Definition at line 89 of file smacc_signal_detector.hpp.

Referenced by pollingLoop(), SignalDetector(), and stop().

◆ executionModel_

ExecutionModel smacc2::SignalDetector::executionModel_
private

Definition at line 103 of file smacc_signal_detector.hpp.

Referenced by pollingLoop(), and SignalDetector().

◆ initialized_

std::atomic<bool> smacc2::SignalDetector::initialized_
private

Definition at line 91 of file smacc_signal_detector.hpp.

Referenced by initialize(), pollingLoop(), and SignalDetector().

◆ lastState_

std::atomic<int64_t> smacc2::SignalDetector::lastState_
private

Definition at line 80 of file smacc_signal_detector.hpp.

Referenced by initialize().

◆ loop_rate_hz

double smacc2::SignalDetector::loop_rate_hz
private

Definition at line 87 of file smacc_signal_detector.hpp.

Referenced by initialize(), pollingLoop(), and SignalDetector().

◆ processorHandle_

SmaccFifoScheduler::processor_handle smacc2::SignalDetector::processorHandle_
private

Definition at line 99 of file smacc_signal_detector.hpp.

Referenced by postEvent(), and setProcessorHandle().

◆ scheduler_

SmaccFifoScheduler* smacc2::SignalDetector::scheduler_
private

Definition at line 97 of file smacc_signal_detector.hpp.

Referenced by postEvent(), and SignalDetector().

◆ signalDetectorThread_

boost::thread smacc2::SignalDetector::signalDetectorThread_
private

Definition at line 101 of file smacc_signal_detector.hpp.

Referenced by join(), and runThread().

◆ smaccStateMachine_

ISmaccStateMachine* smacc2::SignalDetector::smaccStateMachine_
private

◆ statusPub_

rclcpp::Publisher<smacc2_msgs::msg::SmaccStatus>::SharedPtr smacc2::SignalDetector::statusPub_
private

Definition at line 93 of file smacc_signal_detector.hpp.

◆ updatableClients_

std::vector<ISmaccUpdatable *> smacc2::SignalDetector::updatableClients_
private

Definition at line 77 of file smacc_signal_detector.hpp.

Referenced by findUpdatableClientsAndComponents(), and pollOnce().

◆ updatableStateElements_

std::vector<std::vector<ISmaccUpdatable *> > smacc2::SignalDetector::updatableStateElements_
private

The documentation for this class was generated from the following files: