diff --git a/.gitignore b/.gitignore index 2a13ff4e6..bc0cb7e33 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 77592e776..1edbc2656 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,7 @@ option(BTCPP_EXAMPLES "Build tutorials and examples" ON) option(BTCPP_UNIT_TESTS "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) @@ -134,6 +135,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) @@ -163,6 +168,18 @@ target_link_libraries(${BTCPP_LIBRARY} ${BTCPP_EXTRA_LIBRARIES} ) +if(BTCPP_PYTHON) + find_package(Python COMPONENTS Interpreter Development) + find_package(pybind11 CONFIG) + + 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 56dd97006..0f2e22339 100644 --- a/conanfile.txt +++ b/conanfile.txt @@ -2,6 +2,7 @@ gtest/1.12.1 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..fe76b57e7 --- /dev/null +++ b/include/behaviortree_cpp/contrib/pybind11_json.hpp @@ -0,0 +1,226 @@ +/*************************************************************************** +* 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 "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; + } + 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 8fc6d2c25..d9bf55e42 100644 --- a/include/behaviortree_cpp/json_export.h +++ b/include/behaviortree_cpp/json_export.h @@ -23,12 +23,8 @@ namespace BT */ class JsonExporter{ - - public: - static JsonExporter& get() { - static JsonExporter global_instance; - return global_instance; - } +public: + static JsonExporter& get(); /** * @brief toJson adds the content of "any" to the JSON "destination". @@ -43,6 +39,27 @@ class JsonExporter{ dst = val; } + /** + * @brief fromJson tries to convert arbitrary JSON data into the type T. + * + * Calls only compile if `nlohmann::from_json(const nlohmann::json&, T&)` is + * defined in T's namespace. + */ + template + T fromJson(const nlohmann::json& src) const + { + // We don't need to implement a similar `type_converters` interface as + // `toJson` here because the type T must be know statically. There is no + // opaque BT::Any wrapper here requiring RTTI. + return src.template get(); + } + + template + void fromJson(const nlohmann::json& src, T& dst) const + { + dst = fromJson(src); + } + /// Register new JSON converters with addConverter(), /// But works only if this function is implemented: /// diff --git a/include/behaviortree_cpp/python/types.h b/include/behaviortree_cpp/python/types.h new file mode 100644 index 000000000..bcd53a7e3 --- /dev/null +++ b/include/behaviortree_cpp/python/types.h @@ -0,0 +1,39 @@ +#pragma once + +#include + +#include "behaviortree_cpp/json_export.h" +#include "behaviortree_cpp/contrib/json.hpp" +#include "behaviortree_cpp/contrib/pybind11_json.hpp" +#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) +{ + if constexpr (nlohmann::detail::is_getable::value) + { + JsonExporter::get().fromJson(obj, dest); + 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 4dc44405f..e0aba462f 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -25,6 +25,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 @@ -442,11 +449,33 @@ inline Result TreeNode::getInput(const std::string& key, T& destination) const auto val = any_ref.get(); if(!val->empty()) { - if (!std::is_same_v && - val->type() == typeid(std::string)) + // Trivial conversion (T -> T) + if (val->type() == typeid(T)) + { + destination = val->cast(); + } + else if (!std::is_same_v && val->type() == typeid(std::string)) { destination = ParseString(val->cast()); } +#ifdef BTCPP_PYTHON + // py::object -> C++ + else if (val->type() == typeid(pybind11::object)) + { + if (!fromPythonObject(val->cast(), destination)) + { + return nonstd::make_unexpected("Cannot convert from Python object"); + } + } + // C++ -> py::object + else if constexpr (std::is_same_v) + { + if (!toPythonObject(*val, destination)) + { + return nonstd::make_unexpected("Cannot convert to Python object"); + } + } +#endif else { destination = val->cast(); diff --git a/package.xml b/package.xml index 5abad954d..2530a1f35 100644 --- a/package.xml +++ b/package.xml @@ -23,6 +23,7 @@ libsqlite3-dev libzmq3-dev + pybind11-dev ament_cmake_gtest diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 000000000..b07da1aa8 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,8 @@ +[build-system] +requires = [ + "setuptools>=42", + "wheel", + "ninja", + "cmake>=3.16", +] +build-backend = "setuptools.build_meta" 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..8e6e3f436 --- /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"]) +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) + + 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..c10bb8b2c --- /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("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 9c0a228f5..347996c4f 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); +} + } diff --git a/sample_nodes/dummy_nodes.h b/sample_nodes/dummy_nodes.h index c0b1df87c..b57fa8c8e 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 { @@ -121,6 +122,72 @@ 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 +199,10 @@ inline void RegisterNodes(BT::BehaviorTreeFactory& factory) factory.registerSimpleAction("CloseGripper", std::bind(&GripperInterface::close, &grip_singleton)); factory.registerNodeType("ApproachObject"); factory.registerNodeType("SaySomething"); + factory.registerNodeType("RandomVector"); + factory.registerNodeType("PrintMapOfVectors"); + + BT::JsonExporter::get().addConverter(); } } // end namespace diff --git a/setup.py b/setup.py new file mode 100644 index 000000000..9e97b01bd --- /dev/null +++ b/setup.py @@ -0,0 +1,141 @@ +import os +import re +import subprocess +import sys +from pathlib import Path + +from setuptools import Extension, setup +from setuptools.command.build_ext import build_ext + +# Convert distutils Windows platform specifiers to CMake -A arguments +PLAT_TO_CMAKE = { + "win32": "Win32", + "win-amd64": "x64", + "win-arm32": "ARM", + "win-arm64": "ARM64", +} + + +# A CMakeExtension needs a sourcedir instead of a file list. +# The name must be the _single_ output extension from the CMake build. +# If you need multiple extensions, see scikit-build. +class CMakeExtension(Extension): + def __init__(self, name: str, sourcedir: str = "") -> None: + super().__init__(name, sources=[]) + self.sourcedir = os.fspath(Path(sourcedir).resolve()) + + +class CMakeBuild(build_ext): + def build_extension(self, ext: CMakeExtension) -> None: + # Must be in this form due to bug in .resolve() only fixed in Python 3.10+ + ext_fullpath = Path.cwd() / self.get_ext_fullpath(ext.name) + extdir = ext_fullpath.parent.resolve() + + # Using this requires trailing slash for auto-detection & inclusion of + # auxiliary "native" libs + + debug = int(os.environ.get("DEBUG", 0)) if self.debug is None else self.debug + cfg = "Debug" if debug else "Release" + + # CMake lets you override the generator - we need to check this. + # Can be set with Conda-Build, for example. + cmake_generator = os.environ.get("CMAKE_GENERATOR", "") + + # Set Python_EXECUTABLE instead if you use PYBIND11_FINDPYTHON + # EXAMPLE_VERSION_INFO shows you how to pass a value into the C++ code + # from Python. + cmake_args = [ + f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY={extdir}{os.sep}", + f"-DPYTHON_EXECUTABLE={sys.executable}", + f"-DCMAKE_BUILD_TYPE={cfg}", # not used on MSVC, but no harm + # BehaviorTree.CPP specific CMake options + "-DBTCPP_BUILD_TOOLS=OFF", + "-DBTCPP_EXAMPLES=OFF", + "-DBTCPP_UNIT_TESTS=OFF", + ] + build_args = [] + # Adding CMake arguments set as environment variable + # (needed e.g. to build for ARM OSx on conda-forge) + if "CMAKE_ARGS" in os.environ: + cmake_args += [item for item in os.environ["CMAKE_ARGS"].split(" ") if item] + + if self.compiler.compiler_type != "msvc": + # Using Ninja-build since it a) is available as a wheel and b) + # multithreads automatically. MSVC would require all variables be + # exported for Ninja to pick it up, which is a little tricky to do. + # Users can override the generator with CMAKE_GENERATOR in CMake + # 3.15+. + if not cmake_generator or cmake_generator == "Ninja": + try: + import ninja + + ninja_executable_path = Path(ninja.BIN_DIR) / "ninja" + cmake_args += [ + "-GNinja", + f"-DCMAKE_MAKE_PROGRAM:FILEPATH={ninja_executable_path}", + ] + except ImportError: + pass + + else: + # Single config generators are handled "normally" + single_config = any(x in cmake_generator for x in {"NMake", "Ninja"}) + + # CMake allows an arch-in-generator style for backward compatibility + contains_arch = any(x in cmake_generator for x in {"ARM", "Win64"}) + + # Specify the arch if using MSVC generator, but only if it doesn't + # contain a backward-compatibility arch spec already in the + # generator name. + if not single_config and not contains_arch: + cmake_args += ["-A", PLAT_TO_CMAKE[self.plat_name]] + + # Multi-config generators have a different way to specify configs + if not single_config: + cmake_args += [ + f"-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{cfg.upper()}={extdir}" + ] + build_args += ["--config", cfg] + + if sys.platform.startswith("darwin"): + # Cross-compile support for macOS - respect ARCHFLAGS if set + archs = re.findall(r"-arch (\S+)", os.environ.get("ARCHFLAGS", "")) + if archs: + cmake_args += ["-DCMAKE_OSX_ARCHITECTURES={}".format(";".join(archs))] + + # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level + # across all generators. + if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ: + # self.parallel is a Python 3 only way to set parallel jobs by hand + # using -j in the build_ext call, not supported by pip or PyPA-build. + if hasattr(self, "parallel") and self.parallel: + # CMake 3.12+ only. + build_args += [f"-j{self.parallel}"] + + build_temp = Path(self.build_temp) / ext.name + if not build_temp.exists(): + build_temp.mkdir(parents=True) + + subprocess.run( + ["cmake", ext.sourcedir, *cmake_args], cwd=build_temp, check=True + ) + subprocess.run( + ["cmake", "--build", ".", *build_args], cwd=build_temp, check=True + ) + + +# The information here can also be placed in setup.cfg - better separation of +# logic and declaration, and simpler if you include description/version in a file. +setup( + name="btpy", + version="0.0.1", + author="Davide Faconti", + author_email="davide.faconti@gmail.com", + description="Python bindings to the BehaviorTree.CPP library", + long_description="", + packages=["btpy"], + ext_modules=[CMakeExtension("btcpp")], + cmdclass={"build_ext": CMakeBuild}, + zip_safe=False, + python_requires=">=3.7", +) diff --git a/src/json_export.cpp b/src/json_export.cpp index 41084e914..766dac314 100644 --- a/src/json_export.cpp +++ b/src/json_export.cpp @@ -3,7 +3,14 @@ namespace BT { -bool JsonExporter::toJson(const Any &any, nlohmann::json &dst) const +static JsonExporter global_instance; + +JsonExporter& JsonExporter::get() +{ + return global_instance; +} + +bool JsonExporter::toJson(const Any& any, nlohmann::json& dst) const { nlohmann::json json; auto const& type = any.castedType(); diff --git a/src/python/bindings.cpp b/src/python/bindings.cpp new file mode 100644 index 000000000..3f2d118ab --- /dev/null +++ b/src/python/bindings.cpp @@ -0,0 +1,205 @@ +#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.description = ""; + + // Use the type's docstring as the node description, if it exists. + if (const auto doc = type.attr("__doc__"); !doc.is_none()) + { + manifest.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..717aa6aca --- /dev/null +++ b/src/python/types.cpp @@ -0,0 +1,25 @@ +#include "behaviortree_cpp/python/types.h" + +#include +#include + +#include "behaviortree_cpp/json_export.h" +#include "behaviortree_cpp/contrib/json.hpp" +#include "behaviortree_cpp/contrib/pybind11_json.hpp" + +namespace BT +{ + +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