def test_rgbd_camera(self): sdf_path = FindResourceOrThrow( "drake/systems/sensors/test/models/nothing.sdf") tree = RigidBodyTree() with open(sdf_path) as f: sdf_string = f.read() AddModelInstancesFromSdfString(sdf_string, FloatingBaseType.kFixed, None, tree) frame = RigidBodyFrame("rgbd camera frame", tree.FindBody("link")) tree.addFrame(frame) # Use HDTV size. width = 1280 height = 720 camera = attic_mut.RgbdCamera(name="camera", tree=tree, frame=frame, z_near=0.5, z_far=5.0, fov_y=np.pi / 4, show_window=False, width=width, height=height) def check_info(camera_info): self.assertIsInstance(camera_info, mut.CameraInfo) self.assertEqual(camera_info.width(), width) self.assertEqual(camera_info.height(), height) check_info(camera.color_camera_info()) check_info(camera.depth_camera_info()) self.assertIsInstance(camera.color_camera_optical_pose(), Isometry3) self.assertIsInstance(camera.depth_camera_optical_pose(), Isometry3) self.assertTrue(camera.tree() is tree) # N.B. `RgbdCamera` copies the input frame. self.assertEqual(camera.frame().get_name(), frame.get_name()) self._check_ports(camera) # Test discrete camera. period = attic_mut.RgbdCameraDiscrete.kDefaultPeriod discrete = attic_mut.RgbdCameraDiscrete(camera=camera, period=period, render_label_image=True) self.assertTrue(discrete.camera() is camera) self.assertTrue(discrete.mutable_camera() is camera) self.assertEqual(discrete.period(), period) self._check_ports(discrete) # That we can access the state as images. context = discrete.CreateDefaultContext() values = context.get_abstract_state() self.assertIsInstance(values.get_value(0), Value[mut.ImageRgba8U]) self.assertIsInstance(values.get_value(1), Value[mut.ImageDepth32F]) self.assertIsInstance(values.get_value(2), Value[mut.ImageLabel16I])
def test_frame_api(self): tree = RigidBodyTree(FindResourceOrThrow( "drake/examples/pendulum/Pendulum.urdf")) # xyz + rpy frame = RigidBodyFrame( name="frame_1", body=tree.world(), xyz=[0, 0, 0], rpy=[0, 0, 0]) self.assertEqual(frame.get_name(), "frame_1") tree.addFrame(frame) self.assertTrue(frame.get_frame_index() < 0, frame.get_frame_index()) self.assertTrue(frame.get_rigid_body() is tree.world()) self.assertIsInstance(frame.get_transform_to_body(), Isometry3) self.assertTrue(tree.findFrame(frame_name="frame_1") is frame)
def test_rgbd_camera(self): sdf_path = FindResourceOrThrow( "drake/systems/sensors/test/models/nothing.sdf") tree = RigidBodyTree() with open(sdf_path) as f: sdf_string = f.read() AddModelInstancesFromSdfString( sdf_string, FloatingBaseType.kFixed, None, tree) frame = RigidBodyFrame("rgbd camera frame", tree.FindBody("link")) tree.addFrame(frame) # Use HDTV size. width = 1280 height = 720 camera = attic_mut.RgbdCamera( name="camera", tree=tree, frame=frame, z_near=0.5, z_far=5.0, fov_y=np.pi / 4, show_window=False, width=width, height=height) def check_info(camera_info): self.assertIsInstance(camera_info, mut.CameraInfo) self.assertEqual(camera_info.width(), width) self.assertEqual(camera_info.height(), height) check_info(camera.color_camera_info()) check_info(camera.depth_camera_info()) self.assertIsInstance(camera.color_camera_optical_pose(), Isometry3) self.assertIsInstance(camera.depth_camera_optical_pose(), Isometry3) self.assertTrue(camera.tree() is tree) # N.B. `RgbdCamera` copies the input frame. self.assertEqual(camera.frame().get_name(), frame.get_name()) self._check_ports(camera) # Test discrete camera. period = attic_mut.RgbdCameraDiscrete.kDefaultPeriod discrete = attic_mut.RgbdCameraDiscrete( camera=camera, period=period, render_label_image=True) self.assertTrue(discrete.camera() is camera) self.assertTrue(discrete.mutable_camera() is camera) self.assertEqual(discrete.period(), period) self._check_ports(discrete) # That we can access the state as images. context = discrete.CreateDefaultContext() values = context.get_abstract_state() self.assertIsInstance(values.get_value(0), Value[mut.ImageRgba8U]) self.assertIsInstance(values.get_value(1), Value[mut.ImageDepth32F]) self.assertIsInstance(values.get_value(2), Value[mut.ImageLabel16I])
from pydrake.systems.sensors import CameraInfo from pydrake.attic.systems.sensors import RgbdCamera # Create tree describing scene. urdf_path = FindResourceOrThrow( "drake/multibody/models/box.urdf") tree = RigidBodyTree() AddModelInstanceFromUrdfFile( urdf_path, FloatingBaseType.kFixed, None, tree) # - Add frame for camera fixture. frame = RigidBodyFrame( name="rgbd camera frame", body=tree.world(), xyz=[-2, 0, 2], # Ensure that the box is within range. rpy=[0, np.pi / 4, 0]) tree.addFrame(frame) # Create camera. camera = RgbdCamera( name="camera", tree=tree, frame=frame, z_near=0.5, z_far=5.0, fov_y=np.pi / 4, show_window=True) # - Describe state. x = np.zeros(tree.get_num_positions() + tree.get_num_velocities()) # Allocate context and render. context = camera.CreateDefaultContext() context.FixInputPort(0, BasicVector(x)) output = camera.AllocateOutput() camera.CalcOutput(context, output)