-
Notifications
You must be signed in to change notification settings - Fork 682
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
d8c5436
commit 0a08f78
Showing
4 changed files
with
129 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,45 @@ | ||
#pragma once | ||
|
||
#include "behaviortree_cpp/behavior_tree.h" | ||
#include "behaviortree_cpp/json_export.h" | ||
|
||
// my custom type | ||
struct Vector4D | ||
{ | ||
double w; | ||
double x; | ||
double y; | ||
double z; | ||
}; | ||
|
||
// add this just in case, if it is necessary to register it with | ||
// Groot2 publisher. | ||
// You will need to add `RegisterJsonDefinition<Vector4D>(ToJson);` in you main | ||
inline void ToJson(nlohmann::json& dest, const Vector4D& pose) | ||
{ | ||
dest["w"] = pose.w; | ||
dest["x"] = pose.x; | ||
dest["y"] = pose.y; | ||
dest["z"] = pose.z; | ||
} | ||
|
||
namespace BT { | ||
|
||
template <> inline | ||
Vector4D convertFromString(StringView key) | ||
{ | ||
const auto parts = BT::splitString(key, ','); | ||
if (parts.size() != 4) | ||
{ | ||
throw BT::RuntimeError("invalid input)"); | ||
} | ||
|
||
Vector4D output; | ||
output.w = convertFromString<double>(parts[0]); | ||
output.x = convertFromString<double>(parts[1]); | ||
output.y = convertFromString<double>(parts[2]); | ||
output.z = convertFromString<double>(parts[3]); | ||
return output; | ||
} | ||
|
||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,35 @@ | ||
#include "t13_custom_type.hpp" | ||
#include "behaviortree_cpp/bt_factory.h" | ||
|
||
class ShowVector : public BT::SyncActionNode | ||
{ | ||
public: | ||
ShowVector(const std::string& name, const BT::NodeConfig& config) | ||
: BT::SyncActionNode(name, config) | ||
{ | ||
} | ||
|
||
BT::NodeStatus tick() override | ||
{ | ||
const auto v = getInput<Vector4D>("value").value(); | ||
printf("x:%f y:%f z:%f w:%f\n", v.x, v.y, v.z, v.w); | ||
return BT::NodeStatus::SUCCESS; | ||
} | ||
|
||
// It is mandatory to define this static method. | ||
static BT::PortsList providedPorts() | ||
{ | ||
return{ BT::InputPort<Vector4D>("value") }; | ||
} | ||
}; | ||
|
||
// Function used to register ShowVector automatically, when | ||
// loading the plugin. | ||
// Remember that is mandatory to add to toy CMakeLists.txtx file | ||
// this: | ||
// target_compile_definitions(<target_name> PRIVATE BT_PLUGIN_EXPORT) | ||
// | ||
BT_REGISTER_NODES(factory) | ||
{ | ||
factory.registerNodeType<ShowVector>("ShowVector"); | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,37 @@ | ||
#include "behaviortree_cpp/bt_factory.h" | ||
#include "t13_custom_type.hpp" | ||
|
||
static const char* xml_text = R"( | ||
// clang-format off | ||
<root BTCPP_format="4" main_tree_to_execute="MainTree" > | ||
<BehaviorTree ID="MainTree"> | ||
<Sequence> | ||
<Script code="vect:='1,2,3,4'"/> | ||
<ShowVector value="{vect}"/>; | ||
<SubTree ID="MySub" v4="{vect}"/> | ||
</Sequence> | ||
</BehaviorTree> | ||
<BehaviorTree ID="MySub"> | ||
<ShowVector value="{v4}"/>; | ||
</BehaviorTree> | ||
</root> | ||
)"; | ||
// clang-format on | ||
|
||
int main(int argc, char** argv) | ||
{ | ||
using namespace BT; | ||
BehaviorTreeFactory factory; | ||
factory.registerFromPlugin("t13_plugin_action.so"); | ||
|
||
// Not mandatory, since we don't have a Groot2 publisher | ||
RegisterJsonDefinition<Vector4D>(ToJson); | ||
|
||
auto tree = factory.createTreeFromText(xml_text); | ||
tree.tickWhileRunning(); | ||
|
||
return 0; | ||
} | ||
|