def create_dataset(path):
try:
shutil.rmtree(path + "/mav0")
except:
print("Removed before")
os.makedirs(path + "/mav0/cam0/data")
os.makedirs(path + "/mav0/cam1/data")
print('Argument List:', str(sys.argv[1]))
sim.simxFinish(-1) # just in case, close all opened connections
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to CoppeliaSim
print('Connected to remote API server')
#Now try to retrieve data in a blocking fashion (i.e. a service call):
res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all,
sim.simx_opmode_blocking)
er, t_rightWheel = sim.simxGetObjectHandle(clientID,
'Pioneer_p3dx_rightMotor',
sim.simx_opmode_blocking)
er, t_leftWheel = sim.simxGetObjectHandle(clientID,
'Pioneer_p3dx_leftMotor',
sim.simx_opmode_blocking)
er, cam_handle_left = sim.simxGetObjectHandle(
clientID, 'anaglyphStereoSensor_leftSensor',
sim.simx_opmode_blocking)
er, cam_handle_right = sim.simxGetObjectHandle(
clientID, 'anaglyphStereoSensor_rightSensor',
sim.simx_opmode_blocking)
i = 0
fl = open(path + "/mav0/timestamps.txt", "w")
while (True):
err, resolution_left, colorCam_left = sim.simxGetVisionSensorImage(
clientID, cam_handle_left, 0, sim.simx_opmode_oneshot_wait)
err, resolution_right, colorCam_right = sim.simxGetVisionSensorImage(
clientID, cam_handle_right, 0, sim.simx_opmode_oneshot_wait)
img_left_1 = np.array(colorCam_left, dtype=np.uint8)
img_right_1 = np.array(colorCam_right, dtype=np.uint8)
img_left_1.resize([resolution_left[1], resolution_left[0], 3])
img_right_1.resize([resolution_right[1], resolution_right[0], 3])
img_left_2 = np.flipud(img_left_1)
img_right_2 = np.flipud(img_right_1)
img_left_3 = img_left_2[..., ::-1].copy()
img_right_3 = img_right_2[..., ::-1].copy()
milli_time = int(round(time.time() * 1000)) * 1000000000
st = str(milli_time) + '\n'
st_left = path + '/mav0/cam0/data/{}.png'.format(milli_time)
st_right = path + '/mav0/cam1/data/{}.png'.format(milli_time)
cv2.imwrite(st_left, img_left_3)
cv2.imwrite(st_right, img_right_3)
cv2.waitKey(1)
i = i + 1
fl.write(st)
sim.simxSetJointTargetVelocity(clientID, t_rightWheel, 1,
sim.simx_opmode_streaming)
sim.simxSetJointTargetVelocity(clientID, t_leftWheel, 1,
sim.simx_opmode_streaming)
fl.close()
def getColor(sensor):
min_color_value = 200
erro, resolution, Image = sim.simxGetVisionSensorImage(
glob.clientID, sensor, 0, sim.simx_opmode_buffer)
while (erro != 0):
erro, resolution, Image = sim.simxGetVisionSensorImage(
glob.clientID, sensor, 0, sim.simx_opmode_buffer)
img = np.array(Image, dtype=np.uint8)
#print(resolution, img)
rgb_color = 0
if (img[0] > min_color_value):
rgb_color += 100
if (img[1] > min_color_value):
rgb_color += 10
if (img[2] > min_color_value):
rgb_color += 1
if (rgb_color == 1):
return AZUL
if (rgb_color == 10):
return VERDE
if (rgb_color == 100):
return VERMELHO
if (rgb_color == 110):
return AMARELO
if (rgb_color == 111):
return BRANCO
return PRETO
def getCamImage(self, save=False, number=1):
# Save or print Sensor Vision Image
image = None
if self.craneStatus and self.connectionStatus:
if number == 1:
err_code, resolution, image = sim.simxGetVisionSensorImage(
self.clientID, self.cam, 0, sim.simx_opmode_buffer)
elif number == 2:
err_code, resolution, image = sim.simxGetVisionSensorImage(
self.clientID, self.cam2, 0, sim.simx_opmode_buffer)
if save:
if err_code == sim.simx_return_ok:
# basewidth = 192
height, width = resolution
img = np.array(image, dtype=np.uint8)
img.resize([resolution[0], resolution[1], 3])
img = np.array(img[::-1], dtype=np.uint8)
bytesPerLine = 3 * width
qImg = QtGui.QImage(img, width, height, bytesPerLine,
QtGui.QImage.Format_RGB888)
return qImg
else:
return None
return None
def get_vision_sensor_image():
"""
Purpose:
---
This function should first get the handle of the Vision Sensor object from the scene.
After that it should get the Vision Sensor's image array from the CoppeliaSim scene.
Input Arguments:
---
None
Returns:
---
`vision_sensor_image` : [ list ]
the image array returned from the get vision sensor image remote API
`image_resolution` : [ list ]
the image resolution returned from the get vision sensor image remote API
`return_code` : [ integer ]
the return code generated from the remote API
Example call:
---
vision_sensor_image, image_resolution, return_code = get_vision_sensor_image()
NOTE: This function will be automatically called by test_task_2a executable at regular intervals.
"""
global client_id
vision_sensor_image = []
image_resolution = []
return_code = 0
############## ADD YOUR CODE HERE ##############
ret, handle = sim.simxGetObjectHandle(client_id, 'vision_sensor_1',
sim.simx_opmode_blocking)
#imageAcquisitionTime=sim.simxGetLastCmdTime(client_id)
#print(imageAcquisitionTime)
return_code, image_resolution, vision_sensor_image = sim.simxGetVisionSensorImage(
client_id, handle, 0, sim.simx_opmode_streaming)
while (return_code != 0):
return_code, image_resolution, vision_sensor_image = sim.simxGetVisionSensorImage(
client_id, handle, 0, sim.simx_opmode_buffer)
#if(return_code==0):break
#print(vision_sensor_image)
temp = np.array(vision_sensor_image)
while (np.all(temp == 0)):
return_code, image_resolution, vision_sensor_image = sim.simxGetVisionSensorImage(
client_id, handle, 0, sim.simx_opmode_buffer)
temp = np.array(vision_sensor_image)
imageAcquisitionTime = sim.simxGetLastCmdTime(client_id)
##################################################
return vision_sensor_image, image_resolution, return_code
def stepSimulation():
if client.runInSynchronousMode:
currentStep = client.stepCounter
sim.simxSynchronousTrigger(client.id)
while client.stepCounter == currentStep:
retCode, s = sim.simxGetIntegerSignal(
client.id, client.intSignalName,
sim.simx_opmode_buffer)
if retCode == sim.simx_return_ok:
client.stepCounter = s
retCode, res, img = sim.simxGetVisionSensorImage(
client.id, client.visionSensorHandle, 0,
sim.simx_opmode_buffer)
client.lastImageAcquisitionTime = sim.simxGetLastCmdTime(
client.id)
if retCode == sim.simx_return_ok:
sim.simxSetVisionSensorImage(
client.id, client.passiveVisionSensorHandle, img, 0,
sim.simx_opmode_oneshot)
else:
retCode, res, img = sim.simxGetVisionSensorImage(
client.id, client.visionSensorHandle, 0,
sim.simx_opmode_buffer)
if retCode == sim.simx_return_ok:
imageSimTime = sim.simxGetLastCmdTime(client.id)
if client.lastImageAcquisitionTime != imageSimTime:
client.lastImageAcquisitionTime = imageSimTime
sim.simxSetVisionSensorImage(
client.id, client.passiveVisionSensorHandle, img,
0, sim.simx_opmode_oneshot)
def get_image():
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, cameraID, 0, vrep.simx_opmode_streaming)
time.sleep(0.1)
err, resolution, image = vrep.simxGetVisionSensorImage(
clientID, cameraID, 0, vrep.simx_opmode_buffer)
img = np.array(image, dtype=np.uint8)
img.resize([256, 256, 3])
time.sleep(0.1)
return img
def startSimulation(self):
# start simulation
vrep.simxStartSimulation(self.clientId, vrep.simx_opmode_blocking)
# open streaming
vrep.simxGetVisionSensorImage(self.clientId, self.cameraId, 0,
vrep.simx_opmode_streaming)
vrep.simxReadForceSensor(self.clientId, self.sensorId,
vrep.simx_opmode_streaming)
time.sleep(1)
self.beginTime = time.time()
def streamVisionSensor():
# Mouse function
def select_point(event, x, y, flags, params):
global point, point_selected, old_points
if event == cv2.EVENT_LBUTTONDOWN:
point = (x, y)
point_selected = True
old_points = np.array([[x, y]], dtype=np.float32)
print("TRU")
cv2.namedWindow('frame')
point_selected = False
point = ()
old_points = np.array([[]])
#Get the handle of vision sensor
errorCode, visionSensorHandle = sim.simxGetObjectHandle(
clientID, 'Vision_sensor', sim.simx_opmode_oneshot_wait)
#Get the image
errorCode, resolution, image = sim.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, sim.simx_opmode_streaming)
time.sleep(0.5)
errorCode, resolution, image = sim.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, sim.simx_opmode_buffer)
sensorImage = np.array(image, dtype=np.uint8)
sensorImage.resize([resolution[1], resolution[0], 3])
old_image = sensorImage.copy()
while (sim.simxGetConnectionId(clientID) != -1):
cv2.setMouseCallback('frame', select_point)
#Get the image of the vision sensor
errorCode, resolution, image = sim.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, sim.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
sensorImage = np.array(image, dtype=np.uint8)
sensorImage.resize([resolution[1], resolution[0], 3])
displayedImage = cv2.resize(sensorImage, (480, 480))
if point_selected is True:
print("coco")
new_points, status, error = cv2.calcOpticalFlowPyrLK(
old_image, sensorImage, old_points, None, **lk_params)
old_image = sensorImage.copy() #current frame becomes previous
old_points = new_points #current x,y points become previous
x, y = new_points.ravel()
cv2.circle(sensorImage, (x, y), 8, (0, 0, 0), -1)
cv2.imshow('frame', displayedImage)
if cv2.waitKey(30) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
print('End of Simulation')
def __init__(self, queue_dict, sync=True):
"""
this class loads coppeliasim and is responsible for communication between the sim and python...
api functions can be found https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm
"""
# open queues
self.queues = queue_dict
# close all connection that are remaining
sim.simxFinish(-1)
self.clientID = -1
attempt_num = 0
while self.clientID == -1:
print("attempting to connect to VREP...")
self.clientID = sim.simxStart('127.0.0.1', 19999,
True, True, 5000, 5)
attempt_num += 1
if attempt_num >= 3:
print("could not connect to vrep")
return
print("successful connection!")
self.sync = sync
if self.sync:
# set the simulation to synchronise with api
sim.simxSynchronous(self.clientID, True)
# get coppeliasim object handles
self.motor = {'left': self.getHandle("Pioneer_p3dx_leftMotor"),
'right': self.getHandle("Pioneer_p3dx_rightMotor")}
self.camHandle = self.getHandle("camera")
#self.gripperHandle = {"left": self.getHandle("left_joint"),
# "right": self.getHandle("right_joint")}
# init camera data stream with rgb and depth
sim.simxGetVisionSensorImage(self.clientID, self.camHandle, 0, sim.simx_opmode_streaming)
sim.simxGetVisionSensorDepthBuffer(self.clientID, self.camHandle, sim.simx_opmode_streaming)
# init position data stream with cartesian position and euler orientation
sim.simxGetObjectOrientation(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming)
sim.simxGetObjectPosition(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming)
# setting motor speed
self.speed = 1
self.lr = 0.3
self.images = None
# remote start simulation if not already
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
# used for making keyboard switches
self.keyboard_key = Clicker("c", activated=True)
self.keyboard_controlled = self.keyboard_key.activated
self.gripper_key = Clicker("g", activated=False)
self.gripper_activated = self.gripper_key.activated
def initCamera(clientID):
_, cameraObject0 = sim.simxGetObjectHandle(clientID, "Vision_sensor0",
sim.simx_opmode_oneshot_wait)
_, cameraObject1 = sim.simxGetObjectHandle(clientID, "Vision_sensor1",
sim.simx_opmode_oneshot_wait)
_, resolution0, imageCamera0 = sim.simxGetVisionSensorImage(
clientID, cameraObject0, 0, sim.simx_opmode_streaming)
_, resolution1, imageCamera1 = sim.simxGetVisionSensorImage(
clientID, cameraObject1, 0, sim.simx_opmode_streaming)
time.sleep(1)
return cameraObject0, cameraObject1
def getVisionSensorImage(self, handle, rgb = True, ignoreError = False, initialize = False):
if rgb:
b = 0
else:
b = 1
if initialize:
sim.simxGetVisionSensorImage(self.clientID, handle, b, sim.simx_opmode_streaming)
res, image_resolution, image_data = sim.simxGetVisionSensorImage(self.clientID, handle, b, sim.simx_opmode_buffer)
if res!=sim.simx_return_ok and not ignoreError:
print('Failed to find {} with error {}'.format(name, res))
return image_data, image_resolution
def image_processor(clientID):
pub = rospy.Publisher('offset', Float64, queue_size=10)
rospy.init_node('image_processor_node')
rate = rospy.Rate(10)
res, handle_cam = sim.simxGetObjectHandle(clientID, "cam",
sim.simx_opmode_blocking)
while not rospy.is_shutdown():
err, resolution, frame = sim.simxGetVisionSensorImage(
clientID, handle_cam, 0, sim.simx_opmode_streaming)
if (sim.simxGetConnectionId(clientID) != -1):
err, resolution, frame = sim.simxGetVisionSensorImage(
clientID, handle_cam, 0, sim.simx_opmode_buffer)
frame = frame[::-1] # original was bgr, I converted to rgb
if err == sim.simx_return_ok:
img = np.array(frame, dtype=np.uint8)
img.resize([resolution[0], resolution[1], 3])
# detect red cube
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
l_b = np.array([0, 100, 100])
u_b = np.array([10, 255, 255])
mask = cv2.inRange(hsv, l_b, u_b)
contours, _ = cv2.findContours(mask, cv2.RETR_TREE,
cv2.CHAIN_APPROX_NONE)
if len(contours) != 0:
(x, y, w, h) = cv2.boundingRect(contours[0])
x_medium = (2.0 * x +
w) / 2.0 # find the center of the object
object_center = (
resolution[0]
) / 2.0 - x_medium # calculate offset between object's center and the frame center
pub.publish(object_center)
#rospy.loginfo(res)
#cv2.line(img, (int(x_medium), 0), (int(x_medium), resolution[0]), (0, 0, 255), 1)
else:
pub.publish(0.0)
rate.sleep()
#cv2.imshow('image', img)
#if cv2.waitKey(1) & 0xFF == ord('q'):
#break
else:
print("Failed to connect to the remote API server...")
sim.simxFinish(clientID)
def getCode(_clientID):
global clientID
clientID = _clientID
correction = 0
# Get the camera handle:
erro, camera = sim.simxGetObjectHandle(clientID, 'Vis_Num',
sim.simx_opmode_oneshot_wait)
# Start the Stream
erro, res, image = sim.simxGetVisionSensorImage(clientID, camera, 0,
sim.simx_opmode_streaming)
frame, resol = getImage(camera)
src = frame.copy()
#test(camera)
img = basicFilter(src, 2, correction)
isolImg, nres = isolateFace(frame.copy(), img, resol, 1)
if (isolImg.size == 0):
return -1
while ((nres[0] > 240 or nres[0] < 80 or nres[1] > 240 or nres[1] < 80 or
(abs(nres[0] - nres[1]) > 70)) and correction != 3):
correction = correction + 1
img = basicFilter(src, 2, correction)
isolImg, nres = isolateFace(frame.copy(), img, resol, 1)
print(nres)
cv2.imwrite('./imgs/7new.png', isolImg)
op2 = compareFaces.compareBar(isolImg, nres)
return op2
def resolveVision(_clientID, _sigValue):
global clientID
global sigValue
sigValue = _sigValue
clientID = _clientID
# Get the camera handle:
erro, camera = sim.simxGetObjectHandle(clientID, 'Vision_s',
sim.simx_opmode_oneshot_wait)
# Start the Stream
erro, res, image = sim.simxGetVisionSensorImage(clientID, camera, 0,
sim.simx_opmode_streaming)
frame, resol = getImage(camera)
src = frame.copy()
img = basicFilter(src, 0)
foundShape, foundColors, foundCenters = findUseful(src, img, 0.09)
foundColors = rgbToLetter(foundColors)
foundCubes = createArray(foundCenters, foundColors, resol[0], resol[1])
j = 0
for shapes in foundShape:
shp = np.reshape(shapes, (5, 1, 2))
cv2.drawContours(frame, shp.astype(int), -1, (255, 0, 255), 3)
j += 1
cv2.imwrite('./imgs/7Final.png', frame)
return foundCubes
def vision():
while (sim.simxGetConnectionId(clientID) != -1):
err, resolution, image = sim.simxGetVisionSensorImage(
clientID, v1, 0, sim.simx_opmode_buffer)
if err == sim.simx_return_ok:
img = np.array(image, dtype=np.uint8)
img.resize([resolution[1], resolution[0], 3])
# image was originally upside down, turn it 180 degree
img180 = cv2.flip(img, 0)
#convert image from bgr -> rgb
imgfinal = cv2.cvtColor(img180, cv2.COLOR_RGB2BGR)
# show image
cv2.imshow('image', imgfinal)
# press 'q' to exit
if cv2.waitKey(1) & 0xFF == ord('q'):
break
elif err == sim.simx_return_novalue_flag:
print('no image')
pass
else:
print(err)
else:
print("Failed to connect to remote API Server")
sim.simxFinish(clientID)
def get_video_image(self):
error_code, resolution, image = vrep.simxGetVisionSensorImage(
self.clientID, self.video_cam, 0, vrep.simx_opmode_buffer)
image = np.array(image, dtype=np.uint8)
image.resize([resolution[0], resolution[1], 3])
image = cv2.rotate(image, cv2.ROTATE_180)
return image
def getImageRGB(self):
clientID = self.clientID
cameraRGBHandle = self.cameraRGBHandle
resolutionX = self.resolutionX
resolutionY = self.resolutionY
res1, resolution1, image_rgb = vrep.simxGetVisionSensorImage(
clientID, cameraRGBHandle, 0, vrep.simx_opmode_blocking)
image_rgb_r = [image_rgb[i] for i in range(0, len(image_rgb), 3)]
image_rgb_r = np.array(image_rgb_r)
image_rgb_r = image_rgb_r.reshape(resolutionY, resolutionX)
image_rgb_r = image_rgb_r.astype(np.uint8)
image_rgb_g = [image_rgb[i] for i in range(1, len(image_rgb), 3)]
image_rgb_g = np.array(image_rgb_g)
image_rgb_g = image_rgb_g.reshape(resolutionY, resolutionX)
image_rgb_g = image_rgb_g.astype(np.uint8)
image_rgb_b = [image_rgb[i] for i in range(2, len(image_rgb), 3)]
image_rgb_b = np.array(image_rgb_b)
image_rgb_b = image_rgb_b.reshape(resolutionY, resolutionX)
image_rgb_b = image_rgb_b.astype(np.uint8)
result_rgb = cv2.merge([image_rgb_b, image_rgb_g, image_rgb_r])
# 镜像翻转, opencv在这里返回的是一张翻转的图
result_rgb = cv2.flip(result_rgb, 0)
return result_rgb
def main():
Pioneer = Robot(clientID)
Pioneer.turnLeft()
time.sleep(2)
Pioneer.moveForward()
time.sleep(2)
Pioneer.turnRight()
time.sleep(2)
Pioneer.moveBackward()
time.sleep(2)
Pioneer.moveForward()
time.sleep(6)
Pioneer.stop()
# errorCode, left_motor_handle = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_leftMotor', sim.simx_opmode_oneshot_wait)
# errorCode, right_motor_handle = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_rightMotor', sim.simx_opmode_oneshot_wait)
# errorCode=sim.simxSetJointTargetVelocity(clientID, left_motor_handle, 0.8, sim.simx_opmode_streaming)
# errorCode=sim.simxSetJointTargetVelocity(clientID, right_motor_handle, 0.8, sim.simx_opmode_streaming)
# errorCode, sensor1 = sim.simxGetObjectHandle(clientID, 'Pioneer_p3dx_ultrasonicSensor1', sim.simx_opmode_oneshot_wait)
# errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
# clientID, sensor1, sim.simx_opmode_streaming)
# errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
# clientID, sensor1, sim.simx_opmode_buffer)
errorCam0, camera_handle = sim.simxGetObjectHandle(
clientID, cam_name, sim.simx_opmode_oneshot_wait)
# Start the Stream
errorCam1, res, image = sim.simxGetVisionSensorImage(
clientID, camera_handle, 0, sim.simx_opmode_streaming)
def get_vision_feedback(self):
"""Получить визуальную обратную связь
Returns
-------
ndarray
Изображение с камеры в RGB
ndarray
Карта глубины стереоизображения
"""
# stereo images
imgs = []
for i in range(2):
_, res, img = sim.simxGetVisionSensorImage(
self.client, self.cameras[i], False, sim.simx_opmode_blocking)
img = np.asarray(img, dtype=np.uint8)
img = np.reshape(img, (res[0], res[1], 3))
img = np.flip(img, axis=0)
imgs.append(img)
# depth map
left = cv.cvtColor(imgs[1], cv.COLOR_RGB2GRAY)
right = cv.cvtColor(imgs[0], cv.COLOR_RGB2GRAY)
depth_map = self.stereo_matcher.compute(left, right)
return imgs[1], depth_map / 752
def get_input_image(self):
error_code, resolution, image = vrep.simxGetVisionSensorImage(
self.clientID, self.input_cam, 0, vrep.simx_opmode_buffer)
image = np.array(image, dtype=np.uint8)
image.resize([resolution[0], resolution[1], 3])
image = np.dot(image[..., :3], [0.2989, 0.5870, 0.1140])
return image
def __init__(self, clientID):
# Connection ID to communicate with simulator
self.clientID = clientID
self.name = 'Vision_sensor'
self.camera = 0
# Get the Vision Sensor Handle
error,self.camera = sim.simxGetObjectHandle(self.clientID,self.name,sim.simx_opmode_oneshot_wait)
error, Resol_XY, image = sim.simxGetVisionSensorImage(self.clientID,self.camera,0,sim.simx_opmode_streaming)
def ResetSimulationScene(self):
errorCode, resolution, image = sim.simxGetVisionSensorImage(
self.clientID, self.camera, 0, sim.simx_opmode_streaming)
pos = sim.simxGetObjectPosition(self.clientID, self.khepera, -1,
sim.simx_opmode_streaming)
ori_body = sim.simxGetObjectOrientation(self.clientID, self.khepera,
-1, sim.simx_opmode_streaming)
tar = sim.simxGetObjectPosition(self.clientID, self.target, -1,
sim.simx_opmode_streaming)
def getImageBGR(clientID, cameraObject):
_, resolution, image = sim.simxGetVisionSensorImage(
clientID, cameraObject, 0, sim.simx_opmode_buffer)
img = np.array(image, dtype=np.uint8)
img.resize([resolution[1], resolution[0], 3])
img = np.rot90(img, 2)
img = np.fliplr(img)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
return img
def image_processor(clientID):
pub = rospy.Publisher('center', Float64, queue_size=10)
rospy.init_node('image_processor_node')
rate = rospy.Rate(10)
res, handle_cam = sim.simxGetObjectHandle(clientID, "cam",
sim.simx_opmode_blocking)
err, resolution, frame = sim.simxGetVisionSensorImage(
clientID, handle_cam, 0, sim.simx_opmode_streaming)
print(err)
def sensor_ir(self, idcam):
if(self.activo == False):
return
_, resolution, image=sim.simxGetVisionSensorImage(self.clientID, idcam, 1, sim.simx_opmode_streaming)
if(len(resolution) == 0):
print("No pudo conectarse a la camara {}".format(idcam))
return None
img = np.array(image, dtype = np.uint8)
return img[0]
def get_image(clientID, idcam):
_, resolution, image=sim.simxGetVisionSensorImage(clientID, idcam, 0, sim.simx_opmode_oneshot)
if(len(resolution) == 0):
print(idcam)
return None
img = np.array(image, dtype = np.uint8)
img.resize([resolution[0], resolution[1], 3])
img = np.fliplr(img)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
return img
def get_SnapShot(self):
# Get Resolution and Image in RGB scale from simxGetVisionSensorImage
error, Resol_XY, image = sim.simxGetVisionSensorImage(self.clientID,self.camera,0,sim.simx_opmode_buffer)
# Reshape the array to 3d matrix
img = np.array(image, dtype = np.uint8)
img.resize([Resol_XY[0],Resol_XY[1],3])
# Flip the figure
img = np.flip(img, 0)
# Return the Resolution and the Image Matrix
return Resol_XY, img
def get_image(self, idcam):
if(self.activo == False):
return
_, resolution, image=sim.simxGetVisionSensorImage(self.clientID, idcam, 0, sim.simx_opmode_streaming)
if(len(resolution) == 0):
print("No pudo conectarse a la camara {}".format(idcam))
return None
img = np.array(image, dtype = np.uint8)
img.resize([resolution[0], resolution[1], 3])
img = np.fliplr(img)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
return img
def raw_image(self, is_grey_scale=False):
"""
Retrieves the image of a vision sensor.
@return the image data
"""
num_of_clr = 3
if is_grey_scale:
num_of_clr = 1
code, resolution, image = s.simxGetVisionSensorImage(
self._id, self._handle, int(is_grey_scale), self._def_op_mode)
return image
def ResetSimulationScene(self):
errorCode, resolution, image = sim.simxGetVisionSensorImage(
self.clientID, self.camera, 0, sim.simx_opmode_streaming)
pos = sim.simxGetObjectPosition(self.clientID, self.khepera, -1,
sim.simx_opmode_streaming)
ori_body = sim.simxGetObjectOrientation(self.clientID, self.khepera,
-1, sim.simx_opmode_streaming)
tar = sim.simxGetObjectPosition(self.clientID, self.target, -1,
sim.simx_opmode_streaming)
for i in range(8):
handle = self.sensor[i]
errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor(
self.clientID, handle, sim.simx_opmode_streaming)