diff --git a/systems/sensors/BUILD.bazel b/systems/sensors/BUILD.bazel index ea003397287d..01a390ae8d82 100644 --- a/systems/sensors/BUILD.bazel +++ b/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 = [ diff --git a/systems/sensors/lcm_image_array_receive_example.cc b/systems/sensors/lcm_image_array_receive_example.cc new file mode 100644 index 000000000000..534fb6d792fc --- /dev/null +++ b/systems/sensors/lcm_image_array_receive_example.cc @@ -0,0 +1,105 @@ +#include + +#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 { + public: + ImageSink() { + DeclareAbstractInputPort(systems::Value()); + DeclareAbstractInputPort(systems::Value()); + LeafSystem::DeclarePeriodicPublish(0.1); + } + + void DoPublish( + const Context& context, + const std::vector*>&) const { + const systems::AbstractValue* input = + this->EvalAbstractInput(context, 0); + input = this->EvalAbstractInput(context, 1); + unused(input); + } +}; + +int DoMain() { + drake::lcm::DrakeLcm lcm; + DiagramBuilder builder; + + auto image_sub = builder.AddSystem( + systems::lcm::LcmSubscriberSystem::Make( + FLAGS_channel_name, &lcm)); + auto array_to_images = builder.AddSystem(); + builder.Connect(image_sub->get_output_port(), + array_to_images->image_array_t_input_port()); + auto image_sink = builder.AddSystem(); + 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(true); + builder.Connect( + array_to_images->color_image_output_port(), + image_to_lcm_image_array->DeclareImageInputPort( + "color")); + builder.Connect(array_to_images->depth_image_output_port(), + image_to_lcm_image_array->DeclareImageInputPort( + "depth")); + + auto image_array_lcm_publisher = builder.AddSystem( + lcm::LcmPublisherSystem::Make( + 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>( + *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(); +} diff --git a/systems/sensors/lcm_image_array_to_images.cc b/systems/sensors/lcm_image_array_to_images.cc new file mode 100644 index 000000000000..020c82971af3 --- /dev/null +++ b/systems/sensors/lcm_image_array_to_images.cc @@ -0,0 +1,272 @@ +#include "drake/systems/sensors/lcm_image_array_to_images.h" + +#include +#include +#include +#include + +#include "robotlocomotion/image_array_t.hpp" + +#include "drake/common/text_logging.h" +#include "drake/common/unused.h" + +using robotlocomotion::image_array_t; +using robotlocomotion::image_t; + +namespace drake { +namespace systems { +namespace sensors { +namespace { + +bool is_color_image(int8_t type) { + switch (type) { + case image_t::PIXEL_FORMAT_RGB: + case image_t::PIXEL_FORMAT_BGR: + case image_t::PIXEL_FORMAT_RGBA: + case image_t::PIXEL_FORMAT_BGRA: { + return true; + } + default: { + break; + } + } + return false; +} + +bool image_has_alpha(int8_t type) { + switch (type) { + case image_t::PIXEL_FORMAT_RGBA: + case image_t::PIXEL_FORMAT_BGRA: { + return true; + } + default: { + break; + } + } + return false; +} + +// TODO(sam.creasey) Unfortunately vtkPNGReader always attempt to open a file, +// even when used in the same manner as vtkJPEGReader below. We should +// eventually find another way to add PNG support. +void DecompressJpeg(const unsigned char* data, int size, void* out) { + std::vector buffer(size); + memcpy(buffer.data(), data, size); + + vtkNew jpg_reader; + jpg_reader->SetMemoryBuffer(buffer.data()); + jpg_reader->SetMemoryBufferLength(size); + jpg_reader->Update(); + + vtkNew exporter; + exporter->SetInputConnection(jpg_reader->GetOutputPort(0)); + exporter->ImageLowerLeftOff(); + exporter->Update(); + exporter->Export(out); +} + +} // namespace + +LcmImageArrayToImages::LcmImageArrayToImages() + : image_array_t_input_port_index_( + this->DeclareAbstractInputPort( + "image_array_t", Value()).get_index()), + color_image_output_port_index_( + this->DeclareAbstractOutputPort( + "color_image", &LcmImageArrayToImages::CalcColorImage) + .get_index()), + depth_image_output_port_index_( + this->DeclareAbstractOutputPort( + "depth_image", &LcmImageArrayToImages::CalcDepthImage) + .get_index()) { + // TODO(sammy-tri) Calculating our output ports can be kinda expensive. We + // should cache the images. +} + +void LcmImageArrayToImages::CalcColorImage( + const Context& context, ImageRgba8U* color_image) const { + const systems::AbstractValue* input = + this->EvalAbstractInput(context, image_array_t_input_port_index_); + DRAKE_ASSERT(input != nullptr); + const auto& images = input->GetValue(); + + // Look through the image array and just grab the first color image. + const image_t* image = nullptr; + for (int i = 0; i < images.num_images; i++) { + if (is_color_image(images.images[i].pixel_format)) { + image = &images.images[i]; + break; + } + } + + if (!image) { + *color_image = ImageRgba8U(); + return; + } + + color_image->resize(image->width, image->height); + const bool has_alpha = image_has_alpha(image->pixel_format); + ImageRgb8U rgb_image; + if (!has_alpha) { + rgb_image.resize(image->width, image->height); + } + + switch (image->compression_method) { + case image_t::COMPRESSION_METHOD_NOT_COMPRESSED: { + if (has_alpha) { + memcpy(color_image->at(0, 0), image->data.data(), color_image->size()); + } else { + memcpy(rgb_image.at(0, 0), image->data.data(), rgb_image.size()); + } + break; + } + case image_t::COMPRESSION_METHOD_ZLIB: { + int status = 0; + if (has_alpha) { + unsigned long dest_len = color_image->size(); + status = uncompress(color_image->at(0, 0), &dest_len, + image->data.data(), image->size); + } else { + unsigned long dest_len = rgb_image.size(); + status = uncompress(rgb_image.at(0, 0), &dest_len, + image->data.data(), image->size); + } + if (status != Z_OK) { + drake::log()->error("zlib decompression failed on incoming LCM image"); + *color_image = ImageRgba8U(); + return; + } + break; + } + case image_t::COMPRESSION_METHOD_JPEG: { + if (has_alpha) { + DecompressJpeg(image->data.data(), image->size, + color_image->at(0, 0)); + } else { + DecompressJpeg(image->data.data(), image->size, + rgb_image.at(0, 0)); + } + break; + } + default: { + drake::log()->error("Unsupported LCM compression method: {}", + image->compression_method); + *color_image = ImageRgba8U(); + return; + } + } + + if (!has_alpha) { + for (int x = 0; x < image->width; x++) { + for (int y = 0; y < image->height; y++) { + color_image->at(x, y)[0] = rgb_image.at(x, y)[0]; + color_image->at(x, y)[1] = rgb_image.at(x, y)[1]; + color_image->at(x, y)[2] = rgb_image.at(x, y)[2]; + color_image->at(x, y)[3] = 0xff; + } + } + } + + // TODO(sam.creasey) Handle BGR images, or at least error. +} + +void LcmImageArrayToImages::CalcDepthImage( + const Context& context, ImageDepth32F* depth_image) const { + const systems::AbstractValue* input = + this->EvalAbstractInput(context, image_array_t_input_port_index_); + DRAKE_ASSERT(input != nullptr); + const auto& images = input->GetValue(); + + // Look through the image array and just grab the first depth image. + const image_t* image = nullptr; + for (int i = 0; i < images.num_images; i++) { + if (images.images[i].pixel_format == image_t::PIXEL_FORMAT_DEPTH) { + image = &images.images[i]; + break; + } + } + + if (!image) { + *depth_image = ImageDepth32F(); + return; + } + + depth_image->resize(image->width, image->height); + + ImageDepth16U image_16u; + bool is_32f = false; + + switch (image->channel_type) { + case image_t::CHANNEL_TYPE_UINT16: { + is_32f = false; + image_16u.resize(image->width, image->height); + break; + } + case image_t::CHANNEL_TYPE_FLOAT32: { + is_32f = true; + break; + } + default: { + drake::log()->error("Unsupported depth image channel type: {}", + image->channel_type); + *depth_image = ImageDepth32F(); + return; + } + } + + switch (image->compression_method) { + case image_t::COMPRESSION_METHOD_NOT_COMPRESSED: { + if (is_32f) { + memcpy(depth_image->at(0, 0), image->data.data(), + depth_image->size() * depth_image->kPixelSize); + } else { + memcpy(image_16u.at(0, 0), image->data.data(), + image_16u.size() * image_16u.kPixelSize); + } + break; + } + case image_t::COMPRESSION_METHOD_ZLIB: { + int status = 0; + if (is_32f) { + unsigned long dest_len = depth_image->size() * depth_image->kPixelSize; + status = uncompress( + reinterpret_cast(depth_image->at(0, 0)), &dest_len, + image->data.data(), image->size); + } else { + unsigned long dest_len = image_16u.size() * image_16u.kPixelSize; + status = uncompress( + reinterpret_cast(image_16u.at(0, 0)), &dest_len, + image->data.data(), image->size); + + } + if (status != Z_OK) { + drake::log()->error("zlib decompression failed on incoming LCM image: {}", status); + *depth_image = ImageDepth32F(); + return; + } + break; + } + default: { + drake::log()->error("Unsupported LCM compression method: {}", + image->compression_method); + *depth_image = ImageDepth32F(); + return; + } + } + + if (!is_32f) { + for (int x = 0; x < image->width; x++) { + for (int y = 0; y < image->height; y++) { + depth_image->at(x, y)[0] = + static_cast(image_16u.at(x, y)[0]) / 1e3; + } + } + } +} + + + + +} // namespace sensors +} // namespace systems +} // namespace drake diff --git a/systems/sensors/lcm_image_array_to_images.h b/systems/sensors/lcm_image_array_to_images.h new file mode 100644 index 000000000000..ceca202ff7dd --- /dev/null +++ b/systems/sensors/lcm_image_array_to_images.h @@ -0,0 +1,60 @@ +#pragma once + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/systems/framework/leaf_system.h" +#include "drake/systems/sensors/image.h" + +namespace drake { +namespace systems { +namespace sensors { + +/// An LcmImageArrayToImages takes as input an AbstractValue containing a +/// `Value` LCM message that defines an array +/// of images (image_t). The system has output ports for one color image as +/// an ImageRgba8U and one depth image as ImageDepth32F. +class LcmImageArrayToImages : public LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(LcmImageArrayToImages) + + /// An %ImageToLcmImageArrayT constructor. + /// + /// @param color_frame_name The frame name used for color image. + /// @param depth_frame_name The frame name used for depth image. + /// @param label_frame_name The frame name used for label image. + /// @param do_compress When true, zlib compression will be performed. The + /// default is false. + LcmImageArrayToImages(); + + /// Returns the abstract valued input port that expects a + /// `Value`. + const InputPort& image_array_t_input_port() const { + return this->get_input_port(image_array_t_input_port_index_); + } + + /// Returns the abstract valued output port that contains a RGBA image of the + /// type ImageRgba8U. + const OutputPort& color_image_output_port() const { + return this->get_output_port(color_image_output_port_index_); + } + + /// Returns the abstract valued output port that contains an ImageDepth32F. + const OutputPort& depth_image_output_port() const { + return this->get_output_port(depth_image_output_port_index_); + } + + private: + void CalcColorImage(const Context& context, + ImageRgba8U* color_image) const; + void CalcDepthImage(const Context& context, + ImageDepth32F* depth_image) const; + + const InputPortIndex image_array_t_input_port_index_{}; + const OutputPortIndex color_image_output_port_index_{}; + const OutputPortIndex depth_image_output_port_index_{}; +}; + +} // namespace sensors +} // namespace systems +} // namespace drake