/* Copyright (C) 2015-2018 Michele Colledanchise - All Rights Reserved
* Copyright (C) 2018-2025 Davide Faconti, Eurecat - All Rights Reserved
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "behaviortree_cpp/tree_node.h"
#include
#include
#include
#include
namespace BT
{
struct TreeNode::PImpl
{
PImpl(std::string name, NodeConfig config)
: name(std::move(name)), config(std::move(config))
{}
const std::string name;
NodeStatus status = NodeStatus::IDLE;
std::condition_variable state_condition_variable;
mutable std::mutex state_mutex;
StatusChangeSignal state_change_signal;
NodeConfig config;
std::string registration_ID;
PreTickCallback pre_tick_callback;
PostTickCallback post_tick_callback;
TickMonitorCallback tick_monitor_callback;
std::mutex callback_injection_mutex;
std::shared_ptr wake_up;
std::array pre_parsed;
std::array post_parsed;
};
TreeNode::TreeNode(std::string name, NodeConfig config)
: _p(new PImpl(std::move(name), std::move(config)))
{}
TreeNode::TreeNode(TreeNode&& other) noexcept : _p(std::move(other._p))
{}
TreeNode& TreeNode::operator=(TreeNode&& other) noexcept
{
this->_p = std::move(other._p);
return *this;
}
TreeNode::~TreeNode() = default;
NodeStatus TreeNode::executeTick()
{
auto new_status = _p->status;
PreTickCallback pre_tick;
PostTickCallback post_tick;
TickMonitorCallback monitor_tick;
{
const std::scoped_lock lk(_p->callback_injection_mutex);
pre_tick = _p->pre_tick_callback;
post_tick = _p->post_tick_callback;
monitor_tick = _p->tick_monitor_callback;
}
// a pre-condition may return the new status.
// In this case it override the actual tick()
if(auto precond = checkPreConditions())
{
new_status = precond.value();
}
else
{
// injected pre-callback
bool substituted = false;
if(pre_tick && !isStatusCompleted(_p->status))
{
auto override_status = pre_tick(*this);
if(isStatusCompleted(override_status))
{
// don't execute the actual tick()
substituted = true;
new_status = override_status;
}
}
// Call the ACTUAL tick
if(!substituted)
{
using namespace std::chrono;
// Use atomic_thread_fence to prevent compiler reordering of time measurements.
// See issue #861 for details.
const auto t1 = steady_clock::now();
std::atomic_thread_fence(std::memory_order_seq_cst);
try
{
new_status = tick();
}
catch(const NodeExecutionError&)
{
// Already wrapped by a child node, re-throw as-is to preserve original info
throw;
}
catch(const std::exception& ex)
{
// Wrap the exception with this node's context
throw NodeExecutionError({ name(), fullPath(), registrationName() }, ex.what());
}
std::atomic_thread_fence(std::memory_order_seq_cst);
const auto t2 = steady_clock::now();
if(monitor_tick)
{
monitor_tick(*this, new_status, duration_cast(t2 - t1));
}
}
}
// injected post callback
if(isStatusCompleted(new_status))
{
checkPostConditions(new_status);
}
if(post_tick)
{
auto override_status = post_tick(*this, new_status);
if(isStatusCompleted(override_status))
{
new_status = override_status;
}
}
// preserve the IDLE state if skipped, but communicate SKIPPED to parent
if(new_status != NodeStatus::SKIPPED)
{
setStatus(new_status);
}
return new_status;
}
void TreeNode::haltNode()
{
halt();
const auto& parse_executor = _p->post_parsed[size_t(PostCond::ON_HALTED)];
if(parse_executor)
{
Ast::Environment env = { config().blackboard, config().enums };
parse_executor(env);
}
}
void TreeNode::setStatus(NodeStatus new_status)
{
if(new_status == NodeStatus::IDLE)
{
throw RuntimeError("Node [", name(),
"]: you are not allowed to set manually the status to IDLE. "
"If you know what you are doing (?) use resetStatus() instead.");
}
NodeStatus prev_status = NodeStatus::IDLE;
{
const std::unique_lock UniqueLock(_p->state_mutex);
prev_status = _p->status;
_p->status = new_status;
}
if(prev_status != new_status)
{
_p->state_condition_variable.notify_all();
_p->state_change_signal.notify(std::chrono::high_resolution_clock::now(), *this,
prev_status, new_status);
}
}
TreeNode::PreScripts& TreeNode::preConditionsScripts()
{
return _p->pre_parsed;
}
TreeNode::PostScripts& TreeNode::postConditionsScripts()
{
return _p->post_parsed;
}
Expected TreeNode::checkPreConditions()
{
Ast::Environment env = { config().blackboard, config().enums };
// Check pre-conditions in order: FAILURE_IF, SUCCESS_IF, SKIP_IF, WHILE_TRUE.
// IMPORTANT: _failureIf, _successIf, and _skipIf are evaluated ONLY when the
// node is IDLE or SKIPPED. They are NOT re-evaluated while the node is RUNNING.
for(size_t index = 0; index < size_t(PreCond::COUNT_); index++)
{
const auto& parse_executor = _p->pre_parsed[index];
if(!parse_executor)
{
continue;
}
const auto preID = static_cast(index);
// _failureIf, _successIf, _skipIf: only checked when IDLE or SKIPPED
// _while: checked here AND also when RUNNING (see below)
if(_p->status == NodeStatus::IDLE || _p->status == NodeStatus::SKIPPED)
{
// what to do if the condition is true
if(parse_executor(env).cast())
{
if(preID == PreCond::FAILURE_IF)
{
return NodeStatus::FAILURE;
}
if(preID == PreCond::SUCCESS_IF)
{
return NodeStatus::SUCCESS;
}
if(preID == PreCond::SKIP_IF)
{
return NodeStatus::SKIPPED;
}
}
// if the conditions is false
else if(preID == PreCond::WHILE_TRUE)
{
return NodeStatus::SKIPPED;
}
}
else if(_p->status == NodeStatus::RUNNING && preID == PreCond::WHILE_TRUE)
{
// _while is the ONLY precondition checked while RUNNING.
// If the condition becomes false, halt the node and return SKIPPED.
if(!parse_executor(env).cast())
{
haltNode();
return NodeStatus::SKIPPED;
}
}
}
return nonstd::make_unexpected(""); // no precondition
}
void TreeNode::checkPostConditions(NodeStatus status)
{
auto ExecuteScript = [this](const PostCond& cond) {
const auto& parse_executor = _p->post_parsed[size_t(cond)];
if(parse_executor)
{
Ast::Environment env = { config().blackboard, config().enums };
parse_executor(env);
}
};
if(status == NodeStatus::SUCCESS)
{
ExecuteScript(PostCond::ON_SUCCESS);
}
else if(status == NodeStatus::FAILURE)
{
ExecuteScript(PostCond::ON_FAILURE);
}
ExecuteScript(PostCond::ALWAYS);
}
void TreeNode::resetStatus()
{
NodeStatus prev_status = NodeStatus::IDLE;
{
const std::unique_lock lock(_p->state_mutex);
prev_status = _p->status;
_p->status = NodeStatus::IDLE;
}
if(prev_status != NodeStatus::IDLE)
{
_p->state_condition_variable.notify_all();
_p->state_change_signal.notify(std::chrono::high_resolution_clock::now(), *this,
prev_status, NodeStatus::IDLE);
}
}
NodeStatus TreeNode::status() const
{
const std::lock_guard lock(_p->state_mutex);
return _p->status;
}
NodeStatus TreeNode::waitValidStatus()
{
std::unique_lock lock(_p->state_mutex);
while(isHalted())
{
_p->state_condition_variable.wait(lock);
}
return _p->status;
}
const std::string& TreeNode::name() const
{
return _p->name;
}
bool TreeNode::isHalted() const
{
return _p->status == NodeStatus::IDLE;
}
TreeNode::StatusChangeSubscriber
TreeNode::subscribeToStatusChange(TreeNode::StatusChangeCallback callback)
{
return _p->state_change_signal.subscribe(std::move(callback));
}
void TreeNode::setPreTickFunction(PreTickCallback callback)
{
const std::unique_lock lk(_p->callback_injection_mutex);
_p->pre_tick_callback = std::move(callback);
}
void TreeNode::setPostTickFunction(PostTickCallback callback)
{
const std::unique_lock lk(_p->callback_injection_mutex);
_p->post_tick_callback = std::move(callback);
}
void TreeNode::setTickMonitorCallback(TickMonitorCallback callback)
{
const std::unique_lock lk(_p->callback_injection_mutex);
_p->tick_monitor_callback = std::move(callback);
}
uint16_t TreeNode::UID() const
{
return _p->config.uid;
}
const std::string& TreeNode::fullPath() const
{
return _p->config.path;
}
const std::string& TreeNode::registrationName() const
{
return _p->registration_ID;
}
const NodeConfig& TreeNode::config() const
{
return _p->config;
}
NodeConfig& TreeNode::config()
{
return _p->config;
}
StringView TreeNode::getRawPortValue(const std::string& key) const
{
auto remap_it = _p->config.input_ports.find(key);
if(remap_it == _p->config.input_ports.end())
{
remap_it = _p->config.output_ports.find(key);
if(remap_it == _p->config.output_ports.end())
{
throw std::logic_error(StrCat("[", key, "] not found"));
}
}
return remap_it->second;
}
bool TreeNode::isBlackboardPointer(StringView str, StringView* stripped_pointer)
{
if(str.size() < 3)
{
return false;
}
// strip leading and following spaces
size_t front_index = 0;
size_t last_index = str.size() - 1;
while(front_index wake_up)
{
_p->wake_up->emitSignal();
}
}
bool TreeNode::requiresWakeUp() const
{
return bool(_p->wake_up);
}
void TreeNode::setRegistrationID(StringView ID)
{
_p->registration_ID.assign(ID.data(), ID.size());
}
void TreeNode::setWakeUpInstance(std::shared_ptr instance)
{
_p->wake_up = instance;
}
void TreeNode::modifyPortsRemapping(const PortsRemapping& new_remapping)
{
for(const auto& new_it : new_remapping)
{
auto it = _p->config.input_ports.find(new_it.first);
if(it != _p->config.input_ports.end())
{
it->second = new_it.second;
}
it = _p->config.output_ports.find(new_it.first);
if(it != _p->config.output_ports.end())
{
it->second = new_it.second;
}
}
}
template
std::string toStr(const PreCond& cond)
{
if(cond < PreCond::COUNT_)
{
return BT::PreCondNames[static_cast(cond)];
}
return "Undefined";
}
template
std::string toStr(const PostCond& cond)
{
if(cond < BT::PostCond::COUNT_)
{
return BT::PostCondNames[static_cast(cond)];
}
return "Undefined";
}
AnyPtrLocked BT::TreeNode::getLockedPortContent(const std::string& key)
{
if(auto remapped_key = getRemappedKey(key, getRawPortValue(key)))
{
const auto bb_key = std::string(*remapped_key);
auto result = _p->config.blackboard->getAnyLocked(bb_key);
if(!result && _p->config.manifest != nullptr)
{
// Entry doesn't exist yet. Create it using the port's type info
// from the manifest so that getLockedPortContent works even when
// the port is not explicitly declared in XML. Issue #942.
auto port_it = _p->config.manifest->ports.find(key);
if(port_it != _p->config.manifest->ports.end())
{
_p->config.blackboard->createEntry(bb_key, port_it->second);
result = _p->config.blackboard->getAnyLocked(bb_key);
}
}
return result;
}
return {};
}
} // namespace BT