diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index c78e825d..6431e4ed 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -132,6 +132,8 @@ void export_core(pybind11::module& m) { ; py::class_(m, "InterfaceState") + .def(py::init(), py::arg("scene")) + .def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties), py::return_value_policy::reference_internal) ; auto stage = properties::class_>(m, "Stage") @@ -178,6 +180,7 @@ void export_core(pybind11::module& m) { .def(py::init(), py::arg("name") = std::string("generator")) .def("canCompute", &Generator::canCompute) .def("compute", &Generator::compute) + .def("spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); }) ; properties::class_>(m, "MonitoringGenerator") diff --git a/core/python/test/rostest_trampoline.py b/core/python/test/rostest_trampoline.py index 2690d8a8..1b61b18e 100755 --- a/core/python/test/rostest_trampoline.py +++ b/core/python/test/rostest_trampoline.py @@ -5,6 +5,7 @@ import unittest import rostest from moveit.python_tools import roscpp_init from moveit.task_constructor import core, stages +from moveit.core.planning_scene import PlanningScene from geometry_msgs.msg import Vector3Stamped, Vector3 from std_msgs.msg import Header @@ -20,6 +21,9 @@ class PyGenerator(core.Generator): core.Generator.__init__(self, name) self.reset() + def init(self, robot_model): + self.ps = PlanningScene(robot_model) + def reset(self): core.Generator.reset(self) self.num = self.max_calls @@ -29,6 +33,7 @@ class PyGenerator(core.Generator): def compute(self): self.num = self.num - 1 + self.spawn(core.InterfaceState(self.ps), self.num) class PyMonitoringGenerator(core.MonitoringGenerator):