stop1_executor.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019 Pilz GmbH & Co. KG
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8 
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13 
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
19 
20 namespace prbt_hardware_support
21 {
22 Stop1Executor::Stop1Executor(const TServiceCallFunc& hold_func, const TServiceCallFunc& unhold_func,
23  const TServiceCallFunc& recover_func, const TServiceCallFunc& halt_func)
24 {
25  state_machine_ = std::unique_ptr<RunPermittedStateMachine>(
26  new RunPermittedStateMachine(recover_func, halt_func, hold_func, unhold_func));
27 
28  state_machine_->start();
29 
30  worker_thread_ = std::thread(&Stop1Executor::workerThreadFun, this);
31 }
32 
34 {
35  if (worker_thread_.joinable())
36  {
37  {
38  std::lock_guard<std::mutex> lock(sm_mutex_);
39  terminate_ = true;
40  }
41  worker_cv_.notify_one();
42  worker_thread_.join();
43  }
44 
46 }
47 
48 void Stop1Executor::updateRunPermitted(const bool run_permitted)
49 {
50  ROS_DEBUG_STREAM("updateRunPermitted(" << std::boolalpha << run_permitted << std::noboolalpha << ")");
51  {
52  std::lock_guard<std::mutex> lock(sm_mutex_);
53  state_machine_->process_event(typename RunPermittedStateMachine::run_permitted_updated(run_permitted));
54  }
55  worker_cv_.notify_one();
56 }
57 
58 bool Stop1Executor::updateRunPermittedCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res)
59 {
60  updateRunPermitted(req.data);
61  res.success = true;
62  return true;
63 }
64 
66 {
67  std::unique_lock<std::mutex> sm_lock(sm_mutex_);
68  while (!terminate_)
69  {
70  worker_cv_.wait(sm_lock, [this]() { return (!this->state_machine_->task_queue_.empty() || this->terminate_); });
71  if (terminate_)
72  {
73  break;
74  }
75 
76  AsyncRunPermittedTask task = state_machine_->task_queue_.front();
77  state_machine_->task_queue_.pop();
78 
79  sm_lock.unlock(); // | This part is executed async from
80  task.execute(); // | the state machine since new run_permitted updates need to be handled
81  // | during service calls.
82  sm_lock.lock(); // |
83 
84  task.signalCompletion(); // Could add Task to Queue and does process_event on the state machine. Needs lock.
85  }
86 }
87 
88 } // namespace prbt_hardware_support
bool updateRunPermittedCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
Stop1Executor(const TServiceCallFunc &hold_func, const TServiceCallFunc &unhold_func, const TServiceCallFunc &recover_func, const TServiceCallFunc &halt_func)
Create required service clients and state machine; start worker-thread and state machine.
std::atomic_bool terminate_
Flag indicating if the worker-thread should terminate.
std::condition_variable worker_cv_
Condition variable for notifying a waiting worker-thread.
void updateRunPermitted(const bool run_permitted)
This is called everytime an updated run_permitted value is obtained.
std::thread worker_thread_
Worker-thread.
void stopStateMachine()
Stop the state machine.
#define ROS_DEBUG_STREAM(args)
void workerThreadFun()
This is executed in the worker-thread and allows asynchronous handling of run_permitted updates...
std::function< bool()> TServiceCallFunc
msm::back::state_machine< RunPermittedStateMachine_ > RunPermittedStateMachine
The top-level (back-end) state machine.
An AsyncRunPermittedTask is represented by a task execution and a completion signalling.
void signalCompletion()
Signal completion of the task execution.
std::unique_ptr< RunPermittedStateMachine > state_machine_
State machine.
virtual ~Stop1Executor()
Stop state machine and terminate worker-thread.


prbt_hardware_support
Author(s):
autogenerated on Thu Sep 10 2020 03:14:36