def setParams(self, params):
SCENE_FILE = '../../etc/basic_scene.ttt'
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.robot = TurtleBot()
self.drone = Shape('Quadricopter')
self.cameras = {}
self.front_camera_name = "frontCamera"
cam = VisionSensor(self.front_camera_name)
self.cameras["frontCamera"] = {"handle": cam,
"id": 0,
"angle": np.radians(cam.get_perspective_angle()),
"width": cam.get_resolution()[0],
"height": cam.get_resolution()[1],
"focal": (cam.get_resolution()[0] / 2) / np.tan(np.radians(cam.get_perspective_angle() / 2)),
"rgb": np.array(0),
"depth": np.ndarray(0)}
self.joystick_newdata = []
self.speed_robot = []
self.speed_robot_ant = []
self.last_received_data_time = 0
self.once = False
def setParams(self, params):
SCENE_FILE = '../etc/gen3-robolab.ttt'
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.mode = 0
self.bloqueo = True
#self.robot = Viriato()
#self.robot = YouBot()
self.robot_object = Shape("gen3")
self.cameras = {}
self.camera_arm_name = "camera_arm"
cam = VisionSensor(self.camera_arm_name)
self.cameras[self.camera_arm_name] = {
"handle":
cam,
"id":
0,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"focal": (cam.get_resolution()[0] / 2) /
np.tan(np.radians(cam.get_perspective_angle() / 2)),
"rgb":
np.array(0),
"depth":
np.ndarray(0)
}
self.joystick_newdata = []
self.last_received_data_time = 0
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)
def setParams(self, params):
SCENE_FILE = params["scene_dir"]
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.cameras = {}
cam = VisionSensor("Camera_Shoulder")
self.cameras["Camera_Shoulder"] = {
"handle":
cam,
"id":
1,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"depth":
3,
"focal":
cam.get_resolution()[0] /
np.tan(np.radians(cam.get_perspective_angle())),
"position":
cam.get_position(),
"rotation":
cam.get_quaternion(),
"image_rgb":
np.array(0),
"image_rgbd":
np.array(0),
"depth":
np.ndarray(0)
}
self.grasping_objects = {}
can = Shape("can")
self.grasping_objects["002_master_chef_can"] = {
"handle": can,
"sim_pose": None,
"pred_pose_rgb": None,
"pred_pose_rgbd": None
}
with (open("objects_pcl.pickle", "rb")) as file:
self.object_pcl = pickle.load(file)
self.intrinsics = np.array(
[[
self.cameras["Camera_Shoulder"]["focal"], 0.00000000e+00,
self.cameras["Camera_Shoulder"]["width"] / 2.0
],
[
0.00000000e+00, self.cameras["Camera_Shoulder"]["focal"],
self.cameras["Camera_Shoulder"]["height"] / 2.0
], [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
self.arm_ops = {
"MoveToHome": 1,
"MoveToObj": 2,
"CloseGripper": 3,
"OpenGripper": 4
}
self.grasping_iter = 10
self.arm_base = Shape("gen3")
self.arm_target = Dummy("target")
self.gripper = Joint("RG2_openCloseJoint")
def simulate(scene_dir, cls_indices):
# read 3d point cloud vertices npy (for visualization)
vertex_npy = np.load("mesh_data/custom_vertex.npy")
# loop over all scene files in scenes directory
for scene_path in os.listdir(scene_dir):
# check whether it's a scene file or not
if not scene_path[-3:] == 'ttt':
continue
# create an output directory for each scene
scene_out_dir = os.path.join('./sim_data/', scene_path[:-4])
if not os.path.isdir(scene_out_dir):
os.mkdir(scene_out_dir)
# launch scene file
SCENE_FILE = os.path.join(os.path.dirname(os.path.abspath(__file__)), os.path.join(scene_dir, scene_path))
pr = PyRep()
pr.launch(SCENE_FILE, headless=True)
pr.start()
pr.step()
# define vision sensor
camera = VisionSensor('cam')
# set camera absolute pose to world coordinates
camera.set_pose([0,0,0,0,0,0,1])
pr.step()
# define scene shapes
shapes = []
shapes.append(Shape('Shape1'))
shapes.append(Shape('Shape2'))
shapes.append(Shape('Shape3'))
shapes.append(Shape('Shape4'))
pr.step()
print("Getting vision sensor intrinsics ...")
# get vision sensor parameters
cam_res = camera.get_resolution()
cam_per_angle = camera.get_perspective_angle()
ratio = cam_res[0]/cam_res[1]
cam_angle_x = 0.0
cam_angle_y = 0.0
if (ratio > 1):
cam_angle_x = cam_per_angle
cam_angle_y = 2 * degrees(atan(tan(radians(cam_per_angle / 2)) / ratio))
else:
cam_angle_x = 2 * degrees(atan(tan(radians(cam_per_angle / 2)) / ratio))
cam_angle_y = cam_per_angle
# get vision sensor intrinsic matrix
cam_focal_x = float(cam_res[0] / 2.0) / tan(radians(cam_angle_x / 2.0))
cam_focal_y = float(cam_res[1] / 2.0) / tan(radians(cam_angle_y / 2.0))
intrinsics = np.array([[cam_focal_x, 0.00000000e+00, float(cam_res[0]/2.0)],
[0.00000000e+00, cam_focal_y, float(cam_res[1]/2.0)],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
# loop to get 5000 samples per scene
for i in range(5000):
print("Randomizing objects' poses ...")
# set random pose to objects in the scene
obj_colors = []
for shape in shapes:
shape.set_pose([
random.uniform(-0.25,0.25), random.uniform(-0.25,0.25), random.uniform(0.25,2),
random.uniform(-1,1), random.uniform(-1,1), random.uniform(-1,1),
random.uniform(-1,1)
])
obj_colors.append([random.uniform(0,1), random.uniform(0,1), random.uniform(0,1)])
pr.step()
print("Reading vision sensor RGB signal ...")
# read vision sensor RGB image
img = camera.capture_rgb()
img = np.uint8(img * 255.0)
print("Reading vision sensor depth signal ...")
# read vision sensor depth image
depth = camera.capture_depth()
depth = np.uint8(depth * 255.0)
print("Generating front mask for RGB signal ...")
# generate RGB front mask
front_mask = np.sum(img, axis=2)
front_mask[front_mask != 0] = 255
front_mask = Image.fromarray(np.uint8(front_mask))
alpha_img = Image.fromarray(img)
alpha_img.putalpha(front_mask)
print("Saving sensor output ...")
# save sensor output
alpha_img.save(scene_out_dir+f'/{str(i).zfill(6)}-color.png')
imsave(scene_out_dir+f'/{str(i).zfill(6)}-depth.png', depth)
print("Getting objects' poses ...")
# get poses for all objects in scene
poses = []
for shape in shapes:
poses.append(get_object_pose(shape, camera))
pose_mat = np.stack(poses, axis=2)
# save pose visualization on RGB image
img_viz = visualize_predictions(poses, cls_indices[scene_path], img, vertex_npy, intrinsics)
imsave(scene_out_dir+f'/{str(i).zfill(6)}-viz.png', img_viz)
print("Saving meta-data ...")
# save meta-data .mat
meta_dict = {
'cls_indexes' : np.array(cls_indices[scene_path]),
'intrinsic_matrix' : intrinsics,
'poses' : pose_mat
}
savemat(scene_out_dir+f'/{str(i).zfill(6)}-meta.mat', meta_dict)
print("Performing semantic segmentation of RGB signal ...")
# perform semantic segmentation of RGB image
seg_img = camera.capture_rgb()
seg_img = perform_segmentation(seg_img, cls_indices[scene_path], poses, vertex_npy, intrinsics)
imsave(scene_out_dir+f'/{str(i).zfill(6)}-label.png', seg_img)
# stop simulation
pr.stop()
pr.shutdown()
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")
def setParams(self, params):
SCENE_FILE = '../../etc/informatica.ttt'
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.robot_object = Shape("Pioneer")
self.back_left_wheel = Joint("p3at_back_left_wheel_joint")
self.back_right_wheel = Joint("p3at_back_right_wheel_joint")
self.front_left_wheel = Joint("p3at_front_left_wheel_joint")
self.front_right_wheel = Joint("p3at_front_right_wheel_joint")
self.radius = 110 # mm
self.semi_width = 140 # mm
# cameras
self.cameras_write = {}
self.cameras_read = {}
self.front_left_camera_name = "pioneer_camera_left"
cam = VisionSensor(self.front_left_camera_name)
self.cameras_write[self.front_left_camera_name] = {
"handle":
cam,
"id":
0,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"focal": (cam.get_resolution()[0] / 2) /
np.tan(np.radians(cam.get_perspective_angle() / 2)),
"rgb":
np.array(0),
"depth":
np.ndarray(0)
}
self.front_right_camera_name = "pioneer_camera_right"
cam = VisionSensor(self.front_right_camera_name)
self.cameras_write[self.front_right_camera_name] = {
"handle":
cam,
"id":
1,
"angle":
np.radians(cam.get_perspective_angle()),
"width":
cam.get_resolution()[0],
"height":
cam.get_resolution()[1],
"focal": (cam.get_resolution()[0] / 2) /
np.tan(np.radians(cam.get_perspective_angle() / 2)),
"rgb":
np.array(0),
"depth":
np.ndarray(0)
}
self.cameras_read = self.cameras_write.copy()
self.mutex_c = Lock()
# PoseEstimation
self.robot_full_pose_write = RoboCompFullPoseEstimation.FullPoseEuler()
self.robot_full_pose_read = RoboCompFullPoseEstimation.FullPoseEuler()
self.mutex = Lock()
self.ldata = []
self.joystick_newdata = []
self.speed_robot = []
self.speed_robot_ant = []
self.last_received_data_time = 0
