diff --git a/.gitignore b/.gitignore index 9d5bd4326..086b4251c 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ build* site/* /.vscode/ .vs/ +__pycache__ # clangd cache /.cache/* diff --git a/CMakeLists.txt b/CMakeLists.txt index e69c9e96c..491cd863c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ option(BTCPP_EXAMPLES "Build tutorials and examples" ON) option(BUILD_TESTING "Build the unit tests" ON) option(BTCPP_GROOT_INTERFACE "Add Groot2 connection. Requires ZeroMQ" ON) option(BTCPP_SQLITE_LOGGING "Add SQLite logging." ON) +option(BTCPP_PYTHON "Add Python bindings" ON) option(USE_V3_COMPATIBLE_NAMES "Use some alias to compile more easily old 3.x code" OFF) option(ENABLE_FUZZING "Enable fuzzing builds" OFF) @@ -154,6 +155,10 @@ if(BTCPP_SQLITE_LOGGING) list(APPEND BT_SOURCE src/loggers/bt_sqlite_logger.cpp ) endif() +if(BTCPP_PYTHON) + list(APPEND BT_SOURCE src/python/types.cpp) +endif() + ###################################################### if (UNIX) @@ -185,6 +190,19 @@ target_link_libraries(${BTCPP_LIBRARY} ${BTCPP_EXTRA_LIBRARIES} ) +if(BTCPP_PYTHON) + find_package(Python COMPONENTS Interpreter Development) + find_package(pybind11 CONFIG) + message("PYTHON_EXECUTABLE: ${Python_EXECUTABLE}") + + pybind11_add_module(btpy_cpp src/python/bindings.cpp) + target_compile_options(btpy_cpp PRIVATE -Wno-gnu-zero-variadic-macro-arguments) + target_link_libraries(btpy_cpp PRIVATE ${BTCPP_LIBRARY}) + + target_link_libraries(${BTCPP_LIBRARY} PUBLIC Python::Python pybind11::pybind11) + target_compile_definitions(${BTCPP_LIBRARY} PUBLIC BTCPP_PYTHON) +endif() + target_include_directories(${BTCPP_LIBRARY} PUBLIC $ diff --git a/btpy/__init__.py b/btpy/__init__.py new file mode 100644 index 000000000..e664741f1 --- /dev/null +++ b/btpy/__init__.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 + +""" +Top-level module of the BehaviorTree.CPP Python bindings. +""" + +# re-export +from btpy_cpp import ( + BehaviorTreeFactory, + NodeStatus, + StatefulActionNode, + SyncActionNode, + Tree, +) + + +def ports(inputs: list[str] = [], outputs: list[str] = []): + """Decorator to specify input and outputs ports for an action node.""" + + def specify_ports(cls): + cls.input_ports = list(inputs) + cls.output_ports = list(outputs) + return cls + + return specify_ports + + +class AsyncActionNode(StatefulActionNode): + """An abstract action node implemented via cooperative multitasking. + + Subclasses must implement the `run()` method as a generator. Optionally, + this method can return a final `NodeStatus` value to indicate its exit + condition. + + Optionally, subclasses can override the `on_halted()` method which is called + when the tree halts. The default implementation does nothing. The `run()` + method will never be called again after a halt. + + Note: + It is the responsibility of the action author to not block the main + behavior tree loop with long-running tasks. `yield` calls should be + placed whenever a pause is appropriate. + """ + + def __init__(self, name, config): + super().__init__(name, config) + + def on_start(self) -> NodeStatus: + self.coroutine = self.run() + return NodeStatus.RUNNING + + def on_running(self) -> NodeStatus: + # The library logic should never allow this to happen, but users can + # still manually call `on_running` without an associated `on_start` + # call. Make sure to print a useful error when this happens. + if self.coroutine is None: + raise "AsyncActionNode run without starting" + + # Resume the coroutine (generator). As long as the generator is not + # exhausted, keep this action in the RUNNING state. + try: + next(self.coroutine) + return NodeStatus.RUNNING + except StopIteration as e: + # If the action returns a status then propagate it upwards. + if e.value is not None: + return e.value + # Otherwise, just assume the action finished successfully. + else: + return NodeStatus.SUCCESS + + def on_halted(self): + # Default action: do nothing + pass + + +# Specify the symbols to be imported with `from btpy import *`, as described in +# [1]. +# +# [1]: https://docs.python.org/3/tutorial/modules.html#importing-from-a-package +__all__ = [ + "ports", + "AsyncActionNode", + "BehaviorTreeFactory", + "NodeStatus", + "StatefulActionNode", + "SyncActionNode", + "Tree", +] diff --git a/conanfile.txt b/conanfile.txt index 7b81d1d6d..bf225e730 100644 --- a/conanfile.txt +++ b/conanfile.txt @@ -2,6 +2,7 @@ gtest/1.14.0 zeromq/4.3.4 sqlite3/3.40.1 +pybind11/2.10.4 [generators] CMakeDeps diff --git a/include/behaviortree_cpp/contrib/pybind11_json.hpp b/include/behaviortree_cpp/contrib/pybind11_json.hpp new file mode 100644 index 000000000..041b930d2 --- /dev/null +++ b/include/behaviortree_cpp/contrib/pybind11_json.hpp @@ -0,0 +1,232 @@ +/*************************************************************************** +* Copyright (c) 2019, Martin Renou * +* * +* Distributed under the terms of the BSD 3-Clause License. * +* * +* The full license is in the file LICENSE, distributed with this software. * +****************************************************************************/ + +#ifndef PYBIND11_JSON_HPP +#define PYBIND11_JSON_HPP + +#include +#include +#include +#include + +#include "behaviortree_cpp/contrib/json.hpp" + +#include "pybind11/pybind11.h" + +namespace pyjson +{ + namespace py = pybind11; + namespace nl = nlohmann; + + inline py::object from_json(const nl::json& j) + { + if (j.is_null()) + { + return py::none(); + } + else if (j.is_boolean()) + { + return py::bool_(j.get()); + } + else if (j.is_number_unsigned()) + { + return py::int_(j.get()); + } + else if (j.is_number_integer()) + { + return py::int_(j.get()); + } + else if (j.is_number_float()) + { + return py::float_(j.get()); + } + else if (j.is_string()) + { + return py::str(j.get()); + } + else if (j.is_array()) + { + py::list obj(j.size()); + for (std::size_t i = 0; i < j.size(); i++) + { + obj[i] = from_json(j[i]); + } + return obj; + } + else // Object + { + py::dict obj; + for (nl::json::const_iterator it = j.cbegin(); it != j.cend(); ++it) + { + obj[py::str(it.key())] = from_json(it.value()); + } + return obj; + } + } + + inline nl::json to_json(const py::handle& obj) + { + if (obj.ptr() == nullptr || obj.is_none()) + { + return nullptr; + } + if (py::isinstance(obj)) + { + return obj.cast(); + } + if (py::isinstance(obj)) + { + try + { + nl::json::number_integer_t s = obj.cast(); + if (py::int_(s).equal(obj)) + { + return s; + } + } + catch (...) + { + } + try + { + nl::json::number_unsigned_t u = obj.cast(); + if (py::int_(u).equal(obj)) + { + return u; + } + } + catch (...) + { + } + throw std::runtime_error("to_json received an integer out of range for both nl::json::number_integer_t and nl::json::number_unsigned_t type: " + py::repr(obj).cast()); + } + if (py::isinstance(obj)) + { + return obj.cast(); + } + if (py::isinstance(obj)) + { + py::module base64 = py::module::import("base64"); + return base64.attr("b64encode")(obj).attr("decode")("utf-8").cast(); + } + if (py::isinstance(obj)) + { + return obj.cast(); + } + if (py::isinstance(obj) || py::isinstance(obj)) + { + auto out = nl::json::array(); + for (const py::handle value : obj) + { + out.push_back(to_json(value)); + } + return out; + } + if (py::isinstance(obj)) + { + auto out = nl::json::object(); + for (const py::handle key : obj) + { + out[py::str(key).cast()] = to_json(obj[key]); + } + return out; + } + if (py::isinstance(obj)) + { + return obj.cast(); + } + throw std::runtime_error("to_json not implemented for this type of object: " + py::repr(obj).cast()); + } +} + +// nlohmann_json serializers +namespace nlohmann +{ + namespace py = pybind11; + + #define MAKE_NLJSON_SERIALIZER_DESERIALIZER(T) \ + template <> \ + struct adl_serializer \ + { \ + inline static void to_json(json& j, const T& obj) \ + { \ + j = pyjson::to_json(obj); \ + } \ + \ + inline static T from_json(const json& j) \ + { \ + return pyjson::from_json(j); \ + } \ + } + + #define MAKE_NLJSON_SERIALIZER_ONLY(T) \ + template <> \ + struct adl_serializer \ + { \ + inline static void to_json(json& j, const T& obj) \ + { \ + j = pyjson::to_json(obj); \ + } \ + } + + MAKE_NLJSON_SERIALIZER_DESERIALIZER(py::object); + + MAKE_NLJSON_SERIALIZER_DESERIALIZER(py::bool_); + MAKE_NLJSON_SERIALIZER_DESERIALIZER(py::int_); + MAKE_NLJSON_SERIALIZER_DESERIALIZER(py::float_); + MAKE_NLJSON_SERIALIZER_DESERIALIZER(py::str); + + MAKE_NLJSON_SERIALIZER_DESERIALIZER(py::list); + MAKE_NLJSON_SERIALIZER_DESERIALIZER(py::tuple); + MAKE_NLJSON_SERIALIZER_DESERIALIZER(py::dict); + + MAKE_NLJSON_SERIALIZER_ONLY(py::handle); + MAKE_NLJSON_SERIALIZER_ONLY(py::detail::item_accessor); + MAKE_NLJSON_SERIALIZER_ONLY(py::detail::list_accessor); + MAKE_NLJSON_SERIALIZER_ONLY(py::detail::tuple_accessor); + MAKE_NLJSON_SERIALIZER_ONLY(py::detail::sequence_accessor); + MAKE_NLJSON_SERIALIZER_ONLY(py::detail::str_attr_accessor); + MAKE_NLJSON_SERIALIZER_ONLY(py::detail::obj_attr_accessor); + + #undef MAKE_NLJSON_SERIALIZER + #undef MAKE_NLJSON_SERIALIZER_ONLY +} + +// pybind11 caster +namespace pybind11 +{ + namespace detail + { + template <> struct type_caster + { + public: + PYBIND11_TYPE_CASTER(nlohmann::json, _("json")); + + bool load(handle src, bool) + { + try + { + value = pyjson::to_json(src); + return true; + } + catch (...) + { + return false; + } + } + + static handle cast(nlohmann::json src, return_value_policy /* policy */, handle /* parent */) + { + object obj = pyjson::from_json(src); + return obj.release(); + } + }; + } +} + +#endif diff --git a/include/behaviortree_cpp/json_export.h b/include/behaviortree_cpp/json_export.h index 8369656d6..48ee7159d 100644 --- a/include/behaviortree_cpp/json_export.h +++ b/include/behaviortree_cpp/json_export.h @@ -1,12 +1,16 @@ #pragma once -#include "behaviortree_cpp/basic_types.h" #include "behaviortree_cpp/utils/safe_any.hpp" #include "behaviortree_cpp/basic_types.h" - // Use the version nlohmann::json embedded in BT.CPP #include "behaviortree_cpp/contrib/json.hpp" +#ifdef BTCPP_PYTHON +#include +#include +#include "behaviortree_cpp/contrib/pybind11_json.hpp" +#endif + namespace BT { @@ -45,7 +49,6 @@ namespace BT /** * Use RegisterJsonDefinition(); */ - class JsonExporter { public: @@ -120,7 +123,14 @@ class JsonExporter std::unordered_map from_json_converters_; std::unordered_map type_names_; }; - +#ifdef BTCPP_PYTHON +template <> +inline Expected +JsonExporter::fromJson(const nlohmann::json& source) const +{ + return pyjson::from_json(source); +} +#endif template inline Expected JsonExporter::fromJson(const nlohmann::json& source) const { diff --git a/include/behaviortree_cpp/python/types.h b/include/behaviortree_cpp/python/types.h new file mode 100644 index 000000000..7501cb758 --- /dev/null +++ b/include/behaviortree_cpp/python/types.h @@ -0,0 +1,38 @@ +#pragma once + +#include + +#include "behaviortree_cpp/json_export.h" + +#include "behaviortree_cpp/utils/safe_any.hpp" + +namespace BT +{ + +/** + * @brief Generic method to convert Python objects to type T via JSON. + * + * For this function to succeed, the type T must be convertible from JSON via + * the JsonExporter interface. + */ +template +bool fromPythonObject(const pybind11::object& obj, T& dest) +{ + auto dest_maybe = JsonExporter::get().fromJson(obj); + if(dest_maybe.has_value()) + { + dest = dest_maybe.value(); + return true; + } + return false; +} + +/** + * @brief Convert a BT::Any to a Python object via JSON. + * + * For this function to succeed, the type stored inside the Any must be + * convertible to JSON via the JsonExporter interface. + */ +bool toPythonObject(const BT::Any& val, pybind11::object& dest); + +} // namespace BT diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index 1dc73986d..3c4e12ea6 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -24,6 +24,13 @@ #include "behaviortree_cpp/utils/wakeup_signal.hpp" #include "behaviortree_cpp/scripting/script_parser.hpp" +#ifdef BTCPP_PYTHON +#include +#include + +#include "behaviortree_cpp/python/types.h" +#endif + #ifdef _MSC_VER #pragma warning(disable : 4127) #endif @@ -564,6 +571,24 @@ inline Expected TreeNode::getInputStamped(const std::string& key, { destination = parseString(any_value.cast()); } +#ifdef BTCPP_PYTHON + // py::object -> C++ + else if(any_value.type() == typeid(pybind11::object)) + { + if(!fromPythonObject(any_value.cast(), destination)) + { + return nonstd::make_unexpected("Cannot convert from Python object"); + } + } + // C++ -> py::object + else if constexpr(std::is_same_v) + { + if(!toPythonObject(any_value, destination)) + { + return nonstd::make_unexpected("Cannot convert to Python object"); + } + } +#endif else { destination = any_value.cast(); diff --git a/package.xml b/package.xml index 1d245b625..5eb0ed9ce 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,7 @@ libsqlite3-dev libzmq3-dev + pybind11-dev ament_cmake_gtest diff --git a/python_examples/README.md b/python_examples/README.md new file mode 100644 index 000000000..0c82171eb --- /dev/null +++ b/python_examples/README.md @@ -0,0 +1,3 @@ +1. Create a Python virtualenv in the root directory: `python3 -m venv venv && source venv/bin/activate` +2. Build and install the BehaviorTree Python package: `pip install -v .` +3. Run an example, e.g. `python3 python_examples/ex01_sample.py` diff --git a/python_examples/ex01_sample.py b/python_examples/ex01_sample.py new file mode 100644 index 000000000..e1967bc4f --- /dev/null +++ b/python_examples/ex01_sample.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 + +""" +Demo adapted from [btcpp_sample](https://github.com/BehaviorTree/btcpp_sample). +""" + +from btpy import BehaviorTreeFactory, SyncActionNode, NodeStatus, ports + + +xml_text = """ + + + + + + + + + + + + +""" + + +@ports(inputs=["message"]) +class SaySomething(SyncActionNode): + def tick(self): + msg = self.get_input("message") + print(msg) + return NodeStatus.SUCCESS + + +@ports(outputs=["text"]) +class ThinkWhatToSay(SyncActionNode): + def tick(self): + self.set_output("text", "The answer is 42") + return NodeStatus.SUCCESS + + +factory = BehaviorTreeFactory() +factory.register(SaySomething) +factory.register(ThinkWhatToSay) + +tree = factory.create_tree_from_text(xml_text) +tree.tick_while_running() diff --git a/python_examples/ex02_generic_data.py b/python_examples/ex02_generic_data.py new file mode 100644 index 000000000..5f13d4648 --- /dev/null +++ b/python_examples/ex02_generic_data.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 + +""" +Demonstration of passing generic data between nodes. +""" + +import numpy as np +from btpy import BehaviorTreeFactory, SyncActionNode, NodeStatus, ports + +xml_text = """ + + + + + + + + + + + + +""" + + +@ports(inputs=["position", "theta"], outputs=["out", "position"]) +class Rotate(SyncActionNode): + def tick(self): + # Build a rotation matrix which rotates points by `theta` degrees. + theta = np.deg2rad(self.get_input("theta")) + c, s = np.cos(theta), np.sin(theta) + M = np.array([[c, -s], [s, c]]) + + # Apply the rotation to the input position. + position = self.get_input("position") + rotated = M @ position + + # Set the output. + self.set_output("out", rotated) + self.set_output("position", position) + + return NodeStatus.SUCCESS + + +@ports(inputs=["position", "offset"], outputs=["out"]) +class Translate(SyncActionNode): + def tick(self): + offset = np.asarray(self.get_input("offset")) + + # Apply the translation to the input position. + position = np.asarray(self.get_input("position")) + translated = position + offset + + # Set the output. + self.set_output("out", translated) + + return NodeStatus.SUCCESS + + +@ports(inputs=["value"]) +class Print(SyncActionNode): + def tick(self): + value = self.get_input("value") + if value is not None: + print(value) + return NodeStatus.SUCCESS + + +factory = BehaviorTreeFactory() +factory.register(Rotate) +factory.register(Translate) +factory.register(Print) + +tree = factory.create_tree_from_text(xml_text) +tree.tick_while_running() diff --git a/python_examples/ex03_stateful_nodes.py b/python_examples/ex03_stateful_nodes.py new file mode 100644 index 000000000..aadca07f0 --- /dev/null +++ b/python_examples/ex03_stateful_nodes.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 + +""" +Demonstration of stateful action nodes. +""" + +import numpy as np +from btpy import ( + BehaviorTreeFactory, + StatefulActionNode, + SyncActionNode, + NodeStatus, + ports, +) + + +xml_text = """ + + + + + + + + + + + +""" + + +@ports(inputs=["x0", "x1"], outputs=["out"]) +class Interpolate(StatefulActionNode): + def on_start(self): + self.t = 0.0 + self.x0 = np.asarray(self.get_input("x0")) + self.x1 = np.asarray(self.get_input("x1")) + return NodeStatus.RUNNING + + def on_running(self): + if self.t < 1.0: + x = (1.0 - self.t) * self.x0 + self.t * self.x1 + self.set_output("out", x) + self.t += 0.1 + return NodeStatus.RUNNING + else: + return NodeStatus.SUCCESS + + def on_halted(self): + pass + + +@ports(inputs=["value"]) +class Print(SyncActionNode): + def tick(self): + value = self.get_input("value") + if value is not None: + print(value) + return NodeStatus.SUCCESS + + +factory = BehaviorTreeFactory() +factory.register(Interpolate) +factory.register(Print) + +tree = factory.create_tree_from_text(xml_text) +tree.tick_while_running() diff --git a/python_examples/ex04_ros_interop.py b/python_examples/ex04_ros_interop.py new file mode 100644 index 000000000..966f60d3f --- /dev/null +++ b/python_examples/ex04_ros_interop.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 + +""" +Demonstrates interop of BehaviorTree.CPP Python bindings and ROS2 via rclpy. + +You can publish the transform expected in the tree below using this command: + + ros2 run tf2_ros static_transform_publisher \ + --frame-id odom --child-frame-id base_link \ + --x 1.0 --y 2.0 +""" + +import rclpy +from rclpy.node import Node +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +from btpy import ( + BehaviorTreeFactory, + StatefulActionNode, + SyncActionNode, + NodeStatus, + ports, +) + + +xml_text = """ + + + + + + + + + + +""" + + +@ports(inputs=["frame_id", "child_frame_id"], outputs=["tf"]) +class GetRosTransform(StatefulActionNode): + def __init__(self, name, config, node): + super().__init__(name, config) + + self.node = node + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self.node) + + def on_start(self): + return NodeStatus.RUNNING + + def on_running(self): + frame_id = self.get_input("frame_id") + child_frame_id = self.get_input("child_frame_id") + + time = self.node.get_clock().now() + if self.tf_buffer.can_transform(frame_id, child_frame_id, time): + tf = self.tf_buffer.lookup_transform(frame_id, child_frame_id, time) + self.set_output("tf", tf) + + return NodeStatus.RUNNING + + def on_halted(self): + pass + + +@ports(inputs=["value"]) +class Print(SyncActionNode): + def tick(self): + value = self.get_input("value") + if value is not None: + print(value) + return NodeStatus.SUCCESS + + +rclpy.init() +node = Node("ex04_ros_interop") + +factory = BehaviorTreeFactory() +factory.register(GetRosTransform, node) +factory.register(Print) + +tree = factory.create_tree_from_text(xml_text) + +node.create_timer(0.01, lambda: tree.tick_once()) +rclpy.spin(node) diff --git a/python_examples/ex05_type_interop.py b/python_examples/ex05_type_interop.py new file mode 100644 index 000000000..d8e82e3ee --- /dev/null +++ b/python_examples/ex05_type_interop.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 + +""" +Demo of seamless conversion between C++ and Python types. + +NOTE: To run this example, make sure that the path +`sample_nodes/bin/libdummy_nodes_dyn.so` is accessible from the current working +directory. After building the project, this path will exist in your CMake build +root. +""" + +from btpy import BehaviorTreeFactory, SyncActionNode, NodeStatus, ports + + +xml_text = """ + + + + + + + + + + + + + + + + + + + +""" + + +@ports(outputs=["output"]) +class PutVector(SyncActionNode): + def tick(self): + # Schema matching std::unordered_map + # (defined in dummy_nodes.h, input type of PrintMapOfVectors) + self.set_output( + "output", + { + "a": {"x": 0.0, "y": 42.0, "z": 9.0}, + "b": {"x": 1.0, "y": -2.0, "z": 1.0}, + }, + ) + return NodeStatus.SUCCESS + + +@ports(inputs=["value"]) +class Print(SyncActionNode): + def tick(self): + value = self.get_input("value") + if value is not None: + print("Python:", value) + return NodeStatus.SUCCESS + + +factory = BehaviorTreeFactory() +factory.register_from_plugin("build/sample_nodes/bin/libdummy_nodes_dyn.so") +factory.register(PutVector) +factory.register(Print) + +tree = factory.create_tree_from_text(xml_text) +tree.tick_while_running() diff --git a/python_examples/ex06_async_nodes.py b/python_examples/ex06_async_nodes.py new file mode 100644 index 000000000..cccebcd97 --- /dev/null +++ b/python_examples/ex06_async_nodes.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 + +""" +Demonstration of an asynchronous action node implemented conveniently as a +Python coroutine. This enables simple synchronous code to be written in place of +complex asynchronous state machines. +""" + +import time +import numpy as np +from btpy import ( + AsyncActionNode, + BehaviorTreeFactory, + SyncActionNode, + NodeStatus, + ports, +) + + +xml_text = """ + + + + + + + + + + +""" + + +@ports(inputs=["start", "goal"], outputs=["command"]) +class MyAsyncNode(AsyncActionNode): + def run(self): + start = np.asarray(self.get_input("start")) + goal = np.asarray(self.get_input("goal")) + + # Here we write an imperative-looking loop, but we place a `yield` call + # at each iteration. This causes the coroutine to yield back to the + # caller until the next iteration of the tree, rather than block the + # main thread. + t0 = time.time() + while (t := time.time() - t0) < 1.0: + command = (1.0 - t) * start + t * goal + self.set_output("command", command) + yield + + print("Trajectory finished!") + return NodeStatus.SUCCESS + + def on_halted(self): + print("Trajectory halted!") + + +@ports(inputs=["value"]) +class Print(SyncActionNode): + def tick(self): + value = self.get_input("value") + if value is not None: + print(value) + return NodeStatus.SUCCESS + + +factory = BehaviorTreeFactory() +factory.register(MyAsyncNode) +factory.register(Print) + +tree = factory.create_tree_from_text(xml_text) + +# Run for a bit, then halt early. +for i in range(0, 10): + tree.tick_once() +tree.halt_tree() diff --git a/sample_nodes/dummy_nodes.cpp b/sample_nodes/dummy_nodes.cpp index 1ce2ade8d..8406ff3e2 100644 --- a/sample_nodes/dummy_nodes.cpp +++ b/sample_nodes/dummy_nodes.cpp @@ -72,4 +72,16 @@ BT::NodeStatus SaySomethingSimple(BT::TreeNode& self) return BT::NodeStatus::SUCCESS; } +void to_json(nlohmann::json& j, const Vector3& p) +{ + j = nlohmann::json{ { "x", p.x }, { "y", p.y }, { "z", p.z } }; +} + +void from_json(const nlohmann::json& j, Vector3& p) +{ + j.at("x").get_to(p.x); + j.at("y").get_to(p.y); + j.at("z").get_to(p.z); +} + } // namespace DummyNodes diff --git a/sample_nodes/dummy_nodes.h b/sample_nodes/dummy_nodes.h index 6781eaba8..3703a3f03 100644 --- a/sample_nodes/dummy_nodes.h +++ b/sample_nodes/dummy_nodes.h @@ -3,6 +3,7 @@ #include "behaviortree_cpp/behavior_tree.h" #include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/json_export.h" namespace DummyNodes { @@ -119,6 +120,71 @@ class SleepNode : public BT::StatefulActionNode std::chrono::system_clock::time_point deadline_; }; +struct Vector3 +{ + float x; + float y; + float z; +}; + +void to_json(nlohmann::json& j, const Vector3& p); + +void from_json(const nlohmann::json& j, Vector3& p); + +class RandomVector : public BT::SyncActionNode +{ +public: + RandomVector(const std::string& name, const BT::NodeConfig& config) + : BT::SyncActionNode(name, config) + {} + + // You must override the virtual function tick() + NodeStatus tick() override + { + setOutput("vector", Vector3{ 1.0, 2.0, 3.0 }); + return BT::NodeStatus::SUCCESS; + } + + // It is mandatory to define this static method. + static BT::PortsList providedPorts() + { + return { BT::OutputPort("vector") }; + } +}; + +class PrintMapOfVectors : public BT::SyncActionNode +{ +public: + PrintMapOfVectors(const std::string& name, const BT::NodeConfig& config) + : BT::SyncActionNode(name, config) + {} + + // You must override the virtual function tick() + NodeStatus tick() override + { + auto input = getInput>("input"); + if(input.has_value()) + { + std::cerr << "{"; + for(const auto& [key, value] : *input) + { + std::cerr << key << ": (" << value.x << ", " << value.y << ", " << value.z + << "), "; + } + std::cerr << "}" << std::endl; + ; + } + + return BT::NodeStatus::SUCCESS; + } + + // It is mandatory to define this static method. + static BT::PortsList providedPorts() + { + return { BT::InputPort>("input") }; + } +}; + inline void RegisterNodes(BT::BehaviorTreeFactory& factory) { static GripperInterface grip_singleton; @@ -132,6 +198,8 @@ inline void RegisterNodes(BT::BehaviorTreeFactory& factory) std::bind(&GripperInterface::close, &grip_singleton)); factory.registerNodeType("ApproachObject"); factory.registerNodeType("SaySomething"); + factory.registerNodeType("RandomVector"); + factory.registerNodeType("PrintMapOfVectors"); } } // namespace DummyNodes diff --git a/src/python/bindings.cpp b/src/python/bindings.cpp new file mode 100644 index 000000000..49067fde8 --- /dev/null +++ b/src/python/bindings.cpp @@ -0,0 +1,208 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "behaviortree_cpp/basic_types.h" +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/tree_node.h" + +namespace BT +{ + +namespace py = pybind11; + +class Py_SyncActionNode : public SyncActionNode +{ +public: + Py_SyncActionNode(const std::string& name, const NodeConfig& config) + : SyncActionNode(name, config) + {} + + NodeStatus tick() override + { + PYBIND11_OVERRIDE_PURE_NAME(NodeStatus, Py_SyncActionNode, "tick", tick); + } +}; + +class Py_StatefulActionNode : public StatefulActionNode +{ +public: + Py_StatefulActionNode(const std::string& name, const NodeConfig& config) + : StatefulActionNode(name, config) + {} + + NodeStatus onStart() override + { + PYBIND11_OVERRIDE_PURE_NAME(NodeStatus, Py_StatefulActionNode, "on_start", onStart); + } + + NodeStatus onRunning() override + { + PYBIND11_OVERRIDE_PURE_NAME(NodeStatus, Py_StatefulActionNode, "on_running", + onRunning); + } + + void onHalted() override + { + PYBIND11_OVERRIDE_PURE_NAME(void, Py_StatefulActionNode, "on_halted", onHalted); + } +}; + +py::object Py_getInput(const TreeNode& node, const std::string& name) +{ + py::object obj; + + // The input could not exist on the blackboard, in which case we return Python + // `None` instead of an invalid object. + if(!node.getInput(name, obj).has_value()) + { + return py::none(); + } + return obj; +} + +void Py_setOutput(TreeNode& node, const std::string& name, const py::object& value) +{ + node.setOutput(name, value); +} + +// Add a conversion specialization from string values into general py::objects +// by evaluating as a Python expression. +template <> +inline py::object convertFromString(StringView str) +{ + try + { + // First, try evaluating the string as-is. Maybe it's a number, a list, a + // dict, an object, etc. + return py::eval(str); + } + catch(py::error_already_set& e) + { + // If that fails, then assume it's a string literal with quotation marks + // omitted. + return py::str(str); + } +} + +PortsList extractPortsList(const py::type& type) +{ + PortsList ports; + + const auto input_ports = type.attr("input_ports").cast(); + for(const auto& name : input_ports) + { + ports.insert(InputPort(name.cast())); + } + + const auto output_ports = type.attr("output_ports").cast(); + for(const auto& name : output_ports) + { + ports.insert(OutputPort(name.cast())); + } + + return ports; +} + +NodeBuilder makeTreeNodeBuilderFn(const py::type& type, const py::args& args, + const py::kwargs& kwargs) +{ + return [=](const auto& name, const auto& config) -> auto { + py::object obj; + obj = type(name, config, *args, **kwargs); + + // TODO: Increment the object's reference count or else it + // will be GC'd at the end of this scope. The downside is + // that, unless we can decrement the ref when the unique_ptr + // is destroyed, then the object will live forever. + obj.inc_ref(); + + if(py::isinstance(obj)) + { + return std::unique_ptr(obj.cast()); + } + else + { + throw std::runtime_error("invalid node type of " + name); + } + }; +} + +PYBIND11_MODULE(btpy_cpp, m) +{ + py::class_(m, "BehaviorTreeFactory") + .def(py::init()) + .def("register", + [](BehaviorTreeFactory& factory, const py::object& type, const py::args& args, + const py::kwargs& kwargs) { + const std::string name = type.attr("__name__").cast(); + + TreeNodeManifest manifest; + manifest.type = NodeType::ACTION; + manifest.registration_ID = name; + manifest.ports = extractPortsList(type); + manifest.metadata = KeyValueVector{ + { "description", "" }, + }; + + // Use the type's docstring as the node description, if it exists. + if(const auto doc = type.attr("__doc__"); !doc.is_none()) + { + manifest.metadata = KeyValueVector{ + { "description", doc.cast() }, + }; + } + + factory.registerBuilder(manifest, makeTreeNodeBuilderFn(type, args, kwargs)); + }) + .def("register_from_plugin", &BehaviorTreeFactory::registerFromPlugin) + .def("create_tree_from_text", + [](BehaviorTreeFactory& factory, const std::string& text) -> Tree { + return factory.createTreeFromText(text); + }); + + py::class_(m, "Tree") + .def("tick_once", &Tree::tickOnce) + .def("tick_exactly_once", &Tree::tickExactlyOnce) + .def("tick_while_running", &Tree::tickWhileRunning, + py::arg("sleep_time") = std::chrono::milliseconds(10)) + .def("halt_tree", &Tree::haltTree); + + py::enum_(m, "NodeStatus") + .value("IDLE", NodeStatus::IDLE) + .value("RUNNING", NodeStatus::RUNNING) + .value("SUCCESS", NodeStatus::SUCCESS) + .value("FAILURE", NodeStatus::FAILURE) + .value("SKIPPED", NodeStatus::SKIPPED) + .export_values(); + + py::class_(m, "NodeConfig"); + + // Register the C++ type hierarchy so that we can refer to Python subclasses + // by their superclass ptr types in generic C++ code. + py::class_(m, "_TreeNode") + .def("get_input", &Py_getInput) + .def("set_output", &Py_setOutput); + py::class_(m, "_ActionNodeBase"); + py::class_(m, "_SyncActionNode"); + py::class_(m, "_StatefulActionNode"); + + py::class_(m, "SyncActionNode") + .def(py::init()) + .def("tick", &Py_SyncActionNode::tick); + + py::class_(m, "StatefulActionNode") + .def(py::init()) + .def("on_start", &Py_StatefulActionNode::onStart) + .def("on_running", &Py_StatefulActionNode::onRunning) + .def("on_halted", &Py_StatefulActionNode::onHalted); +} + +} // namespace BT diff --git a/src/python/types.cpp b/src/python/types.cpp new file mode 100644 index 000000000..83b0889dd --- /dev/null +++ b/src/python/types.cpp @@ -0,0 +1,29 @@ +#include "behaviortree_cpp/python/types.h" + +#include +#include + +#include "behaviortree_cpp/json_export.h" +#include "behaviortree_cpp/contrib/json.hpp" + +namespace BT +{ + +#if defined(_WIN32) +__declspec(dllexport) +#else +__attribute__((visibility("default"))) +#endif + bool toPythonObject(const BT::Any& val, pybind11::object& dest) +{ + nlohmann::json json; + if(JsonExporter::get().toJson(val, json)) + { + dest = json; + return true; + } + + return false; +} + +} // namespace BT