def test_depth_image_to_point_cloud_api(self):
camera_info = CameraInfo(width=640, height=480, fov_y=np.pi / 4)
dut = mut.DepthImageToPointCloud(camera_info=camera_info)
self.assertIsInstance(dut.depth_image_input_port(), InputPort)
self.assertIsInstance(dut.point_cloud_output_port(), OutputPort)
dut = mut.DepthImageToPointCloud(
camera_info=camera_info,
pixel_type=PixelType.kDepth16U,
scale=0.001)
object_file_path = \
"drake/examples/manipulation_station/models/061_foam_brick.sdf"
brick = AddModelFromSdfFile(
FindResourceOrThrow(object_file_path),
station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph() )
station.Finalize()
# add systems to convert the depth images from ManipulationStation to
# PointClouds
left_camera_info = pc_to_pose.camera_configs["left_camera_info"]
middle_camera_info = pc_to_pose.camera_configs["middle_camera_info"]
right_camera_info = pc_to_pose.camera_configs["right_camera_info"]
left_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=left_camera_info))
middle_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=middle_camera_info))
right_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=right_camera_info))
left_name_prefix = \
"camera_" + pc_to_pose.camera_configs["left_camera_serial"]
middle_name_prefix = \
"camera_" + pc_to_pose.camera_configs["middle_camera_serial"]
right_name_prefix = \
"camera_" + pc_to_pose.camera_configs["right_camera_serial"]
# connect the depth images to the DepthImageToPointCloud converters
builder.Connect(station.GetOutputPort(left_name_prefix + "_depth_image"),
left_dut.depth_image_input_port())
def add_camera_system(builder, config_file, viz, build_station_real_world,
build_station_simulation):
# create the PointCloudToPoseSystem
if "sim.yml" in config_file:
pc_to_pose_left = builder.AddSystem(
PointCloudToPoseSystem(config_file,
viz,
SegmentDoorSimulationLeft,
ComputeLeftDoorPose,
name="left_door"))
pc_to_pose_right = builder.AddSystem(
PointCloudToPoseSystem(config_file,
viz,
SegmentDoorSimulationRight,
ComputeRightDoorPose,
name="right_door"))
elif "station_1.yml" in config_file or "station_2.yml" in config_file:
pc_to_pose_left = builder.AddSystem(
PointCloudToPoseSystem(config_file,
viz,
SegmentDoorRealWorldLeft,
ComputeLeftDoorPose,
name="left_door"))
pc_to_pose_right = builder.AddSystem(
PointCloudToPoseSystem(config_file,
viz,
SegmentDoorRealWorldRight,
ComputeRightDoorPose,
name="right_door"))
else:
raise ValueError(
"Could not detect whether config file is a simulation or station config."
)
# realsense serial numbers are >> 100
use_hardware = int(
pc_to_pose_left.camera_configs["left_camera_serial"]) > 100
if use_hardware:
camera_ids = [
pc_to_pose_left.camera_configs["left_camera_serial"],
pc_to_pose_left.camera_configs["middle_camera_serial"],
pc_to_pose_left.camera_configs["right_camera_serial"]
]
station = build_station_real_world(builder, camera_ids)
else:
station, brick = build_station_simulation(builder)
# add systems to convert the depth images from ManipulationStation to
# PointClouds
left_camera_info = pc_to_pose_left.camera_configs["left_camera_info"]
middle_camera_info = pc_to_pose_left.camera_configs["middle_camera_info"]
right_camera_info = pc_to_pose_left.camera_configs["right_camera_info"]
left_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=left_camera_info))
middle_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=middle_camera_info))
right_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=right_camera_info))
left_name_prefix = \
"camera_" + pc_to_pose_left.camera_configs["left_camera_serial"]
middle_name_prefix = \
"camera_" + pc_to_pose_left.camera_configs["middle_camera_serial"]
right_name_prefix = \
"camera_" + pc_to_pose_left.camera_configs["right_camera_serial"]
# connect the depth images to the DepthImageToPointCloud converters
builder.Connect(station.GetOutputPort(left_name_prefix + "_depth_image"),
left_dut.depth_image_input_port())
builder.Connect(station.GetOutputPort(middle_name_prefix + "_depth_image"),
middle_dut.depth_image_input_port())
builder.Connect(station.GetOutputPort(right_name_prefix + "_depth_image"),
right_dut.depth_image_input_port())
for pc_to_pose in [pc_to_pose_left, pc_to_pose_right]:
# connect the rgb images to the PointCloudToPoseSystem
builder.Connect(station.GetOutputPort(left_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_left_rgb"))
builder.Connect(
station.GetOutputPort(middle_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_middle_rgb"))
builder.Connect(
station.GetOutputPort(right_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_right_rgb"))
# connect the XYZ point clouds to PointCloudToPoseSystem
builder.Connect(left_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("left_point_cloud"))
builder.Connect(middle_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("middle_point_cloud"))
builder.Connect(right_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("right_point_cloud"))
if use_hardware:
return [use_hardware, station, pc_to_pose_left, pc_to_pose_right]
else:
return [
use_hardware, station, pc_to_pose_left, pc_to_pose_right, brick
]
def setUp(self):
builder = DiagramBuilder()
station = builder.AddSystem(ManipulationStation())
station.SetupDefaultStation()
station.Finalize()
# create the PointCloudToPoseSystem
config_file = "perception/config/sim.yml"
self.pc_to_pose = builder.AddSystem(
PointCloudToPoseSystem(config_file, viz=False))
# add systems to convert the depth images from ManipulationStation to
# PointClouds
left_camera_info = self.pc_to_pose.camera_configs["left_camera_info"]
middle_camera_info = \
self.pc_to_pose.camera_configs["middle_camera_info"]
right_camera_info = self.pc_to_pose.camera_configs["right_camera_info"]
left_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=left_camera_info))
middle_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=middle_camera_info))
right_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=right_camera_info))
left_name_prefix = "camera_" + \
self.pc_to_pose.camera_configs["left_camera_serial"]
middle_name_prefix = "camera_" + \
self.pc_to_pose.camera_configs["middle_camera_serial"]
right_name_prefix = "camera_" + \
self.pc_to_pose.camera_configs["right_camera_serial"]
# connect the depth images to the DepthImageToPointCloud converters
builder.Connect(
station.GetOutputPort(left_name_prefix + "_depth_image"),
left_dut.depth_image_input_port())
builder.Connect(
station.GetOutputPort(middle_name_prefix + "_depth_image"),
middle_dut.depth_image_input_port())
builder.Connect(
station.GetOutputPort(right_name_prefix + "_depth_image"),
right_dut.depth_image_input_port())
# connect the rgb images to the PointCloudToPoseSystem
builder.Connect(station.GetOutputPort(left_name_prefix + "_rgb_image"),
self.pc_to_pose.GetInputPort("camera_left_rgb"))
builder.Connect(
station.GetOutputPort(middle_name_prefix + "_rgb_image"),
self.pc_to_pose.GetInputPort("camera_middle_rgb"))
builder.Connect(
station.GetOutputPort(right_name_prefix + "_rgb_image"),
self.pc_to_pose.GetInputPort("camera_right_rgb"))
# connect the XYZ point clouds to PointCloudToPoseSystem
builder.Connect(left_dut.point_cloud_output_port(),
self.pc_to_pose.GetInputPort("left_point_cloud"))
builder.Connect(middle_dut.point_cloud_output_port(),
self.pc_to_pose.GetInputPort("middle_point_cloud"))
builder.Connect(right_dut.point_cloud_output_port(),
self.pc_to_pose.GetInputPort("right_point_cloud"))
diagram = builder.Build()
simulator = Simulator(diagram)
self.context = diagram.GetMutableSubsystemContext(
self.pc_to_pose, simulator.get_mutable_context())
def GetBrickPose(config_file, viz=False):
"""Estimates the pose of the foam brick in a ManipulationStation setup.
@param config_file str. The path to a camera configuration file.
@param viz bool. If True, save point clouds to numpy arrays.
@return An Isometry3 representing the pose of the brick.
"""
builder = DiagramBuilder()
# create the PointCloudToPoseSystem
pc_to_pose = builder.AddSystem(
PointCloudToPoseSystem(config_file, viz, SegmentFoamBrick,
GetFoamBrickPose))
# realsense serial numbers are >> 100
use_hardware = int(pc_to_pose.camera_configs["left_camera_serial"]) > 100
if use_hardware:
camera_ids = [
pc_to_pose.camera_configs["left_camera_serial"],
pc_to_pose.camera_configs["middle_camera_serial"],
pc_to_pose.camera_configs["right_camera_serial"]
]
station = builder.AddSystem(
ManipulationStationHardwareInterface(camera_ids))
station.Connect()
else:
station = builder.AddSystem(ManipulationStation())
station.AddCupboard()
object_file_path = \
"drake/examples/manipulation_station/models/061_foam_brick.sdf"
brick = AddModelFromSdfFile(FindResourceOrThrow(object_file_path),
station.get_mutable_multibody_plant(),
station.get_mutable_scene_graph())
station.Finalize()
# add systems to convert the depth images from ManipulationStation to
# PointClouds
left_camera_info = pc_to_pose.camera_configs["left_camera_info"]
middle_camera_info = pc_to_pose.camera_configs["middle_camera_info"]
right_camera_info = pc_to_pose.camera_configs["right_camera_info"]
left_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=left_camera_info))
middle_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=middle_camera_info))
right_dut = builder.AddSystem(
mut.DepthImageToPointCloud(camera_info=right_camera_info))
left_name_prefix = \
"camera_" + pc_to_pose.camera_configs["left_camera_serial"]
middle_name_prefix = \
"camera_" + pc_to_pose.camera_configs["middle_camera_serial"]
right_name_prefix = \
"camera_" + pc_to_pose.camera_configs["right_camera_serial"]
# connect the depth images to the DepthImageToPointCloud converters
builder.Connect(station.GetOutputPort(left_name_prefix + "_depth_image"),
left_dut.depth_image_input_port())
builder.Connect(station.GetOutputPort(middle_name_prefix + "_depth_image"),
middle_dut.depth_image_input_port())
builder.Connect(station.GetOutputPort(right_name_prefix + "_depth_image"),
right_dut.depth_image_input_port())
# connect the rgb images to the PointCloudToPoseSystem
builder.Connect(station.GetOutputPort(left_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_left_rgb"))
builder.Connect(station.GetOutputPort(middle_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_middle_rgb"))
builder.Connect(station.GetOutputPort(right_name_prefix + "_rgb_image"),
pc_to_pose.GetInputPort("camera_right_rgb"))
# connect the XYZ point clouds to PointCloudToPoseSystem
builder.Connect(left_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("left_point_cloud"))
builder.Connect(middle_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("middle_point_cloud"))
builder.Connect(right_dut.point_cloud_output_port(),
pc_to_pose.GetInputPort("right_point_cloud"))
diagram = builder.Build()
simulator = Simulator(diagram)
if not use_hardware:
X_WObject = Isometry3.Identity()
X_WObject.set_translation([.6, 0, 0])
station_context = diagram.GetMutableSubsystemContext(
station, simulator.get_mutable_context())
station.get_mutable_multibody_plant().tree().SetFreeBodyPoseOrThrow(
station.get_mutable_multibody_plant().GetBodyByName(
"base_link", brick), X_WObject,
station.GetMutableSubsystemContext(
station.get_mutable_multibody_plant(), station_context))
context = diagram.GetMutableSubsystemContext(
pc_to_pose, simulator.get_mutable_context())
# returns the pose of the brick, of type Isometry3
return pc_to_pose.GetOutputPort("X_WObject").Eval(context)