forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
91 lines
3.4 KiB
91 lines
3.4 KiB
2 years ago
|
#include "drake/systems/sensors/sim_rgbd_sensor.h"
|
||
|
|
|
||
|
|
#include <memory>
|
||
|
|
#include <optional>
|
||
|
|
#include <regex>
|
||
|
|
|
||
|
|
#include "drake/systems/lcm/lcm_publisher_system.h"
|
||
|
|
#include "drake/systems/sensors/image_to_lcm_image_array_t.h"
|
||
|
|
|
||
|
|
namespace drake {
|
||
|
|
namespace systems {
|
||
|
|
namespace sensors {
|
||
|
|
namespace internal {
|
||
|
|
|
||
|
|
using geometry::SceneGraph;
|
||
|
|
using geometry::render::ColorRenderCamera;
|
||
|
|
using geometry::render::DepthRenderCamera;
|
||
|
|
using math::RigidTransformd;
|
||
|
|
using multibody::MultibodyPlant;
|
||
|
|
|
||
|
|
RgbdSensor* AddSimRgbdSensor(const SceneGraph<double>& scene_graph,
|
||
|
|
const MultibodyPlant<double>& plant,
|
||
|
|
const SimRgbdSensor& sim_rgbd_sensor,
|
||
|
|
DiagramBuilder<double>* builder) {
|
||
|
|
DRAKE_DEMAND(builder != nullptr);
|
||
|
|
// TODO(eric.cousineau): Simplify this if drake#10247 is resolved.
|
||
|
|
|
||
|
|
// 'A' is the body for the parent sensor frame.
|
||
|
|
// `P` is the parent of the sensor frame.
|
||
|
|
// `B` is the sensor frame.
|
||
|
|
|
||
|
|
const auto& frame_P = sim_rgbd_sensor.frame();
|
||
|
|
const auto& body_A = frame_P.body();
|
||
|
|
const std::optional<geometry::FrameId> body_A_id =
|
||
|
|
plant.GetBodyFrameIdIfExists(body_A.index());
|
||
|
|
DRAKE_THROW_UNLESS(body_A_id.has_value());
|
||
|
|
|
||
|
|
// We must include the offset of the body to ensure that we posture the camera
|
||
|
|
// appropriately.
|
||
|
|
const RigidTransformd X_AP = frame_P.GetFixedPoseInBodyFrame();
|
||
|
|
const RigidTransformd X_AB = X_AP * sim_rgbd_sensor.X_PB();
|
||
|
|
|
||
|
|
auto* rgbd_sensor_sys = builder->AddSystem<RgbdSensor>(
|
||
|
|
*body_A_id, X_AB, sim_rgbd_sensor.color_properties(),
|
||
|
|
sim_rgbd_sensor.depth_properties());
|
||
|
|
// `set_name` is only used for debugging.
|
||
|
|
rgbd_sensor_sys->set_name("rgbd_sensor_" + sim_rgbd_sensor.serial());
|
||
|
|
builder->Connect(scene_graph.get_query_output_port(),
|
||
|
|
rgbd_sensor_sys->query_object_input_port());
|
||
|
|
return rgbd_sensor_sys;
|
||
|
|
}
|
||
|
|
|
||
|
|
void AddSimRgbdSensorLcmPublisher(const SimRgbdSensor& sim_rgbd_sensor,
|
||
|
|
const OutputPort<double>* rgb_port,
|
||
|
|
const OutputPort<double>* depth_16u_port,
|
||
|
|
bool do_compress,
|
||
|
|
DiagramBuilder<double>* builder,
|
||
|
|
drake::lcm::DrakeLcmInterface* lcm) {
|
||
|
|
DRAKE_DEMAND(builder != nullptr);
|
||
|
|
DRAKE_DEMAND(lcm != nullptr);
|
||
|
|
if (!rgb_port && !depth_16u_port) return;
|
||
|
|
|
||
|
|
auto image_to_lcm_image_array =
|
||
|
|
builder->AddSystem<ImageToLcmImageArrayT>(do_compress);
|
||
|
|
image_to_lcm_image_array->set_name("image_to_lcm_" +
|
||
|
|
sim_rgbd_sensor.serial());
|
||
|
|
if (depth_16u_port) {
|
||
|
|
const auto& lcm_depth_port =
|
||
|
|
image_to_lcm_image_array->DeclareImageInputPort<PixelType::kDepth16U>(
|
||
|
|
"depth");
|
||
|
|
builder->Connect(*depth_16u_port, lcm_depth_port);
|
||
|
|
}
|
||
|
|
if (rgb_port) {
|
||
|
|
const auto& lcm_rgb_port =
|
||
|
|
image_to_lcm_image_array->DeclareImageInputPort<PixelType::kRgba8U>(
|
||
|
|
"rgb");
|
||
|
|
builder->Connect(*rgb_port, lcm_rgb_port);
|
||
|
|
}
|
||
|
|
auto image_array_lcm_publisher =
|
||
|
|
builder->AddSystem(lcm::LcmPublisherSystem::Make<lcmt_image_array>(
|
||
|
|
"DRAKE_RGBD_CAMERA_IMAGES_" + sim_rgbd_sensor.serial(), lcm,
|
||
|
|
1. / sim_rgbd_sensor.rate_hz()));
|
||
|
|
builder->Connect(image_to_lcm_image_array->image_array_t_msg_output_port(),
|
||
|
|
image_array_lcm_publisher->get_input_port());
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace internal
|
||
|
|
} // namespace sensors
|
||
|
|
} // namespace systems
|
||
|
|
} // namespace drake
|