Skip to content

Commit

Permalink
Render multiple camera images in drake visualizer
Browse files Browse the repository at this point in the history
  • Loading branch information
@RussTedrake
RussTedrake committed Oct 23, 2018
1 parent 04ef960 commit a3f913e
Show file tree
Hide file tree
Showing 4 changed files with 224 additions and 80 deletions.
2 changes: 1 addition & 1 deletion examples/manipulation_station/manipulation_station.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -348,7 +348,7 @@ void ManipulationStation<T>::Finalize() {
builder.template AddSystem<systems::sensors::dev::RgbdCamera>(
"camera" + std::to_string(i),
geometry::dev::SceneGraph<double>::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());

Expand Down
31 changes: 27 additions & 4 deletions examples/manipulation_station/proof_of_life.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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<systems::sensors::ImageToLcmImageArrayT>();
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<systems::sensors::PixelType::kRgba8U>(
"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<robotlocomotion::image_array_t>(
"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();

Expand Down
250 changes: 180 additions & 70 deletions systems/sensors/image_to_lcm_image_array_t.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,21 +52,99 @@ void Pack(const Image<kPixelType>& image, image_t* msg) {
memcpy(&msg->data[0], image.at(0, 0), size);
}

template <PixelFormat>
struct LcmPixelTraits;

template <>
struct LcmPixelTraits<PixelFormat::kRgb> {
static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_RGB;
};

template <>
struct LcmPixelTraits<PixelFormat::kBgr> {
static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_BGR;
};

template <>
struct LcmPixelTraits<PixelFormat::kRgba> {
static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_RGBA;
};

template <>
struct LcmPixelTraits<PixelFormat::kBgra> {
static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_BGRA;
};

template <>
struct LcmPixelTraits<PixelFormat::kGrey> {
static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_GRAY;
};

template <>
struct LcmPixelTraits<PixelFormat::kDepth> {
static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_DEPTH;
};

template <>
struct LcmPixelTraits<PixelFormat::kLabel> {
static constexpr uint8_t kPixelFormat = image_t::PIXEL_FORMAT_LABEL;
};

template <PixelType>
struct LcmImageTraits;

template <>
struct LcmImageTraits<PixelType::kRgb8U> {
static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_UINT8;
};

template <>
struct LcmImageTraits<PixelType::kBgr8U> {
static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_UINT8;
};

template <>
struct LcmImageTraits<PixelType::kRgba8U> {
static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_UINT8;
};

template <>
struct LcmImageTraits<PixelType::kBgra8U> {
static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_UINT8;
};

template <>
struct LcmImageTraits<PixelType::kGrey8U> {
static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_FLOAT32;
};

template <>
struct LcmImageTraits<PixelType::kDepth16U> {
static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_UINT16;
};

template <>
struct LcmImageTraits<PixelType::kDepth32F> {
static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_FLOAT32;
};

template <>
struct LcmImageTraits<PixelType::kLabel16I> {
static constexpr uint8_t kChannelType = image_t::CHANNEL_TYPE_INT16;
};

template <PixelType kPixelType>
void PackImageToLcmImageT(const Image<kPixelType>& image, int64_t utime,
uint8_t pixel_format, uint8_t channel_type,
const string& frame_name, image_t* msg,
void PackImageToLcmImageT(const Image<kPixelType>& 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<ImageTraits<kPixelType>::kPixelFormat>::kPixelFormat;
msg->channel_type = LcmImageTraits<kPixelType>::kChannelType;

if (do_compress) {
Compress(image, msg);
Expand All @@ -75,47 +153,118 @@ void PackImageToLcmImageT(const Image<kPixelType>& 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<Image<PixelType::kRgb8U>>();
PackImageToLcmImageT(image_value, msg, do_compress);
break;
}
case PixelType::kBgr8U: {
const auto& image_value =
untyped_image.GetValue<Image<PixelType::kBgr8U>>();
PackImageToLcmImageT(image_value, msg, do_compress);
break;
}
case PixelType::kRgba8U: {
const auto& image_value =
untyped_image.GetValue<Image<PixelType::kRgba8U>>();
PackImageToLcmImageT(image_value, msg, do_compress);
break;
}
case PixelType::kBgra8U: {
const auto& image_value =
untyped_image.GetValue<Image<PixelType::kBgra8U>>();
PackImageToLcmImageT(image_value, msg, do_compress);
break;
}
case PixelType::kGrey8U: {
const auto& image_value =
untyped_image.GetValue<Image<PixelType::kGrey8U>>();
PackImageToLcmImageT(image_value, msg, do_compress);
break;
}
case PixelType::kDepth16U: {
const auto& image_value =
untyped_image.GetValue<Image<PixelType::kDepth16U>>();
PackImageToLcmImageT(image_value, msg, do_compress);
break;
}
case PixelType::kDepth32F: {
const auto& image_value =
untyped_image.GetValue<Image<PixelType::kDepth32F>>();
PackImageToLcmImageT(image_value, msg, do_compress);
break;
}
case PixelType::kLabel16I: {
const auto& image_value =
untyped_image.GetValue<Image<PixelType::kLabel16I>>();
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<ImageRgba8U>()).get_index();

DeclareImageInputPort<PixelType::kRgba8U>(color_frame_name).get_index();
depth_image_input_port_index_ =
DeclareAbstractInputPort(systems::Value<ImageDepth32F>()).get_index();

DeclareImageInputPort<PixelType::kDepth32F>(depth_frame_name).get_index();
label_image_input_port_index_ =
DeclareAbstractInputPort(systems::Value<ImageLabel16I>()).get_index();
DeclareImageInputPort<PixelType::kLabel16I>(label_frame_name).get_index();

image_array_t_msg_output_port_index_ =
DeclareAbstractOutputPort(&ImageToLcmImageArrayT::CalcImageArray)
.get_index();
}

const InputPort<double>&
ImageToLcmImageArrayT::color_image_input_port() const {
template <PixelType kPixelType>
const InputPort<double>& ImageToLcmImageArrayT::DeclareImageInputPort(
const std::string& name) {
input_port_pixel_type_.push_back(kPixelType);
return this->DeclareAbstractInputPort(name,
systems::Value<Image<kPixelType>>());
}

const InputPort<double>& 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<double>&
ImageToLcmImageArrayT::depth_image_input_port() const {
const InputPort<double>& 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<double>&
ImageToLcmImageArrayT::label_image_input_port() const {
const InputPort<double>& 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<double>&
ImageToLcmImageArrayT::image_array_t_msg_output_port() const {
const OutputPort<double>& ImageToLcmImageArrayT::image_array_t_msg_output_port()
const {
return System<double>::get_output_port(image_array_t_msg_output_port_index_);
}

Expand All @@ -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<ImageRgba8U>();

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<ImageDepth32F>();
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<ImageLabel16I>();

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++;
}
}
Expand Down
Loading

0 comments on commit a3f913e

Please sign in to comment.