Skip to content

Commit f9204a6

Browse files
JafarAbdiJafarAbdi
committed
BT::any input/output
1 parent 53463eb commit f9204a6

File tree

3 files changed

+187
-15
lines changed

3 files changed

+187
-15
lines changed

‎CMakeLists.txt

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
1313
find_package(pybind11_vendor REQUIRED)
1414
find_package(pybind11 REQUIRED)
1515
find_package(fmt REQUIRED)
16+
find_package(py_binding_tools REQUIRED)
1617

1718
ament_python_install_package(behaviortree_py PACKAGE_DIR behaviortree_py)
1819

@@ -27,8 +28,10 @@ target_include_directories(
2728

2829
pybind11_add_module(behaviortree_py src/behaviortree_py.cpp)
2930
target_compile_features(behaviortree_py PRIVATE cxx_std_20)
30-
target_link_libraries(behaviortree_py PRIVATE behaviortree_cpp::behaviortree_cpp
31-
fmt::fmt ${PROJECT_NAME}_headers)
31+
target_link_libraries(
32+
behaviortree_py
33+
PRIVATE behaviortree_cpp::behaviortree_cpp fmt::fmt ${PROJECT_NAME}_headers
34+
py_binding_tools::py_binding_tools)
3235

3336
add_custom_command(
3437
TARGET behaviortree_py

‎examples/scripting.py

Lines changed: 19 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,7 @@
11
import behaviortree_py as bt
2+
from geometry_msgs.msg import Point
3+
from std_msgs.msg import String
4+
from moveit_msgs.msg import OrientedBoundingBox
25

36
xml_text = """
47
<root BTCPP_format="4">
@@ -8,6 +11,7 @@
811
<Script code=" A:=THE_ANSWER; B:=3.14; color:=RED " />
912
<Precondition if="A>B && color != BLUE" else="FAILURE">
1013
<Sequence>
14+
<Vision />
1115
<SetValue value="{double_value}"/>
1216
<SaySomething message="{A}"/>
1317
<SaySomething message="{B}"/>
@@ -25,30 +29,37 @@
2529
def say_something_double(
2630
tree_node: bt.TreeNode,
2731
) -> bt.NodeStatus:
28-
print(bt.get_input_double(tree_node, "message"))
32+
print(tree_node.get_input("message"))
2933
return bt.NodeStatus.SUCCESS
3034

3135

3236
def say_something(tree_node: bt.TreeNode) -> bt.NodeStatus:
33-
print(bt.get_input_string(tree_node, "message"))
37+
print(tree_node.get_input("message"))
3438
return bt.NodeStatus.SUCCESS
3539

3640

3741
def set_value(tree_node: bt.TreeNode) -> bt.NodeStatus:
38-
if not (result := bt.set_output(tree_node, "value", 24.9)):
42+
if not (result := tree_node.set_output("value", 24.9)):
43+
print(f"Failed to set value '{result.error()}'")
44+
return bt.NodeStatus.FAILURE
45+
return bt.NodeStatus.SUCCESS
46+
47+
48+
def vision(tree_node: bt.TreeNode) -> bt.NodeStatus:
49+
if not (result := tree_node.set_output("bbox", OrientedBoundingBox())):
50+
print(f"Failed to set value '{result.error()}'")
3951
return bt.NodeStatus.FAILURE
4052
return bt.NodeStatus.SUCCESS
4153

4254

4355
factory = bt.BehaviorTreeFactory()
56+
factory.register_simple_action("Vision", vision, dict([bt.output_port("bbox")]))
57+
factory.register_simple_action("SetValue", set_value, dict([bt.output_port("value")]))
4458
factory.register_simple_action(
45-
"SetValue", set_value, dict([bt.output_port_double("value")])
46-
)
47-
factory.register_simple_action(
48-
"SaySomething", say_something, dict([bt.input_port_string("message")])
59+
"SaySomething", say_something, dict([bt.input_port("message")])
4960
)
5061
factory.register_simple_action(
51-
"SaySomethingDouble", say_something_double, dict([bt.input_port_double("message")])
62+
"SaySomethingDouble", say_something_double, dict([bt.input_port("message")])
5263
)
5364
factory.register_scripting_enum("RED", 1)
5465
factory.register_scripting_enum("BLUE", 2)

‎src/behaviortree_py.cpp

Lines changed: 163 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
#include <behaviortree_cpp/loggers/groot2_publisher.h>
1212
#include <behaviortree_cpp/tree_node.h>
1313
#include <fmt/args.h>
14+
#include <py_binding_tools/ros_msg_typecasters.h>
1415
#include <pybind11/chrono.h>
1516
#include <pybind11/functional.h>
1617
#include <pybind11/pybind11.h>
@@ -22,6 +23,150 @@
2223

2324
namespace py = pybind11;
2425

26+
// Mention that it's copied from MTC
27+
namespace {
28+
29+
/** In order to assign new property values in Python, we need to convert the
30+
*Python object to a boost::any instance of the correct type. As the C++ type
31+
*cannot be inferred from the Python type, we can support this assignment only
32+
*for a few basic types (see fromPython()) as well as ROS message types. For
33+
*other types, a generic assignment via stage.properties["property"] = value is
34+
*not possible. Instead, use the .property<Type> declaration on the stage to
35+
*allow for direct assignment like this: stage.property = value
36+
**/
37+
class PropertyConverterRegistry {
38+
using to_python_converter_function = pybind11::object (*)(const BT::Any&);
39+
using from_python_converter_function = BT::Any (*)(const pybind11::object&);
40+
41+
struct Entry {
42+
to_python_converter_function to_;
43+
from_python_converter_function from_;
44+
};
45+
46+
// map from type_index to corresponding converter functions
47+
typedef std::map<std::type_index, Entry> RegistryMap;
48+
RegistryMap types_;
49+
// map from ros-msg-names to entry in types_
50+
using RosMsgTypeNameMap = std::map<std::string, RegistryMap::iterator>;
51+
RosMsgTypeNameMap msg_names_;
52+
53+
public:
54+
PropertyConverterRegistry();
55+
56+
inline bool insert(const std::type_index& type_index,
57+
const std::string& ros_msg_name,
58+
to_python_converter_function to,
59+
from_python_converter_function from);
60+
61+
static py::object toPython(const BT::Any& value);
62+
63+
static BT::Any fromPython(const py::object& bpo);
64+
};
65+
66+
inline constexpr static PropertyConverterRegistry REGISTRY_SINGLETON;
67+
68+
/// utility class to register C++ / Python converters for a property of type T
69+
template <typename T>
70+
class PropertyConverter {
71+
public:
72+
PropertyConverter() { REGISTRY_SINGLETON.insert(typeid(T), rosMsgName<T>(), &toPython, &fromPython); }
73+
74+
private:
75+
static pybind11::object toPython(const BT::Any& value) { return pybind11::cast(value.cast<T>()); }
76+
77+
static BT::Any fromPython(const pybind11::object& po) { return BT::Any(pybind11::cast<T>(po)); }
78+
79+
template <class Q = T>
80+
typename std::enable_if<rosidl_generator_traits::is_message<Q>::value, std::string>::type rosMsgName() {
81+
return rosidl_generator_traits::name<Q>();
82+
}
83+
84+
template <class Q = T>
85+
typename std::enable_if<!rosidl_generator_traits::is_message<Q>::value, std::string>::type rosMsgName() {
86+
return std::string();
87+
}
88+
};
89+
90+
PropertyConverterRegistry::PropertyConverterRegistry() {
91+
// register property converters
92+
PropertyConverter<bool>();
93+
PropertyConverter<int>();
94+
PropertyConverter<unsigned int>();
95+
PropertyConverter<long>();
96+
PropertyConverter<float>();
97+
PropertyConverter<double>();
98+
PropertyConverter<std::string>();
99+
PropertyConverter<std::set<std::string>>();
100+
PropertyConverter<std::map<std::string, double>>();
101+
}
102+
103+
bool PropertyConverterRegistry::insert(const std::type_index& type_index,
104+
const std::string& ros_msg_name,
105+
to_python_converter_function to,
106+
from_python_converter_function from) {
107+
auto it_inserted = types_.insert(std::make_pair(type_index, Entry{to, from}));
108+
if (!it_inserted.second) return false;
109+
110+
if (!ros_msg_name.empty()) // is this a ROS msg type?
111+
msg_names_.insert(std::make_pair(ros_msg_name, it_inserted.first));
112+
113+
return true;
114+
}
115+
116+
py::object PropertyConverterRegistry::toPython(const BT::Any& value) {
117+
if (value.empty()) return py::object();
118+
for (const auto& [name, entry] : REGISTRY_SINGLETON.msg_names_) {
119+
std::cout << name << " " << BT::demangle(entry->first) << std::endl;
120+
}
121+
122+
auto it = REGISTRY_SINGLETON.types_.find(value.type());
123+
if (it == REGISTRY_SINGLETON.types_.end()) {
124+
std::string name = BT::demangle(value.type());
125+
throw py::type_error("No Python -> C++ conversion for: " + name);
126+
}
127+
128+
return it->second.to_(value);
129+
}
130+
131+
std::string rosMsgName(PyObject* object) {
132+
py::object o = py::reinterpret_borrow<py::object>(object);
133+
auto cls = o.attr("__class__");
134+
auto name = cls.attr("__name__").cast<std::string>();
135+
auto module = cls.attr("__module__").cast<std::string>();
136+
auto pos = module.find(".msg");
137+
if (pos == std::string::npos)
138+
// object is not a ROS message type, return it's class name instead
139+
return module + "." + name;
140+
else
141+
return module.substr(0, pos) + "/msg/" + name;
142+
}
143+
144+
BT::Any PropertyConverterRegistry::fromPython(const py::object& po) {
145+
PyObject* o = po.ptr();
146+
147+
if (PyBool_Check(o)) return BT::Any((o == Py_True));
148+
if (PyLong_Check(o)) return BT::Any(PyLong_AS_LONG(o));
149+
if (PyFloat_Check(o)) return BT::Any(PyFloat_AS_DOUBLE(o));
150+
if (PyUnicode_Check(o)) return BT::Any(py::cast<std::string>(o));
151+
152+
const std::string& ros_msg_name = rosMsgName(o);
153+
auto it = REGISTRY_SINGLETON.msg_names_.find(ros_msg_name);
154+
if (it == REGISTRY_SINGLETON.msg_names_.end())
155+
throw py::type_error("No C++ conversion available for (property) type: " + ros_msg_name);
156+
157+
return it->second->second.from_(po);
158+
}
159+
160+
} // end anonymous namespace
161+
162+
// WTF??
163+
namespace BT {
164+
template <>
165+
inline BT::Any convertFromString<BT::Any>(BT::StringView str) {
166+
return BT::Any(str);
167+
}
168+
} // namespace BT
169+
25170
PYBIND11_MODULE(behaviortree_py, m) {
26171
m.doc() = "Python wrapper for BehaviorTree.CPP";
27172

@@ -48,7 +193,14 @@ PYBIND11_MODULE(behaviortree_py, m) {
48193
py::class_<BT::TreeNode, std::shared_ptr<BT::TreeNode>>(m, "TreeNode")
49194
.def("name", &BT::TreeNode::name)
50195
.def("status", &BT::TreeNode::status)
51-
.def("type", &BT::TreeNode::type);
196+
.def("type", &BT::TreeNode::type)
197+
.def("get_input",
198+
[](BT::TreeNode* self, const std::string& key) {
199+
return PropertyConverterRegistry::toPython(self->getInput<BT::Any>(key).value());
200+
})
201+
.def("set_output", [](BT::TreeNode* self, const std::string& key, py::object value) {
202+
return self->setOutput(key, PropertyConverterRegistry::fromPython(value));
203+
});
52204

53205
py::class_<BT::BehaviorTreeFactory>(m, "BehaviorTreeFactory")
54206
.def(py::init<>())
@@ -123,8 +275,14 @@ PYBIND11_MODULE(behaviortree_py, m) {
123275
},
124276
py::arg("root_tree"));
125277

126-
bind_port_methods<int>(m, "int");
127-
bind_port_methods<double>(m, "double");
128-
bind_port_methods<std::string>(m, "string");
129-
bind_port_methods<bool>(m, "bool");
278+
m.def(
279+
"input_port",
280+
[](BT::StringView name, BT::StringView description) { return BT::InputPort<BT::Any>(name, description); },
281+
py::arg("name"),
282+
py::arg("description") = "")
283+
.def(
284+
"output_port",
285+
[](BT::StringView name, BT::StringView description) { return BT::OutputPort<BT::Any>(name, description); },
286+
py::arg("name"),
287+
py::arg("description") = "");
130288
}

0 commit comments

Comments
 (0)