diff --git a/examples/manipulation_station/manipulation_station.cc b/examples/manipulation_station/manipulation_station.cc index 743d92831d44..bfb9596e47cd 100644 --- a/examples/manipulation_station/manipulation_station.cc +++ b/examples/manipulation_station/manipulation_station.cc @@ -348,7 +348,7 @@ void ManipulationStation::Finalize() { builder.template AddSystem( "camera" + std::to_string(i), geometry::dev::SceneGraph::world_frame_id(), - get_camera_pose(i), camera_properties, true); + get_camera_pose(i), camera_properties, false); builder.Connect(render_scene_graph->get_query_output_port(), camera->query_object_input_port()); diff --git a/examples/manipulation_station/proof_of_life.cc b/examples/manipulation_station/proof_of_life.cc index 10756d665839..3e6888c78c6c 100644 --- a/examples/manipulation_station/proof_of_life.cc +++ b/examples/manipulation_station/proof_of_life.cc @@ -8,6 +8,7 @@ #include "drake/systems/framework/diagram.h" #include "drake/systems/framework/diagram_builder.h" #include "drake/systems/sensors/image.h" +#include "drake/systems/sensors/image_to_lcm_image_array_t.h" #include "drake/systems/sensors/visualization/sensors_visualization.h" namespace drake { @@ -39,10 +40,32 @@ int do_main(int argc, char* argv[]) { geometry::ConnectDrakeVisualizer(&builder, station->get_mutable_scene_graph(), station->GetOutputPort("pose_bundle")); - systems::sensors::ConnectRgbdCameraToDrakeVisualizer( - &builder, station->GetOutputPort("camera0_rgb_image"), - station->GetOutputPort("camera0_depth_image"), - station->GetOutputPort("camera0_label_image")); + /* + systems::sensors::ConnectRgbdCameraToDrakeVisualizer( + &builder, station->GetOutputPort("camera0_rgb_image"), + station->GetOutputPort("camera0_depth_image"), + station->GetOutputPort("camera0_label_image")); + */ + auto image_to_lcm_image_array = + builder.template AddSystem(); + image_to_lcm_image_array->set_name("converter"); + for (int i = 0; i < 3; i++) { + const auto& cam_port = + image_to_lcm_image_array + ->DeclareImageInputPort( + "camera" + std::to_string(i)); + builder.Connect( + station->GetOutputPort("camera" + std::to_string(i) + "_rgb_image"), + cam_port); + } + auto image_array_lcm_publisher = builder.template AddSystem( + systems::lcm::LcmPublisherSystem::Make( + "DRAKE_RGBD_CAMERA_IMAGES", nullptr)); + + image_array_lcm_publisher->set_name("rgbd_publisher"); + image_array_lcm_publisher->set_publish_period(1. / 10 /* 10 fps */); + builder.Connect(image_to_lcm_image_array->image_array_t_msg_output_port(), + image_array_lcm_publisher->get_input_port()); auto diagram = builder.Build(); diff --git a/systems/sensors/image_to_lcm_image_array_t.cc b/systems/sensors/image_to_lcm_image_array_t.cc index f900f8348303..f75b26b2c0cf 100644 --- a/systems/sensors/image_to_lcm_image_array_t.cc +++ b/systems/sensors/image_to_lcm_image_array_t.cc @@ -52,21 +52,99 @@ void Pack(const Image& image, image_t* msg) { memcpy(&msg->data[0], image.at(0, 0), size); } +template +struct LcmPixelTraits; + +template <> +struct LcmPixelTraits { + static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_RGB; +}; + +template <> +struct LcmPixelTraits { + static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_BGR; +}; + +template <> +struct LcmPixelTraits { + static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_RGBA; +}; + +template <> +struct LcmPixelTraits { + static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_BGRA; +}; + +template <> +struct LcmPixelTraits { + static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_GRAY; +}; + +template <> +struct LcmPixelTraits { + static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_DEPTH; +}; + +template <> +struct LcmPixelTraits { + static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_LABEL; +}; + +template +struct LcmImageTraits; + +template <> +struct LcmImageTraits { + static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_UINT8; +}; + +template <> +struct LcmImageTraits { + static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_UINT8; +}; + +template <> +struct LcmImageTraits { + static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_UINT8; +}; + +template <> +struct LcmImageTraits { + static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_UINT8; +}; + +template <> +struct LcmImageTraits { + static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_FLOAT32; +}; + +template <> +struct LcmImageTraits { + static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_UINT16; +}; + +template <> +struct LcmImageTraits { + static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_FLOAT32; +}; + +template <> +struct LcmImageTraits { + static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_INT16; +}; + template -void PackImageToLcmImageT(const Image& image, int64_t utime, - uint8_t pixel_format, uint8_t channel_type, - const string& frame_name, image_t* msg, +void PackImageToLcmImageT(const Image& image, image_t* msg, bool do_compress) { // TODO(kunimatsu-tri) Fix seq here that is always set to zero. msg->header.seq = 0; - msg->header.utime = utime; - msg->header.frame_name = frame_name; msg->width = image.width(); msg->height = image.height(); msg->row_stride = image.kPixelSize * msg->width; msg->bigendian = false; - msg->pixel_format = pixel_format; - msg->channel_type = channel_type; + msg->pixel_format = + LcmPixelTraits::kPixelFormat>::kPixelFormat; + msg->channel_type = LcmImageTraits::kChannelType; if (do_compress) { Compress(image, msg); @@ -75,47 +153,118 @@ void PackImageToLcmImageT(const Image& image, int64_t utime, } } +void PackImageToLcmImageT(const AbstractValue& untyped_image, + PixelType pixel_type, int64_t utime, + const string& frame_name, image_t* msg, + bool do_compress) { + msg->header.utime = utime; + msg->header.frame_name = frame_name; + + switch (pixel_type) { + case PixelType::kRgb8U: { + const auto& image_value = + untyped_image.GetValue>(); + PackImageToLcmImageT(image_value, msg, do_compress); + break; + } + case PixelType::kBgr8U: { + const auto& image_value = + untyped_image.GetValue>(); + PackImageToLcmImageT(image_value, msg, do_compress); + break; + } + case PixelType::kRgba8U: { + const auto& image_value = + untyped_image.GetValue>(); + PackImageToLcmImageT(image_value, msg, do_compress); + break; + } + case PixelType::kBgra8U: { + const auto& image_value = + untyped_image.GetValue>(); + PackImageToLcmImageT(image_value, msg, do_compress); + break; + } + case PixelType::kGrey8U: { + const auto& image_value = + untyped_image.GetValue>(); + PackImageToLcmImageT(image_value, msg, do_compress); + break; + } + case PixelType::kDepth16U: { + const auto& image_value = + untyped_image.GetValue>(); + PackImageToLcmImageT(image_value, msg, do_compress); + break; + } + case PixelType::kDepth32F: { + const auto& image_value = + untyped_image.GetValue>(); + PackImageToLcmImageT(image_value, msg, do_compress); + break; + } + case PixelType::kLabel16I: { + const auto& image_value = + untyped_image.GetValue>(); + PackImageToLcmImageT(image_value, msg, do_compress); + break; + } + case PixelType::kExpr: + DRAKE_ABORT_MSG("PixelType::kExpr is not supported."); + } +} + } // anonymous namespace +ImageToLcmImageArrayT::ImageToLcmImageArrayT(bool do_compress) + : do_compress_(do_compress) { + image_array_t_msg_output_port_index_ = + DeclareAbstractOutputPort(&ImageToLcmImageArrayT::CalcImageArray) + .get_index(); +} + ImageToLcmImageArrayT::ImageToLcmImageArrayT(const string& color_frame_name, const string& depth_frame_name, const string& label_frame_name, bool do_compress) - : color_frame_name_(color_frame_name), - depth_frame_name_(depth_frame_name), - label_frame_name_(label_frame_name), - do_compress_(do_compress) { + : do_compress_(do_compress) { color_image_input_port_index_ = - DeclareAbstractInputPort(systems::Value()).get_index(); - + DeclareImageInputPort(color_frame_name).get_index(); depth_image_input_port_index_ = - DeclareAbstractInputPort(systems::Value()).get_index(); - + DeclareImageInputPort(depth_frame_name).get_index(); label_image_input_port_index_ = - DeclareAbstractInputPort(systems::Value()).get_index(); + DeclareImageInputPort(label_frame_name).get_index(); image_array_t_msg_output_port_index_ = DeclareAbstractOutputPort(&ImageToLcmImageArrayT::CalcImageArray) .get_index(); } -const InputPort& -ImageToLcmImageArrayT::color_image_input_port() const { +template +const InputPort& ImageToLcmImageArrayT::DeclareImageInputPort( + const std::string& name) { + input_port_pixel_type_.push_back(kPixelType); + return this->DeclareAbstractInputPort(name, + systems::Value>()); +} + +const InputPort& ImageToLcmImageArrayT::color_image_input_port() const { + DRAKE_DEMAND(color_image_input_port_index_ >= 0); return this->get_input_port(color_image_input_port_index_); } -const InputPort& -ImageToLcmImageArrayT::depth_image_input_port() const { +const InputPort& ImageToLcmImageArrayT::depth_image_input_port() const { + DRAKE_DEMAND(depth_image_input_port_index_ >= 0); return this->get_input_port(depth_image_input_port_index_); } -const InputPort& -ImageToLcmImageArrayT::label_image_input_port() const { +const InputPort& ImageToLcmImageArrayT::label_image_input_port() const { + DRAKE_DEMAND(label_image_input_port_index_ >= 0); return this->get_input_port(label_image_input_port_index_); } -const OutputPort& -ImageToLcmImageArrayT::image_array_t_msg_output_port() const { +const OutputPort& ImageToLcmImageArrayT::image_array_t_msg_output_port() + const { return System::get_output_port(image_array_t_msg_output_port_index_); } @@ -126,53 +275,14 @@ void ImageToLcmImageArrayT::CalcImageArray( msg->num_images = 0; msg->images.clear(); - const AbstractValue* color_image_value = - this->EvalAbstractInput(context, color_image_input_port_index_); - - const AbstractValue* depth_image_value = - this->EvalAbstractInput(context, depth_image_input_port_index_); - - const AbstractValue* label_image_value = - this->EvalAbstractInput(context, label_image_input_port_index_); - - if (color_image_value) { - const ImageRgba8U& color_image = - color_image_value->GetValue(); - - image_t color_image_msg; - PackImageToLcmImageT(color_image, msg->header.utime, - image_t::PIXEL_FORMAT_RGBA, - image_t::CHANNEL_TYPE_UINT8, color_frame_name_, - &color_image_msg, - do_compress_); - msg->images.push_back(color_image_msg); - msg->num_images++; - } - - if (depth_image_value) { - const ImageDepth32F& depth_image = - depth_image_value->GetValue(); - image_t depth_image_msg; - PackImageToLcmImageT(depth_image, msg->header.utime, - image_t::PIXEL_FORMAT_DEPTH, - image_t::CHANNEL_TYPE_FLOAT32, depth_frame_name_, - &depth_image_msg, - do_compress_); - msg->images.push_back(depth_image_msg); - msg->num_images++; - } + for (int i = 0; i < get_num_input_ports(); i++) { + const AbstractValue* image_value = this->EvalAbstractInput(context, i); - if (label_image_value) { - const ImageLabel16I& label_image = - label_image_value->GetValue(); - - image_t label_image_msg; - PackImageToLcmImageT(label_image, msg->header.utime, - image_t::PIXEL_FORMAT_LABEL, - image_t::CHANNEL_TYPE_INT16, label_frame_name_, - &label_image_msg, - do_compress_); - msg->images.push_back(label_image_msg); + image_t image_msg; + PackImageToLcmImageT(*image_value, input_port_pixel_type_[i], + msg->header.utime, this->get_input_port(i).get_name(), + &image_msg, do_compress_); + msg->images.push_back(image_msg); msg->num_images++; } } diff --git a/systems/sensors/image_to_lcm_image_array_t.h b/systems/sensors/image_to_lcm_image_array_t.h index 8664654b60fb..a58606e54320 100644 --- a/systems/sensors/image_to_lcm_image_array_t.h +++ b/systems/sensors/image_to_lcm_image_array_t.h @@ -6,6 +6,7 @@ #include "drake/common/drake_copyable.h" #include "drake/systems/framework/leaf_system.h" +#include "drake/systems/sensors/pixel_types.h" namespace drake { namespace systems { @@ -26,7 +27,8 @@ class ImageToLcmImageArrayT : public systems::LeafSystem { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ImageToLcmImageArrayT) - /// An %ImageToLcmImageArrayT constructor. + /// An %ImageToLcmImageArrayT constructor. Declares three input ports -- + /// one color image, one depth image, and one label image. /// /// @param color_frame_name The frame name used for color image. /// @param depth_frame_name The frame name used for depth image. @@ -39,31 +41,40 @@ class ImageToLcmImageArrayT : public systems::LeafSystem { bool do_compress = false); /// Returns the input port containing a color image. + /// Note: Only valid if the color/depth/label constructor is used. const InputPort& color_image_input_port() const; /// Returns the input port containing a depth image. + /// Note: Only valid if the color/depth/label constructor is used. const InputPort& depth_image_input_port() const; /// Returns the input port containing a label image. + /// Note: Only valid if the color/depth/label constructor is used. const InputPort& label_image_input_port() const; /// Returns the abstract valued output port that contains a /// `Value`. const OutputPort& image_array_t_msg_output_port() const; + + /// Default constructor doesn't declare any ports. Use the Add*Input() + /// methods to declare them. + explicit ImageToLcmImageArrayT(bool do_compress = false); + + template + const InputPort& DeclareImageInputPort(const std::string& name); + private: void CalcImageArray(const systems::Context& context, robotlocomotion::image_array_t* msg) const; + int color_image_input_port_index_{-1}; int depth_image_input_port_index_{-1}; int label_image_input_port_index_{-1}; int image_array_t_msg_output_port_index_{-1}; - const std::string color_frame_name_; - const std::string depth_frame_name_; - const std::string label_frame_name_; - + std::vector input_port_pixel_type_{}; const bool do_compress_; };