def setup_devices():
""" Assign the devices from the simulator to specific IDs """
global robotID, left_motorID, right_motorID, visonID
# res: result (1(OK), -1(error), 0(not called))
# robot
res, robotID = vrep.simxGetObjectHandle(clientID, 'robot#', WAIT)
# motors
res, left_motorID = vrep.simxGetObjectHandle(clientID, 'leftMotor#', WAIT)
res, right_motorID = vrep.simxGetObjectHandle(clientID, 'rightMotor#',
WAIT)
res, visonID = vrep.simxGetObjectHandle(clientID, "Vision_sensor#", WAIT)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visonID, 0, STREAMING)
time.sleep(0.5)
# start up devices
# wheels
vrep.simxSetJointTargetVelocity(clientID, left_motorID, 0, STREAMING)
vrep.simxSetJointTargetVelocity(clientID, right_motorID, 0, STREAMING)
# pose
vrep.simxGetObjectPosition(clientID, robotID, -1, MODE_INI)
vrep.simxGetObjectOrientation(clientID, robotID, -1, MODE_INI)
# vision sensor
vrep.simxGetVisionSensorImage(clientID, visonID, 1, STREAMING)
return
def initialize_vrep_cameras(self):
#vrep setup
vrep.simxFinish(-1)
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if(self.clientID == -1):
raise RuntimeError("ClientID -1, could not connect to vrep")
else:
#Top camera
#Get the handle of the top vision sensor
res1, self.top_camera_handle = vrep.simxGetObjectHandle(
self.clientID, self.top_camera_name, vrep.simx_opmode_oneshot_wait)
if(res1 != 0):
raise RuntimeError("Could not get top camera handle. res = " + str(res1))
#Get the handle of the bottom vision sensor
res1, self.bottom_camera_handle = vrep.simxGetObjectHandle(
self.clientID, self.bottom_camera_name, vrep.simx_opmode_oneshot_wait)
if(res1 != 0):
raise RuntimeError("Could not get bottom camera handle. res = " + str(res1))
#Initialize top camera stream
_, _, _ = vrep.simxGetVisionSensorImage(
self.clientID, self.top_camera_handle, 0, vrep.simx_opmode_streaming)
#Initialize bottom camera stream
_, _, _ = vrep.simxGetVisionSensorImage(
self.clientID, self.bottom_camera_handle, 0, vrep.simx_opmode_streaming)
def analysisVideo(vision_sensor_name, client_id, config,
handle_video_coordlist):
res1, visionSensorHandle = vrep.simxGetObjectHandle(
client_id, vision_sensor_name, vrep.simx_opmode_oneshot_wait)
# config["vision_sensor_handle"] = visionSensorHandle
vrep.simxGetVisionSensorImage(client_id, visionSensorHandle, 0,
vrep.simx_opmode_streaming)
time.sleep(1)
while (vrep.simxGetConnectionId(client_id) != -1):
# if visionSensorHandle != config["vision_sensor_handle"]:
# continue
# print "config_handle:", config["vision_sensor_handle"]
# print "handle:", visionSensorHandle
res, resolution, image = vrep.simxGetVisionSensorImage(
client_id, visionSensorHandle, 0, vrep.simx_opmode_buffer)
while (not image):
pass
if len(resolution) == 0:
continue
image_byte_array = array.array('b', image)
_img = I.frombuffer("RGB", (resolution[0], resolution[1]),
image_byte_array, "raw", "RGB", 0, 1)
im = np.array(_img)[::-1, :, ::-1].copy()
coordlist = detect(im) # 检测目标物体坐标
handle_video_coordlist(coordlist, config)
# if config["play_video"]:
cv2.imshow(config["video_label"], im)
if cv2.waitKey(15) & 0xFF == 27: # Press ESC to exit :)
break
print 'End of Analysis'
def streamVisionSensor(visionSensorName, clientID, pause, sample_time):
#Get the handle of the vision sensor
res1, visionSensorHandle = vrep.simxGetObjectHandle(
clientID, visionSensorName, vrep.simx_opmode_oneshot_wait)
#Get the image
res2, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_streaming)
#Allow the display to be refreshed
plt.ion()
#Initialiazation of the figure
time.sleep(0.5)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
print resolution
im = I.new("RGB", (resolution[0], resolution[1]), "white")
#Give a title to the figure
fig = plt.figure(1)
fig.canvas.set_window_title(visionSensorName)
#inverse the picture
plotimg = plt.imshow(im, origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
img = I.new("RGB", (resolution[0], resolution[1]))
while (vrep.simxGetConnectionId(clientID) != -1):
start = time.time()
#Get the image of the vision sensor
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b', image)
im = I.frombuffer("RGB", (resolution[0], resolution[1]),
image_byte_array, "raw", "RGB", 0, 1)
#Update the image
plotimg.set_data(im)
#Refresh the display
plt.draw()
#The mandatory pause ! (or it'll not work)
plt.pause(pause)
#Set the image for naoqi
imagepixel = [] #*(resolution[0]*resolution[1])
for i in xrange(resolution[0] * resolution[1]):
r = i * 3
g = r + 1
b = r + 2
imagepixel.append((image[b], image[g], image[r]))
img.putdata(imagepixel)
# print image
# print imagepixel
data = img.tostring()
im = im.rotate(180)
videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0],
resolution[1], im.tostring())
end = time.time()
dt = end - start
if dt < sample_time:
time.sleep(sample_time - dt)
else:
print "Can't keep a period (%gs>%gs)" % (dt, sample_time)
print 'End of Simulation'
def __init__(self, clientID, printFlag = False):
self.clientID = clientID;
# retrieve motor handles
errorCode, self.steer_handle = vrep.simxGetObjectHandle(self.clientID, 'steer_joint', vrep.simx_opmode_oneshot_wait);
errorCode, self.motor_handle = vrep.simxGetObjectHandle(self.clientID, 'motor_joint', vrep.simx_opmode_oneshot_wait);
errorCode, self.fl_brake_handle = vrep.simxGetObjectHandle(self.clientID, 'fl_brake_joint', vrep.simx_opmode_oneshot_wait);
errorCode, self.fr_brake_handle = vrep.simxGetObjectHandle(self.clientID, 'fr_brake_joint', vrep.simx_opmode_oneshot_wait);
errorCode, self.bl_brake_handle = vrep.simxGetObjectHandle(self.clientID, 'bl_brake_joint', vrep.simx_opmode_oneshot_wait);
errorCode, self.br_brake_handle = vrep.simxGetObjectHandle(self.clientID, 'br_brake_joint', vrep.simx_opmode_oneshot_wait);
errorCode, self.camera_f_handle = vrep.simxGetObjectHandle(self.clientID, 'cam_f', vrep.simx_opmode_oneshot_wait);
vrep.simxGetVisionSensorImage(self.clientID, self.camera_f_handle, 0, vrep.simx_opmode_streaming)
print('Received Handles...');
self.factor = 30/(2.68*3.6);
self.max_throttle = 19; # Kmph
self.max_reverse_throttle = -19; #Kmph
self.max_steer = 30; # Degrees
self.printFlag = printFlag;
# Self test the camera
print('Setting up the camera system...');
self.lastFrame = None;
err = 0;
while(err != 1):
err, self.lastFrame = self.get_image();
print('Camera setup successful.')
def streamVisionSensor(visionSensorName, clientID, pause=0.0001):
#Get the handle of the vision sensor
res1, visionSensorHandle = vrep.simxGetObjectHandle(
clientID, visionSensorName, vrep.simx_opmode_oneshot_wait)
#Get the image
res2, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_streaming)
#Initialiazation of the figure
time.sleep(0.5)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
# Init figure
plt.ion()
fig = plt.figure(1)
plotimg = plt.imshow(im, origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
c = 0
while (vrep.simxGetConnectionId(clientID) != -1):
#Get the image of the vision sensor
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
#Transform the image so it can be displayed
img = np.array(image, dtype=np.uint8).reshape(
[resolution[1], resolution[0], 3])
#Update the image
fig.canvas.set_window_title(visionSensorName + '_' + str(c))
plotimg.set_data(img)
plt.draw()
plt.pause(pause)
c += 1
print('End of Simulation')
def get_image(self):
# camera1 1
res, resolution, image1 = vrep.simxGetVisionSensorImage(
self.client_id,
self.camera_handles[0],
options=0,
operationMode=vrep.simx_opmode_blocking)
image1 = np.array(image1, dtype=np.uint8)
image1 = np.reshape(image1, [resolution[1], resolution[0], 3])[::-1,
...]
# camera1 2
res, resolution, image2 = vrep.simxGetVisionSensorImage(
self.client_id,
self.camera_handles[1],
options=0,
operationMode=vrep.simx_opmode_blocking)
image2 = np.array(image2, dtype=np.uint8)
image2 = np.reshape(image2, [resolution[1], resolution[0], 3])[::-1,
...]
# camera1 3
res, resolution, image3 = vrep.simxGetVisionSensorImage(
self.client_id,
self.camera_handles[2],
options=0,
operationMode=vrep.simx_opmode_blocking)
image3 = np.array(image3, dtype=np.uint8)
image3 = np.reshape(image3, [resolution[1], resolution[0], 3])[::-1,
...]
resolution = resolution[::-1]
return resolution, [image1, image2, image3]
def streamVisionSensor(visionSensorName,clientID,pause=0.0001):
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
#Allow the display to be refreshed
plt.ion()
#Initialiazation of the figure
time.sleep(0.5)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
#Give a title to the figure
fig = plt.figure(1)
fig.canvas.set_window_title(visionSensorName)
#inverse the picture
plotimg = plt.imshow(im,origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
while (vrep.simxGetConnectionId(clientID)!=-1):
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
#Update the image
plotimg.set_data(im)
#Refresh the display
plt.draw()
#The mandatory pause ! (or it'll not work)
plt.pause(pause)
print 'End of Simulation'
def obtener_camara_handler(clientID):
"""
Funcion que obtiene un handler de la camara, el cual se puede utilizar
para acceder al laser mas adelante. Ademas, inicializa el visor de la
camara y del laser
Args:
clientID: ID del cliente que ha establecido una conexion con el
servidor de V-REP.
Return:
Devuelve el handler de la camara
"""
# Obtener handler
_, camhandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor',
vrep.simx_opmode_oneshot_wait)
# Inicializar camara y laser
vrep.simxGetVisionSensorImage(clientID, camhandle, 0,
vrep.simx_opmode_streaming)
vrep.simxGetStringSignal(clientID, 'LaserData', vrep.simx_opmode_streaming)
# Esperar 1 segundo hasta que se rellene el buffer
time.sleep(1)
return camhandle
def main():
# Initialize two Vision Sensors. If we don't do this, we can't retrieve images from them.
vrep.simxGetVisionSensorImage(clientID, line_sensor, 0, vrep.simx_opmode_streaming)
vrep.simxGetVisionSensorImage(clientID, obstacle_sensor, 0, vrep.simx_opmode_streaming)
time.sleep(1)
# PID Parameters
# Motor Kp Ki Kd
# 5 0.01 0 0
# 10 0.0001 0.0005
pid = pid_controller(1, 0.0001, 0.0005)
# Set the Power!
motor(10)
# Start the main loop.
while vrep.simxGetConnectionId(clientID) != -1:
raw = get_line_image()
# Crop the line image.
raw = raw[128:192, :]
# Find the desired moment.
ret = get_moment(raw)
if ret is None:
continue
cx, cy, img = ret
# Calculate the angle we want to steer.
angle = math.atan((128 - cx) / cy)
steer(pid(angle))
cv2.imshow("result", img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
def getVisionSensor(visionSensorName,clientID,sample_time):
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
time.sleep(0.5)
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
print resolution
img = I.new("RGB", (resolution[0], resolution[1]))
while (vrep.simxGetConnectionId(clientID)!=-1):
start = time.time()
#Get the image of the vision sensor
res,resolution,image = vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
# #Set the image for naoqi
# imagepixel = []#*(resolution[0]*resolution[1])
# for i in xrange(resolution[0] * resolution[1]):
# r = i*3
# g = r+1
# b = r+2
# imagepixel.append((image[b], image[g], image[r]))
# img.putdata(imagepixel)
# print image
# print imagepixel
videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0], resolution[1], im.rotate(180).tostring())
end = time.time()
dt = end-start
# if dt < sample_time:
# time.sleep(sample_time - dt)
# else:
# print "Can't keep a period (%gs>%gs)" % (dt,sample_time)
print 'End of Simulation'
def getDroneImage(clientID, height):
position = np.asarray([0, 0, height])
# Get Drone Handle
errorCode, droneHandle = vrep.simxGetObjectHandle(
clientID, 'drone', vrep.simx_opmode_oneshot_wait)
# Set Drone Height (in meters)
vrep.simxSetObjectPosition(clientID, droneHandle, -1, position,
vrep.simx_opmode_oneshot_wait)
# Get Drone Camera Image (First Call)
errorDrone, resolution, droneImage = vrep.simxGetVisionSensorImage(
clientID, droneHandle, 0, vrep.simx_opmode_streaming)
count = 0
while (vrep.simxGetConnectionId(clientID) != -1):
# Read in drone camera image (Note operating mode changed to buffer)
errorDrone, resolution, droneImage = vrep.simxGetVisionSensorImage(
clientID, droneHandle, 0, vrep.simx_opmode_buffer)
count += 1
# quit after 5 images read (first images may be empty)
if (errorDrone == vrep.simx_return_ok and count >= 1):
droneImage = np.array(droneImage, dtype=np.uint8)
droneImage.resize([resolution[1], resolution[0], 3])
break
return resolution, droneImage
def streamVisionSensor(visionSensorName,clientID,pause=0.0001):
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
#Allow the display to be refreshed
plt.ion()
#Initialiazation of the figure
time.sleep(0.5)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
#Give a title to the figure
fig = plt.figure(1)
fig.canvas.set_window_title(visionSensorName)
#inverse the picture
plotimg = plt.imshow(im,origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
while (vrep.simxGetConnectionId(clientID)!=-1):
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
#Update the image
plotimg.set_data(im)
#Refresh the display
plt.draw()
#The mandatory pause ! (or it'll not work)
plt.pause(pause)
print 'End of Simulation'
def startDataStreaming():
if (not clientID):
print('connect first')
elif (clientID[0] == -1):
print('connect first')
else:
#Position
for i in Objects_handle:
vrep.simxGetObjectPosition(clientID[0], i, UR3_handle[0],
vrep.simx_opmode_streaming)
#Dummy position
vrep.simxGetObjectPosition(clientID[0], end_effector[0], UR3_handle[0],
vrep.simx_opmode_streaming)
#vision
for i in vision_sensor:
vrep.simxGetVisionSensorImage(clientID[0], i, 0,
vrep.simx_opmode_streaming)
#collision
for i in collision_handle:
vrep.simxReadCollision(clientID[0], i, vrep.simx_opmode_streaming)
#UR3 Joint
for i in UR3_joint_handle:
vrep.simxGetJointPosition(clientID[0], i,
vrep.simx_opmode_streaming)
def initialize_vrep_cameras(top_camera_name, bottom_camera_name,
clientID):
#Top camera
#Get the handle of the top vision sensor
res1, top_camera_handle = vrep.simxGetObjectHandle(
clientID, top_camera_name, vrep.simx_opmode_oneshot_wait)
if (res1 != 0):
raise RuntimeError("Could not get top camera handle. res = " +
str(res1))
#Get the handle of the bottom vision sensor
res1, bottom_camera_handle = vrep.simxGetObjectHandle(
clientID, bottom_camera_name, vrep.simx_opmode_oneshot_wait)
if (res1 != 0):
raise RuntimeError(
"Could not get bottom camera handle. res = " + str(res1))
#Initialize top camera stream
_, _, _ = vrep.simxGetVisionSensorImage(clientID,
top_camera_handle, 0,
vrep.simx_opmode_streaming)
#Initialize bottom camera stream
_, _, _ = vrep.simxGetVisionSensorImage(clientID,
bottom_camera_handle, 0,
vrep.simx_opmode_streaming)
time.sleep(0.5)
return top_camera_handle, bottom_camera_handle
def main():
vrep.simxFinish(-1) # just in case, close all opened connections
clientID = vrep.simxStart('127.0.0.1', 20000, True, True, 1300,
5) # Connect to V-REP
if clientID == -1:
sys.exit()
print('Connected to remote API server')
res, camhandle = vrep.simxGetObjectHandle(clientID, 'camara_1',
vrep.simx_opmode_oneshot_wait)
print(res)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, camhandle, 0, vrep.simx_opmode_streaming)
##############
args = cli()
# load model
model, _ = nets.factory_from_args(args)
model = model.to(args.device)
processor = decoder.factory_from_args(args, model)
visualizer = None
while True:
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, camhandle, 0, vrep.simx_opmode_buffer)
if len(image) == 0:
continue
img = np.array(image, dtype=np.uint8)
img.resize([resolution[1], resolution[0], 3])
img = np.rot90(img, 2)
img = np.fliplr(img)
cv2.imshow('t', img)
cv2.waitKey(1)
image = cv2.resize(img, None, fx=args.scale, fy=args.scale)
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
if visualizer is None:
visualizer = Visualizer(processor, args)(image)
visualizer.send(None)
start = time.time()
image_pil = PIL.Image.fromarray(image)
processed_image_cpu, _, __ = transforms.EVAL_TRANSFORM(
image_pil, [], None)
processed_image = processed_image_cpu.contiguous().to(
args.device, non_blocking=True)
#print('preprocessing time', time.time() - start)
fields = processor.fields(torch.unsqueeze(processed_image, 0))[0]
visualizer.send((image, fields))
#print('loop time = {:.3}s, FPS = {:.3}'.format(
# time.time() - last_loop, 1.0 / (time.time() - last_loop)))
last_loop = time.time()
vrep.simxFinish(clientID)
def streamVisionSensor(visionSensorName, clientID, pause=0.0001):
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, 1)
# start the simulation:
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
# enable streaming of the iteration counter:
res, iteration1 = vrep.simxGetIntegerSignal(clientID, "iteration",
vrep.simx_opmode_streaming)
print("iteration1: ", iteration1)
#Get the handle of the vision sensor
res1, visionSensorHandle = vrep.simxGetObjectHandle(
clientID, visionSensorName, vrep.simx_opmode_oneshot_wait)
#Get the image
res2, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_streaming)
#Initialiazation of the figure
time.sleep(0.5)
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
# Init figure
plt.ion()
fig = plt.figure(1)
plotimg = plt.imshow(im, origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
while (vrep.simxGetConnectionId(clientID) != -1):
if iteration1 < 30:
vrep.simxSynchronousTrigger(clientID)
res, iteration1 = vrep.simxGetIntegerSignal(
clientID, "iteration", vrep.simx_opmode_buffer)
if res != vrep.simx_return_ok: iteration1 = -1
iteration2 = iteration1
while iteration2 == iteration1: # wait until the iteration counter has changed
res, iteration2 = vrep.simxGetIntegerSignal(
clientID, "iteration", vrep.simx_opmode_buffer)
if res != vrep.simx_return_ok:
iteration2 = -1
print(iteration2)
#Get the image of the vision sensor
res, resolution, image = vrep.simxGetVisionSensorImage(
clientID, visionSensorHandle, 0, vrep.simx_opmode_buffer)
#Transform the image so it can be displayed
img = np.array(image, dtype=np.uint8).reshape(
[resolution[1], resolution[0], 3])
#Update the image
fig.canvas.set_window_title(visionSensorName + '_' +
str(iteration2))
plotimg.set_data(img)
plt.draw()
plt.pause(pause)
# vrep.simxSynchronousTrigger(clientID)
print('End of Simulation')
def setup(self):
if self.clientID != -1:
errorCode, handles, intData, floatData, array = vrep.simxGetObjectGroupData(self.clientID,
vrep.sim_appobj_object_type,
0,
vrep.simx_opmode_oneshot_wait)
data = dict(zip(array, handles))
self.camera = [value for key, value in data.items() if "Vision_sensor" in key][0]
self.target = [value for key, value in data.items() if "Quadricopter_target" == key][0]
vrep.simxGetVisionSensorImage(self.clientID, self.camera, 1, vrep.simx_opmode_streaming)
def streamVisionSensor(visionSensorName,clientID,pause=0.0001):
nn = newNeuralNetwork()
#print("Neural Network num_inputs:{n}".format(n=nn.num_inputs))
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
#Allow the display to be refreshed
plt.ion()
#Initialiazation of the figure
time.sleep(0.5)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
im = I.new("RGB", (resolution[0], resolution[1]), "white")
#Give a title to the figure
im = im.crop((160, 0, 480, 480))
fig = plt.figure(1)
fig.canvas.set_window_title(visionSensorName)
#inverse the picture
plotimg = plt.imshow(im,origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
count = COUNTER
while (vrep.simxGetConnectionId(clientID)!=-1):
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
im = im.crop((160, 0, 480, 480))
#Update the image
plotimg.set_data(im)
#Refresh the display
plt.axis("off")
plt.show()
#The mandatory pause ! (or it'll not work)
plt.pause(pause)
if (nn):
# feed the neural network with the image and get its prediction (if it has a plant)
trainer = Trainer("live", imageIntoFeatures(im))
answer = nn.predict(trainer.inputs)
print(answer)
#assertAnswer(answer)
#assertAnswerStrong(answer)
if (SAVE_VISION and count % 60 == 0):
name = str(count//60) + ".png"
print("Saving " + name)
im.save(name)
count += 1
print('End of Simulation')
def pdControl(clientID, youBotCam, goal):
print("begin PD control")
wheelJoints = move.getWheelJoints(clientID)
err, res, image = vrep.simxGetVisionSensorImage(clientID, youBotCam, 0,
vrep.simx_opmode_buffer)
cv2Image = colorDet.convertToCv2Format(image, res)
#cv2.imshow("Current Image of youBot", cv2Image)
#cv2.waitKey(0)
# initialize variables for while loop
cor = getRedBlobPicture(cv2Image)
if len(cor) == 0:
cor = getBlueBlobPicture(cv2Image)
preCor = cor
rotVel = 0.0
forwBackVel = 999 # just a random number to initialize while loop
leftRightVel = 999 # just a random number to initialize while loop
while not velOk(forwBackVel, leftRightVel):
# retrieve new coordinate of red blob
err, res, image = vrep.simxGetVisionSensorImage(
clientID, youBotCam, 0, vrep.simx_opmode_buffer)
cv2Image = colorDet.convertToCv2Format(image, res)
cor = getRedBlobPicture(cv2Image)
if len(cor) == 0:
cor = getBlueBlobPicture(cv2Image)
# calculate new velocities
forwBackVel = 0.1 * (cor[0][0] - goal[0]) - 0.002 * (cor[0][0] -
preCor[0][0])
leftRightVel = 0.08 * (cor[0][1] - goal[1]) - 0.001 * (cor[0][1] -
preCor[0][1])
# save current coordinates as previous coordinates
preCor = cor
# update velocities
move.setWheelVelocity(clientID, 0.0)
vrep.simxPauseCommunication(clientID, True)
for i in range(0, 4):
vrep.simxSetJointTargetVelocity(
clientID, wheelJoints[i],
move.wheelVel(forwBackVel / 10.0, leftRightVel / 10.0,
rotVel)[i], vrep.simx_opmode_oneshot)
vrep.simxPauseCommunication(clientID, False)
# stop moving
move.setWheelVelocity(clientID, 0.0)
#cv2.imshow("Current Image of youBot", cv2Image)
#cv2.waitKey(0)
print("end PD control")
def collectImageData(clientID, action_arg):
list_of_images = []
collector = []
if clientID!=-1:
res,v0=vrep.simxGetObjectHandle(clientID,'Vision_sensor',vrep.simx_opmode_oneshot_wait)
res,v1=vrep.simxGetObjectHandle(clientID,'PassiveVision_sensor',vrep.simx_opmode_oneshot_wait)
ret_code, left_handle = vrep.simxGetObjectHandle(clientID,'DynamicLeftJoint', vrep.simx_opmode_oneshot_wait)
ret_code, right_handle = vrep.simxGetObjectHandle(clientID,'DynamicRightJoint', vrep.simx_opmode_oneshot_wait)
ret_code, base_handle = vrep.simxGetObjectHandle(clientID, 'LineTracerBase', vrep.simx_opmode_oneshot_wait)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v0,0,vrep.simx_opmode_streaming)
ret_code, euler_angles = vrep.simxGetObjectOrientation(clientID, base_handle, -1, vrep.simx_opmode_streaming)
count = 0
action = 2
inference_counter = 0
steps = 50
action_length = 0
while (vrep.simxGetConnectionId(clientID)!=-1 and count < steps):
tim = time.time()
for i in range(10):
vrep.simxSynchronousTrigger(clientID)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,v0,0,vrep.simx_opmode_buffer)
if res==vrep.simx_return_ok:
img = np.array(image,dtype=np.uint8)
img.resize([resolution[1],resolution[0],3])
rotate_img = img.copy()
rotate_img = np.flipud(img)
list_of_images.append(rotate_img)
ret_code, pos = vrep.simxGetObjectPosition(clientID, base_handle, -1, vrep.simx_opmode_oneshot)
ret_code, velo, angle_velo = vrep.simxGetObjectVelocity(clientID, base_handle, vrep.simx_opmode_oneshot)
ret_code, euler_angles = vrep.simxGetObjectOrientation(clientID, base_handle, -1, vrep.simx_opmode_buffer)
collector.append([pos[0], pos[1], pos[2], velo[0], velo[1], velo[2], euler_angles[0], euler_angles[1], euler_angles[2], action])
if action_length < 1 and action_arg == -1:
action_length = random.randint(2,10)
action = np.random.randint(0, 5)
elif action_arg > -1:
action = action_arg
action_length = action_length - 1
velo = (action -2) * 15
return_val = vrep.simxSetJointTargetVelocity(clientID, left_handle, velo, vrep.simx_opmode_oneshot)
return_val2 = vrep.simxSetJointTargetVelocity(clientID, right_handle, velo, vrep.simx_opmode_oneshot_wait)
count+=1
return list_of_images, collector
else:
sys.exit()
def getVisionSensor(visionSensorName,clientID):
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
time.sleep(1)
while (vrep.simxGetConnectionId(clientID)!=-1):
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
print resolution
print 'End of Simulation'
def getVisionSensor(visionSensorName,clientID):
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
time.sleep(1)
while (vrep.simxGetConnectionId(clientID)!=-1):
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
print resolution
print 'End of Simulation'
def streamVisionSensor(visionSensorName,clientID,pause,sample_time):
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
#Allow the display to be refreshed
plt.ion()
#Initialiazation of the figure
time.sleep(0.5)
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
print resolution
im = I.new("RGB", (resolution[0], resolution[1]), "white")
#Give a title to the figure
fig = plt.figure(1)
fig.canvas.set_window_title(visionSensorName)
#inverse the picture
plotimg = plt.imshow(im,origin='lower')
#Let some time to Vrep in order to let him send the first image, otherwise the loop will start with an empty image and will crash
time.sleep(1)
img = I.new("RGB", (resolution[0], resolution[1]))
while (vrep.simxGetConnectionId(clientID)!=-1):
start = time.time()
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "RGB", 0, 1)
#Update the image
plotimg.set_data(im)
#Refresh the display
plt.draw()
#The mandatory pause ! (or it'll not work)
plt.pause(pause)
#Set the image for naoqi
imagepixel = []#*(resolution[0]*resolution[1])
for i in xrange(resolution[0] * resolution[1]):
r = i*3
g = r+1
b = r+2
imagepixel.append((image[b], image[g], image[r]))
img.putdata(imagepixel)
# print image
# print imagepixel
data = img.tostring()
im = im.rotate(180)
videoDeviceProxy.putImage(vision_definitions.kTopCamera, resolution[0], resolution[1], im.tostring())
end = time.time()
dt = end-start
if dt < sample_time:
time.sleep(sample_time - dt)
else:
print "Can't keep a period (%gs>%gs)" % (dt,sample_time)
print 'End of Simulation'
def read_vision_sensor(self): """ Reads the image raw data from vrep vision sensor. Returns: resolution: Tuple with the image resolution. image: List with the image data. """ res, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.vision_handle, 0, vrep.simx_opmode_streaming) while(res != vrep.simx_return_ok): res, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.vision_handle, 0, vrep.simx_opmode_buffer) return resolution, image
def getDepthImage(clientID):
imageCount = 0 # to keep track of how many images have been processed
processLimit = 5
# print ('Vision Sensor object handling')
errorCode, rgbCamHandle = vrep.simxGetObjectHandle(
clientID, 'kinect_rgb', vrep.simx_opmode_oneshot_wait)
errorCode, xyzCamHandle = vrep.simxGetObjectHandle(
clientID, 'kinect_pointcloud', vrep.simx_opmode_oneshot_wait)
# print ('Setting up First Calls for Reading Images and Sensor Values')
# Note that the image takes a while to display so cannot load first image but
# have to set up a buffer
errorRGB, resolution, rgbImage = vrep.simxGetVisionSensorImage(
clientID, rgbCamHandle, 0, vrep.simx_opmode_streaming)
# api return value for the coordinate extraction filter in camera is as follows:
# (1) point count along x, (2) point count along Y, (3) point x1, (4) point y1,
# (5) point z1, (6) distance point1, (7) point x2, etc.
# and auxPacket[1] holds data in the format described above
errorXYZ, detectionState, auxPackets = vrep.simxReadVisionSensor(
clientID, xyzCamHandle, vrep.simx_opmode_streaming)
# loop while connected to simulation
while (vrep.simxGetConnectionId(clientID) != -1):
# Read in camera information (Note operating mode changed to buffer)
errorRGB, resolution, rgbImage = vrep.simxGetVisionSensorImage(
clientID, rgbCamHandle, 0, vrep.simx_opmode_buffer)
errorXYZ, detectionState, auxPackets = vrep.simxReadVisionSensor(
clientID, xyzCamHandle, vrep.simx_opmode_buffer)
errorCodes = [errorRGB, errorXYZ]
# Check if have both image and xyz coordinate data available
if (all(errors == vrep.simx_return_ok for errors in errorCodes)):
# print ("image OK!!!")
imageCount += 1
elif (all(errors == vrep.simx_return_novalue_flag
for errors in errorCodes)):
# print ("no image yet")
pass
else:
# print (errorCodes)
pass
# exit if processed more than certain amount of images
if imageCount > processLimit:
break
return auxPackets, resolution, rgbImage
def renderSensorImage(clientID, camera, fname):
errRender, resolution, image = vrep.simxGetVisionSensorImage(
clientID, camera, 0, vrep.simx_opmode_streaming)
errRender, resolution, image = vrep.simxGetVisionSensorImage(
clientID, camera, 0, vrep.simx_opmode_buffer)
if errRender == vrep.simx_return_ok:
img = np.array(image, dtype=np.uint8)
img.resize([resolution[0], resolution[1], 3])
img = cv2.flip(img, 0)
img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
cv2.imwrite(fname, img)
pass
def getVisionSensor(self):
_, resolution, image = vrep.simxGetVisionSensorImage(
self.clientID, self.visionSensor, 0, vrep.simx_opmode_streaming)
time.sleep(0.1)
_, resolution, image = vrep.simxGetVisionSensorImage(
self.clientID, self.visionSensor, 0, vrep.simx_opmode_buffer)
sensorImage = np.array(image, dtype=np.uint8)
image_1 = sensorImage.flatten()
sensorImage.resize([resolution[0], resolution[1], 3])
mpl.imshow(sensorImage, origin='lower')
return image_1, sensorImage
def getImage(self, vision_handle_number):
if self.synchronous:
err, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.vision_handles[vision_handle_number], 0, vrep.simx_opmode_blocking)
while err != vrep.simx_return_ok:
self.step_forward()
err, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.vision_handles[vision_handle_number], 0, vrep.simx_opmode_blocking)
else:
err, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.vision_handles[vision_handle_number], 0, vrep.simx_opmode_streaming)
while err != vrep.simx_return_ok:
err, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.vision_handles[vision_handle_number], 0, vrep.simx_opmode_buffer)
print("Image OK!")
img = np.array(image,dtype=np.uint8)
img.resize([resolution[1],resolution[0],3])
return img
def get_camera_image(self):
data = {
'res': -1,
'err_list': ["camera isn't connected to client"],
'data': {
'image': np.zeros((128, 128, 3)),
'resolution': (128, 128, 3)
}
}
if self.components['camera']['id'] != None:
res, resolution, image = vrep.simxGetVisionSensorImage(
self.client_id, self.components['camera']['id'], 0,
vrep.simx_opmode_streaming)
data['res'] = res
data['err_list'] = err_list = parse_error(res)
print resolution
if res not in (0, 1) and self.debug:
err_print(prefix='CAMERA', message=err_list)
elif len(resolution) > 1:
resolution = (resolution[0], resolution[1], 3)
data['data']['resolution'] = resolution
mat_image = np.array(image, dtype=np.uint8)
mat_image.resize([resolution[0], resolution[1], 3])
data['data']['image'] = mat_image[::-1]
return data
def get_camera_data(self):
"""
-- Read images data from vrep and convert into np array
"""
# Get color image from simulation
res, resolution, raw_image = vrep.simxGetVisionSensorImage(
self.clientID, self.kinectRGB_handle, 0,
vrep.simx_opmode_oneshot_wait)
self._error_catch(res)
color_img = np.array(raw_image, dtype=np.uint8)
color_img.shape = (resolution[1], resolution[0], 3)
color_img = color_img.astype(np.float) / 255
color_img[color_img < 0] += 1
color_img *= 255
color_img = np.fliplr(color_img)
color_img = color_img.astype(np.uint8)
# Get depth image from simulation
res, resolution, depth_buffer = vrep.simxGetVisionSensorDepthBuffer(
self.clientID, self.kinectDepth_handle,
vrep.simx_opmode_oneshot_wait)
self._error_catch(res)
depth_img = np.array(depth_buffer)
depth_img.shape = (resolution[1], resolution[0])
depth_img = np.fliplr(depth_img)
# zNear = 0.01
# zFar = 10
# depth_img = depth_img * (zFar - zNear) + zNear
depth_img[depth_img < 0] = 0
depth_img[depth_img > 1] = 1
depth_img = depth_img * self.INT16
return depth_img, color_img
def __init__(self):
vrep.simxFinish(-1)
self.clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
# start simulation
rc = vrep.simxStartSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
# object handles
res, self.quad_obj = vrep.simxGetObjectHandle(
self.clientID, 'Quadricopter', vrep.simx_opmode_oneshot_wait)
res, self.camera = vrep.simxGetObjectHandle(
self.clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait)
# Initialise data streaming from V-REP
err, resolution, image = vrep.simxGetVisionSensorImage(
self.clientID, self.camera, 0, vrep.simx_opmode_streaming)
_, pos = vrep.simxGetObjectPosition(self.clientID, self.quad_obj, -1,
vrep.simx_opmode_streaming)
time.sleep(2)
# Variables
_, self.last_pos = vrep.simxGetObjectPosition(self.clientID,
self.quad_obj, -1,
vrep.simx_opmode_buffer)
def _vrep_get_vision_sensor_image(self,
camera_handle,
opmode=vrep.simx_opmode_buffer,
a=0):
return vrep.unwrap_vrep(
vrep.simxGetVisionSensorImage(self._clientID, camera_handle, a,
opmode))
def dvs_vision(cid, handle, height=32, width=32):
err, res, image = vrep.simxGetVisionSensorImage(cid, handle, 1, vrep_mode)
# Return zeroes if nothing is in the input buffer
if err == 1:
return np.zeros(height * width)
#return np.array(image).ravel()==0
return (np.array(image).ravel()==0) + (np.array(image).ravel()==-1)
def findAllBlobs(clientId, youBotCam, homoMatrix):
currentDegree = 0
blobList = []
#we will rotate for 180 degree for spotting the blobs
i = 0
while (i < 4):
i += 1
err, res, image = vrep.simxGetVisionSensorImage(
clientId, youBotCam, 0, vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
# do some image stuff ----------------------------------------------------------------------------------
cv2Image = convertToCv2Format(image, res)
tempBlobs = getBlobsGlobal(cv2Image, homoMatrix, clientId)
count = 0
for tb in tempBlobs:
count = 0
for b in blobList:
if move.isSamePoint(tb[0], b[0]) and tb[1] == b[1]:
count = count + 1
if count == 0:
print("added", tb, " to blobList")
blobList.append(tb)
move.rotate(10, clientId, True)
return blobList
def exercise4_action(clientID, youBotCam):
counter = 1
while counter < 5:
# get further images from vision sensor
err, res, image = vrep.simxGetVisionSensorImage(
clientID, youBotCam, 0, vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
# do some image stuff ----------------------------------------------------------------------------------
cv2Image = convertToCv2Format(image, res)
cv2ImageCopy = cv2Image
imageContours = getContours(cv2Image, boundariesBlue)
cv2.drawContours(cv2ImageCopy, imageContours, 0, (255, 0, 0), 1)
cv2.imshow("Current Image of youBot", cv2ImageCopy)
# calculate center of green blob
x, y = calcCenter(imageContours[0])
print("Current center of the green blob: x={} y={}".format(x, y))
cv2.waitKey(0) # key to get out of waiting is ESC
# end some image stuff ---------------------------------------------------------------------------------
move.forward(0.1, clientID)
counter += 1
def get_H_matrix(chessboard_corners, youBotCam, clientID):
print("Begin calculation of H-matrix, please wait ...")
err, res, image = vrep.simxGetVisionSensorImage(clientID, youBotCam, 0,
vrep.simx_opmode_buffer)
image = convertToCv2Format(image, res)
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
found, prime_corners = cv2.findChessboardCorners(image, (3, 4))
prime_corners = addOne(prime_corners)
# convert all global corners of the chessboard in egocentric world space
ego_corners = []
for gc in chessboard_corners:
newCorner = globalToEgocentric(gc, clientID)
ego_corners.append(newCorner)
# add a 1 in every row (globalToEgocentric only returns x,y coordinates
ego_corners = addOne(ego_corners)
# convert ego_corners in numpy array
ego_corners = np.asarray(ego_corners)
# calculate H-matrix
A = getA(prime_corners, ego_corners)
return getH(A)
def getImage (self):
#errorCode, resolution, image=vrep.simxGetVisionSensorImage(self.clientID, self.camera, 0, vrep.simx_opmode_streaming)
#print "pillando imagen"
#time.sleep(1) #necesario
errorCode, resolution, image=vrep.simxGetVisionSensorImage(self.clientID, self.camera, 0, vrep.simx_opmode_oneshot_wait)
im=np.array(image, dtype=np.uint8)
im.resize([resolution[1], resolution[0],3])
im=np.flipud(im)
return im
def moveAndReadLights(self):
"rotate in place and print light readings"
eCode, res, rightEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.rightEye, 0, vrep.simx_opmode_streaming)
ecode, res, leftEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.leftEye, 0, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(self.simulID)
for step in xrange(self.noSteps):
rightSpeed = 25
leftSpeed = rightSpeed
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.rightMotor, rightSpeed, vrep.simx_opmode_oneshot_wait)
eCode = vrep.simxSetJointTargetVelocity(self.simulID, self.leftMotor, leftSpeed, vrep.simx_opmode_oneshot_wait)
vrep.simxSynchronousTrigger(self.simulID)
eCodeR, res, rightEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.rightEye, 0, vrep.simx_opmode_buffer)
eCodeL, res, leftEyeRead = vrep.simxGetVisionSensorImage(self.simulID, self.leftEye, 0, vrep.simx_opmode_buffer)
vrep.simxSynchronousTrigger(self.simulID)
# print "Right eCode:\t", eCodeR,
# print "Left eCode:\t", eCodeL
# leftImg = np.array(leftEyeRead, np.uint8)
# rightImg.resize(res[0],res[1],3)
print "Right:\t%d, %d\tLeft:\t%d, %d"% (len(rightEyeRead),sum(rightEyeRead), len(leftEyeRead),sum(leftEyeRead))
def get_camera_image(self): 'Get the image from the camera' err, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.camera, 0, vrep.simx_opmode_buffer) img = np.array(image,dtype=np.uint8) img.resize([resolution[1],resolution[0],3]) img = np.fliplr(img) temp = np.copy(img[:,:,0]) img[:,:,0] = np.copy(img[:,:,2]) img[:,:,2] = temp return img
def fetchKinect(depthSTR,rgbSTR):
errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,rgbSTR,vrep.simx_opmode_oneshot_wait)
errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_oneshot_wait)
errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_oneshot_wait)
img,imgArr=Raftaar.ProcessImage(image,resolution)
rgbArr=np.array(imgArr)
errorHere,resol,depth=vrep.simxGetVisionSensorDepthBuffer(clientID,kinectDepth,vrep.simx_opmode_oneshot_wait)
depthArr=np.array(depth)
return depthArr,rgbArr
def get_and_process_image(self): """ returns the aggregated data of bubbleRobs Vision Sensor """ err,res,img = vrep.simxGetVisionSensorImage(self.clientID,self.visionSensorHandle,0,vrep.simx_opmode_oneshot_wait) colval = numpy.zeros((res[0]*res[1],3)) i = 0 for pix in range(res[0]*res[1]): for col in range(3): if img[i] >= 0: colval[pix][col] = img[i] else: colval[pix][col] = img[i] + 256 i += 1 red = 0.0 red_position = 0.0 green = 0.0 green_position = 0.0 blue = 0.0 blue_position = 0.0 i = 0 for pix in colval: i += 1 position = (i % 32) if (pix[0] > 200 and pix[1] < 100 and pix[2] < 100): red += 1 red_position += -1 + position * 0.0625 elif (pix[0] < 100 and pix[1] > 200 and pix[2] < 100): green += 1 green_position += -1 + position * 0.0625 elif (pix[0] < 100 and pix[1] < 100 and pix[2] > 200): blue += 1 blue_position += -1 + position * 0.0625 if (red > 0): red_position = red_position / red if (green > 0): green_position = green_position / green if (blue > 0): blue_position = blue_position / blue input_vars = [red / 256, red_position, green / 256, green_position, blue / 256, blue_position] return input_vars
def __init__(self,port_number):
vrep.simxFinish(-1)
self.clientID = vrep.simxStart('127.0.0.1', port_number, True, True, 5000, 5)
# start simulation
rc = vrep.simxStartSimulation(self.clientID, vrep.simx_opmode_oneshot_wait)
# object handles
res, self.quad_obj = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter', vrep.simx_opmode_oneshot_wait)
res, self.camera = vrep.simxGetObjectHandle(self.clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait)
res, self.target = vrep.simxGetObjectHandle(self.clientID, 'Quadricopter_target', vrep.simx_opmode_oneshot_wait)
# Initialise data streaming from V-REP
err, resolution, image = vrep.simxGetVisionSensorImage(self.clientID, self.camera, 0, vrep.simx_opmode_streaming)
_,pos = vrep.simxGetObjectPosition(self.clientID, self.quad_obj, -1, vrep.simx_opmode_streaming)
_,target_pos = vrep.simxGetObjectPosition(self.clientID, self.target, -1, vrep.simx_opmode_streaming)
time.sleep(2)
# Variables
_,self.last_pos = vrep.simxGetObjectPosition(self.clientID, self.quad_obj, -1, vrep.simx_opmode_buffer)
def initilize_vrep_api(self):
# initialize ePuck handles and variables
_, self.bodyelements=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_bodyElements', vrep.simx_opmode_oneshot_wait)
_, self.leftMotor=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
_, self.rightMotor=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
_, self.ePuck=vrep.simxGetObjectHandle(
self.clientID, 'ePuck', vrep.simx_opmode_oneshot_wait)
_, self.ePuckBase=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_base', vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of ePuck
_, self.xyz = vrep.simxGetObjectPosition(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
# initialize overhead cam
_, self.overheadCam=vrep.simxGetObjectHandle(
self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
_, self.resolution, self.image = vrep.simxGetVisionSensorImage(
self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)
# initialize goal handle + odom
_, self.goalHandle=vrep.simxGetObjectHandle(
self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
_, self.goalPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# STOP Motor Velocities. Not sure if this is necessary
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.leftMotor,0,vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.rightMotor,0,vrep.simx_opmode_oneshot_wait)
turnToTargetAngle (getTargetAngle(target_x,target_y))
moveRobot(1,1)
time.sleep(0.2)
print getDistanceToTarget(target_x, target_y)
print "punto!"
moveRobot(0,0)
#moveRobot(-0.5,0.5);4
printRobotLocation()
#print getAngleToTarget(5,5)
errorCode, resolution, image=vrep.simxGetVisionSensorImage(clientID, camera, 0, vrep.simx_opmode_streaming)
time.sleep(1)
errorCode, resolution, image=vrep.simxGetVisionSensorImage(clientID, camera, 0, vrep.simx_opmode_buffer)
errorCode, resolution, image=vrep.simxGetVisionSensorImage(clientID, camera, 0, vrep.simx_opmode_buffer)
errorCode, resolution, image=vrep.simxGetVisionSensorImage(clientID, camera, 0, vrep.simx_opmode_buffer)
errorCode, resolution, image=vrep.simxGetVisionSensorImage(clientID, camera, 0, vrep.simx_opmode_buffer)
errorCode, resolution, image=vrep.simxGetVisionSensorImage(clientID, camera, 0, vrep.simx_opmode_buffer)
print "hola"
im=np.array(image, dtype=np.uint8)
im.resize([resolution[1], resolution[0],3])
#mlp.imshow(im)
def robot_code(self):
t = time.time()
self.curr_goal = 0
while (time.time() - t) < 200:
# global map
_,resolution,image = vrep.simxGetVisionSensorImage(self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait) # Get image
im = self.global_map_preprocess(resolution, image)
self.pose = self.robot_pose_get()
x, y, theta = self.pose
# theta = self.theta_history.weighted_average(scheme='last', mathfunc=lambda x: np.exp(x))
# initialize worldNorthTheta for the first time
if self.worldNorthTheta is None:
self.worldNorthTheta = self.pose[2]
_ = [self.theta_history.append(self.pose[2]) for _ in range(self.history_length)]
self.pose_pixel = np.array(odom2pixelmap(x, y, self.map_side_length, im.shape[0]))
m, n = self.pose_pixel
self.paths, self.blurred_paths = self.global_map_process(im)
# calculate intermediate goals once
if self.GOALS is None:
raw_input("")
# acquire pixel location of goal
_, finishPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_buffer)
self.finish_pixel = odom2pixelmap(finishPose[0], finishPose[1], self.map_side_length, im.shape[0])
contiguousPath = AStarBinaryGrid(self.blurred_paths).calculate_path(self.pose_pixel, self.finish_pixel)
self.GOALS = contiguousPath[::self.path_skip]
# SKIP THIS FIRST LOOP AND CONTINUE
continue
else:
self.goal_pose_pixel = np.array(self.GOALS[self.curr_goal])
goal_m, goal_n = self.goal_pose_pixel
lidarValues = self.lidar_scan_get(window_size=21)
#############################
# Potential Field Algorithm #
#############################
repulsionVectors = self.repulsion_vectors_compute(lidarValues, k_repulse=10.0)
attractionVector = self.attraction_vector_compute(k_attract=100.0)
finalVector = np.sum(np.vstack((repulsionVectors, attractionVector)), axis=0)
finalUnitVector = finalVector / np.linalg.norm(finalVector)
print "finalUnitVector: ", finalUnitVector
# TODO: do we use the unit vector (for direction) only, or the finalVector?
finalValue, finalAngle = cart2pol(finalUnitVector[0], finalUnitVector[1])
error_theta = angle_diff(mapTheta2worldTheta(finalAngle, self.worldNorthTheta), theta)
self.e_theta_h.append(error_theta)
k_angular_p = 2.0 * self.maxVelocity
k_angular_D = 0.25
k_angular_S = 1.0
k_angular_I = 2.5
omega = k_angular_p * (
error_theta
+ k_angular_D / k_angular_S * (self.e_theta_h[-1] - self.e_theta_h[-2])
+ k_angular_S / k_angular_I * sum(self.e_theta_h)
)
print "Omega: ", round(omega,1)
#############################
# StateController Algorithm #
#############################
# if desired heading is not directly in front
if np.abs(error_theta) > np.pi / 3:
# turn in place
forward_vel = self.maxVelocity
# omega *= 0.25
else:
# Direct yourself to the goal
goal_distance = np.linalg.norm(self.goal_pose_pixel - self.pose_pixel)
print "distance_from_goal: ", goal_distance
if goal_distance <= 4:
# Achieved Goal!
forward_vel = self.maxVelocity * 0.25 # Slow down to prepare for the next one
self.curr_goal += 1
else:
forward_vel = self.maxVelocity
# control the motors
ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel, omega, g=1)
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.leftMotor,ctrl_sig_left,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.rightMotor,ctrl_sig_right,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
self.idash.add(lambda: self.plot_maze(self.blurred_paths*1.0, m, n, goal_m, goal_n))
self.idash.add(lambda: self.plot_maze(im, m, n, goal_m, goal_n))
def plot_current_and_desired_heading():
self.plot_unit_quiver(finalUnitVector, 'r')
self.plot_unit_quiver(pol2cart(1, worldTheta2mapTheta(theta, self.worldNorthTheta)), 'k')
plt.title("Error Theta: %f" % error_theta)
self.idash.add(plot_current_and_desired_heading)
self.idash.add(self.plot_theta_history)
self.idash.plotframe()
if self.killer.kill_now:
self.clean_exit()
Implemetasikan algoritma Kalman Filter 2D pada class kalman2d.kalman_prediction_2d
'''
#Get motor handle
errorCode,left_motor=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_leftMotor',vrep.simx_opmode_oneshot_wait)
errorCode,right_motor=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_rightMotor',vrep.simx_opmode_oneshot_wait)
# Get ultrasound sensors handle
errorCode,usensor1=vrep.simxGetObjectHandle(clientID,'Pioneer_p3dx_ultrasonicSensor8',vrep.simx_opmode_oneshot_wait)
# Get camera sensors handle
errorCode,cam_handle=vrep.simxGetObjectHandle(clientID,'Camera',vrep.simx_opmode_oneshot_wait)
errorCode,resolution,image=vrep.simxGetVisionSensorImage(clientID,cam_handle,0,vrep.simx_opmode_oneshot_wait)
pX = pY = 0.0
for i in range (1,30) :
errorCode,gpsX=vrep.simxGetFloatSignal(clientID,'gpsX',vrep.simx_opmode_oneshot_wait);
errorCode,gpsY=vrep.simxGetFloatSignal(clientID,'gpsY',vrep.simx_opmode_oneshot_wait);
pX += gpsX
pY += gpsY
offSetX = (pX/30) - initX
offSetY = (pY/30) - initY
for i in range(1,11):
#while True :
print 'Iteration : ', i
try:
postureProxy = ALProxy("ALRobotPosture", robotIP, robotPort)
except Exception, e:
print "Could not create proxy to ALRobotPosture"
print "Error was: ", e
#INIT V-REP IMAGE SENSOR
vrep.simxFinish(-1)
clientID=vrep.simxStart('127.0.0.2',19998,True,True,5000,5)
if clientID!=-1:
print('Connected to remote API server')
#Get and display the pictures from the camera
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,'NAO_vision1',vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_streaming)
#Allow the display to be refreshed
#plt.ion()
#Initialiazation of the figure
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
time.sleep(1)
im = I.new("RGB", (640,480), "white")
#Give a title to the figure
#fig = plt.figure(1)
#fig.canvas.set_window_title('NAO_vision1')
#inverse the picture
#plotimg = plt.imshow(im,origin='lower')
# Set NAO in Stiffness On
StiffnessOn(motionProxy)
vrep.simxPauseCommunication(clientID,False)
# give the arm some time to move
time.sleep(1)
# verify that the arm has approximately reached its target position
err0 = vrep.simxGetJointPosition(clientID,armJoint0Handle,vrep.simx_opmode_streaming)[1] - rand_base_rotate
err1 = vrep.simxGetJointPosition(clientID,armJoint1Handle,vrep.simx_opmode_streaming)[1] - rand_base_lower
err2 = vrep.simxGetJointPosition(clientID,armJoint2Handle,vrep.simx_opmode_streaming)[1] - rand_elbow_bend
err3 = vrep.simxGetJointPosition(clientID,armJoint3Handle,vrep.simx_opmode_streaming)[1] - rand_hand_bend
err4 = vrep.simxGetJointPosition(clientID,armJoint4Handle,vrep.simx_opmode_streaming)[1] - 0.0
joint_pos_err = abs(err0) + abs(err1) + abs(err2) + abs(err3) + abs(err4)
print "joint positioning error =", joint_pos_err
# get image from vision sensor 1
err,res,img = vrep.simxGetVisionSensorImage(clientID,visionSensor1Handle,0,vrep.simx_opmode_oneshot_wait)
colval1 = img_to_numpy(res,img)
# get the binary image marking the cyan ball in Youbot's gripper
binval1 = img_binarise(colval1,0.0,0.15,0.7,0.99,0.9,1.0)
# get the x- and y-coordinates of the cyan ball
binval1 = numpy.reshape(binval1,(res[1],res[0]))
pos_img1 = img_find_cm(binval1)
print "pos_img1 =", pos_img1
# get image from vision sensor 2
err,res,img = vrep.simxGetVisionSensorImage(clientID,visionSensor2Handle,0,vrep.simx_opmode_oneshot_wait)
colval2 = img_to_numpy(res,img)
# get the binary image marking the cyan ball in Youbot's gripper
binval2 = img_binarise(colval2,0.0,0.15,0.7,0.99,0.9,1.0)
# get the x- and y-coordinates of the cyan ball
binval2 = numpy.reshape(binval2,(res[1],res[0]))
def JointControlWhileTakingPictures(clientID,motionProxy,i,Body, naoDirectory, visionSensorName):
#Get the handle of the vision sensor
res1,visionSensorHandle=vrep.simxGetObjectHandle(clientID,visionSensorName,vrep.simx_opmode_oneshot_wait)
#Get the image
res2,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_buffer)
time.sleep(1)
#Head
while(vrep.simxGetConnectionId(clientID)!=-1):
#Getting joint's angles from Choregraphe (please check your robot's IP)
commandAngles = motionProxy.getAngles('Body', False)
#Allow the robot to move in VRep using choregraphe's angles
vrep.simxSetJointTargetPosition(clientID,Body[0][i],commandAngles[0],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[1][i],commandAngles[1],vrep.simx_opmode_streaming)
#Left Leg
vrep.simxSetJointTargetPosition(clientID,Body[2][i],commandAngles[8],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[3][i],commandAngles[9],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[4][i],commandAngles[10],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[5][i],commandAngles[11],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[6][i],commandAngles[12],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[7][i],commandAngles[13],vrep.simx_opmode_streaming)
#Right Leg
vrep.simxSetJointTargetPosition(clientID,Body[8][i],commandAngles[14],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[9][i],commandAngles[15],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[10][i],commandAngles[16],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[11][i],commandAngles[17],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[12][i],commandAngles[18],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[13][i],commandAngles[19],vrep.simx_opmode_streaming)
#Left Arm
vrep.simxSetJointTargetPosition(clientID,Body[14][i],commandAngles[2],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[15][i],commandAngles[3],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[16][i],commandAngles[4],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[17][i],commandAngles[5],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[18][i],commandAngles[6],vrep.simx_opmode_streaming)
#Right Arm
vrep.simxSetJointTargetPosition(clientID,Body[19][i],commandAngles[20],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[20][i],commandAngles[21],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[21][i],commandAngles[22],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[22][i],commandAngles[23],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[23][i],commandAngles[24],vrep.simx_opmode_streaming)
#Left Fingers
vrep.simxSetJointTargetPosition(clientID,Body[25][i][0],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][1],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][2],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][3],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][4],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][5],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][6],1.0-commandAngles[7],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[25][i][7],1.0-commandAngles[7],vrep.simx_opmode_streaming)
#Right Fingers
vrep.simxSetJointTargetPosition(clientID,Body[27][i][0],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][1],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][2],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][3],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][4],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][5],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][6],1.0-commandAngles[25],vrep.simx_opmode_streaming)
vrep.simxSetJointTargetPosition(clientID,Body[27][i][7],1.0-commandAngles[25],vrep.simx_opmode_streaming)
#Get the image of the vision sensor
res,resolution,image=vrep.simxGetVisionSensorImage(clientID,visionSensorHandle,0,vrep.simx_opmode_oneshot_wait)
#Transform the image so it can be displayed using pyplot
image_byte_array = array.array('b',image)
im = I.frombuffer("RGB", (640,480), image_byte_array, "raw", "RGB", 0, 1)
#Save the image to disk
date_string = datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")
naoDirectory = 'D:\_dev\mo416-final-project\Images\Semaforo_verde'
file_path = naoDirectory + '\img_' + date_string + '.png'
print file_path
rotated_im = im.transpose(I.ROTATE_180)
rotated_im.save(file_path,"PNG")
print 'End of simulation'
if clientID!=-1: print 'Connected to V-REP' # get vision sensor object handle res, vs = vrep.simxGetObjectHandle(clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait) # getting other object handles res, quad = vrep.simxGetObjectHandle(clientID, 'Quadricopter', vrep.simx_opmode_oneshot_wait) time.sleep(2) res, marker = vrep.simxGetObjectHandle(clientID, 'Plane1', vrep.simx_opmode_oneshot_wait) time.sleep(2) print quad, marker # start getting data from vision sensor err, resolution, image = vrep.simxGetVisionSensorImage(clientID, vs, 0, vrep.simx_opmode_streaming) time.sleep(2) while True: err, resolution, image = vrep.simxGetVisionSensorImage(clientID, vs, 0, vrep.simx_opmode_buffer) img = np.array(image,dtype=np.uint8) img.resize([resolution[1],resolution[0],3]) img = np.fliplr(img) temp = np.copy(img[:,:,0]) img[:,:,0] = np.copy(img[:,:,2]) img[:,:,2] = temp
time1 = input("Time of flight: ")
vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_z, z_vel, vrep.simx_opmode_blocking)
vrep.simxSetObjectFloatParameter(clientID, obj1, vrep.sim_shapefloatparam_init_velocity_x, x_vel, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
t_end = time.time() + time1
#while time.time() < t_end:
for i in range(0,10):
errorCodeKinectRGB,kinectRGB=vrep.simxGetObjectHandle(clientID,'kinect_rgb',vrep.simx_opmode_blocking)
#errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_blocking)
img_time=vrep.simxGetLastCmdTime(clientID)
#errorCodeKinectDepth,kinectDepth=vrep.simxGetObjectHandle(clientID,depthSTR,vrep.simx_opmode_blocking)
errorHere,resolution,image=vrep.simxGetVisionSensorImage(clientID,kinectRGB,0,vrep.simx_opmode_oneshot_wait)
image_byte_array = array.array('b', image)
image_buffer = I.frombuffer("RGB", (resolution[0],resolution[1]), image_byte_array, "raw", "BGR", 0, 1)
image_buffer = image_buffer.transpose(Image.FLIP_LEFT_RIGHT)
image_buffer = image_buffer.transpose(Image.ROTATE_180)
img2 = np.asarray(image_buffer)
img_time=vrep.simxGetLastCmdTime(clientID)
#print(img_time)
#cv2.imwrite(str(img_time)+'.png',img2)
time1=vrep.simxGetLastCmdTime(clientID)
pose = getPosition(clientID)
pose_list.append(pose)
def _read_camera(self):
data = vrep.simxGetVisionSensorImage(self.clientID,self.camera,1,vrep.simx_opmode_buffer)
if data[0] == vrep.simx_return_ok :
return data
return None
