diff --git a/systems/sensors/BUILD.bazel b/systems/sensors/BUILD.bazel index 091bfcefb0aa..2e41f4609ac5 100644 --- a/systems/sensors/BUILD.bazel +++ b/systems/sensors/BUILD.bazel @@ -29,6 +29,7 @@ drake_cc_package_library( ":gyroscope", ":image", ":image_to_lcm_image_array_t", + ":image_writer", ":lcm_image_array_to_images", ":lcm_image_traits", ":optitrack_sender", @@ -394,6 +395,20 @@ drake_cc_library( ], ) +drake_cc_library( + name = "image_writer", + srcs = ["image_writer.cc"], + hdrs = ["image_writer.h"], + deps = [ + ":image", + "//common:essential", + "//systems/framework", + "@spruce", + "@vtk//:vtkCommonDataModel", + "@vtk//:vtkIOImage", + ], +) + # === test/ === drake_cc_googletest( @@ -486,6 +501,18 @@ filegroup( ], ) +drake_cc_googletest( + name = "image_writer_test", + tags = vtk_test_tags(), + deps = [ + ":image_writer", + "//common:temp_directory", + "//common/test_utilities", + "@spruce", + "@vtk//:vtkIOImage", + ], +) + drake_cc_googletest( name = "rgbd_camera_test", data = [ diff --git a/systems/sensors/image_writer.cc b/systems/sensors/image_writer.cc new file mode 100644 index 000000000000..949c0899d8cd --- /dev/null +++ b/systems/sensors/image_writer.cc @@ -0,0 +1,248 @@ +#include "drake/systems/sensors/image_writer.h" + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include "fmt/ostream.h" + +namespace drake { +namespace systems { +namespace sensors { + +template +void SaveToFileHelper(const std::string& file_path, + const Image& image) { + const int width = image.width(); + const int height = image.height(); + const int num_channels = Image::kNumChannels; + + vtkSmartPointer writer; + vtkNew vtk_image; + vtk_image->SetDimensions(width, height, 1); + + // NOTE: This excludes *many* of the defined `PixelType` values. + switch (kPixelType) { + case PixelType::kRgba8U: + vtk_image->AllocateScalars(VTK_UNSIGNED_CHAR, num_channels); + writer = vtkSmartPointer::New(); + break; + case PixelType::kDepth32F: + vtk_image->AllocateScalars(VTK_FLOAT, num_channels); + writer = vtkSmartPointer::New(); + break; + case PixelType::kLabel16I: + vtk_image->AllocateScalars(VTK_UNSIGNED_SHORT, num_channels); + writer = vtkSmartPointer::New(); + break; + default: + throw std::logic_error( + "Unsupported image type; cannot be written to file"); + } + + auto image_ptr = reinterpret_cast::T*>( + vtk_image->GetScalarPointer()); + const int num_scalar_components = vtk_image->GetNumberOfScalarComponents(); + DRAKE_DEMAND(num_scalar_components == num_channels); + + for (int v = height - 1; v >= 0; --v) { + for (int u = 0; u < width; ++u) { + for (int c = 0; c < num_channels; ++c) { + image_ptr[c] = + static_cast::T>(image.at(u, v)[c]); + } + image_ptr += num_scalar_components; + } + } + + writer->SetFileName(file_path.c_str()); + writer->SetInputData(vtk_image.GetPointer()); + writer->Write(); +} + +void SaveToPng(const std::string& file_path, const ImageRgba8U& image) { + SaveToFileHelper(file_path, image); +} + +void SaveToTiff(const std::string& file_path, const ImageDepth32F& image) { + SaveToFileHelper(file_path, image); +} + +void SaveToPng(const std::string& file_path, const ImageLabel16I& image) { + SaveToFileHelper(file_path, image); +} + +ImageWriter::ImageWriter() { + // NOTE: This excludes *many* of the defined `PixelType` values. + labels_[PixelType::kRgba8U] = "color"; + extensions_[PixelType::kRgba8U] = ".png"; + labels_[PixelType::kDepth32F] = "depth"; + extensions_[PixelType::kLabel16I] = ".png"; + labels_[PixelType::kLabel16I] = "label"; + extensions_[PixelType::kDepth32F] = ".tiff"; +} + +template +const InputPort& ImageWriter::DeclareImageInputPort( + std::string port_name, std::string file_name_format, double publish_period, + double start_time) { + // Test to confirm valid pixel type. + static_assert(kPixelType == PixelType::kRgba8U || + kPixelType == PixelType::kDepth32F || + kPixelType == PixelType::kLabel16I, + "ImageWriter: the only supported pixel types are: kRgba8U, " + "kDepth32F, and kLabel16I"); + + if (publish_period <= 0) { + throw std::logic_error("ImageWriter: publish period must be positive"); + } + + // Confirms the implied directory is valid. + spruce::path test_dir = + DirectoryFromFormat(file_name_format, port_name, kPixelType); + FolderState folder_state = ValidateDirectory(test_dir); + if (folder_state != FolderState::kValid) { + std::string reason; + switch (folder_state) { + case FolderState::kMissing: + reason = "the directory does not exist"; + break; + case FolderState::kIsFile: + reason = "the directory is actually a file"; + break; + case FolderState::kUnwritable: + reason = "no permissions to write the directory"; + break; + default: + DRAKE_ABORT_MSG("Directory is not valid; unhandled failure condition"); + } + throw std::logic_error( + fmt::format("ImageWriter: The format string `{}` implied the invalid " + "directory: '{}'; {}", + file_name_format, test_dir.getStr(), reason)); + } + + // Confirms file has appropriate extension. + const std::string& extension = extensions_[kPixelType]; + if (file_name_format.substr(file_name_format.size() - extension.size()) != + extension) { + file_name_format += extension; + } + // TODO(SeanCurtis-TRI): Handle other issues that may arise with filename: + // - invalid symbols + // - invalid length + // - more? + + // Now configure the system for the valid port declaration. + const auto& port = + DeclareAbstractInputPort(port_name, systems::Value>()); + + PublishEvent event( + Event::TriggerType::kPeriodic, + [this, port_index = port.get_index()](const Context& context, + const PublishEvent&) { + WriteImage(context, port_index); + }); + DeclarePeriodicEvent>(publish_period, start_time, event); + port_info_.emplace_back(std::move(file_name_format), kPixelType); + + return port; +} + +template +void ImageWriter::WriteImage(const Context& context, int index) const { + const ImagePortInfo& data = port_info_[index]; + const Image* image = + this->EvalInputValue>(context, index); + if (image) { + const std::string& port_name = get_input_port(index).get_name(); + SaveToFileHelper(MakeFileName(data.format, data.pixel_type, + context.get_time(), port_name, data.count++), + *image); + return; + } + throw std::logic_error( + fmt::format("ImageWriter: {} image input port {} is not connected", + labels_.at(data.pixel_type), index)); +} + +std::string ImageWriter::MakeFileName(const std::string& format, + PixelType pixel_type, double time, + const std::string& port_name, + int count) const { + DRAKE_DEMAND(labels_.count(pixel_type) > 0); + + int64_t u_time = static_cast(time * 1e6 + 0.5); + int m_time = static_cast(time * 1e3 + 0.5); + return fmt::format(format, fmt::arg("port_name", port_name), + fmt::arg("image_type", labels_.at(pixel_type)), + fmt::arg("time_double", time), + fmt::arg("time_usec", u_time), + fmt::arg("time_msec", m_time), fmt::arg("count", count)); +} + +spruce::path ImageWriter::DirectoryFromFormat(const std::string& format, + const std::string& port_name, + PixelType pixel_type) const { + // Extract the directory. + size_t index = format.rfind('/'); + std::string dir_format = format.substr(0, index + 1); + // NOTE: [bcdelmosu] are all the characters in: double, msec, and usec. + // Technically, this will also key on '{time_mouse}', but if someone is + // putting that in their file path, they deserve whatever they get. + std::regex invalid_args("\\{count|time_[bcdelmosu]+\\}"); + std::smatch match; + std::regex_search(dir_format, match, invalid_args); + if (!match.empty()) { + throw std::logic_error( + "ImageWriter: The directory path cannot include time or image count"); + } + std::string dir = MakeFileName(dir_format, pixel_type, 0, port_name, 0); + return spruce::path(dir); +} + +ImageWriter::FolderState ImageWriter::ValidateDirectory( + const spruce::path& file_path) { + if (file_path.exists()) { + if (file_path.isDir()) { + if (access(file_path.getStr().c_str(), W_OK) == 0) { + return FolderState::kValid; + } else { + return FolderState::kUnwritable; + } + } else { + return FolderState::kIsFile; + } + } else { + return FolderState::kMissing; + } +} + +template const InputPort& ImageWriter::DeclareImageInputPort< + PixelType::kRgba8U>(std::string port_name, std::string file_name_format, + double publish_period, double start_time); +template const InputPort& ImageWriter::DeclareImageInputPort< + PixelType::kDepth32F>(std::string port_name, std::string file_name_format, + double publish_period, double start_time); +template const InputPort& ImageWriter::DeclareImageInputPort< + PixelType::kLabel16I>(std::string port_name, std::string file_name_format, + double publish_period, double start_time); + +template void ImageWriter::WriteImage( + const Context& context, int index) const; +template void ImageWriter::WriteImage( + const Context& context, int index) const; +template void ImageWriter::WriteImage( + const Context& context, int index) const; + +} // namespace sensors +} // namespace systems +} // namespace drake diff --git a/systems/sensors/image_writer.h b/systems/sensors/image_writer.h new file mode 100644 index 000000000000..0626cfec6348 --- /dev/null +++ b/systems/sensors/image_writer.h @@ -0,0 +1,258 @@ +#pragma once + +/** @file Provides utilities for writing images to disk. + + This file provides two sets of utilities: stand alone methods that can be + invoked in any context and a System that can be connected into a diagram to + automatically capture images during simulation at a fixed frequency. */ + +#include +#include +#include +#include + +#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 { + +/** @name Utility functions for writing common image types to disk. + + Given a fully-specified path to the file to write and corresponding image data, + these functions will _attempt_ to write the image data to the file. The + functions assume that the path is valid and writable. These functions will + attempt to write the image to the given file path. The file format will be + that indicated by the function name, but the extension will be whatever is + provided as input. + + These function do not do validation on the provided file path (existence, + writability, correspondence with image type, etc.) It relies on the caller to + have done so. */ +//@{ + +/** Writes the color (8-bit, RGBA) image data to disk. */ +void SaveToPng(const std::string& file_path, const ImageRgba8U& image); + +/** Writes the depth (32-bit) image data to disk. Png files do not support + channels larger than 16-bits and its support for floating point values is + also limited at best. So, depth images can only be written as tiffs. */ +void SaveToTiff(const std::string& file_path, const ImageDepth32F& image); + +/** Writes the label (16-bit) image data to disk. */ +void SaveToPng(const std::string& file_path, const ImageLabel16I& image); + +//@} + +/** A system for periodically writing images to the file system. The system does + not have a fixed set of input ports; the system can have an arbitrary number of + image input ports. Each input port is independently configured with respect to: + + - publish frequency, + - write location (directory) and image name, + - input image format (which, in turn, implies a file-system format), + - port name (which needs to be unique across all input ports in this system), + and + - context time at which output starts. + + By design, this system is intended to work with RgbdCamera, but can connect to + any output port that provides images. + + @system{ImageWriter, + @input_port{declared_image1} + @input_port{declared_image2} + @input_port{...} + @input_port{declared_imageN}, + } + + %ImageWriter supports three specific types of images: + + - ImageRgba8U - typically a color image written to disk as .png images. + - ImageDepth32F - typically a depth image, written to disk as .tiff images. + - ImageLabel16I - typically a label image, written to disk as .png images. + + Input ports are added to an %ImageWriter via DeclareImageInputPort(). See + that function's documentation for elaboration on how to configure image output. + It is important to note, that every declared image input port _must_ be + connected; otherwise, attempting to write an image from that port, will cause + an error in the system. */ +class ImageWriter : public LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ImageWriter) + + /** Constructs default instance with no image ports. */ + ImageWriter(); + + /** Declares and configures a new image input port. A port is configured by + providing: + + - a unique port name, + - an output file format string, + - a publish period, + - a start time, and + - an image type. + + Each port is evaluated independently, so that two ports on the same + %ImageWriter can write images to different locations at different + frequencies, etc. If images are to be kept in sync (e.g., registered color + and depth images), they should be given the same period and start time. + +

