class TestVisionSensors(TestCore): def setUp(self): super().setUp() [self.pyrep.step() for _ in range(10)] self.cam = VisionSensor('cam0') def test_handle_explicitly(self): cam = VisionSensor.create((640, 480)) # blank image rgb = cam.capture_rgb() self.assertEqual(rgb.sum(), 0) # non-blank image cam.set_explicit_handling(value=1) cam.handle_explicitly() rgb = cam.capture_rgb() self.assertNotEqual(rgb.sum(), 0) cam.remove() def test_capture_rgb(self): img = self.cam.capture_rgb() self.assertEqual(img.shape, (16, 16, 3)) # Check that it's not a blank image self.assertFalse(img.min() == img.max() == 0.0) def test_capture_depth(self): img = self.cam.capture_depth() self.assertEqual(img.shape, (16, 16)) # Check that it's not a blank depth map self.assertFalse(img.min() == img.max() == 1.0) def test_create(self): cam = VisionSensor.create([640, 480], perspective_mode=True, view_angle=35.0, near_clipping_plane=0.25, far_clipping_plane=5.0, render_mode=RenderMode.OPENGL3) self.assertEqual(cam.capture_rgb().shape, (480, 640, 3)) self.assertEqual(cam.get_perspective_mode(), PerspectiveMode.PERSPECTIVE) self.assertAlmostEqual(cam.get_perspective_angle(), 35.0, 3) self.assertEqual(cam.get_near_clipping_plane(), 0.25) self.assertEqual(cam.get_far_clipping_plane(), 5.0) self.assertEqual(cam.get_render_mode(), RenderMode.OPENGL3) def test_get_set_resolution(self): self.cam.set_resolution([320, 240]) self.assertEqual(self.cam.get_resolution(), [320, 240]) self.assertEqual(self.cam.capture_rgb().shape, (240, 320, 3)) def test_get_set_perspective_mode(self): for perspective_mode in PerspectiveMode: self.cam.set_perspective_mode(perspective_mode) self.assertEqual(self.cam.get_perspective_mode(), perspective_mode) def test_get_set_render_mode(self): for render_mode in [RenderMode.OPENGL, RenderMode.OPENGL3]: self.cam.set_render_mode(render_mode) self.assertEqual(self.cam.get_render_mode(), render_mode) def test_get_set_perspective_angle(self): self.cam.set_perspective_angle(45.0) self.assertAlmostEqual(self.cam.get_perspective_angle(), 45.0, 3) def test_get_set_orthographic_size(self): self.cam.set_orthographic_size(1.0) self.assertEqual(self.cam.get_orthographic_size(), 1.0) def test_get_set_near_clipping_plane(self): self.cam.set_near_clipping_plane(0.5) self.assertEqual(self.cam.get_near_clipping_plane(), 0.5) def test_get_set_far_clipping_plane(self): self.cam.set_far_clipping_plane(0.5) self.assertEqual(self.cam.get_far_clipping_plane(), 0.5) def test_get_set_windowed_size(self): self.cam.set_windowed_size((640, 480)) self.assertEqual(self.cam.get_windowed_size(), (640, 480)) def test_get_set_entity_to_render(self): self.cam.set_entity_to_render(-1) self.assertEqual(self.cam.get_entity_to_render(), -1)
class PyrepDevice(object): """ PyrepDevice handles communication with CoppeliaSim Vision Sensors via Pyrep. Retrieved images are converted to cv2 format and passed to registered callback functions. """ @staticmethod def autodetect_nicoeyes(): """ Returns a tuple containing the detected names of left and right eye sensor :return: Devicenames as (left, right) tuple (None if not found) :rtype: tuple """ eye_sensors = ["left_eye", "right_eye"] for i in range(len(eye_sensors)): if not VisionSensor.exists(eye_sensors[i]): logger.warning( "Could not find vision sensor named '%s'", eye_sensors[i] ) eye_sensors[i] = None return tuple(eye_sensors) def __init__( self, sensor_name, width=640, height=480, near_clipping_plane=1e-2, far_clipping_plane=10.0, view_angle=60.0, ): """ Connects to Vision Sensor with given name and updates parameters accordingly. :param sensor_name: name (or handle) of the vision sensor in the scene :type sensor_name: str :param width: horizontal camera resolution :type width: int :param heigth: vertical camera resolution :type heigth: int :param near_clipping_plane: the minimum distance from which the sensor will be able to detect. :type near_clipping_plane: float :param far_clipping_plane: the maximum distance from which the sensor will be able to detect. :type far_clipping_plane: float :param view_angle: field of view of the camera in degree. :type view_angle: float """ self._sensor = VisionSensor(sensor_name) self.resolution = (width, height) self.near_clipping_plane = near_clipping_plane self.far_clipping_plane = far_clipping_plane self.view_angle = view_angle self._callback = [] self._removed_ids = [] ref = weakref.ref(self, StepListener.unregister) StepListener.register(ref) @property def sensor_name(self): """ :return: name of the sensor in the scene :rtype: str """ return self._sensor.get_name() @property def resolution(self): """ :return: camera resolution (width, height) :rtype: tuple(int, int) """ return tuple(self._sensor.get_resolution()) @resolution.setter def resolution(self, res): """ :param res: camera resolution (width, height) :type res: tuple(int, int) """ self._sensor.set_resolution(res) @property def near_clipping_plane(self): """ :return: the minimum distance from which the sensor will be able to detect. :rtype: float """ return self._sensor.get_near_clipping_plane() @near_clipping_plane.setter def near_clipping_plane(self, val): """ :param val: the minimum distance from which the sensor will be able to detect. :type val: float """ self._sensor.set_near_clipping_plane(val) @property def far_clipping_plane(self): """ :return: the maximum distance from which the sensor will be able to detect. :rtype: float """ return self._sensor.get_far_clipping_plane() @far_clipping_plane.setter def far_clipping_plane(self, val): """ :param val: the maximum distance from which the sensor will be able to detect. :type val: float """ self._sensor.set_far_clipping_plane(val) @property def view_angle(self): """ :return: field of view of the camera in degree. :rtype: float """ return self._sensor.get_perspective_angle() @view_angle.setter def view_angle(self, val): """ :param val: field of view of the camera in degree. :type val: float """ self._sensor.set_perspective_angle(val) def get_image(self): """ Captures an image for the current simulation step from the vision sensor and converts it to cv2 format :return: the image :rtype: cv2 image """ frame = self._sensor.capture_rgb() frame = np.uint8(frame[:, :, [2, 1, 0]] * 255) return frame def step(self): """ Executes all registered callbacks with the sensor image for the current simulation step. """ frame = self.get_image() id_start = 0 # iterate through callbacks while skipping removed ids (if any) for id_end in self._removed_ids: for id in range(id_start, id_end): self._callback[id](frame) id_start = id_end # iterate through (remaining) callbacks for id in range(id_start, len(self._callback)): self._callback[id](frame) def add_callback(self, function): """ Adds a callback for the event loop Whenever step is called, all registered callbacks are executed. The callback must take exactly 1 argument through which it receives the frame. :param function: Function to add as callback :type function: function :return: id of the added callback :rtype: int """ if not (inspect.isfunction(function) or inspect.ismethod(function)): logger.error("Trying to add non-function callback") raise ValueError("Trying to add non-function callback") if len(self._removed_ids) == 0: self._callback += [function] id = len(self._callback) - 1 else: id = min(self._removed_ids) self._callback[id] self._removed_ids.remove(id) logger.info("Added callback with id %i", id) return id def remove_callback(self, id): """ Removes callback with given id :param id: id of the callback :type id: int """ if id in self._removed_ids or id not in range(0, len(self._callback)): logger.warning("Callback with id %i does not exist", id) return if id == len(self._callback) - 1: i = id for removed in reversed(self._removed_ids): if i - 1 != removed: break self._removed_ids[:-1] i -= 1 self._callback = self._callback[:i] else: self._callback[id] = None self._removed_ids += [id] self._removed_ids.sort() logger.info("Removed callback with id %i", id) def clean_callbacks(self): """ Removes all saved callbacks """ self._callback.clear() self._removed_ids.clear() logger.info("Cleared callbacks")