Adds bindings for the Sensor convenience class (gazebosim#2042)

Signed-off-by: Voldivh <[email protected]>
Signed-off-by: Michael Carroll <[email protected]>
Signed-off-by: Eloy Briceno <[email protected]>
Co-authored-by: Michael Carroll <[email protected]>

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
   )

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 <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include "Sensor.hh"
+
+namespace py = pybind11;
+
+namespace gz
+{
+namespace sim
+{
+namespace python
+{
+void defineSimSensor(py::object module)
+{
+  py::class_<gz::sim::Sensor>(module, "Sensor")
+  .def(py::init<gz::sim::Entity>())
+  .def(py::init<gz::sim::Sensor>())
+  .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

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 <pybind11/pybind11.h>
+
+#include <gz/sim/Sensor.hh>
+
+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_

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);

python/test/joint_test.sdf

@@ -1,6 +1,8 @@
 <?xml version="1.0" ?>
 <sdf version="1.6">
     <world name="world_test">
+    <plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics" />
+    <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque" />
         <model name="model_test">
 
             <link name="link_test_1">
@@ -16,7 +18,10 @@
                 <axis>
                     <xyz>0 0 1</xyz>
                 </axis>
+
                 <sensor type="force_torque" name="sensor_test">
+                    <pose>0 1 0 0 0 0</pose>
+                    <topic>sensor_topic_test</topic>
                 </sensor>
             </joint>
         </model>

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()