Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "behaviortree_cpp/decorators/timeout_node.h"
00013 #include "behaviortree_cpp/action_node.h"
00014
00015 namespace BT
00016 {
00017
00018 TimeoutNode::TimeoutNode(const std::string& name, unsigned milliseconds)
00019 : DecoratorNode(name, {} ),
00020 child_halted_(false),
00021 timer_id_(0),
00022 msec_(milliseconds),
00023 read_parameter_from_ports_(false),
00024 timeout_started_(false)
00025 {
00026 setRegistrationID("Timeout");
00027 }
00028
00029 TimeoutNode::TimeoutNode(const std::string& name, const NodeConfiguration& config)
00030 : DecoratorNode(name, config),
00031 child_halted_(false),
00032 timer_id_(0),
00033 msec_(0),
00034 read_parameter_from_ports_(true),
00035 timeout_started_(false)
00036 {
00037 }
00038
00039 NodeStatus TimeoutNode::tick()
00040 {
00041 if( read_parameter_from_ports_ )
00042 {
00043 if( !getInput("msec", msec_) )
00044 {
00045 throw RuntimeError("Missing parameter [msec] in TimeoutNode");
00046 }
00047 }
00048
00049 if ( !timeout_started_ )
00050 {
00051 timeout_started_ = true;
00052 setStatus(NodeStatus::RUNNING);
00053 child_halted_ = false;
00054
00055 if (msec_ > 0)
00056 {
00057 timer_id_ = timer_.add(std::chrono::milliseconds(msec_),
00058 [this](bool aborted)
00059 {
00060 std::unique_lock<std::mutex> lk( timeout_mutex_ );
00061 if (!aborted && child()->status() == NodeStatus::RUNNING)
00062 {
00063 child_halted_ = true;
00064 child()->halt();
00065 child()->setStatus(NodeStatus::IDLE);
00066 }
00067 });
00068 }
00069 }
00070
00071 std::unique_lock<std::mutex> lk( timeout_mutex_ );
00072
00073 if (child_halted_)
00074 {
00075 timeout_started_ = false;
00076 return NodeStatus::FAILURE;
00077 }
00078 else
00079 {
00080 auto child_status = child()->executeTick();
00081 if (child_status != NodeStatus::RUNNING)
00082 {
00083 timeout_started_ = false;
00084 timeout_mutex_.unlock();
00085 timer_.cancel(timer_id_);
00086 timeout_mutex_.lock();
00087 }
00088 return child_status;
00089 }
00090 }
00091
00092 }