From b16de11b35a7bc64ab922cedd567a34ff5d4440b Mon Sep 17 00:00:00 2001 From: Eloy Briceno <51831786+Voldivh@users.noreply.github.com> Date: Mon, 21 Aug 2023 14:12:29 -0500 Subject: [PATCH] Adds bindings for the Sensor convenience class (#2042) Signed-off-by: Voldivh Signed-off-by: Michael Carroll Signed-off-by: Eloy Briceno <51831786+Voldivh@users.noreply.github.com> Co-authored-by: Michael Carroll --- python/CMakeLists.txt | 2 + python/src/gz/sim/Sensor.cc | 71 ++++++++++++++++++++++++ python/src/gz/sim/Sensor.hh | 40 ++++++++++++++ python/src/gz/sim/_gz_sim_pybind11.cc | 2 + python/test/joint_test.sdf | 5 ++ python/test/sensor_TEST.py | 80 +++++++++++++++++++++++++++ 6 files changed, 200 insertions(+) create mode 100644 python/src/gz/sim/Sensor.cc create mode 100644 python/src/gz/sim/Sensor.hh create mode 100755 python/test/sensor_TEST.py diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 45d103278a..8abdd85254 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -48,6 +48,7 @@ pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/gz/sim/Link.cc src/gz/sim/Model.cc src/gz/sim/TestFixture.cc + src/gz/sim/Sensor.cc src/gz/sim/Server.cc src/gz/sim/ServerConfig.cc src/gz/sim/UpdateInfo.cc @@ -92,6 +93,7 @@ if (BUILD_TESTING) joint_TEST link_TEST model_TEST + sensor_TEST testFixture_TEST world_TEST ) diff --git a/python/src/gz/sim/Sensor.cc b/python/src/gz/sim/Sensor.cc new file mode 100644 index 0000000000..36e4d23083 --- /dev/null +++ b/python/src/gz/sim/Sensor.cc @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include +#include + +#include "Sensor.hh" + +namespace py = pybind11; + +namespace gz +{ +namespace sim +{ +namespace python +{ +void defineSimSensor(py::object module) +{ + py::class_(module, "Sensor") + .def(py::init()) + .def(py::init()) + .def("entity", &gz::sim::Sensor::Entity, + "Get the entity which this sensor is related to.") + .def("reset_entity", &gz::sim::Sensor::ResetEntity, + "Reset Entity to a new one.") + .def("valid", &gz::sim::Sensor::Valid, + py::arg("ecm"), + "Check whether this sensor correctly refers to an entity that" + "has a components::Sensor.") + .def("name", &gz::sim::Sensor::Name, + py::arg("ecm"), + "Get the sensor's unscoped name.") + .def("pose", &gz::sim::Sensor::Pose, + py::arg("ecm"), + "Get the pose of the sensor. " + "If the sensor has a trajectory, this will only return the origin" + "pose of the trajectory and not the actual world pose of the sensor.") + .def("topic", &gz::sim::Sensor::Topic, + py::arg("ecm"), + "Get the topic of the sensor.") + .def("parent", &gz::sim::Sensor::Parent, + py::arg("ecm"), + "Get the parent entity. This can be a link or a joint.") + .def("__copy__", + [](const gz::sim::Sensor &self) + { + return gz::sim::Sensor(self); + }) + .def("__deepcopy__", + [](const gz::sim::Sensor &self, pybind11::dict) + { + return gz::sim::Sensor(self); + }); +} +} // namespace python +} // namespace sim +} // namespace gz diff --git a/python/src/gz/sim/Sensor.hh b/python/src/gz/sim/Sensor.hh new file mode 100644 index 0000000000..00f09598c2 --- /dev/null +++ b/python/src/gz/sim/Sensor.hh @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef GZ_SIM_PYTHON__SENSOR_HH_ +#define GZ_SIM_PYTHON__SENSOR_HH_ + +#include + +#include + +namespace gz +{ +namespace sim +{ +namespace python +{ +/// Define a pybind11 wrapper for a gz::sim::Sensor +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +defineSimSensor(pybind11::object module); +} // namespace python +} // namespace sim +} // namespace gz + +#endif // GZ_SIM_PYTHON__SENSOR_HH_ diff --git a/python/src/gz/sim/_gz_sim_pybind11.cc b/python/src/gz/sim/_gz_sim_pybind11.cc index 60a13b93f4..c509948c2a 100644 --- a/python/src/gz/sim/_gz_sim_pybind11.cc +++ b/python/src/gz/sim/_gz_sim_pybind11.cc @@ -23,6 +23,7 @@ #include "Joint.hh" #include "Link.hh" #include "Model.hh" +#include "Sensor.hh" #include "Server.hh" #include "ServerConfig.hh" #include "TestFixture.hh" @@ -38,6 +39,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { gz::sim::python::defineSimJoint(m); gz::sim::python::defineSimLink(m); gz::sim::python::defineSimModel(m); + gz::sim::python::defineSimSensor(m); gz::sim::python::defineSimServer(m); gz::sim::python::defineSimServerConfig(m); gz::sim::python::defineSimTestFixture(m); diff --git a/python/test/joint_test.sdf b/python/test/joint_test.sdf index f4c780c35b..4c9d79345c 100644 --- a/python/test/joint_test.sdf +++ b/python/test/joint_test.sdf @@ -1,6 +1,8 @@ + + @@ -16,7 +18,10 @@ 0 0 1 + + 0 1 0 0 0 0 + sensor_topic_test diff --git a/python/test/sensor_TEST.py b/python/test/sensor_TEST.py new file mode 100755 index 0000000000..7c4c420bf6 --- /dev/null +++ b/python/test/sensor_TEST.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +# Copyright (C) 2023 Open Source Robotics Foundation + +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at + +# http://www.apache.org/licenses/LICENSE-2.0 + +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest + +from gz.common import set_verbosity +from gz_test_deps.sim import (K_NULL_ENTITY, TestFixture, + Joint, Model, Sensor, World, world_entity) +from gz_test_deps.math import Pose3d + + +class TestSensor(unittest.TestCase): + post_iterations = 0 + iterations = 0 + pre_iterations = 0 + + def test_model(self): + set_verbosity(4) + + file_path = os.path.dirname(os.path.realpath(__file__)) + fixture = TestFixture(os.path.join(file_path, 'joint_test.sdf')) + + def on_post_udpate_cb(_info, _ecm): + self.post_iterations += 1 + + def on_pre_udpate_cb(_info, _ecm): + self.pre_iterations += 1 + world_e = world_entity(_ecm) + self.assertNotEqual(K_NULL_ENTITY, world_e) + w = World(world_e) + m = Model(w.model_by_name(_ecm, 'model_test')) + j = Joint(m.joint_by_name(_ecm, 'joint_test')) + sensor = Sensor(j.sensor_by_name(_ecm, 'sensor_test')) + # Entity Test + self.assertNotEqual(K_NULL_ENTITY, sensor.entity()) + # Valid Test + self.assertTrue(sensor.valid(_ecm)) + # Name Test + self.assertEqual('sensor_test', sensor.name(_ecm)) + # Pose Test + self.assertEqual(Pose3d(0, 1, 0, 0, 0, 0), sensor.pose(_ecm)) + # Topic Test + if self.pre_iterations <= 1: + self.assertEqual(None, sensor.topic(_ecm)) + else: + self.assertEqual('sensor_topic_test', sensor.topic(_ecm)) + # Parent Test + self.assertEqual(j.entity(), sensor.parent(_ecm)) + + def on_udpate_cb(_info, _ecm): + self.iterations += 1 + + fixture.on_post_update(on_post_udpate_cb) + fixture.on_update(on_udpate_cb) + fixture.on_pre_update(on_pre_udpate_cb) + fixture.finalize() + + server = fixture.server() + server.run(True, 2, False) + + self.assertEqual(2, self.pre_iterations) + self.assertEqual(2, self.iterations) + self.assertEqual(2, self.post_iterations) + + +if __name__ == '__main__': + unittest.main()