def get_vision_sensor_info_from_name(self, vision_sensor_name):
#returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, vision_sensor_name, self.operation_mode)
returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, vision_sensor_name, vrep.simx_opmode_oneshot_wait)
# Skip non-existent objects
if (returnCode != 0):
return None
returnCode, param_near_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1000, self.operation_mode)
returnCode, param_far_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1001, self.operation_mode)
returnCode, param_resolution_x = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1002, self.operation_mode)
returnCode, param_resolution_y = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1003, self.operation_mode)
returnCode, param_perspective_angle = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1004, self.operation_mode)
try:
ratio = param_resolution_x/param_resolution_y
except ZeroDivisionError:
return None
if (ratio > 1):
param_perspective_angle_x = param_perspective_angle
param_perspective_angle_y = 2*np.arctan(np.tan(param_perspective_angle/2)/ratio)
else:
param_perspective_angle_x = 2*np.arctan(np.tan(param_perspective_angle/2)*ratio)
param_perspective_angle_y = param_perspective_angle
return VisionSensor(vision_sensor_name, \
param_resolution_x, \
param_resolution_y, \
param_perspective_angle, \
param_perspective_angle_x, \
param_perspective_angle_y, \
param_near_clipping, \
param_far_clipping, \
obj_handle)
def get_resolution(self):
"""Retrieve resolution."""
if self._handle < 0:
if self._handle == MISSING_HANDLE:
raise RuntimeError(
"Could not retrieve resolution of {}: missing name or "
"handle.".format(self._name))
if self._handle == REMOVED_OBJ_HANDLE:
raise RuntimeError("Could not retrieve resolution of {}: "
"object removed.".format(self._name))
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not retrieve resolution of {}: not connected to V-REP "
"remote API server.".format(self._name))
res, resolution_x = vrep.simxGetObjectIntParameter(
client_id, self._handle, vrep.sim_visionintparam_resolution_x,
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not retrieve resolution of {}."
"".format(self._name))
res, resolution_y = vrep.simxGetObjectIntParameter(
client_id, self._handle, vrep.sim_visionintparam_resolution_y,
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not retrieve resolution of {}."
"".format(self._name))
return resolution_x, resolution_y
def get_vision_sensor_info_from_name(self, vision_sensor_name):
returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, vision_sensor_name, self.operation_mode)
if (returnCode == 0):
returnCode, param_near_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1000, self.operation_mode)
returnCode, param_far_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1001, self.operation_mode)
returnCode, param_resolution_x = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1002, self.operation_mode)
returnCode, param_resolution_y = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1003, self.operation_mode)
returnCode, param_perspective_angle = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1004, self.operation_mode)
ratio = param_resolution_x/param_resolution_y
if (ratio > 1):
param_perspective_angle_x = param_perspective_angle
param_perspective_angle_y = 2*np.arctan(np.tan(param_perspective_angle/2)/ratio)
else:
param_perspective_angle_x = 2*np.arctan(np.tan(param_perspective_angle/2)*ratio)
param_perspective_angle_y = param_perspective_angle
return VisionSensor(vision_sensor_name, \
param_resolution_x, \
param_resolution_y, \
param_perspective_angle, \
param_perspective_angle_x, \
param_perspective_angle_y, \
param_near_clipping, \
param_far_clipping, \
obj_handle)
def getVirtualCamAdditionalMatrix(sensorHandle):
"获得深度相机的近景/远景距离/视场角/分辨率"
err, nearClip = vrep.simxGetObjectFloatParameter(
clientID, sensorHandle, vrep.sim_visionfloatparam_near_clipping, vrep.simx_opmode_oneshot_wait)
err, farClip = vrep.simxGetObjectFloatParameter(
clientID, sensorHandle, vrep.sim_visionfloatparam_far_clipping, vrep.simx_opmode_oneshot_wait)
err, angle = vrep.simxGetObjectFloatParameter(
clientID, sensorHandle, vrep.sim_visionfloatparam_perspective_angle, vrep.simx_opmode_oneshot_wait)
err, res_x = vrep.simxGetObjectIntParameter(
clientID, sensorHandle, vrep.sim_visionintparam_resolution_x, vrep.simx_opmode_oneshot_wait)
err, res_y = vrep.simxGetObjectIntParameter(
clientID, sensorHandle, vrep.sim_visionintparam_resolution_y, vrep.simx_opmode_oneshot_wait)
return nearClip, farClip, angle, (res_x, res_y)
def getObjectParameter(clientID, handle, parameter_code, is_float=True):
if is_float:
ret, value = vrep.simxGetObjectFloatParameter(clientID, handle,
parameter_code, blocking)
else:
ret, value = vrep.simxGetObjectIntParameter(clientID, handle,
parameter_code, blocking)
if ret != return_ok:
raise Exception('FAIL: getObjectParameter failed')
return value
def get_object(self, name: str) -> VRepObject or VRepDepthSensor:
ret, handle = vrep.simxGetObjectHandle(self.id, name,
vrep.simx_opmode_blocking)
if ret != 0:
raise ConnectionError(VRepError.create(ret))
else:
"""
Kludge ahead.
To find out whether the object is a depth sensor, we query its x-resolution (parameter 1002).
If we don't get a server error, we know it is a sensor.
"""
ret, __ = vrep.simxGetObjectIntParameter(self.id, handle, 1002,
vrep.simx_opmode_blocking)
if ret == 0:
return VRepDepthSensor(self.id, handle, name)
else:
return VRepObject(self.id, handle, name)
def __init__(self, client_id: int, handle: int, name: str):
"""Initialize sensor and get specific information
Initialize
"""
super().__init__(client_id, handle, name)
# Assume that the sensor returns a square image
__, self.res = vrep.simxGetObjectIntParameter(
client_id, handle, vrep.sim_visionintparam_resolution_x,
self.BLOCK)
__, self.max_depth = vrep.simxGetObjectFloatParameter(
client_id, handle, vrep.sim_visionfloatparam_far_clipping,
self.BLOCK)
__, angle_radians = vrep.simxGetObjectFloatParameter(
client_id, handle, vrep.sim_visionfloatparam_perspective_angle,
self.BLOCK)
self.angle = round(degrees(angle_radians), 3)
def get_parameter(self, sensor_name, parameter_name):
if parameter_name in self.params_f:
_, value = vrep.simxGetObjectFloatParameter(
self.client_id,
self.handles[sensor_name + self.postfix],
self.params_f[parameter_name],
ONE_SHOT_MODE
)
if parameter_name == 'perspective_angle':
value = value * 180 * 2 / math.pi
return value
elif parameter_name in self.params_i:
_, value = vrep.simxGetObjectIntParameter(
self.client_id,
self.handles[sensor_name + self.postfix],
self.params_i[parameter_name],
ONE_SHOT_MODE
)
return value
else:
return 'Parameter not found'
vs_index_list = [
objects_names.index(i) for i in objects_names
if re.match(r'(#\d|)\b|'.join(vs_list), i)
]
print('list size: ', len(vs_index_list))
vs_list = []
for i in vs_index_list:
returnCode, param_near_clipping = vrep.simxGetObjectFloatParameter(
clientID, objects_handles[i], 1000, vrep.simx_opmode_blocking)
returnCode, param_far_clipping = vrep.simxGetObjectFloatParameter(
clientID, objects_handles[i], 1001, vrep.simx_opmode_blocking)
returnCode, param_resolution_x = vrep.simxGetObjectIntParameter(
clientID, objects_handles[i], 1002, vrep.simx_opmode_blocking)
returnCode, param_resolution_y = vrep.simxGetObjectIntParameter(
clientID, objects_handles[i], 1003, vrep.simx_opmode_blocking)
returnCode, param_perspective_angle = vrep.simxGetObjectFloatParameter(
clientID, objects_handles[i], 1004, vrep.simx_opmode_blocking)
ratio = param_resolution_x / param_resolution_y
if (ratio > 1):
param_perspective_angle_x = param_perspective_angle
param_perspective_angle_y = 2 * np.arctan(
np.tan(param_perspective_angle / 2) / ratio)
else:
param_perspective_angle_x = 2 * np.arctan(
np.tan(param_perspective_angle / 2) * ratio)
param_perspective_angle_y = param_perspective_angle