Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "behaviortree_cpp/action_node.h"
00015 #include "coroutine/coroutine.h"
00016
00017 namespace BT
00018 {
00019 ActionNodeBase::ActionNodeBase(const std::string& name, const NodeConfiguration& config)
00020 : LeafNode::LeafNode(name, config)
00021 {
00022 }
00023
00024
00025
00026
00027 SimpleActionNode::SimpleActionNode(const std::string& name,
00028 SimpleActionNode::TickFunctor tick_functor,
00029 const NodeConfiguration& config)
00030 : SyncActionNode(name, config), tick_functor_(std::move(tick_functor))
00031 {
00032 }
00033
00034 NodeStatus SimpleActionNode::tick()
00035 {
00036 NodeStatus prev_status = status();
00037
00038 if (prev_status == NodeStatus::IDLE)
00039 {
00040 setStatus(NodeStatus::RUNNING);
00041 prev_status = NodeStatus::RUNNING;
00042 }
00043
00044 NodeStatus status = tick_functor_(*this);
00045 if (status != prev_status)
00046 {
00047 setStatus(status);
00048 }
00049 return status;
00050 }
00051
00052
00053
00054 AsyncActionNode::AsyncActionNode(const std::string& name, const NodeConfiguration& config)
00055 : ActionNodeBase(name, config),
00056 keep_thread_alive_(true),
00057 start_action_(false)
00058 {
00059 thread_ = std::thread(&AsyncActionNode::asyncThreadLoop, this);
00060 }
00061
00062 AsyncActionNode::~AsyncActionNode()
00063 {
00064 if (thread_.joinable())
00065 {
00066 stopAndJoinThread();
00067 }
00068 }
00069
00070 void AsyncActionNode::waitStart()
00071 {
00072 std::unique_lock<std::mutex> lock(start_mutex_);
00073 while (!start_action_)
00074 {
00075 start_signal_.wait(lock);
00076 }
00077 start_action_ = false;
00078 }
00079
00080 void AsyncActionNode::notifyStart()
00081 {
00082 std::unique_lock<std::mutex> lock(start_mutex_);
00083 start_action_ = true;
00084 start_signal_.notify_all();
00085 }
00086
00087 void AsyncActionNode::asyncThreadLoop()
00088 {
00089 while (keep_thread_alive_.load())
00090 {
00091 waitStart();
00092
00093
00094
00095 if (keep_thread_alive_)
00096 {
00097
00098 try {
00099 setStatus(tick());
00100 }
00101 catch (std::exception&)
00102 {
00103 std::cerr << "\nUncaught exception from the method tick() of an AsyncActionNode: ["
00104 << registrationName() << "/" << name() << "]\n" << std::endl;
00105 exptr_ = std::current_exception();
00106 keep_thread_alive_ = false;
00107 }
00108 }
00109 }
00110 }
00111
00112 NodeStatus AsyncActionNode::executeTick()
00113 {
00114
00115
00116 if (status() == NodeStatus::IDLE)
00117 {
00118 setStatus( NodeStatus::RUNNING );
00119 notifyStart();
00120 }
00121
00122 if( exptr_ )
00123 {
00124 std::rethrow_exception(exptr_);
00125 }
00126 return status();
00127 }
00128
00129 void AsyncActionNode::stopAndJoinThread()
00130 {
00131 keep_thread_alive_.store(false);
00132 notifyStart();
00133 if (thread_.joinable())
00134 {
00135 thread_.join();
00136 }
00137 }
00138
00139
00140 struct CoroActionNode::Pimpl
00141 {
00142 coroutine::routine_t coro;
00143 std::atomic<bool> pending_destroy;
00144
00145 };
00146
00147
00148 CoroActionNode::CoroActionNode(const std::string &name,
00149 const NodeConfiguration& config):
00150 ActionNodeBase (name, config),
00151 _p(new Pimpl)
00152 {
00153 _p->coro = 0;
00154 _p->pending_destroy = false;
00155 }
00156
00157 CoroActionNode::~CoroActionNode()
00158 {
00159 if( _p->coro != 0 )
00160 {
00161 coroutine::destroy(_p->coro);
00162 }
00163 }
00164
00165 void CoroActionNode::setStatusRunningAndYield()
00166 {
00167 setStatus( NodeStatus::RUNNING );
00168 coroutine::yield();
00169 }
00170
00171 NodeStatus CoroActionNode::executeTick()
00172 {
00173 if( _p->pending_destroy && _p->coro != 0 )
00174 {
00175 coroutine::destroy(_p->coro);
00176 _p->coro = 0;
00177 _p->pending_destroy = false;
00178 }
00179
00180 if ( _p->coro == 0)
00181 {
00182 _p->coro = coroutine::create( [this]()
00183 {
00184 setStatus(tick());
00185 } );
00186 }
00187
00188 if( _p->coro != 0 )
00189 {
00190 if( _p->pending_destroy ||
00191 coroutine::resume(_p->coro) == coroutine::ResumeResult::FINISHED )
00192 {
00193 coroutine::destroy(_p->coro);
00194 _p->coro = 0;
00195 _p->pending_destroy = false;
00196 }
00197 }
00198 return status();
00199 }
00200
00201 void CoroActionNode::halt()
00202 {
00203 _p->pending_destroy = true;
00204 }
00205
00206 SyncActionNode::SyncActionNode(const std::string &name, const NodeConfiguration& config):
00207 ActionNodeBase(name, config)
00208 {}
00209
00210 NodeStatus SyncActionNode::executeTick()
00211 {
00212 auto stat = ActionNodeBase::executeTick();
00213 if( stat == NodeStatus::RUNNING)
00214 {
00215 throw LogicError("SyncActionNode MUST never return RUNNING");
00216 }
00217 return stat;
00218 }
00219
00220
00221
00222 }