Commit 133e53ed authored by Max  Bauer's avatar Max Bauer
Browse files

Merge branch 'bosch-transfer/22-01-14-formats-and-improvements' into 'master'

bosch-transfer/22-01-14-formats-and-improvements

See merge request !6
parents 67582db5 5788e1a6
---
Language: Cpp
BasedOnStyle: Google
ColumnLimit: 120
BinPackParameters: false
DerivePointerAlignment: false
PointerAlignment: Left
IndentPPDirectives: AfterHash
MaxEmptyLinesToKeep: 2
......@@ -13,22 +13,19 @@
#include "agnostic_behavior_tree/behavior_node.h"
namespace yase
{
namespace yase {
// The base for all action nodes
class ActionNode : public BehaviorNode
{
public:
explicit ActionNode(const std::string &name)
: ActionNode(name, nullptr){};
// The base for all action nodes
class ActionNode : public BehaviorNode {
public:
explicit ActionNode(const std::string& name) : ActionNode(name, nullptr){};
ActionNode(const std::string &name, Extension::Ptr extension_ptr)
: BehaviorNode(std::string("Action::").append(name), std::move(extension_ptr)){};
ActionNode(const std::string& name, Extension::Ptr extension_ptr)
: BehaviorNode(std::string("Action::").append(name), std::move(extension_ptr)){};
virtual ~ActionNode() override = default;
};
virtual ~ActionNode() override = default;
};
} // namespace yase
} // namespace yase
#endif // AGNOSTIC_BEHAVIOR_TREE_ACTION_NODE_H
#endif // AGNOSTIC_BEHAVIOR_TREE_ACTION_NODE_H
......@@ -11,139 +11,110 @@
#ifndef AGNOSTIC_BEHAVIOR_TREE_ACTIONS_ANALYSE_NODES_H
#define AGNOSTIC_BEHAVIOR_TREE_ACTIONS_ANALYSE_NODES_H
#include <string>
#include <iostream>
#include <agnostic_behavior_tree/action_node.h>
namespace yase
{
// Returns N Times until it returns preffered NodeStatus. Tracks the method accesses
class AnalyseNode : public ActionNode
{
public:
explicit AnalyseNode(const size_t max_repeat_ticks,
const NodeStatus return_status = NodeStatus::kSuccess)
: AnalyseNode("", max_repeat_ticks, return_status, nullptr){};
AnalyseNode(const std::string name,
const size_t max_repeat_ticks,
const NodeStatus return_status,
Extension::Ptr extension_ptr)
: ActionNode(std::string("AnalyseNode").append(name), std::move(extension_ptr)),
m_max_repeat_ticks(max_repeat_ticks),
m_return_status(return_status){};
virtual ~AnalyseNode() override = default;
bool isInitialised() const
{
return m_initialised;
}
size_t overallTicks() const
{
return m_overall_ticks;
}
size_t ticksSinceInit() const
{
return m_ticks_since_init;
}
size_t onInitCalls() const
{
return m_on_init_calls;
}
size_t onTerminateCalls() const
{
return m_on_terminate_calls;
}
protected:
NodeStatus tick() final
{
m_overall_ticks++;
m_ticks_since_init++;
if (m_repeat_counter < m_max_repeat_ticks)
{
m_repeat_counter++;
executionInfo(std::string("Ticked ")
.append(std::to_string(m_repeat_counter))
.append(" of ")
.append(std::to_string(m_max_repeat_ticks))
.append(" times"));
return NodeStatus::kRunning;
}
return m_return_status;
};
void onInit() override
{
m_repeat_counter = 0;
m_initialised = true;
m_ticks_since_init = 0;
m_on_init_calls++;
}
#include <iostream>
#include <string>
void onTerminate() override
{
m_initialised = false;
m_on_terminate_calls++;
namespace yase {
// Returns N Times until it returns preffered NodeStatus. Tracks the method accesses
class AnalyseNode : public ActionNode {
public:
explicit AnalyseNode(const size_t max_repeat_ticks,
const NodeStatus return_status = NodeStatus::kSuccess,
Extension::Ptr extension_ptr = nullptr)
: AnalyseNode("", max_repeat_ticks, return_status, extension_ptr){};
AnalyseNode(const std::string name,
const size_t max_repeat_ticks,
const NodeStatus return_status = NodeStatus::kSuccess,
Extension::Ptr extension_ptr = nullptr)
: ActionNode(std::string("AnalyseNode").append(name), std::move(extension_ptr)),
m_max_repeat_ticks(max_repeat_ticks),
m_return_status(return_status){};
virtual ~AnalyseNode() override = default;
bool isInitialised() const { return m_initialised; }
size_t overallTicks() const { return m_overall_ticks; }
size_t ticksSinceInit() const { return m_ticks_since_init; }
size_t onInitCalls() const { return m_on_init_calls; }
size_t onTerminateCalls() const { return m_on_terminate_calls; }
protected:
NodeStatus tick() final {
m_overall_ticks++;
m_ticks_since_init++;
if (m_repeat_counter < m_max_repeat_ticks) {
m_repeat_counter++;
executionInfo(std::string("Ticked ")
.append(std::to_string(m_repeat_counter))
.append(" of ")
.append(std::to_string(m_max_repeat_ticks))
.append(" times"));
return NodeStatus::kRunning;
}
private:
const size_t m_max_repeat_ticks;
const NodeStatus m_return_status;
// Variables to be reset at onInit
bool m_initialised{false};
size_t m_repeat_counter{0};
// Variables which will not be reset:
size_t m_overall_ticks{0};
size_t m_on_init_calls{0};
size_t m_ticks_since_init{0};
size_t m_on_terminate_calls{0};
};
// Returns always a Running
class AlwaysRunning : public AnalyseNode
{
public:
explicit AlwaysRunning() : AlwaysRunning(nullptr){};
AlwaysRunning(Extension::Ptr extension_ptr)
: AnalyseNode("AlwaysRunning", 0, NodeStatus::kRunning, std::move(extension_ptr)){};
virtual ~AlwaysRunning() override = default;
};
// Returns always a Success
class AlwaysSuccess : public AnalyseNode
{
public:
AlwaysSuccess() : AlwaysSuccess(nullptr){};
AlwaysSuccess(Extension::Ptr extension_ptr)
: AnalyseNode("AlwaysSuccess", 0, NodeStatus::kSuccess, std::move(extension_ptr)){};
~AlwaysSuccess() override = default;
};
// Returns always a Failure
class AlwaysFailure : public AnalyseNode
{
public:
AlwaysFailure() : AlwaysFailure(nullptr){};
AlwaysFailure(Extension::Ptr extension_ptr)
: AnalyseNode("AlwaysFailure", 0, NodeStatus::kFailure, std::move(extension_ptr)){};
~AlwaysFailure() override = default;
return m_return_status;
};
} // namespace yase
#endif // AGNOSTIC_BEHAVIOR_TREE_ACTIONS_ANALYSE_NODES_H
void onInit() override {
m_repeat_counter = 0;
m_initialised = true;
m_ticks_since_init = 0;
m_on_init_calls++;
}
void onTerminate() override {
m_initialised = false;
m_on_terminate_calls++;
}
private:
const size_t m_max_repeat_ticks;
const NodeStatus m_return_status;
// Variables to be reset at onInit
bool m_initialised{false};
size_t m_repeat_counter{0};
// Variables which will not be reset:
size_t m_overall_ticks{0};
size_t m_on_init_calls{0};
size_t m_ticks_since_init{0};
size_t m_on_terminate_calls{0};
};
// Returns always a Running
class AlwaysRunning : public AnalyseNode {
public:
AlwaysRunning(Extension::Ptr extension_ptr = nullptr)
: AnalyseNode("AlwaysRunning", 0, NodeStatus::kRunning, std::move(extension_ptr)){};
virtual ~AlwaysRunning() override = default;
};
// Returns always a Success
class AlwaysSuccess : public AnalyseNode {
public:
AlwaysSuccess(Extension::Ptr extension_ptr = nullptr)
: AnalyseNode("AlwaysSuccess", 0, NodeStatus::kSuccess, std::move(extension_ptr)){};
~AlwaysSuccess() override = default;
};
// Returns always a Failure
class AlwaysFailure : public AnalyseNode {
public:
AlwaysFailure(Extension::Ptr extension_ptr = nullptr)
: AnalyseNode("AlwaysFailure", 0, NodeStatus::kFailure, std::move(extension_ptr)){};
~AlwaysFailure() override = default;
};
} // namespace yase
#endif // AGNOSTIC_BEHAVIOR_TREE_ACTIONS_ANALYSE_NODES_H
......@@ -15,35 +15,31 @@
#include <functional>
namespace yase
{
// A simple action node which allows to express logic via a lambda.
// This node eases to execute generic logic with a Functor.
class FunctorActionNode : public ActionNode
{
public:
using TickFunctor = std::function<NodeStatus()>;
namespace yase {
// A simple action node which allows to express logic via a lambda.
// This node eases to execute generic logic with a Functor.
class FunctorActionNode : public ActionNode {
public:
using TickFunctor = std::function<NodeStatus()>;
FunctorActionNode(const std::string &name,
FunctorActionNode::TickFunctor tick_functor)
: ActionNode(name), m_tick_functor(std::move(tick_functor)){};
FunctorActionNode(const std::string& name,
FunctorActionNode::TickFunctor tick_functor,
Extension::Ptr extension_ptr = nullptr)
: ActionNode(name, extension_ptr), m_tick_functor(std::move(tick_functor)){};
explicit FunctorActionNode(FunctorActionNode::TickFunctor tick_functor)
: FunctorActionNode("UnnamedFunctorAction", std::move(tick_functor)){};
explicit FunctorActionNode(FunctorActionNode::TickFunctor tick_functor, Extension::Ptr extension_ptr = nullptr)
: FunctorActionNode("UnnamedFunctorAction", std::move(tick_functor), extension_ptr){};
virtual ~FunctorActionNode() override = default;
virtual ~FunctorActionNode() override = default;
void onInit() override{};
void onInit() override{};
protected:
NodeStatus tick() final
{
return m_tick_functor();
};
protected:
NodeStatus tick() final { return m_tick_functor(); };
TickFunctor m_tick_functor;
};
TickFunctor m_tick_functor;
};
} // namespace yase
} // namespace yase
#endif // AGNOSTIC_BEHAVIOR_TREE_ACTIONS_FUNCTOR_ACTION_NODE_H
#endif // AGNOSTIC_BEHAVIOR_TREE_ACTIONS_FUNCTOR_ACTION_NODE_H
......@@ -13,196 +13,175 @@
#include "agnostic_behavior_tree/scoped_blackboard.h"
#include <string>
#include <exception>
#include <string>
namespace yase
{
namespace yase {
// Execution states of a node
enum class NodeStatus
{
kIdle = 0,
kRunning,
kSuccess,
kFailure
};
// Execution states of a node
enum class NodeStatus { kIdle = 0, kRunning, kSuccess, kFailure };
// Convert the status into an ouput string
std::string toStr(NodeStatus status, bool use_ansi_escape_code = false);
// Convert the status into an ouput string
std::string toStr(NodeStatus status, bool use_ansi_escape_code = false);
// Simulator specific Extensions can be inherited and composed to behavior nodes
class Extension
{
public:
Extension() = default;
virtual ~Extension() = default;
// Simulator specific Extensions can be inherited and composed to behavior nodes
class Extension {
public:
Extension() = default;
virtual ~Extension() = default;
using Ptr = std::shared_ptr<Extension>;
};
using Ptr = std::shared_ptr<Extension>;
};
// Abstract base class for Behavior Tree Nodes.
class BehaviorNode
{
public:
virtual ~BehaviorNode()
{
m_blackboard = nullptr;
};
using Ptr = std::shared_ptr<BehaviorNode>;
// Method to invoke tick from the outside while ensuring the consistency of the node status and tick cycle tracking
NodeStatus executeTick()
{
// Track current tick cycle
if (m_parent_node != nullptr)
{
if (m_tick_cycle >= m_parent_node->m_tick_cycle)
{
std::string error_msg = "Error while executing tick() of behavior node [";
error_msg.append(name());
error_msg.append("]: Tick cycle [").append(std::to_string(m_tick_cycle)).append("] of this node is greater/equal then its parent tick cycle [");
error_msg.append(std::to_string(m_parent_node->m_tick_cycle)).append("] - A child is not allowed to proceed faster then its parent");
throw std::logic_error(error_msg);
}
m_tick_cycle = m_parent_node->m_tick_cycle;
}
else
{
(m_tick_cycle)++;
}
// Clear execution info before step
m_execution_info.clear();
m_status = tick();
if (m_status == NodeStatus::kIdle)
{
// Abstract base class for Behavior Tree Nodes.
class BehaviorNode {
public:
virtual ~BehaviorNode() { m_blackboard = nullptr; };
using Ptr = std::shared_ptr<BehaviorNode>;
// Method to invoke tick from the outside while ensuring the consistency of the node status and tick cycle tracking
NodeStatus executeTick() {
// Track current tick cycle
if (m_parent_node != nullptr) {
if (m_tick_cycle >= m_parent_node->m_tick_cycle) {
std::string error_msg = "Error while executing tick() of behavior node [";
error_msg.append(name());
error_msg.append("]: Returned invalid status NodeStatus::kIdle!");
error_msg.append("]: Tick cycle [")
.append(std::to_string(m_tick_cycle))
.append("] of this node is greater/equal then its parent tick cycle [");
error_msg.append(std::to_string(m_parent_node->m_tick_cycle))
.append("] - A child is not allowed to proceed faster then its parent");
throw std::logic_error(error_msg);
}
return status();
};
m_tick_cycle = m_parent_node->m_tick_cycle;
} else {
(m_tick_cycle)++;
}
// Clear execution info before step
m_execution_info.clear();
m_status = tick();
if (m_status == NodeStatus::kIdle) {
std::string error_msg = "Error while executing tick() of behavior node [";
error_msg.append(name());
error_msg.append("]: Returned invalid status NodeStatus::kIdle!");
throw std::logic_error(error_msg);
}
return status();
};
// Called once right before behavior is ticked the first time.
// THE CALL MUST RESET THE BEHAVIOR with all its necessary values!
virtual void onInit() = 0;
// Called once right before behavior is ticked the first time.
// THE CALL MUST RESET THE BEHAVIOR with all its necessary values!
virtual void onInit() = 0;
// Called after behavior is called the last time
virtual void onTerminate(){};
// Called after behavior is called the last time
virtual void onTerminate(){};
// Get node status
NodeStatus status() const { return m_status; };
// Get node status
NodeStatus status() const { return m_status; };
// Get detailed execution info
std::string executionInfo() const { return m_execution_info; };
// Get detailed execution info
std::string executionInfo() const { return m_execution_info; };
// Get tree node name
const std::string &name() const { return m_name; };
// Get tree node name
const std::string& name() const { return m_name; };
// Get BlackboardBase to examine symbols
BlackboardBase::Ptr blackboardBase() const { return m_blackboard; };
// Get BlackboardBase to examine symbols
BlackboardBase::Ptr blackboardBase() const { return m_blackboard; };
// Init the sub tree (node with sub nodes)
virtual void initTree()
{
m_blackboard->clearLocal();
declareSymbolsInBlackBoard(*(m_blackboard));
lookUpSymbolsInBlackBoard(*(m_blackboard));
};
// Init the sub tree (node with sub nodes)
virtual void initTree() {
m_blackboard->clearLocal();
declareSymbolsInBlackBoard(*(m_blackboard));
lookUpSymbolsInBlackBoard(*(m_blackboard));
};
// Get current tick cycle of node
unsigned int tickCycle() const
{
return m_tick_cycle;
}
// Get current tick cycle of node
unsigned int tickCycle() const { return m_tick_cycle; }
// Checks if node was ticked in current tick cycle
bool tickedInCurrentCycle() const
{
if (m_parent_node == nullptr)
{
return true;
}
if (m_parent_node->tickedInCurrentCycle())
{
return (m_parent_node->tickCycle() == m_tick_cycle);
}
return false;
// Checks if node was ticked in current tick cycle
bool tickedInCurrentCycle() const {
if (m_parent_node == nullptr) {
return true;
}
// Check if it is safe to access the extension
bool extensionExists() const
{
return (m_extension != nullptr);
if (m_parent_node->tickedInCurrentCycle()) {
return (m_parent_node->tickCycle() == m_tick_cycle);
}
return false;
}
// Check if it is safe to access the extension
bool extensionExists() const { return (m_extension != nullptr); }
// Get Extension, will throw if null
Extension::Ptr extension() const {
if (!extensionExists()) {
std::string exception_msg = "Trying to access the extension in node [";
exception_msg.append(name());
exception_msg.append("] which is NULL.");
throw std::runtime_error(exception_msg);
}
return m_extension;
};
// Get Extension, will throw if null
Extension::Ptr extension() const
{
if (!extensionExists())