Adds RGB-D subscribers to manipulation station hardware interface

bindings/pydrake/examples/manipulation_station_py.cc

@@ -1,5 +1,6 @@
 #include "pybind11/eigen.h"
 #include "pybind11/pybind11.h"
+#include "pybind11/stl.h"
 
 #include "drake/bindings/pydrake/documentation_pybind.h"
 #include "drake/bindings/pydrake/pydrake_pybind.h"
@@ -58,7 +59,9 @@ PYBIND11_MODULE(manipulation_station, m) {
 
   py::class_<ManipulationStationHardwareInterface, Diagram<double>>(
       m, "ManipulationStationHardwareInterface")
-      .def(py::init<bool>(), py::arg("block_until_connected") = true,
+      .def(py::init<const std::vector<std::string>, bool>(),
+          py::arg("camera_ids") = std::vector<std::string>{},
+          py::arg("block_until_connected") = true,
            doc.ManipulationStationHardwareInterface.ctor.doc_3)
       .def("get_controller_plant",
            &ManipulationStationHardwareInterface::get_controller_plant,

examples/manipulation_station/BUILD.bazel

@@ -62,6 +62,7 @@ drake_cc_library(
         "//systems/framework",
         "//systems/lcm",
         "//systems/primitives",
+        "//systems/sensors:lcm_image_array_to_images",
     ],
 )
 

examples/manipulation_station/joint_teleop.py

@@ -17,6 +17,7 @@
 from pydrake.systems.analysis import Simulator
 from pydrake.util.eigen_geometry import Isometry3
 
+from pydrake.systems.sensors import Image
 
 parser = argparse.ArgumentParser(description=__doc__)
 parser.add_argument(
@@ -35,7 +36,9 @@
 builder = DiagramBuilder()
 
 if args.hardware:
-    station = builder.AddSystem(ManipulationStationHardwareInterface())
+    camera_ids = ["805212060544"]
+    station = builder.AddSystem(ManipulationStationHardwareInterface(
+        camera_ids))
 else:
     station = builder.AddSystem(ManipulationStation())
     station.AddCupboard()

examples/manipulation_station/manipulation_station_hardware_interface.cc

@@ -1,5 +1,7 @@
 #include "drake/examples/manipulation_station/manipulation_station_hardware_interface.h"
 
+#include "robotlocomotion/image_array_t.hpp"
+
 #include "drake/common/find_resource.h"
 #include "drake/examples/kuka_iiwa_arm/iiwa_lcm.h"
 #include "drake/lcm/drake_lcm.h"
@@ -13,6 +15,7 @@
 #include "drake/systems/lcm/lcm_publisher_system.h"
 #include "drake/systems/lcm/lcm_subscriber_system.h"
 #include "drake/systems/primitives/pass_through.h"
+#include "drake/systems/sensors/lcm_image_array_to_images.h"
 
 namespace drake {
 namespace examples {
@@ -22,11 +25,12 @@ using Eigen::Isometry3d;
 using Eigen::Vector3d;
 using multibody::multibody_plant::MultibodyPlant;
 using multibody::parsing::AddModelFromSdfFile;
+using robotlocomotion::image_array_t;
 
 // TODO(russt): Set publishing defaults.
 
 ManipulationStationHardwareInterface::ManipulationStationHardwareInterface(
-    bool block_until_connected)
+    const std::vector<std::string> camera_ids, bool block_until_connected)
     : owned_controller_plant_(std::make_unique<MultibodyPlant<double>>()),
       owned_lcm_(new lcm::DrakeLcm()) {
   owned_lcm_->StartReceiveThread();
@@ -101,12 +105,26 @@ ManipulationStationHardwareInterface::ManipulationStationHardwareInterface(
     std::cout << "Received!" << std::endl;
   }
 
-  builder.ExportOutput(
-      wsg_status_receiver->get_state_output_port(),
-      "wsg_state_measured");
-  builder.ExportOutput(
-      wsg_status_receiver->get_force_output_port(),
-      "wsg_force_measured");
+  int camera_number{0};
+  for (const std::string& id : camera_ids) {
+    auto image_sub = builder.AddSystem(
+        systems::lcm::LcmSubscriberSystem::Make<image_array_t>(
+            "DRAKE_RGBD_CAMERA_IMAGES_" + id, owned_lcm_.get()));
+    auto array_to_images =
+        builder.AddSystem<systems::sensors::LcmImageArrayToImages>();
+    builder.Connect(image_sub->get_output_port(),
+                    array_to_images->image_array_t_input_port());
+    builder.ExportOutput(array_to_images->color_image_output_port(),
+        "camera" + std::to_string(camera_number) + "_rgb_image");
+    builder.ExportOutput(array_to_images->depth_image_output_port(),
+        "camera" + std::to_string(camera_number) + "_depth_image");
+    camera_number++;
+  }
+
+  builder.ExportOutput(wsg_status_receiver->get_state_output_port(),
+                       "wsg_state_measured");
+  builder.ExportOutput(wsg_status_receiver->get_force_output_port(),
+                       "wsg_force_measured");
   builder.Connect(wsg_status_subscriber->get_output_port(),
                   wsg_status_receiver->get_input_port(0));
 

examples/manipulation_station/manipulation_station_hardware_interface.h

@@ -41,7 +41,14 @@ class ManipulationStationHardwareInterface : public systems::Diagram<double> {
  public:
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ManipulationStationHardwareInterface)
 
-  ManipulationStationHardwareInterface(bool block_until_connected = true);
+  /// Subscribes to an incoming camera message on the channel
+  ///   DRAKE_RGBD_CAMERA_IMAGES_<camera_id>
+  /// where @p camera_ids contains the ids, typically serial numbers, and
+  /// declares the output ports camera%d_rgb_image and camera%d_depth_image,
+  /// where %d is the camera number starting at zero.
+  ManipulationStationHardwareInterface(
+      const std::vector<std::string> camera_ids = {},
+      bool block_until_connected = true);
 
   /// @param scene_graph Registers the known geometries with the @p
   ///   scene_graph, and creates a `geometry_pose` output port.  Only the