def stop(): i =0 prev = 0 interceptx = 0.4 interceptz = 0.1 point1 = None point2 = None detectx = 0.6 while(True): [err,signal]=sim.simxGetStringSignal(clientID,'measuredData',sim.simx_opmode_blocking) coord = sim.simxUnpackFloats(signal) if len(coord)!= 0: if point1 is None: point1 = coord if point2 is None and coord[0] < detectx: point2 = coord if point1 is not None and point2 is not None: break i = i+1 print('start:', point1) print('end:', point2) ratio = (point2[1]-point1[1])/(point2[0]-point1[0]+0.000000001) intercepty = point1[1] + ratio*(interceptx-point1[0]) point3 = [interceptx,intercepty,interceptz] hit = [interceptx+0.1,intercepty,interceptz] robot.moveTo(point3) robot.moveTo(hit) robot.moveTo(point3)
def waitForMovementExecuted(id): global executedMovId global stringSignalName while executedMovId!=id: retCode,s=sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_buffer) if retCode==sim.simx_return_ok: executedMovId=s
def __init__(self, clientID): # Connection ID to communicate with simulator self.clientID = clientID self.LaserDataX = np.zeros(181) self.LaserDataY = np.zeros(181) # Get Data from Laser Scanner on V-REP error, data = sim.simxGetStringSignal(self.clientID,'communication',sim.simx_opmode_streaming)
def get_sonar_sensor(): # Sonar reading as distance to closest object detected by it, -1 if no data sonar_dist = -1 return_code, sonar_dist_packed = sim.simxGetStringSignal(clientID=clientID, signalName="sonar_sensor", operationMode=sim.simx_opmode_blocking) if return_code == 0: sonar_dist = sim.simxUnpackFloats(sonar_dist_packed) return sonar_dist
def waitForMovementExecuted(id): global executedMovId global stringSignalName while executedMovId!=id: retCode,s=sim.simxGetStringSignal(clientID,stringSignalName,sim.simx_opmode_buffer) if retCode==sim.simx_return_ok: if type(s)==bytearray: s=s.decode('ascii') # python2/python3 differences executedMovId=s
def waitForMovementExecuted2(id): while client.executedMovId2 != id: retCode, s = sim.simxGetStringSignal(client.id, client.stringSignalName2, sim.simx_opmode_buffer) if retCode == sim.simx_return_ok: if type(s) == bytearray: s = s.decode('ascii') # python2/python3 differences client.executedMovId2 = s
def laser_points(self): return_code, data = sim.simxGetStringSignal(self.client_id, "measuredDataAtThisTime", sim.simx_opmode_blocking) if return_code == sim.simx_return_ok: measured_data = np.array(sim.simxUnpackFloats(data)) measured_data = np.split(measured_data, len(measured_data) / 3) else: print("Eror %d" % return_code) return np.array(measured_data)
def get_image_top_cam(): # Image from the top camera return_code, return_value = sim.simxGetStringSignal(clientID=clientID, signalName="top_cam_image", operationMode=sim.simx_opmode_blocking) if return_code == 0: image = sim.simxUnpackFloats(return_value) res = int(np.sqrt(len(image) / 3)) return image_correction(image, res) else: return return_code
def timerEvent(self, a0: 'QTimerEvent') -> None: self.errorCode, self.points = sim.simxGetStringSignal(self.clientID, 'scan ranges', sim.simx_opmode_buffer) points_ = sim.simxUnpackFloats(self.points) points_ = np.array(points_).reshape(len(points_) // 3,3 ) points_ = np.transpose(points_.transpose()[:-1]) self.clusters = RDP.clust_then_RDP(points_) self.update()
def get_battery(): ''' Returns the battery ------- TYPE DESCRIPTION. ''' return sim.simxGetStringSignal(clientID=clientID, signalName="battery", operationMode=sim.simx_opmode_blocking)
def get_bumper_sensor(): # Bumper reading as 3-dimensional force vector # bumper_force_vector = [0, 0, 0] # If there is a force, return_code, bumper_force_vector_packed = sim.simxGetStringSignal(clientID=clientID, signalName="bumper_sensor", operationMode=sim.simx_opmode_blocking) if return_code == 0: bumper_force_vector = sim.simxUnpackFloats(bumper_force_vector_packed) return bumper_force_vector
def get_laser_2d_data(self): error, signalValue = sim.simxGetStringSignal(self.clientId, "measuredDataAtThisTime", sim.simx_opmode_buffer) if error == sim.simx_return_novalue_flag: print(str(error) + "! signalValue_buffer") data = sim.simxUnpackFloats(signalValue) dataList = [] for i in range(0, int(len(data)), 3): dataList.append(data[i + 1]) if len(dataList) == 182: time.sleep(0.1) return dataList else: return False
def check_self_simx_opmode_streaming_FLAG(self): if self.simxOpmodeStreamingToBuffer_FLAG == 0: error, signalValue = sim.simxGetStringSignal( self.clientId, "measuredDataAtThisTime", sim.simx_opmode_streaming) if error == sim.simx_return_remote_error_flag: print(str(error) + "! simxGetStringSignal_streaming") error, signalValue = sim.simxGetStringSignal( self.clientId, "measuredDataAtThisTime", sim.simx_opmode_buffer) if error == sim.simx_return_novalue_flag: print(str(error) + "! simxGetStringSignal_buffer") sim.simxUnpackFloats(signalValue) error, position = sim.simxGetObjectPosition( self.clientId, self.pioneerHandle, -1, sim.simx_opmode_streaming) if error == sim.simx_return_remote_error_flag: print(str(error) + "! simxGetObjectPosition_streaming") error, position = sim.simxGetObjectPosition( self.clientId, self.pioneerHandle, -1, sim.simx_opmode_buffer) if error == sim.simx_return_novalue_flag: print(str(error) + "! simxGetObjectPosition_buffer") self.simxOpmodeStreamingToBuffer_FLAG = 1 time.sleep(0.1)
def get_LaserData(self,pos,alpha): ## Get Data from Laser Scanner on V-REP error, data = sim.simxGetStringSignal(self.clientID,'communication',sim.simx_opmode_buffer) # Unpack data to array array = sim.simxUnpackFloats(data) if len(array)>181: for i in range(0, 181): # Separate X and Y coordinates self.LaserDataX[i] = array[3*i+1] self.LaserDataY[i] = array[3*i+2] # Adjusting the Map X0 = np.cos(alpha)*self.LaserDataX - np.sin(alpha)*self.LaserDataY + pos[0] Y0 = np.sin(alpha)*self.LaserDataX + np.cos(alpha)*self.LaserDataY + pos[1] self.LaserDataX , self.LaserDataY = X0 , Y0 # @remove get functions normally return values(the X and Y LaserData) return self.LaserDataX, self.LaserDataY
def __init__(self): super(MyWindow, self).__init__() self.setGeometry(300, 300,300,100) self.setWindowTitle('pyqt_tst') self.left_speed = 0 self.right_speed = 0 sim.simxFinish(-1) # если имелась кака-либо незавершенная сессия, она будет завершена self.clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # соединение с портом if self.clientID != -1: print('Server is connected!') else: print('Server is unreachable!') sys.exit(0) self.errorCode, self.points = sim.simxGetStringSignal(self.clientID, 'scan ranges', sim.simx_opmode_streaming) time.sleep(0.5) code, self.left_wheel = sim.simxGetObjectHandle(self.clientID,'Left_motor',sim.simx_opmode_blocking) print(f'code: {code}, left: {self.left_wheel}') code, self.right_wheel = sim.simxGetObjectHandle(self.clientID,'Right_motor',sim.simx_opmode_blocking) print(f'code: {code}, right: {self.right_wheel}') self.clusters = [] self.startTimer(200) print('init_done')
while clientID == -1: clientID = sim.simxStart('127.0.0.1', 19997, True, True, 15000, 5) print('Not connected to remote API server') if clientID != -1: break start_sim = True # simxGetPingTime(clientID) _, drone = sim.simxGetObjectHandle(clientID, 'my_drone', sim.simx_opmode_blocking) _, target = sim.simxGetObjectHandle(clientID, 'Quadcopter_target', sim.simx_opmode_blocking) _, camera = sim.simxGetObjectHandle(clientID, 'Main_camera', sim.simx_opmode_oneshot_wait) _, h_sensor = sim.simxGetObjectHandle(clientID, 'H_sensor', sim.simx_opmode_oneshot_wait) _, gps = sim.simxGetStringSignal(clientID, 'gps', sim.simx_opmode_streaming) _, gyro = sim.simxGetStringSignal(clientID, 'gyro', sim.simx_opmode_streaming) _, accel = sim.simxGetStringSignal(clientID, 'nydavai', sim.simx_opmode_streaming) _, velocity, _ = sim.simxGetObjectVelocity(clientID, drone, sim.simx_opmode_streaming) _, euler = sim.simxGetObjectOrientation(clientID, drone, target, sim.simx_opmode_streaming) _, h_detected, h_point, h_det_obj, h_norm = sim.simxReadProximitySensor(clientID, h_sensor, sim.simx_opmode_streaming) _, orientation_target = sim.simxGetObjectOrientation(clientID, target, -1, sim.simx_opmode_streaming) _, orientation_drone = sim.simxGetObjectOrientation(clientID, drone, -1, sim.simx_opmode_streaming) _, location_drone = sim.simxGetObjectPosition(clientID, drone, -1, sim.simx_opmode_streaming) _, location_target = sim.simxGetObjectPosition(clientID, target, -1, sim.simx_opmode_streaming) _, resolution, image = sim.simxGetVisionSensorImage(clientID, camera, 0, sim.simx_opmode_streaming) pid1 = PID_controll(2, 0, 0) pid1.k_v = -2 pid2 = PID_controll(0.005, 0, 1)
#Check Remote API for function calls and operation modes ERRORCODE, Lidar = vrep.simxGetObjectHandle(clientID, 'laser_sensor#2', vrep.simx_opmode_blocking) #Using absoulte position of the rover as a stand in for Magnetometer ERRORCODE, Magref = vrep.simxGetObjectHandle(clientID,'rover#2',vrep.simx_opmode_blocking) res, v1 = vrep.simxGetObjectHandle(clientID, 'Vision_sensor0#2', vrep.simx_opmode_oneshot_wait) ERRORCODE, Rover3 = vrep.simxGetObjectHandle(clientID, 'rover#3', vrep.simx_opmode_blocking) if clientID != -1: starting_time = time.time() #Setting up Lidar, gyro, Mag, vision sensor for first time detection, simx_opmode_streaming is first call as mentioned in Remote API ERRORCODE, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_streaming) ERRORCODE, detectionState, detectedPoint_lidar, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(clientID, Lidar, vrep.simx_opmode_streaming) ERRORCODE, eulerAngles=vrep.simxGetObjectOrientation( clientID, Magref, -1, vrep.simx_opmode_streaming) ERRORCODE, gyro_out = vrep.simxGetStringSignal(clientID, "myGyroData#2", vrep.simx_opmode_streaming) ERRORCODE, accel_out = vrep.simxGetStringSignal(clientID, "myaccData#2", vrep.simx_opmode_streaming) ERRORCODE, position = vrep.simxGetObjectPosition( clientID, Magref, -1, vrep.simx_opmode_streaming) ERRORCODE, position2 = vrep.simxGetObjectPosition( clientID, Rover3, -1, vrep.simx_opmode_streaming) while vrep.simxGetConnectionId(clientID) != -1: #Setting up Lidar, gyro, Mag, vision sensor for detection values from buffer, simx_opmode_buffer is for all subsequent calls after first call as mentioned in Remote API ERRORCODE, resolution, image = vrep.simxGetVisionSensorImage(clientID, v1, 0, vrep.simx_opmode_buffer) ERRORCODE, detectionState, detectedPoint_lidar, detectedObjectHandle, detectedSurfaceNormalVector = vrep.simxReadProximitySensor(clientID, Lidar, vrep.simx_opmode_buffer) # ERRORCODE, gyro_data = vrep.simxGetFloatSignal(clientID, gyroRef, vrep.simx_opmode_buffer) ERRORCODE, eulerAngles=vrep.simxGetObjectOrientation( clientID, Magref, -1, vrep.simx_opmode_buffer) ERRORCODE, gyro_out = vrep.simxGetStringSignal( clientID, "myGyroData#2", vrep.simx_opmode_buffer) ERRORCODE, accel_out = vrep.simxGetStringSignal( clientID, "myaccData#2", vrep.simx_opmode_buffer)
def readObservations(clientID): returnCode, sensorData = sim.simxGetStringSignal(clientID, "points", sim.simx_opmode_streaming) measureData = sim.simxUnpackFloats(sensorData) return measureData
stringSignalName = targetArm + '_executedMovId' def waitForMovementExecuted(id): global executedMovId global stringSignalName while executedMovId != id: retCode, s = sim.simxGetStringSignal(clientID, stringSignalName, sim.simx_opmode_buffer) if retCode == sim.simx_return_ok: if type(s) == bytearray: s = s.decode('ascii') # python2/python3 differences executedMovId = s # Start streaming stringSignalName string signal: sim.simxGetStringSignal(clientID, stringSignalName, sim.simx_opmode_streaming) # Set-up some movement variables: times = [ 0.000, 0.050, 0.100, 0.150, 0.200, 0.250, 0.300, 0.350, 0.400, 0.450, 0.500, 0.550, 0.600, 0.650, 0.700, 0.750, 0.800, 0.850, 0.900, 0.950, 1.000, 1.050, 1.100, 1.150, 1.200, 1.250, 1.300, 1.350, 1.400, 1.450, 1.500, 1.550, 1.600, 1.650, 1.700, 1.750, 1.800, 1.850 ] j1 = [ 0.000, 0.000, 0.002, 0.009, 0.022, 0.042, 0.068, 0.100, 0.139, 0.185, 0.237, 0.296, 0.360, 0.431, 0.506, 0.587, 0.669, 0.753, 0.838, 0.923, 1.008, 1.091, 1.170, 1.243, 1.308, 1.365, 1.414, 1.455, 1.491, 1.519, 1.541, 1.557, 1.566, 1.569, 1.570, 1.571, 1.571, 1.571 ] j2 = [
def waitForMovementExecuted2(id): while client.executedMovId2!=id: retCode,s=sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_buffer) if retCode==sim.simx_return_ok: client.executedMovId2=s
client.stringSignalName2=targetArm2+'_executedMovId' def waitForMovementExecuted1(id): while client.executedMovId1!=id: retCode,s=sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_buffer) if retCode==sim.simx_return_ok: client.executedMovId1=s def waitForMovementExecuted2(id): while client.executedMovId2!=id: retCode,s=sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_buffer) if retCode==sim.simx_return_ok: client.executedMovId2=s # Start streaming client.stringSignalName1 and client.stringSignalName2 string signals: sim.simxGetStringSignal(client.id,client.stringSignalName1,sim.simx_opmode_streaming) sim.simxGetStringSignal(client.id,client.stringSignalName2,sim.simx_opmode_streaming) # Set-up some movement variables: mVel=100*math.pi/180 mAccel=150*math.pi/180 maxVel=[mVel,mVel,mVel,mVel,mVel,mVel] maxAccel=[mAccel,mAccel,mAccel,mAccel,mAccel,mAccel] targetVel=[0,0,0,0,0,0] # Start simulation: sim.simxStartSimulation(client.id,sim.simx_opmode_blocking) # Wait until ready: waitForMovementExecuted1('ready') waitForMovementExecuted1('ready')
primitives = np.load('primitive_data.npy') ip = '127.0.0.1' port = 19997 sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart(ip, port, True, True, -5000, 5) # Connect to V-REP if clientID == -1: import sys sys.exit('\nV-REP remote API server connection failed (' + ip + ':' + str(port) + '). Is V-REP running?') print('Connected to Remote API Server') # show in the terminal sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot) Body = {} get_first_handles(clientID, Body) errorCode, acc = sim.simxGetStringSignal(clientID, 'Acceleration', sim.simx_opmode_streaming) returnCode, position = sim.simxGetObjectPosition(clientID, Body['HeadYaw'], -1, sim.simx_opmode_streaming) #errorCode, acc = sim.simxGetStringSignal(clientID, 'Gyrometer', sim.simx_opmode_streaming) acc_record_safe = [] acc_record_fall = [] for i in tqdm(range(primitives.shape[0])): sequence_sample = np.concatenate( (primitives[i], primitives[(i + 1) % primitives.shape[0]]), axis=0) for idx_f in range(sequence_sample.shape[0]): angle_recon = converter.frameRecon(sequence_sample[idx_f]) # angles: LSP, LSR, LEY, LER, RSP, RSR, REY, RER, LHP, LHR, LHYP, LKP, RHP, RHR, RHYP, RKP, LAP, LAR, RAP, RAR angles = converter.generateWholeJoints(angle_recon) assert (len(angles) == 20) transmit(clientID, Body, angles)
boxesVolumesSum = 0 num = 0 boxes_generated = 0 '''print('adf') while True: r,data=sim.simxGetStringSignal(clientID,'TOTALBOXES',sim.simx_opmode_blocking) print(data) if data!=bytearray(b''): boxes_generated=int(data.decode()) break print('boxgenerated')''' while True: #r, data = sim.simxReadStringStream(clientID,'STATE',sim.simx_opmode_streaming) r, data = sim.simxGetStringSignal(clientID, 'STATE', sim.simx_opmode_blocking) #print(data) if data != bytearray(b''): state = sim.simxUnpackFloats(data) state = np.array(state) state = state.reshape((120, -1)) r, dim = sim.simxGetStringSignal(clientID, 'DIM', sim.simx_opmode_blocking) dim = sim.simxUnpackFloats(dim) if (boxes_generated == 0): boxes_generated = int(dim[3]) dim = dim[0:3] #print(dim) sim.simxSetStringSignal(clientID, 'STATE', '', sim.simx_opmode_oneshot) if heuristics == 'walle':