Added Python binding for CameraInfo

Signed-off-by: Alejandro Hernández Cordero <[email protected]>

drake_ros/drake_ros/core/__init__.py

@@ -1,5 +1,6 @@
 """Python wrapper for drake_ros.core."""
 
+from drake_ros._cc.core import CameraInfoSystem
 from drake_ros._cc.core import ClockSystem
 from drake_ros._cc.core import init
 from drake_ros._cc.core import Isometry3ToRosPose
@@ -98,6 +99,7 @@ def _make_ros_subscriber_system(
 
 
 __all__ = [
+    'CameraInfoSystem',
     'ClockSystem',
     'DrakeRosInterface',
     'Isometry3ToRosPose',

drake_ros/drake_ros/core/cc_pybind.cc

@@ -6,6 +6,7 @@
 #include <pybind11/stl.h>
 #include <rclcpp/qos.hpp>
 
+#include "drake_ros/core/camera_info_system.h"
 #include "drake_ros/core/clock_system.h"
 #include "drake_ros/core/drake_ros.h"
 #include "drake_ros/core/geometry_conversions.h"
@@ -20,6 +21,7 @@
 namespace drake_ros {
 namespace drake_ros_py DRAKE_ROS_NO_EXPORT {
 
+using drake_ros::core::CameraInfoSystem;
 using drake_ros::core::ClockSystem;
 using drake_ros::core::DrakeRos;
 using drake_ros::core::init;
@@ -96,6 +98,35 @@ void DefCore(py::module m) {
   // them in sync, like pydrake does.
   py::class_<DrakeRos>(m, "DrakeRos");
 
+  py::class_<CameraInfoSystem, LeafSystem<double>>(m, "CameraInfoSystem")
+      .def_static(
+          "AddToBuilder",
+          [](drake::systems::DiagramBuilder<double>* builder, DrakeRos* ros,
+             const std::string& topic_name, const QoS& qos,
+             const std::unordered_set<drake::systems::TriggerType>&
+                 pub_triggers,
+             double publish_period) {
+            auto [camera_info_system, pub_system] =
+                CameraInfoSystem::AddToBuilder(builder, ros, topic_name, qos,
+                                               pub_triggers, publish_period);
+
+            py::object py_builder = py::cast(builder, py_rvp::reference);
+            py::list result;
+            result.append(py::cast(camera_info_system,
+                                   py_rvp::reference_internal, py_builder));
+            result.append(
+                py::cast(pub_system, py_rvp::reference_internal, py_builder));
+            return result;
+          },
+          py::arg("builder"), py::arg("ros"), py::kw_only(),
+          py::arg("topic_name") = std::string{"/image/camera_info"},
+          py::arg("qos") = drake_ros::QoS(rclcpp::SensorDataQoS()),
+          py::arg("publish_triggers") =
+              std::unordered_set<drake::systems::TriggerType>{
+                  RosPublisherSystem::kDefaultTriggerTypes},
+          py::arg("publish_period") = 0.0)
+      .def("set_camera_info", &CameraInfoSystem::SetCameraInfo);
+
   py::class_<ClockSystem, LeafSystem<double>>(m, "ClockSystem")
       .def_static(
           "AddToBuilder",

drake_ros_examples/examples/rgbd/camera_info.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+
+import numpy as np
+
+import drake_ros.core
+from drake_ros.core import ClockSystem
+from drake_ros.core import CameraInfoSystem
+from drake_ros.core import RosInterfaceSystem
+
+from pydrake.common import FindResourceOrThrow
+from pydrake.multibody.parsing import Parser
+from pydrake.systems.analysis import Simulator
+from pydrake.systems.framework import DiagramBuilder
+from pydrake.systems.sensors import CameraInfo
+
+
+def main():
+    # Create a Drake diagram
+    builder = DiagramBuilder()
+    # Initialise the ROS infrastructure
+    drake_ros.core.init()
+    # Create a Drake system to interface with ROS
+    sys_ros_interface = builder.AddSystem(RosInterfaceSystem('camera_info'))
+    ClockSystem.AddToBuilder(builder, sys_ros_interface.get_ros_interface())
+    camera_info_system = CameraInfoSystem.AddToBuilder(builder, sys_ros_interface.get_ros_interface())
+
+    intrinsics = CameraInfo(
+        width=640,
+        height=480,
+        fov_y=np.pi/4,
+    )
+
+    camera_info_system[0].set_camera_info(intrinsics)
+
+    # Build the complete system from the diagram
+    diagram = builder.Build()
+
+    # Create a simulator for the system
+    simulator = Simulator(diagram)
+    simulator.Initialize()
+    simulator_context = simulator.get_mutable_context()
+    simulator.set_target_realtime_rate(1.0)
+
+    # Step the simulator in 0.1s intervals
+    step = 0.1
+    while simulator_context.get_time() < float('inf'):
+        next_time = min(
+            simulator_context.get_time() + step, float('inf'),
+        )
+        simulator.AdvanceTo(next_time)
+
+
+if __name__ == '__main__':
+    main()