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 dist_cbs(self): ## Read position of blocks from V-REP then send to remoteAPI ## if self.firstMsgVerification: err, pos_str = vrep.simxGetStringSignal(clientID, 'cuboidpos', vrep.simx_opmode_streaming) self.firstMsgVerification = False else: err, pos_str = vrep.simxGetStringSignal(clientID, 'cuboidpos', vrep.simx_opmode_buffer) pos = vrep.simxUnpackFloats(pos_str) #for i in range(0,6): #dist2 = pos[2] # print "pos= ",pos if len(pos) == 6: dist = pos[0] dist0 = pos[1] dist1 = pos[2] dist2 = pos[3] dist3 = pos[4] dist4 = pos[5] else: dist = 1.0 dist0 = 1.0 dist1 = 1.0 dist2 = 1.0 dist3 = 1.0 dist4 = 1.0 return dist, dist0, dist1, dist2, dist3, dist4
def get_lidar_data(self): """Returns data measured using LIDAR. Make sure robot you use has a lidar""" error_code, ranges = vrep.simxGetStringSignal(self.client_id, 'scan ranges', STREAM_MODE) time.sleep(0.1) while len(ranges) == 0: error_code, ranges = vrep.simxGetStringSignal(self.client_id, 'scan ranges', BUFFER_MODE) ranges = vrep.simxUnpackFloats(ranges) return ranges
def obtener_datos_laser_simulador(terminar_conexion=True): """ Se conecta al simulador y obtiene los datos del láser, que devuelve como una lista de dos elementos: lista[0] es una lista con las coordenadas X de los puntos y lista[1] una lista con las coordenadas Y. También devuelve el clientID y robothandle. @terminar_conexion Si vale True, se termina la simulación tras obtener los datos del láser. """ vrep.simxFinish(-1) #Terminar todas las conexiones clientID = vrep.simxStart( '127.0.0.1', 19999, True, True, 5000, 5 ) #Iniciar una nueva conexion en el puerto 19999 (direccion por defecto) # Primero lanzar play en la escena y después ejecutar python if clientID != -1: print('Conexion establecida') else: sys.exit( "Error: no se puede conectar. Tienes que iniciar la simulación antes de llamar a este script." ) #Terminar este script #Guardar la referencia al robot _, robothandle = vrep.simxGetObjectHandle(clientID, 'Pioneer_p3dx', vrep.simx_opmode_oneshot_wait) #acceder a los datos del laser _, datosLaserComp = vrep.simxGetStringSignal(clientID, 'LaserData', vrep.simx_opmode_streaming) # Hay que esperar un segundo antes de poder acceder a los datos del láser time.sleep(1) puntosx = [ ] #listas para recibir las coordenadas x, y z de los puntos detectados por el laser puntosy = [] puntosz = [] returnCode, signalValue = vrep.simxGetStringSignal(clientID, 'LaserData', vrep.simx_opmode_buffer) datosLaser = vrep.simxUnpackFloats(signalValue) for indice in range(0, len(datosLaser), 3): puntosx.append(datosLaser[indice + 1]) puntosy.append(datosLaser[indice + 2]) puntosz.append(datosLaser[indice]) if terminar_conexion: #detenemos la simulacion vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot_wait) #cerramos la conexion vrep.simxFinish(clientID) # Devuelvo los puntos y el clientID return [puntosx, puntosy], clientID, robothandle
def get_laser_scan(self): if self.laser_init: errorCode, signalVal = vrep.simxGetStringSignal( self.clientID, "MySignal", vrep.simx_opmode_streaming) else: errorCode, signalVal = vrep.simxGetStringSignal( self.clientID, "MySignal", vrep.simx_opmode_buffer) assert errorCode <= 1, 'Cannot grab laser data' data = vrep.simxUnpackFloats(signalVal) return data[0::2], data[1::2]
def prepare(self): self.clearSignal() ################ GET ROBOT/GOAL POSITIONS, MAP DIM WITH RESOLUTION, TRANSFORM ########################### r, self.robotHandle = vrep.simxGetObjectHandle( self.clientID, 'body#1', vrep.simx_opmode_oneshot_wait) r, self.goalHandle = vrep.simxGetObjectHandle( self.clientID, 'Goal', vrep.simx_opmode_oneshot_wait) r = -1 while (r != vrep.simx_return_ok): r, robotPosition = vrep.simxGetObjectPosition( self.clientID, self.robotHandle, -1, vrep.simx_opmode_streaming ) #-1 specifies we want the absolute position self.env.updateRobotPosition(robotPosition) r = -1 while (r != vrep.simx_return_ok): r, goalPosition = vrep.simxGetObjectPosition( self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming) self.env.updateGoal(goalPosition) rCode = -1 while (rCode != vrep.simx_return_ok): rCode, angle = vrep.simxGetObjectOrientation( self.clientID, self.robotHandle, -1, vrep.simx_opmode_streaming) #just to prepare our vrep robotPosition = self.env.transform(robotPosition) goalPosition = self.env.transform(goalPosition) self.env.updateRobotPosition(robotPosition) self.env.updateGoal(goalPosition) self.env.initializeMap() ################## PREPARE SENSORS ############################ rCode, proxHandle = vrep.simxGetObjectHandle( self.clientID, 'sensor#1', vrep.simx_opmode_oneshot_wait) if rCode != vrep.simx_return_ok or rCode != vrep.simx_return_ok: print("Could not get proximity sensor handle. Exiting.") sys.exit() self.proxHandles = [proxHandle] error, state, point, handle, vector = vrep.simxReadProximitySensor( self.clientID, self.proxHandles[0], vrep.simx_opmode_streaming) rCode = -1 vrep.simxGetFloatSignal( self.clientID, "ProxDistance", vrep.simx_opmode_streaming) #just to get things started vrep.simxGetStringSignal(self.clientID, "threeDimData", vrep.simx_opmode_streaming) ############# INITIALIZE PRIORITY QUEUE ###################### print("OVERALL GOAL POSITION: ", self.env.getGoal()) heapq.heapify(self.open) self.env.setMap(goalPosition[0], goalPosition[1], 1, 0) heapq.heappush(self.open, (self.key(goalPosition), goalPosition))
def read_laser(self): """ Gets the 572 points read by the laser sensor. Each reading contains 3 values (x, y, z) of the point relative to the sensor position. Returns: laser: List with 1716 values of x, y and z from each point. """ res, laser = vrep.simxGetStringSignal(self.clientID,"LasermeasuredDataAtThisTime", vrep.simx_opmode_streaming) laser = vrep.simxUnpackFloats(laser) while(res != vrep.simx_return_ok): res, laser = vrep.simxGetStringSignal(self.clientID,"LasermeasuredDataAtThisTime", vrep.simx_opmode_buffer) laser = vrep.simxUnpackFloats(laser) return laser
def _connect(self): self.__clientID = vrep.simxStart(self.__host_url, self.__port, True, True, 5000, 5) # Connect to V-REP if self.__clientID != -1: print ('Connected to remote API server of Vrep with clientID: ', self.__clientID) self._connectionStatus = True vrep.simxGetStringSignal(self.__clientID, self.__signalName+'_allSens', vrep. simx_opmode_streaming) #first time call vrep.simxGetStringSignal(self.__clientID, self.__signalName+'_camera', vrep. simx_opmode_streaming) #first time call vrep.simxSetStringSignal(self.__clientID, self.__signalName+'_velocities', vrep.simxPackFloats([0.0, 0.0]), vrep.simx_opmode_streaming) #first time call # if self.__synchronous: # self.startsim() else: logging.error('Failed connecting to remote API server of Vrep')
def dvs(cid, handle=None, signal_name="dataFromThisTimestep", dim_x=32, dim_y=32, offset=0, magnitude=1): empty_str="" raw_bytes = (ctypes.c_ubyte * len(empty_str)).from_buffer_copy(empty_str) err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep", vrep.simx_opmode_oneshot) err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes, vrep.simx_opmode_oneshot_wait) image = np.zeros( (dim_x, dim_y), dtype='uint8' ) + offset l = len(data) for i in range( int(l/4) ): b = list(bytearray(data[i*4:i*4+4])) x_coord = b[0] y_coord = b[1] if x_coord >= 128: x_coord -= 128 polarity = 1 else: #polarity = 0 polarity = -1 timestamp = (b[3] * 256) + b[2] # Distinguish between different polarities #image[127-y_coord][127-x_coord] = polarity * magnitude image[dim_y-1-y_coord][dim_x-1-x_coord] = polarity * magnitude # No Distinction between different polarities #image[127-y_coord][127-x_coord] = magnitude return image.ravel()
def getLaserPoints(self): if self.id != -1: if self.sec_first_call: mode = vrepConst.simx_opmode_streaming res = vrep.simxGetStringSignal(self.id, self.tube_name, vrepConst.simx_opmode_streaming) time.sleep(0.05) self.sec_first_call = False else: mode = vrepConst.simx_opmode_buffer res = vrep.simxGetStringSignal(self.id, self.tube_name, vrepConst.simx_opmode_buffer) r = vrep.simxUnpackFloats(res[1]) return r
def getLaserData(self): res, data = vrep.simxGetStringSignal(self.clientID, 'measuredDataAtThisTime', vrep.simx_opmode_buffer) laserData = [] if (data): # data unpacking laserDetectedPoints = vrep.simxUnpackFloats(data) # data loading in robocomp laserData format for i in range(0, len(laserDetectedPoints) - 2, 3): x = laserDetectedPoints[i] y = laserDetectedPoints[i + 1] z = laserDetectedPoints[i + 2] singleLaser = TData() # angle and distance are relative to the laser sensor # x-axis of the laser points in the direction of movement of robot and z-axis is out of the plane # distance is in cm singleLaser.dist = (np.sqrt(x**2 + y**2)) * 100 # angles is in degrees singleLaser.angle = math.degrees((y / x)) laserData.append(singleLaser) # print(f'distance: {distance} angle: {angle}') return laserData
def dvs(cid, handle=None, signal_name="dataFromThisTimestep", dim_x=32, dim_y=32, offset=0, magnitude=1): empty_str = "" raw_bytes = (ctypes.c_ubyte * len(empty_str)).from_buffer_copy(empty_str) err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep", vrep.simx_opmode_oneshot) err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes, vrep.simx_opmode_oneshot_wait) image = np.zeros((dim_x, dim_y), dtype='uint8') + offset l = len(data) for i in range(int(l / 4)): b = list(bytearray(data[i * 4:i * 4 + 4])) x_coord = b[0] y_coord = b[1] if x_coord >= 128: x_coord -= 128 polarity = 1 else: #polarity = 0 polarity = -1 timestamp = (b[3] * 256) + b[2] # Distinguish between different polarities #image[127-y_coord][127-x_coord] = polarity * magnitude image[dim_y - 1 - y_coord][dim_x - 1 - x_coord] = polarity * magnitude # No Distinction between different polarities #image[127-y_coord][127-x_coord] = magnitude return image.ravel()
def turn(self, speedr, speedl, angle): """ turns the unit: giving speed to the left wheel makes the robot to turn right and vice-versa :param speedr: speed of the right wheel. :param speedl: speed of the left wheel. :param angle: turning angle. """ self._term.write('turning, angle = {}'.format(angle)) # will contain cumulative turtning angle z = 0 while z < angle: time.sleep(1) # the second parameter is the velocity # set speed vrep.simxSetJointTargetVelocity(self._clientID, self.wheels_handles["wheel_right"], speedr, self._operation_mode) vrep.simxSetJointTargetVelocity(self._clientID, self.wheels_handles["wheel_left"], speedl, self._operation_mode) # get gyroscope data gyro_data = vrep.simxGetStringSignal(self._clientID, self.signals['gyro_signal'], self._operation_mode) # gyro_data_unpacked_x = (struct.unpack("f", bytearray(gyro_data[1][:4]))[0] * 180) / math.pi # gyro_data_unpacked_y = (struct.unpack("f", bytearray(gyro_data[1][4:8]))[0] * 180) / math.pi # extract degrees per second gyro_data_unpacked_z = (struct.unpack("f", bytearray(gyro_data[1][8:12]))[0] * 180) / math.pi # self._term.write('-------------------------------------------------------\n' # '{} : X-Gyro = {} dps\n Y-Gyro = {} dps\n Z-Gyro = {} dps' # .format(self._port, round(gyro_data_unpacked_x, 2), round(gyro_data_unpacked_y, 2), # round(gyro_data_unpacked_z, 2))) # add up z += abs(gyro_data_unpacked_z) # self._term.write('cumulative angle = {}'.format(z)) self._term.write('turn completed')
def fastSensingOverSignal(self): """ read all non-camera sensors in one signal call, except for pose :return: """ res, values = vrep.simxGetStringSignal(self.__clientID, self.__signalName + '_allSens', vrep.simx_opmode_buffer) if res == vrep.simx_return_ok: sensValues = vrep.simxUnpackFloats(values) self._proximitySensorValues = np.array(sensValues[:8], dtype=np.float) self._lightSensorValues = np.array(sensValues[8:16], dtype=np.float) self._groundSensorValues = np.array(sensValues[16:19], dtype=np.float) self._accelerometerValues = np.array(sensValues[19:22], dtype=np.float) self._wheelEncoderValues = np.array(sensValues[22:], dtype=np.float) logging.debug( 'Remote signal reception for signal EPUCK_allSens ok') else: logging.error( 'Remote signal reception for signal EPUCK_allSens failed')
def wait_for_signal(clientID, signal, mode=vrep.simx_opmode_oneshot_wait): """Waits for a signal from V-REP before continuing main client code.""" r = -1 while r != vrep.simx_return_ok: r, data = vrep.simxGetStringSignal(clientID, signal, mode) time.sleep(0.1) return data
def loop(self): operationMode=vrep.simx_opmode_streaming if self.__initLoop: self.__initLoop=False else: operationMode=vrep.simx_opmode_buffer returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode) if returnCode==vrep.simx_return_ok: self.__orientation=orientation[2] else: self.__orientation = None # print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()" self.__mind.setInput("orientation", self.__orientation) returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode) if returnCode==vrep.simx_return_ok: self.__position=[0.0,0.0] self.__position[0]=position[0] #X self.__position[1]=position[1] #Y else: self.__position=None # print >> sys.stderr, "Error in VRepBubbleRob.getPosition()" returnCode, linearVelocity, angularVelocity = vrep.simxGetObjectVelocity(self.__clientID, self.__bodyHandle, operationMode) if returnCode==vrep.simx_return_ok: try: # self.__velocity=linearVelocity[0]*math.cos(self.__orientation)+linearVelocity[1]*math.sin(self.__orientation) self.__velocity=math.sqrt(linearVelocity[0]**2+linearVelocity[1]**2) self.__mind.setInput("velocity", self.__velocity) except TypeError: pass # if self.__name=="BubbleRob#1": # print self.__velocity, linearVelocity[0], math.cos(self.__orientation), linearVelocity[1], math.sin(self.__orientation) else: self.__velocity=None # print >> sys.stderr, "Error in VRepBubbleRob.getPosition()" returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode) if returnCode==vrep.simx_return_ok: # We succeeded at reading the proximity sensor self.__mind.setInput("lastProximitySensorTime", vrep.simxGetLastCmdTime(self.__clientID)) self.__mind.setInput("sensorTrigger", sensorTrigger) self.__mind.setInput("mostSalientItem", self.__mind.selectMostSalient("I")) # print self.__name, self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName")) self.__mind.setInput("attendedItem", self.__mind.getAttendedItem(self.__mind.getOutput("attendedItemName"))) self.__mind.applyRules() self.__mind.setStates() if self.__mind.getOutput("steering")!=None: self.setSteering(self.__mind.getOutput("steering")) if self.__mind.getOutput("thrust")!=None: self.setThrust(self.__mind.getOutput("thrust")) if self.__mind.getOutput("reward")!=None: if self.__mind.getOutput("reward")>0.5: self.setEmotionalExpression(1) elif self.__mind.getOutput("reward")<-0.5: self.setEmotionalExpression(-1) else: self.setEmotionalExpression(0) getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming) if dMessage!="": print("Debug:"+dMessage) self.__cnt=self.__cnt+1
def get_signal(self, signal): res, signal_val = vrep.simxGetStringSignal(self.client_id, signal, vrep.simx_opmode_blocking) if res <= 8: return signal_val if signal_val != '' else '0' else: err_print("GET SWARM DATA", parse_error(res)) raise Exception("ERROR IN GET SWARM DATA")
def get_data(own=False): ''' Función para recoger datos laser del simulador V-REP y crear un archivo asociado con los puntos detectados. ''' if (own): name = input("Introduce el nombre del fichero: ") tiempo = int(input("Introduce el intervalo de tiempo (s): ")) iteraciones = int(input("Introduce el número de iteraciones: ")) else: name = "laser.dat" tiempo = 1 iteraciones = 1 # Abrimos el fichero para escritura path = "data/{}".format(name) fd = open(path, 'w') for i in range(0, iteraciones): #listas para recibir las coordenadas x, y de los puntos detectados por el laser puntosx = [] puntosy = [] returnCode, signalValue = vrep.simxGetStringSignal( clientID, 'LaserData', vrep.simx_opmode_buffer) time.sleep( tiempo ) #esperamos un tiempo para que el ciclo de lectura de datos no sea muy rápido datosLaser = vrep.simxUnpackFloats(signalValue) for indice in range(0, len(datosLaser), 3): puntosx.append(datosLaser[indice + 1]) puntosy.append(datosLaser[indice + 2]) plt.clf() plt.plot(puntosx, puntosy, 'r.') plt.axis([0, 4, -2, 2]) plt.show() datos = "Iteracion {}_{}".format(i, len(puntosx)) for x, y in zip(puntosx, puntosy): datos += " " + str(x) + " " + str(y) datos += "\n" fd.write(datos) fd.close()
def _get_StateReward(self, prefix="", init=False): v_opmode = vrep.simx_opmode_streaming if init else vrep.simx_opmode_buffer state = np.array( vrep.simxUnpackFloats( vrep.simxGetStringSignal(self.id_, prefix + "states", v_opmode)[1])) reward = vrep.simxGetFloatSignal(self.id_, prefix + "reward", v_opmode)[1] return state, reward
def get_hit_object_name(self): hit_name = '' res, hit_name = vrep.simxGetStringSignal(self.clientID, 'hit_name', vrep.simx_opmode_blocking) if res == vrep.simx_return_ok: pass #print ("hit ", hit_name) # display the reply from V-REP (in this case, the handle of the created dummy) else: print('Remote function call failed: take action') return hit_name
def iniciarCamaraLaser(clientID): #cargamos datos de imagene y camara _, datosLaserComp = vrep.simxGetStringSignal(clientID, 'LaserData', vrep.simx_opmode_streaming) _, camhandle = vrep.simxGetObjectHandle(clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait) _, resolution, image = vrep.simxGetVisionSensorImage( clientID, camhandle, 0, vrep.simx_opmode_streaming) time.sleep(1) return camhandle
def __get(self): vrep.simxGetPingTime(self.__ID) self.state = np.array( vrep.simxUnpackFloats( vrep.simxGetStringSignal(self.__ID, "states", vrep.simx_opmode_buffer)[1])) self.reward = vrep.simxGetFloatSignal(self.__ID, "reward", vrep.simx_opmode_buffer)[1] self.done = bool( vrep.simxGetIntegerSignal(self.__ID, "done", vrep.simx_opmode_buffer)[1])
def get_prox_sensors(self): res, data = vrep.simxGetStringSignal(self.client_id, 'EPUCK_PROXSENS' + self.suffix, vrep.simx_opmode_oneshot_wait) err_list = parse_error(res) if res != 0 and self.debug: err_print(prefix="SET SPEED LEFT: ", message=err_list) data = vrep.simxUnpackFloats(data) self.prox_sensors = data return {'res': res, 'err_list': err_list, 'data': data}
def get_lidar_data(self): point_data, dist_data = [], [] e, data = vrep.simxGetStringSignal(self.client_id, "lidarMeasuredData", vrep.simx_opmode_buffer) if self.check_error_code(e, "simxGetStringSignal lidar distances error"): buf = data.split("!#!") if len(buf) >= 2: dist_data = vrep.simxUnpackFloats(buf[1]) measuredData = vrep.simxUnpackFloats(buf[0]) point_data = np.reshape(measuredData, (int(len(measuredData) / 3), 3)) return e, point_data, dist_data
def obtenerPuntosXY(clientID): puntosx = [ ] #listas para recibir las coordenadas x, y z de los puntos detectados por el laser puntosy = [] #puntosz=[] returnCode, signalValue = vrep.simxGetStringSignal(clientID, 'LaserData', vrep.simx_opmode_buffer) #esperamos un tiempo para que el ciclo de lectura de datos no sea muy rápido datosLaser = vrep.simxUnpackFloats(signalValue) for indice in range(0, len(datosLaser), 3): puntosx.append(datosLaser[indice + 1]) puntosy.append(datosLaser[indice + 2]) #puntosz.append(datosLaser[indice]) return puntosx, puntosy
def _get_Space(self, soa, prefix="", dtype=np.float32): if "state" in soa: max_v = np.array( vrep.simxUnpackFloats( vrep.simxGetStringSignal(self.id_, prefix + "max_state", vrep.simx_opmode_blocking)[1])) min_v = np.array( vrep.simxUnpackFloats( vrep.simxGetStringSignal(self.id_, prefix + "min_state", vrep.simx_opmode_blocking)[1])) elif "action" in soa: max_v = np.array( vrep.simxUnpackFloats( vrep.simxGetStringSignal(self.id_, prefix + "max_action", vrep.simx_opmode_blocking)[1])) min_v = np.array( vrep.simxUnpackFloats( vrep.simxGetStringSignal(self.id_, prefix + "min_action", vrep.simx_opmode_blocking)[1])) try: rtv = spaces.Box(min_v, max_v, dtype=dtype) except: rtv = spaces.Box(min_v, max_v) return rtv
def output(self): if self.__mind.getOutput("steering")!=None: self.setSteering(self.__mind.getOutput("steering")) if self.__mind.getOutput("thrust")!=None: self.setThrust(self.__mind.getOutput("thrust")) if self.__mind.getOutput("reward")!=None: if self.__mind.getOutput("reward")>0.5: self.setEmotionalExpression(1) elif self.__mind.getOutput("reward")<-0.5: self.setEmotionalExpression(-1) else: self.setEmotionalExpression(0) getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming) if dMessage!="": print("Debug:"+dMessage)
def lidarScan360(clientID, servo): scan = [] e = vrep.simxSetJointPosition(clientID, servo, -np.pi / 2, vrep.simx_opmode_blocking) eData = vrep.simxGetStringSignal(clientID, "mySignal", vrep.simx_opmode_blocking) scan = vrep.simxUnpackFloats(eData[1]) e = vrep.simxSetJointPosition(clientID, servo, np.pi / 2, vrep.simx_opmode_blocking) eData = vrep.simxGetStringSignal(clientID, "mySignal", vrep.simx_opmode_blocking) data = vrep.simxUnpackFloats(eData[1]) scan = scan[::-1] data = data[::-1] for x in data: scan.append(x) for i in range(len(scan)): scan[i] = min(50000, 5000 * scan[i]) while len(scan) > 360: scan.pop() # print(scan) return scan
def compute(self): print 'SpecificWorker.compute...' #computeCODE res, data = vrep.simxGetStringSignal(self.clientID, 'MySignal', vrep.simx_opmode_buffer) if (data): laserDetectedPoints = vrep.simxUnpackFloats(data) for i in range(len(laserDetectedPoints) / 2): singleLaser = TData() singleLaser.dist = laserDetectedPoints[2 * i] singleLaser.angle = laserDetectedPoints[2 * i + 1] self.laserData.append(singleLaser) time.sleep(2) return True
def checkGround(self, robotPosition): r, distance = vrep.simxGetFloatSignal(self.clientID, "GroundDistance", vrep.simx_opmode_buffer) r, table = vrep.simxGetStringSignal(self.clientID, "threeDimData", vrep.simx_opmode_buffer) if r == vrep.simx_return_ok: table = vrep.simxUnpackFloats(table) dim = int((len(table) / 3)**(1 / 2)) if dim * dim * 3 != len(table): return set() heights = np.array(table).reshape((dim, dim, 3))[:, :, 0] cliffs = self.env.analyzeCliffs( heights) #returns a set of relative locations of cliffs return cliffs else: return set()
def loop(self): operationMode = vrep.simx_opmode_streaming if self.__initLoop: self.__initLoop = False else: operationMode = vrep.simx_opmode_buffer returnCode, orientation = vrep.simxGetObjectOrientation( self.__clientID, self.__bodyHandle, -1, operationMode) if returnCode == vrep.simx_return_ok: self.__orientation = orientation[2] #Z else: self.__orientation = None print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()" returnCode, position = vrep.simxGetObjectPosition( self.__clientID, self.__bodyHandle, -1, operationMode) if returnCode == vrep.simx_return_ok: self.__position = [0.0, 0.0] self.__position[0] = position[0] #X self.__position[1] = position[1] #Y else: print >> sys.stderr, "Error in VRepBubbleRob.getPosition()" self.__position = None returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor( self.__clientID, self.__sensorHandle, operationMode) if returnCode == vrep.simx_return_ok: # We succeeded at reading the proximity sensor simulationTime = vrep.simxGetLastCmdTime(self.__clientID) thrust = 0.0 steering = 0.0 if simulationTime - self.__driveBackStartTime < 3000: # driving backwards while slightly turning: thrust = -1.0 steering = -1.0 else: # going forward: thrust = 1.0 steering = 0.0 if sensorTrigger: self.__driveBackStartTime = simulationTime # We detected something, and start the backward mode self.setSteering(steering) self.setThrust(thrust) getSignalReturnCode, dMessage = vrep.simxGetStringSignal( self.__clientID, "Debug", vrep.simx_opmode_streaming) if dMessage != "": print("Debug:" + dMessage)
def procesar_ciclo(clientID, segundos, iter): """ Funcion que procesa un ciclo de simulacion. Obtiene los datos y los transforma al formato JSON especificado. Args: clientID: ID del cliente que ha establecido una conexion con el servidor de V-REP. segundos: Numero de segundos que esperar para procesar los datos. iter: Numero de iteracion. Return: Devuelve un objeto en formato JSON especificando la iteracion y los puntos detectados en el eje X y en el eje Y """ # Obtener coordenadas x,y,z detectadas por laser puntosx = [] puntosy = [] puntosz = [] # Obtener señal _, signalValue = vrep.simxGetStringSignal(clientID, 'LaserData', vrep.simx_opmode_buffer) # Esperar para procesar los datos print( f"Leidos datos en iteracion {iter}. Esperando {segundos} seg. para procesarlos..." ) time.sleep(segundos) # Procesar datos datosLaser = vrep.simxUnpackFloats(signalValue) for indice in range(0, len(datosLaser), 3): puntosz.append(datosLaser[indice]) puntosx.append(datosLaser[indice + 1]) puntosy.append(datosLaser[indice + 2]) # Obtener lectura lectura = {"Iteracion": iter, "PuntosX": puntosx, "PuntosY": puntosy} return lectura
def loop(self): operationMode=vrep.simx_opmode_streaming if self.__initLoop: self.__initLoop=False else: operationMode=vrep.simx_opmode_buffer returnCode, orientation = vrep.simxGetObjectOrientation(self.__clientID, self.__bodyHandle, -1, operationMode) if returnCode==vrep.simx_return_ok: self.__orientation=orientation[2] #Z else: self.__orientation = None print >> sys.stderr, "Error in VRepBubbleRob.getOrientation()" returnCode, position = vrep.simxGetObjectPosition(self.__clientID, self.__bodyHandle, -1, operationMode) if returnCode==vrep.simx_return_ok: self.__position=[0.0,0.0] self.__position[0]=position[0] #X self.__position[1]=position[1] #Y else: print >> sys.stderr, "Error in VRepBubbleRob.getPosition()" self.__position=None returnCode, sensorTrigger, dp, doh, dsnv = vrep.simxReadProximitySensor(self.__clientID, self.__sensorHandle, operationMode) if returnCode==vrep.simx_return_ok: # We succeeded at reading the proximity sensor simulationTime=vrep.simxGetLastCmdTime(self.__clientID) thrust=0.0 steering=0.0 if simulationTime-self.__driveBackStartTime<3000: # driving backwards while slightly turning: thrust=-1.0 steering=-1.0 else: # going forward: thrust=1.0 steering=0.0 if sensorTrigger: self.__driveBackStartTime=simulationTime # We detected something, and start the backward mode self.setSteering(steering) self.setThrust(thrust) getSignalReturnCode, dMessage = vrep.simxGetStringSignal(self.__clientID, "Debug", vrep.simx_opmode_streaming) if dMessage!="": print("Debug:"+dMessage)
def _process_events(cid, attr, mode=oneshot_wait, last=None): err, data = vrep.simxGetStringSignal(cid, attr, mode) safe_call(vrep.simxClearStringSignal, cid, attr, mode) x, y, signs, times = [], [], [], [] if err or len(data) == 0: pass elif last is not None and (len(data) == last[0] and data[:4] == last[1]): pass else: last = (len(data), data[:4]) # --- Format data data = np.array(bytearray(data), dtype=np.uint8) data.shape = (-1, 4) signs = data[:, 0] >= 128 data[:, 0] -= signs * 128 x, y = data[:, 0], data[:, 1] times = 256 * data[:, 3] + data[:, 2] return (x, y, signs, times), last
def _read_sensors(self): """ This method is used for read the ePuck's sensors. Don't use directly, instead use 'step()' """ # Read differents sensors for s in self._sensors_to_read: if s == 'a': # Accelerometer sensor in a non filtered way if self._accelerometer_filtered: parameters = ('A', 12, '@III') else: parameters = ('a', 6, '@HHH') self._debug('WARNING: Accelerometer not yet implemented!') elif s == 'n': # Proximity sensors res, prox = vrep.simxGetStringSignal(self._clientID, 'EPUCK_proxSens', vrep.simx_opmode_streaming) if res != vrep.simx_return_ok: self._debug("WARNING: Proximity sensors readout failed: ", res) else: proxVals = vrep.simxUnpackFloats(prox) # TODO: Find out actual needed scaling factor proxVals = [int(x * 1000) for x in proxVals] self._proximity = tuple(proxVals) elif s == 'm': # Floor sensors res, floor1 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_0', vrep.simx_opmode_streaming) if res != vrep.simx_return_ok: self._debug("WARNING: Floor 1 sensor readout failed: ", res) res, floor2 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_1', vrep.simx_opmode_streaming) if res != vrep.simx_return_ok: self._debug("WARNING: Floor 2 sensor readout failed: ", res) res, floor3 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_2', vrep.simx_opmode_streaming) if res != vrep.simx_return_ok: self._debug("WARNING: Floor 3 sensor readout failed: ", res) # Scale returned values to mimic real robot; current factor is just guessed self._floor_sensors = (floor1*1800, floor2*1800, floor3*1800) elif s == 'q': # Motor position sensor # First: Get the handles of both motor joints res, leftMotor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait) if res != vrep.simx_return_ok: self._debug("WARNING: Unable to get handle of left motor: ", res) continue res, rightMotor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait) if res != vrep.simx_return_ok: self._debug("WARNING: Unable to get handle of right motor: ", res) continue # Second: Get the actual motor position (in radians) res, leftPos = vrep.simxGetJointPosition(self._clientID, leftMotor, vrep.simx_opmode_oneshot_wait) if res != vrep.simx_return_ok: self._debug("WARNING: Readout of left motor failed: ", res) continue res, rightPos = vrep.simxGetJointPosition(self._clientID, rightMotor, vrep.simx_opmode_streaming) if res != vrep.simx_return_ok: self._debug("WARNING: Readout of left motor failed: ", res) continue self._motor_position = (leftPos, rightPos) elif s == 'o': # Light sensors parameters = ('O', 16, '@HHHHHHHH') self._debug('WARNING: Light sensors not yet implemented!') elif s == 'u': # Microphone parameters = ('u', 6, '@HHH') self._debug('WARNING: Microphones not yet implemented!') elif s == 'e': # Motor Speed parameters = ('E', 4, '@HH') self._debug('WARNING: Motor speed not yet implemented!') elif s == 'i': # Do nothing for the camera, is an independent process pass else: self._debug('Unknow type of sensor to read')
# Update display detection_display.set_data(img) detection_display.axes.figure.canvas.draw() cid = vrep.simxStart('127.0.0.1',20002,True,True,5000,5) if cid != -1: print ('Connected to V-REP remote API server, client id: %s' % cid) vrep.simxStartSimulation( cid, vrep.simx_opmode_oneshot ) else: print ('Failed connecting to V-REP remote API server') exit(1) while True: err, data = vrep.simxGetStringSignal(cid, "dataFromThisTimeStep", vrep.simx_opmode_oneshot) err = vrep.simxSetStringSignal(cid, "dataFromThisTimeStep", raw_bytes, vrep.simx_opmode_oneshot_wait) l = len(data) for i in range( int(l/4) ): b = list(bytearray(data[i*4:i*4+4])) x_coord = b[0] y_coord = b[1] if x_coord > 128: x_coord -= 128 polarity = 1 else: polarity = 0 timestamp = (b[3] * 256) + b[2] # Distinguish between different polarities
import numpy as np initialCall=True print ('VREP Simulation Program') vrep.simxFinish(-1) # just in case, close all opened connections clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP if clientID!=-1: print ('Connected to remote API server') else: print('Connection Failure') sys.exit('Abort Connection') # Object handle _,Quadbase=vrep.simxGetObjectHandle(clientID,'Quadricopter_base',vrep.simx_opmode_oneshot_wait) while True: # Code for testing... if initialCall: mode = vrep.simx_opmode_streaming initialCall = False else: mode = vrep.simx_opmode_buffer errorFlag,rawStringData=vrep.simxGetStringSignal(clientID,'rawMeasuredData',mode) if errorFlag == vrep.simx_return_ok: rawFloatData=vrep.simxUnpackFloats(rawStringData) print(len(rawFloatData)) else: print('Measurements Awaiting') time.sleep(1.0)
def run_simulation(self, trajectory): """ Trajectory is a list 6 pairs of vectors, each of the same length. For each pair: 1. The first vector is the position of the motor in rad. 2. The second vector is the max velocity of the motor in rad/s. """ self._com(remote_api.simxStopSimulation, get=False) if self.verbose: print("Setting parameters...") traj = self._prepare_traj(trajectory) packed_data = remote_api.simxPackFloats(traj) raw_bytes = (ctypes.c_ubyte * len(packed_data)).from_buffer_copy(packed_data) assert remote_api.simxSetStringSignal(self.api_id, 'trajectory', raw_bytes, remote_api.simx_opmode_oneshot_wait) == 0 self._com(remote_api.simxSetIntegerSignal, 'state', 1, get=False) self._com(remote_api.simxGetIntegerSignal, 'state', get=True, mode=remote_api.simx_opmode_streaming, ret_ok=(0, 1)) time.sleep(0.1) self._com(remote_api.simxStartSimulation, get=False) if self.verbose: print("Simulation started.") time.sleep(0.01) start_time = time.time() while not self.simulation_paused(): if time.time() - start_time > 60.0: print("Simulation seems hung. Exiting...") sys.exit(1) time.sleep(0.005) time.sleep(1.0) if self.verbose: print("Getting resulting parameters.") object_sensors = self._get_signal('object_sensors') collide_data = self._get_signal('collide_data') contact_data = self._get_signal('contact_data') contact_type = self._get_signal('contact_type', int_type=True) contacts = [] for i in range(0, len(contact_type), 3): step = contact_type[i] obj_names = [self.handles[contact_type[i+1]],self.handles[contact_type[i+2]]] data = contact_data[2*i:2*i+6] contacts.append(Contact(step, obj_names, data)) first_c, last_c = None, None max_f, max_c = 0.0, None for c in contacts: if c.obj_names[0] in self.tracked_objects or c.obj_names[1] in self.tracked_objects: if first_c is None: first_c = c last_c = c if max_f < c.force_norm: max_f, max_c = c.force_norm, c salient_contacts = {'first': first_c, 'last': last_c, 'max': max_c} if first_c is not None: print('first contact: {} {:.2f} N'.format('|'.join(first_c.obj_names), first_c.force_norm)) if max_c is not None: print(' max contact: {} {:.2f} N'.format('|'.join(max_c.obj_names), max_c.force_norm)) marker_sensors = None if self.cfg.sprims.tip: marker_sensors = np.array(remote_api.simxUnpackFloats( remote_api.simxGetStringSignal(self.api_id, 'marker_sensors', remote_api.simx_opmode_oneshot_wait))) # assert len(positions) == len(quaternions) == len(velocities) if self.verbose: print("End of simulation.") joint_sensors = None return {'object_sensors' : object_sensors, 'joint_sensors' : joint_sensors, 'marker_sensors' : marker_sensors, 'collide_data' : collide_data, 'contacts' : contacts, 'salient_contacts': salient_contacts}