Add a system to handle incoming image_array_t messages

systems/sensors/BUILD.bazel

@@ -124,6 +124,42 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "lcm_image_array_to_images",
+    srcs = [
+        "lcm_image_array_to_images.cc",
+    ],
+    hdrs = [
+        "lcm_image_array_to_images.h",
+    ],
+    deps = [
+        ":image",
+        "//common:essential",
+        "//systems/framework",
+        "@lcmtypes_robotlocomotion",
+        "@vtk//:vtkIOImage",
+        "@zlib",
+    ],
+)
+
+drake_cc_binary(
+    name = "lcm_image_array_receive_example",
+    srcs = [
+        "lcm_image_array_receive_example.cc",
+    ],
+    deps = [
+        ":image_to_lcm_image_array_t",
+        ":lcm_image_array_to_images",
+        "//common:text_logging_gflags",
+        "//common:unused",
+        "//lcm",
+        "//systems/analysis:simulator",
+        "//systems/lcm:lcm_pubsub_system",
+        "@gflags",
+    ],
+)
+
+
 drake_cc_library(
     name = "depth_sensor",
     srcs = [

systems/sensors/lcm_image_array_receive_example.cc

@@ -0,0 +1,105 @@
+#include <gflags/gflags.h>
+
+#include "robotlocomotion/image_array_t.hpp"
+
+#include "drake/common/text_logging_gflags.h"
+#include "drake/common/unused.h"
+#include "drake/lcm/drake_lcm.h"
+#include "drake/systems/analysis/simulator.h"
+#include "drake/systems/framework/diagram_builder.h"
+#include "drake/systems/lcm/lcm_publisher_system.h"
+#include "drake/systems/lcm/lcm_subscriber_system.h"
+#include "drake/systems/sensors/image.h"
+#include "drake/systems/sensors/image_to_lcm_image_array_t.h"
+#include "drake/systems/sensors/lcm_image_array_to_images.h"
+
+using robotlocomotion::image_array_t;
+
+DEFINE_string(channel_name, "DRAKE_RGBD_CAMERA_IMAGES_IN",
+              "Channel name to receive images on");
+DEFINE_string(publish_name, "DRAKE_RGBD_CAMERA_IMAGES",
+              "Channel name to publish images on");
+DEFINE_double(duration, 5., "Total duration of the simulation in secondes.");
+
+namespace drake {
+namespace systems {
+namespace sensors {
+namespace {
+
+class ImageSink : public LeafSystem<double> {
+ public:
+  ImageSink() {
+    DeclareAbstractInputPort(systems::Value<ImageRgba8U>());
+    DeclareAbstractInputPort(systems::Value<ImageDepth32F>());
+    LeafSystem<double>::DeclarePeriodicPublish(0.1);
+  }
+
+  void DoPublish(
+      const Context<double>& context,
+      const std::vector<const systems::PublishEvent<double>*>&) const {
+    const systems::AbstractValue* input =
+        this->EvalAbstractInput(context, 0);
+    input = this->EvalAbstractInput(context, 1);
+    unused(input);
+  }
+};
+
+int DoMain() {
+  drake::lcm::DrakeLcm lcm;
+  DiagramBuilder<double> builder;
+
+  auto image_sub = builder.AddSystem(
+      systems::lcm::LcmSubscriberSystem::Make<image_array_t>(
+          FLAGS_channel_name, &lcm));
+  auto array_to_images = builder.AddSystem<LcmImageArrayToImages>();
+  builder.Connect(image_sub->get_output_port(),
+                  array_to_images->image_array_t_input_port());
+  auto image_sink = builder.AddSystem<ImageSink>();
+  builder.Connect(array_to_images->color_image_output_port(),
+                  image_sink->get_input_port(0));
+  builder.Connect(array_to_images->depth_image_output_port(),
+                  image_sink->get_input_port(1));
+
+  auto image_to_lcm_image_array =
+      builder.AddSystem<ImageToLcmImageArrayT>(true);
+  builder.Connect(
+      array_to_images->color_image_output_port(),
+      image_to_lcm_image_array->DeclareImageInputPort<PixelType::kRgba8U>(
+          "color"));
+  builder.Connect(array_to_images->depth_image_output_port(),
+      image_to_lcm_image_array->DeclareImageInputPort<PixelType::kDepth32F>(
+          "depth"));
+
+  auto image_array_lcm_publisher = builder.AddSystem(
+      lcm::LcmPublisherSystem::Make<robotlocomotion::image_array_t>(
+          FLAGS_publish_name, &lcm));
+  image_array_lcm_publisher->set_publish_period(0.1);
+  builder.Connect(
+      image_to_lcm_image_array->image_array_t_msg_output_port(),
+      image_array_lcm_publisher->get_input_port());
+
+  auto diagram = builder.Build();
+  auto context = diagram->CreateDefaultContext();
+  auto simulator = std::make_unique<systems::Simulator<double>>(
+      *diagram, std::move(context));
+
+  lcm.StartReceiveThread();
+  simulator->set_publish_at_initialization(true);
+  simulator->set_publish_every_time_step(false);
+  simulator->set_target_realtime_rate(1.0);
+  simulator->Initialize();
+  simulator->StepTo(FLAGS_duration);
+
+  return 0;
+}
+
+}  // namespace
+}  // namespace sensors
+}  // namespace systems
+}  // namespace drake
+
+int main(int argc, char* argv[]) {
+  gflags::ParseCommandLineFlags(&argc, &argv, true);
+  drake::logging::HandleSpdlogGflags();
+  return drake::systems::sensors::DoMain();
+}