Specifying the times at which images are written

+ + Given a _positive_ publish period `p`, images will be written at times + contained in the list of times: `t = [0, 1⋅p, 2⋅p, ...]`. The start time + parameter determines what the _first_ output time will be. Given a "start + time" value `tₛ`, the frames will be written at: + `t = tₛ + [0, 1⋅p, 2⋅p, ...]`. + +

Specifying write location and output file names

+ + When writing image data to disk, the location and name of the output files + are controlled by a user-defined format string. The format string should be + compatible with `fmt::format()`. %ImageWriter provides several _named_ format + arguments that can be referenced in the format string: + + - `port_name` - The name of the port (see below). + - `image_type` - One of `color`, `depth`, or `label`, depending on the + image type requested. + - `time_double` - The time (in seconds) stored in the context at the + invocation of Publish(), represented as a double. + - `time_usec` - The time (in microseconds) stored in the context at the + invocation of Publish(), represented as a 64-bit integer. + - `time_msec` - The time (in milliseconds) stored in the context at the + invocation of Publish(), represented as an integer. + - `count` - The number of images that have been written from this + port (the first image would get zero, the Nᵗʰ would get + N - 1). This value increments _every_ time an image gets + written. + + File names can then be specified as shown in the following examples (assuming + the port was declared as a color image port, with a name of "my_port", a + period of 0.02 s (50 Hz), and a start time of 5 s. + + - `/home/user/images/{port_name}/{time_usec}` creates a sequence like: + - `/home/user/images/my_port/5000000.png` + - `/home/user/images/my_port/5020000.png` + - `/home/user/images/my_port/5040000.png` + - ... + - `/home/user/images/{image_type}/{time_msec:05}` creates a sequence like: + - `/home/user/images/color/05000.png` + - `/home/user/images/color/05020.png` + - `/home/user/images/color/05040.png` + - ... + - `/home/user/{port_name}/my_image_{count:03}.txt` creates a sequence like: + - `/home/user/my_port/my_image_000.txt.png` + - `/home/user/my_port/my_image_001.txt.png` + - `/home/user/my_port/my_image_002.txt.png` + - ... + + We call attention particularly to the following: + + - Note the zero-padding arguments in the second and third examples. Making + use of zero-padding typically facilitates _other_ processes. + - If the file name format does not end with an appropriate extension (e.g., + `.png` or `.tiff`), the extension will be added. + - The directory specified in the format will be tested for validity + (does it exist, is it a directory, can the program write to it). The + full _file name_ will _not_ be validated. If it is invalid (e.g., too + long, invalid characters, bad format substitution), images will silently + not be created. + - The third example uses the count flag -- regardless of start time, the + first file written will always be zero, the second one, etc. + - The directory can *only* depend `port_name` and `image_type`. It _cannot_ + depend on values that change over time (e.g., `time_double`, `count`, + etc. + + @param port_name The name of the port (must be unique among all image + ports). This string is available in the format + string as `port_name`. + @param file_name_format The `fmt::format()`-compatible string which defines + the context-dependent file name to write the image + to. + @param publish_period The period at which images read from this input port + are written in calls to Publish(). + @param start_time The minimum value for the context's time at which + images will be written in calls to Publish(). + @tparam kPixelType The representation of the per-pixel data (see + PixelType). Must be one of {PixelType::kRgba8U, + PixelType::kDepth32F, or PixelType::kLabel16I}. + @throws std::logic_error if (1) the directory encoded in the + `file_name_format` is not "valid" (see + documentation above for definition), + (2) `publish_period` is not positive, or + (3) `port_name` is used by a previous input port. + */ + template + const InputPort& DeclareImageInputPort(std::string port_name, + std::string file_name_format, + double publish_period, + double start_time); + + private: +#ifndef DRAKE_DOXYGEN_CXX + // Friend for facilitating unit testing. + friend class ImageWriterTester; +#endif + + // Does the work of writing image indexed by `i` to the disk. + template + void WriteImage(const Context& context, int index) const; + + // Creates a file name from the given format string and time. + std::string MakeFileName(const std::string& format, PixelType pixel_type, + double time, const std::string& port_name, + int count) const; + + // Given the file format string (and port-specific configuration values), + // extracts, tests, and returns the output folder information. + // The tests are in support of the statement that the directory path cannot + // depend on time. + // Examples: + // "a/b/c/" --> "a/b/c/" + // "a/b/c" --> "a/b" + // "a/{time_usec}/c" --> thrown exception. + // "a/{port_name}/c" --> "a/my_port" (assuming port_name = "my_port"). + spruce::path DirectoryFromFormat(const std::string& format, + const std::string& port_name, + PixelType pixel_type) const; + + enum class FolderState { + kValid, + kMissing, + kIsFile, + kUnwritable + }; + + // Returns true if the directory path provided is valid: it exists, it's a + // directory, and it's writable. + static FolderState ValidateDirectory(const spruce::path& file_path); + + // The per-input port data. + struct ImagePortInfo { + ImagePortInfo(std::string format_in, PixelType pixel_type_in) + : format(std::move(format_in)), pixel_type(pixel_type_in) {} + const std::string format; + const PixelType pixel_type; + // NOTE: This is made mutable as a low-cost mechanism for incrementing + // image writes without involving the overhead of discrete state. + mutable int count{0}; + // TODO(SeanCurtis-TRI): For copying this system, it may be necessary to + // also store the period and start time so that the ports in the copy can + // be properly instantiated. + }; + + // For each input port, this stores the corresponding image data. It is an + // invariant that port_info_.size() == get_num_input_ports(). + std::vector port_info_; + + std::unordered_map labels_; + std::unordered_map extensions_; +}; + +} // namespace sensors +} // namespace systems +} // namespace drake diff --git a/systems/sensors/pixel_types.h b/systems/sensors/pixel_types.h index d3870b50cca9..41785e6e550c 100644 --- a/systems/sensors/pixel_types.h +++ b/systems/sensors/pixel_types.h @@ -2,6 +2,7 @@ #include +#include "drake/common/hash.h" #include "drake/common/symbolic.h" namespace drake { @@ -129,3 +130,10 @@ struct ImageTraits { } // namespace sensors } // namespace systems } // namespace drake + +// Enable the pixel type enumeration to be used as a map key. +namespace std { +template <> +struct hash + : public drake::DefaultHash {}; +} // namespace std diff --git a/systems/sensors/test/image_writer_test.cc b/systems/sensors/test/image_writer_test.cc new file mode 100644 index 000000000000..6282040285bf --- /dev/null +++ b/systems/sensors/test/image_writer_test.cc @@ -0,0 +1,694 @@ +#include "drake/systems/sensors/image_writer.h" + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/temp_directory.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/systems/framework/event_collection.h" + +namespace drake { +namespace systems { +namespace sensors { + +// Friend class to get access to ImageWriter private functions for testing. +class ImageWriterTester { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ImageWriterTester) + + explicit ImageWriterTester(const ImageWriter& writer) : writer_(writer) {} + + spruce::path DirectoryFromFormat(const std::string& format, + const std::string& port_name, + PixelType pixel_type) const { + return writer_.DirectoryFromFormat(format, port_name, pixel_type); + } + + static bool IsDirectoryValid(const spruce::path& file_path) { + return ImageWriter::ValidateDirectory(file_path) == + ImageWriter::FolderState::kValid; + } + + static bool DirectoryIsMissing(const spruce::path& file_path) { + return ImageWriter::ValidateDirectory(file_path) == + ImageWriter::FolderState::kMissing; + } + + static bool DirectoryIsFile(const spruce::path& file_path) { + return ImageWriter::ValidateDirectory(file_path) == + ImageWriter::FolderState::kIsFile; + } + + static bool DirectoryIsUnwritable(const spruce::path& file_path) { + return ImageWriter::ValidateDirectory(file_path) == + ImageWriter::FolderState::kUnwritable; + } + + std::string MakeFileName(const std::string& format, PixelType pixel_type, + double time, const std::string& port_name, + int count) const { + return writer_.MakeFileName(format, pixel_type, time, port_name, count); + } + + const std::string& port_format(int port_index) const { + return writer_.port_info_[port_index].format; + } + + int port_count(int port_index) const { + return writer_.port_info_[port_index].count; + } + + const std::string& label(PixelType pixel_type) const { + return writer_.labels_.at(pixel_type); + } + + const std::string& extension(PixelType pixel_type) const { + return writer_.extensions_.at(pixel_type); + } + + private: + const ImageWriter& writer_; +}; + +namespace { + +// Utility functions for creating test, reference images + +template +static Image test_image() { + throw std::logic_error("No default implementation"); +} + +template <> +Image test_image() { + // Creates a simple 4x1 image consisting of: [red][green][blue][white]. + Image color_image(4, 1); + auto set_color = [&color_image](int x, int y, uint8_t r, uint8_t g, + uint8_t b) { + color_image.at(x, y)[0] = r; + color_image.at(x, y)[1] = g; + color_image.at(x, y)[2] = b; + color_image.at(x, y)[3] = 255; + }; + set_color(0, 0, 255, 0, 0); + set_color(1, 0, 0, 255, 0); + set_color(2, 0, 0, 0, 255); + set_color(3, 0, 255, 255, 255); + return color_image; +} + +template <> +Image test_image() { + // Creates a simple 4x1 image consisting of: 0, 0.25, 0.5, 0.75 + Image depth_image(4, 1); + *depth_image.at(0, 0) = 0.0f; + *depth_image.at(1, 0) = 0.25f; + *depth_image.at(2, 0) = 0.5f; + *depth_image.at(3, 0) = 1.0f; + return depth_image; +} + +template <> +Image test_image() { + // Creates a simple 4x1 image consisting of: 0, 100, 200, 300. + Image label_image(4, 1); + *label_image.at(0, 0) = 0; + *label_image.at(1, 0) = 100; + *label_image.at(2, 0) = 200; + // Note: value > 255 to make sure that values aren't being truncated/wrapped + // to 8-bit values. + *label_image.at(3, 0) = 300; + return label_image; +} + +// Class for testing actual I/O work. This helps manage generated files by +// facilitating temporary file names and registering additional names so that +// they can be cleaned up at the conclusion of the tests. +class ImageWriterTest : public ::testing::Test { + public: + static void SetUpTestCase() { + ASSERT_TRUE(ImageWriterTester::IsDirectoryValid(temp_dir())); + } + + static void TearDownTestCase() { + for (const auto& file_name : files_) { + spruce::path file_path(file_name); + if (file_path.exists()) { + // We'll consider a failure to delete a temporary file as a test + // failure. + unlink(file_path.getStr().c_str()); + EXPECT_FALSE(file_path.exists()) + << "Failed to delete temporary test file: " << file_name; + } + } + } + + // This assumes that the temp_directory() API will *always* return the same + // name during the execution of this test. + static std::string temp_dir() { return temp_directory(); } + + // Returns a unique temporary image name - every requested name will be + // examined at tear down for deletion. When it comes to writing images, all + // names should come from here. + static std::string temp_name() { + spruce::path temp_path; + do { + temp_path.setStr(temp_dir()); + temp_path.append("image_writer_test_" + std::to_string(++img_count_) + + ".png"); + } while (temp_path.exists()); + files_.insert(temp_path.getStr()); + return temp_path.getStr(); + } + + // Arbitrary files that are generated can be added to the set of files that + // require clean up. This should be invoked for _every_ file generated in this + // test suite. + static void add_file_for_cleanup(const std::string& file_name) { + files_.insert(file_name); + } + + template + static ::testing::AssertionResult ReadImage(const std::string& image_name, + Image* image) { + spruce::path image_path(image_name); + if (image_path.exists()) { + vtkSmartPointer reader; + switch (kPixelType) { + case PixelType::kRgba8U: + case PixelType::kLabel16I: + reader = vtkSmartPointer::New(); + break; + case PixelType::kDepth32F: + reader = vtkSmartPointer::New(); + break; + default: + return ::testing::AssertionFailure() + << "Trying to read an unknown image type"; + } + reader->SetFileName(image_name.c_str()); + vtkNew exporter; + exporter->SetInputConnection(reader->GetOutputPort()); + exporter->Update(); + vtkImageData* image_data = exporter->GetInput(); + // Assumes 1-dimensional data -- the 4x1 image. + if (image_data->GetDataDimension() == 1) { + int read_width; + image_data->GetDimensions(&read_width); + if (read_width == image->width()) { + exporter->Export(image->at(0, 0)); + return ::testing::AssertionSuccess(); + } + } + int dims[3]; + exporter->GetDataDimensions(&dims[0]); + return ::testing::AssertionFailure() + << "Expected a " << image->width() << "x" << image->height() + << "image. Read an image of size: " << dims[0] << "x" << dims[1] + << "x" << dims[2]; + } else { + return ::testing::AssertionFailure() + << "The image to be read does not exist: " << image_name; + } + } + + template + static ::testing::AssertionResult MatchesFileOnDisk( + const std::string& file_name, const Image& expected) { + Image read_image(expected.width(), expected.height()); + auto result = ReadImage(file_name, &read_image); + if (result == ::testing::AssertionSuccess()) { + for (int u = 0; u < 4; ++u) { + for (int c = 0; c < ImageTraits::kNumChannels; ++c) { + if (read_image.at(u, 0)[c] != expected.at(u, 0)[c]) { + if (result != ::testing::AssertionFailure()) { + result = ::testing::AssertionFailure(); + } + result << "\nPixel (" << u << ", 0)[" << c << "] doesn't match. " + << "From disk(" << read_image.at(u, 0)[0] + << ", reference image: " << expected.at(u, 0)[0]; + } + } + } + } + return result; + } + + template + static void TestWritingImageOnPort() { + ImageWriter writer; + ImageWriterTester tester(writer); + + // Values for port declaration. + const double period = 1 / 10.0; // 10 Hz. + const double start_time = 0.25; + const std::string port_name = "port"; + spruce::path path(temp_dir()); + path.append("{image_type}_{time_usec}"); + + Image image = test_image(); + const auto& port = writer.DeclareImageInputPort( + port_name, path.getStr(), period, start_time); + auto events = writer.AllocateCompositeEventCollection(); + auto context = writer.AllocateContext(); + context->FixInputPort(port.get_index(), + AbstractValue::Make>(image)); + context->set_time(0.); + writer.CalcNextUpdateTime(*context, events.get()); + + const std::string expected_name = tester.MakeFileName( + tester.port_format(port.get_index()), kPixelType, context->get_time(), + port_name, tester.port_count(port.get_index())); + spruce::path expected_file(expected_name); + EXPECT_FALSE(expected_file.exists()); + writer.Publish(*context, events->get_publish_events()); + EXPECT_TRUE(expected_file.exists()); + EXPECT_EQ(1, tester.port_count(port.get_index())); + add_file_for_cleanup(expected_file.getStr()); + + EXPECT_TRUE(MatchesFileOnDisk(expected_name, image)); + } + + private: + // NOTE: These are static so that they are shared across the entire test + // suite. This allows all tests to have non-conflicting names *and* get + // cleaned up when the test suite shuts down. + + // This presumes the tests in a single test case do *not* run in parallel. + static int img_count_; + + // Files that may need to be cleaned up. It _must_ include every file that has + // been created by these tests but *may* include purely speculative file + // names. + static std::set files_; +}; + +int ImageWriterTest::img_count_{-1}; +std::set ImageWriterTest::files_; + +// ImageWriter contains a number of mappings between pixel type and work strings +// (extensions and image_type values for format args, this confirms that they +// are mapped correctly. +TEST_F(ImageWriterTest, ImageToStringMaps) { + ImageWriter writer; + ImageWriterTester tester(writer); + + EXPECT_EQ("color", tester.label(PixelType::kRgba8U)); + EXPECT_EQ("label", tester.label(PixelType::kLabel16I)); + EXPECT_EQ("depth", tester.label(PixelType::kDepth32F)); + + EXPECT_EQ(".png", tester.extension(PixelType::kRgba8U)); + EXPECT_EQ(".png", tester.extension(PixelType::kLabel16I)); + EXPECT_EQ(".tiff", tester.extension(PixelType::kDepth32F)); +} + +// Tests the processing of file format for extracting the directory. +TEST_F(ImageWriterTest, DirectoryFromFormat) { + ImageWriter writer; + ImageWriterTester tester{writer}; + + EXPECT_EQ("", + tester.DirectoryFromFormat("/root", "port_name", PixelType::kRgba8U) + .getStr()); + EXPECT_EQ( + "/root", + tester.DirectoryFromFormat("/root/", "port_name", PixelType::kRgba8U) + .getStr()); + EXPECT_EQ( + "/root", + tester.DirectoryFromFormat("/root/file", "port_name", PixelType::kRgba8U) + .getStr()); + // Don't use all three image types; the FileNameFormatting test already + // tests those permutations. We just want to make sure it's engaged here. + EXPECT_EQ("/root/color", + tester + .DirectoryFromFormat("/root/{image_type}/file", "port_name", + PixelType::kRgba8U) + .getStr()); + EXPECT_EQ("/root/my_port", + tester + .DirectoryFromFormat("/root/{port_name}/file", "my_port", + PixelType::kRgba8U) + .getStr()); + + // Test against invalid formatting arguments. + DRAKE_EXPECT_THROWS_MESSAGE( + tester.DirectoryFromFormat("/root/{count}/file", "port", + PixelType::kRgba8U), + std::logic_error, + ".*The directory path cannot include time or image count"); + DRAKE_EXPECT_THROWS_MESSAGE( + tester.DirectoryFromFormat("/root/{time_double}/file", "port", + PixelType::kRgba8U), + std::logic_error, + ".*The directory path cannot include time or image count"); + DRAKE_EXPECT_THROWS_MESSAGE( + tester.DirectoryFromFormat("/root/{time_usec}/file", "port", + PixelType::kRgba8U), + std::logic_error, + ".*The directory path cannot include time or image count"); + DRAKE_EXPECT_THROWS_MESSAGE( + tester.DirectoryFromFormat("/root/{time_msec}/file", "port", + PixelType::kRgba8U), + std::logic_error, + ".*The directory path cannot include time or image count"); + + // Make sure it's not fooled by strings that are *almost* format arguments. + EXPECT_EQ("/root/time_double", + tester + .DirectoryFromFormat("/root/time_double/file", "my_port", + PixelType::kRgba8U) + .getStr()); + EXPECT_EQ("/root/time_usec", + tester + .DirectoryFromFormat("/root/time_usec/file", "my_port", + PixelType::kRgba8U) + .getStr()); + EXPECT_EQ("/root/time_msec", + tester + .DirectoryFromFormat("/root/time_msec/file", "my_port", + PixelType::kRgba8U) + .getStr()); + EXPECT_EQ("/root/count", + tester + .DirectoryFromFormat("/root/count/file", "my_port", + PixelType::kRgba8U) + .getStr()); +} + +// Tests the logic for formatting images. +TEST_F(ImageWriterTest, FileNameFormatting) { + auto test_file_name = [](const ImageWriter& writer, const std::string& format, + PixelType pixel_type, double time, + const std::string& port_name, int count, + const std::string expected) { + const std::string path = ImageWriterTester(writer).MakeFileName( + format, pixel_type, time, port_name, count); + EXPECT_EQ(path, expected); + }; + + ImageWriter writer; + + // Completely hard-coded; not dependent on any of ImageWriter's baked values. + test_file_name(writer, "/hard/coded/file.png", PixelType::kRgba8U, 0, "port", + 0, "/hard/coded/file.png"); + + // Use the port name. + test_file_name(writer, "/hard/{port_name}/file.png", PixelType::kRgba8U, 0, + "port", 0, "/hard/port/file.png"); + + // Use the image type. + test_file_name(writer, "/hard/{image_type}/file.png", PixelType::kRgba8U, 0, + "port", 0, "/hard/color/file.png"); + test_file_name(writer, "/hard/{image_type}/file.png", PixelType::kDepth32F, 0, + "port", 0, "/hard/depth/file.png"); + test_file_name(writer, "/hard/{image_type}/file.png", PixelType::kLabel16I, 0, + "port", 0, "/hard/label/file.png"); + + // Use the time values. + test_file_name(writer, "/hard/{time_double:.2f}.png", PixelType::kRgba8U, 0, + "port", 0, "/hard/0.00.png"); + test_file_name(writer, "/hard/{time_double:.2f}.png", PixelType::kRgba8U, + 1.111, "port", 0, "/hard/1.11.png"); + test_file_name(writer, "/hard/{time_double:.2f}.png", PixelType::kRgba8U, + 1.116, "port", 0, "/hard/1.12.png"); + + test_file_name(writer, "/hard/{time_usec:03}.png", PixelType::kRgba8U, 0, + "port", 0, "/hard/000.png"); + test_file_name(writer, "/hard/{time_usec:03}.png", PixelType::kRgba8U, 1.111, + "port", 0, "/hard/1111000.png"); + + test_file_name(writer, "/hard/{time_msec:03}.png", PixelType::kRgba8U, 0, + "port", 0, "/hard/000.png"); + test_file_name(writer, "/hard/{time_msec:03}.png", PixelType::kRgba8U, 1.111, + "port", 0, "/hard/1111.png"); + + // Use the count value. + test_file_name(writer, "/hard/{count:03}.png", PixelType::kRgba8U, 1.111, + "port", 0, "/hard/000.png"); + test_file_name(writer, "/hard/{count:03}.png", PixelType::kRgba8U, 1.111, + "port", 12, "/hard/012.png"); + + // Bad place holders. + EXPECT_THROW( + test_file_name(writer, "/hard/{port}/file.png", PixelType::kRgba8U, 0, + "port", 0, "/hard/{port}/file.png"), + fmt::format_error); +} + +// Tests the write-ability of an image writer based on the validity of the +// directory path. +TEST_F(ImageWriterTest, ValidateDirectory) { + // Case: Non-existent directory. + EXPECT_TRUE(ImageWriterTester::DirectoryIsMissing( + spruce::path("this/path/does/not_exist"))); + + // Case: No write permissions (assuming that this isn't run as root). +EXPECT_TRUE(ImageWriterTester::DirectoryIsUnwritable(spruce::path("/root"))); + + // Case: the path is to a file. + const std::string file_name = temp_name(); + std::ofstream stream(file_name, std::ios::out); + ASSERT_FALSE(stream.fail()); + EXPECT_TRUE(ImageWriterTester::DirectoryIsFile(spruce::path(file_name))); +} + +// Confirm behavior on documented errors in +TEST_F(ImageWriterTest, ConfigureInputPortErrors) { + ImageWriter writer; + + // Bad publish period. + DRAKE_EXPECT_THROWS_MESSAGE(writer.DeclareImageInputPort( + "port", "format", -0.1, 0), + std::logic_error, + ".* publish period must be positive"); + + // Invalid directory -- relies on tested correctness of ValidateDirectory() + // and simply uses _one_ of the mechanisms for implying an invalid folder. + DRAKE_EXPECT_THROWS_MESSAGE( + writer.DeclareImageInputPort("port", "/root/name", + 0.1, 0), + std::logic_error, + ".*The format string .* implied the invalid directory.*"); + + // Now test a port with the same name -- can only happen if one port has + // been successfully declared. + spruce::path path(temp_dir()); + path.append("file_{count:3}"); + const auto& port = writer.DeclareImageInputPort( + "port", path.getStr(), 0.1, 0); + EXPECT_EQ(0, port.get_index()); + { + auto events = writer.AllocateCompositeEventCollection(); + auto context = writer.AllocateContext(); + writer.CalcNextUpdateTime(*context, events.get()); + EXPECT_TRUE(events->HasEvents()); + } + + spruce::path path2(temp_dir()); + path2.append("alt_file_{count:3}"); + DRAKE_EXPECT_THROWS_MESSAGE(writer.DeclareImageInputPort( + "port", path2.getStr(), 0.1, 0), + std::logic_error, + "System .* already has an input port named .*"); +} + +// Helper function for testing the extension produced for a given pixel type. +template +void TestPixelExtension(const std::string& folder, ImageWriter* writer, + int* count) { + using std::to_string; + + ImageWriterTester tester(*writer); + + spruce::path format(folder); + format.append("file"); + const auto& port = writer->DeclareImageInputPort( + "port" + to_string(++(*count)), format.getStr(), 1, 1); + const std::string& final_format = tester.port_format(port.get_index()); + EXPECT_NE(format.getStr(), final_format); + const std::string& ext = tester.extension(kPixelType); + EXPECT_EQ(ext, final_format.substr(final_format.size() - ext.size())); +} + +// This tests that format strings pick up the appropriate extension based on +// image type. +TEST_F(ImageWriterTest, FileExtension) { + using std::to_string; + + ImageWriter writer; + int count = 0; + + // Case: each image type applies the right extension to an extension-less + // format string. + TestPixelExtension(temp_dir(), &writer, &count); + TestPixelExtension(temp_dir(), &writer, &count); + TestPixelExtension(temp_dir(), &writer, &count); + + ImageWriterTester tester(writer); + // Case: Format string with correct extension remains unchanged. + { + spruce::path format(temp_dir()); + format.append("file.png"); + const auto& port = writer.DeclareImageInputPort( + "port" + to_string(++count), format.getStr(), 1, 1); + const std::string& final_format = tester.port_format(port.get_index()); + EXPECT_EQ(format.getStr(), final_format); + } + + // Case: wrong extension gets correct extension appended. + { + spruce::path format(temp_dir()); + format.append("file.txt"); + const auto& port = writer.DeclareImageInputPort( + "port" + to_string(++count), format.getStr(), 1, 1); + const std::string& final_format = tester.port_format(port.get_index()); + const std::string& ext = tester.extension(PixelType::kRgba8U); + EXPECT_EQ(format.getStr() + ext, final_format); + } +} + +// Probes the correctness of a single declared port. +TEST_F(ImageWriterTest, SingleConfiguredPort) { + ImageWriter writer; + ImageWriterTester tester(writer); + + // Freshly constructed, the writer has no timed events. + { + auto events = writer.AllocateCompositeEventCollection(); + auto context = writer.AllocateContext(); + writer.CalcNextUpdateTime(*context, events.get()); + EXPECT_FALSE(events->HasEvents()); + } + + // Values for port declaration. + const double period = 1 / 10.0; // 10 Hz. + const double start_time = 0.25; + const std::string port_name{"single_color_port"}; + const PixelType pixel_type = PixelType::kRgba8U; + spruce::path path(temp_dir()); + path.append("single_port_{time_usec}"); + + const auto& port = writer.DeclareImageInputPort( + port_name, path.getStr(), period, start_time); + + // Count gets properly initialized to zero (no images written from this port). + EXPECT_EQ(0, tester.port_count(port.get_index())); + + // Confirm a reported periodic event. The configuration parameters above are + // called out below in commented lines. + { + auto events = writer.AllocateCompositeEventCollection(); + auto context = writer.AllocateContext(); + context->set_time(0.); + double next_time = writer.CalcNextUpdateTime(*context, events.get()); + // Confirm start time fed into the periodic event. + EXPECT_EQ(start_time, next_time); + + EXPECT_TRUE(events->HasEvents()); + EXPECT_FALSE(events->HasDiscreteUpdateEvents()); + EXPECT_FALSE(events->HasUnrestrictedUpdateEvents()); + EXPECT_TRUE(events->HasPublishEvents()); + + { + const auto& publish_events = + dynamic_cast>&>( + events->get_publish_events()) + .get_events(); + ASSERT_EQ(1u, publish_events.size()); + const auto& event = publish_events.front(); + EXPECT_EQ(Event::TriggerType::kPeriodic, + event->get_trigger_type()); + + // With no connection on the input port, publishing this event will result + // in an error. + DRAKE_EXPECT_THROWS_MESSAGE( + writer.Publish(*context, events->get_publish_events()), + std::logic_error, ".* image input port \\d+ is not connected"); + + // Confirms that a valid publish increments the counter. + context->FixInputPort( + port.get_index(), + AbstractValue::Make(test_image())); + + const std::string expected_name = tester.MakeFileName( + tester.port_format(port.get_index()), pixel_type, context->get_time(), + port_name, tester.port_count(port.get_index())); + spruce::path expected_file(expected_name); + EXPECT_FALSE(expected_file.exists()); + writer.Publish(*context, events->get_publish_events()); + EXPECT_TRUE(expected_file.exists()); + EXPECT_EQ(1, tester.port_count(port.get_index())); + add_file_for_cleanup(expected_file.getStr()); + } + + // Confirm period is correct. + context->set_time(start_time + 0.1 * period); + events->Clear(); + next_time = writer.CalcNextUpdateTime(*context, events.get()); + EXPECT_EQ(start_time + period, next_time); + } +} + +// This simply confirms that the color image gets written to the right format. +TEST_F(ImageWriterTest, WritesColorImage) { + TestWritingImageOnPort(); +} + +// This confirms that the label image gets written properly. +TEST_F(ImageWriterTest, WritesLabelImage) { + TestWritingImageOnPort(); +} + +// This simply confirms that the depth image gets written to the right format. +TEST_F(ImageWriterTest, WritesDepthImage) { + TestWritingImageOnPort(); +} + +// Evaluate the stand-alone test for color images. +TEST_F(ImageWriterTest, SaveToPng_Color) { + ImageRgba8U color_image = test_image(); + + const std::string color_image_name = temp_name(); + SaveToPng(color_image_name, color_image); + + EXPECT_TRUE(MatchesFileOnDisk(color_image_name, color_image)); +} + +// Evaluate the stand-alone test for depth images. +TEST_F(ImageWriterTest, SaveToTiff_Depth) { + ImageDepth32F depth_image = test_image(); + + const std::string depth_image_name = temp_name(); + SaveToTiff(depth_image_name, depth_image); + + EXPECT_TRUE(MatchesFileOnDisk(depth_image_name, depth_image)); +} + +// Evaluate the stand-alone test for label images. +TEST_F(ImageWriterTest, SaveToPng_Label) { + ImageLabel16I label_image = test_image(); + + const std::string label_image_name = temp_name(); + SaveToPng(label_image_name, label_image); + + EXPECT_TRUE(MatchesFileOnDisk(label_image_name, label_image)); +} + +} // namespace +} // namespace sensors +} // namespace systems +} // namespace drake diff --git a/tools/workspace/default.bzl b/tools/workspace/default.bzl index d61c8dec7d72..91932fd60487 100644 --- a/tools/workspace/default.bzl +++ b/tools/workspace/default.bzl @@ -37,6 +37,7 @@ load("@drake//tools/workspace/lcmtypes_robotlocomotion:repository.bzl", "lcmtype load("@drake//tools/workspace/liblz4:repository.bzl", "liblz4_repository") load("@drake//tools/workspace/libpng:repository.bzl", "libpng_repository") load("@drake//tools/workspace/libprotobuf:repository.bzl", "libprotobuf_repository") # noqa +load("@drake//tools/workspace/libtiff:repository.bzl", "libtiff_repository") load("@drake//tools/workspace/mosek:repository.bzl", "mosek_repository") load("@drake//tools/workspace/net_sf_jchart2d:repository.bzl", "net_sf_jchart2d_repository") # noqa load("@drake//tools/workspace/nlopt:repository.bzl", "nlopt_repository") @@ -152,6 +153,8 @@ def add_default_repositories(excludes = [], mirrors = DEFAULT_MIRRORS): libpng_repository(name = "libpng") if "libprotobuf" not in excludes: libprotobuf_repository(name = "libprotobuf") + if "libtiff" not in excludes: + libtiff_repository(name = "libtiff") if "mosek" not in excludes: mosek_repository(name = "mosek") if "net_sf_jchart2d" not in excludes: diff --git a/tools/workspace/libtiff/BUILD.bazel b/tools/workspace/libtiff/BUILD.bazel new file mode 100644 index 000000000000..2e5301dc6578 --- /dev/null +++ b/tools/workspace/libtiff/BUILD.bazel @@ -0,0 +1,8 @@ +# -*- python -*- + +# This file exists to make our directory into a Bazel package, so that our +# neighboring *.bzl file can be loaded elsewhere. + +load("//tools/lint:lint.bzl", "add_lint_tests") + +add_lint_tests() diff --git a/tools/workspace/libtiff/repository.bzl b/tools/workspace/libtiff/repository.bzl new file mode 100644 index 000000000000..40cd5eeabfc6 --- /dev/null +++ b/tools/workspace/libtiff/repository.bzl @@ -0,0 +1,20 @@ +# -*- mode: python -*- + +load( + "@drake//tools/workspace:pkg_config.bzl", + "pkg_config_repository", +) + +def libtiff_repository( + name, + licenses = ["notice"], # Libtiff + modname = "libtiff-4", + pkg_config_paths = ["/usr/local/opt/libtiff/lib/pkgconfig"], + **kwargs): + pkg_config_repository( + name = name, + licenses = licenses, + modname = modname, + pkg_config_paths = pkg_config_paths, + **kwargs + ) diff --git a/tools/workspace/vtk/repository.bzl b/tools/workspace/vtk/repository.bzl index 8bd7d2c03140..201eb95106b0 100644 --- a/tools/workspace/vtk/repository.bzl +++ b/tools/workspace/vtk/repository.bzl @@ -504,6 +504,8 @@ licenses([ "vtkJPEGReader.h", "vtkPNGReader.h", "vtkPNGWriter.h", + "vtkTIFFReader.h", + "vtkTIFFWriter.h", ], deps = [ ":vtkCommonCore", @@ -512,6 +514,7 @@ licenses([ ":vtkmetaio", "@libpng", "@zlib", + "@libtiff", ], )