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 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 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_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 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_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 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
pid2 = PID_controll(0.005, 0, 1) pid3 = PID_controll(-0.005, 0, -1) pid4 = PID_controll(0.1, 0, 2) pid5 = PID_controll(0.25, 0, 2.1) pid6 = PID_controll(-0.25, 0, -2.1) controller = control_positioon() pev_time = 0 flag1, flag2, flag3, flag4, flag5 = [False for _ in range(5)] target_pos, drone_pos, target_orient, drone_orient = [0 for _ in range(4)] lock = threading.Lock() t1 = threading.Thread(target=change_values, args=[massage, pid1, pid2, pid3, pid4, pid5, pid6, clientID]) t1.start() while 1: while start_sim: _, gps = sim.simxGetStringSignal(clientID, 'gps', sim.simx_opmode_buffer) gps = sim.simxUnpackFloats(gps) # 3 floats _, gyro = sim.simxGetStringSignal(clientID, 'gyro', sim.simx_opmode_buffer) gyro = sim.simxUnpackFloats(gyro) # 3 floats _, accel = sim.simxGetStringSignal(clientID, 'nydavai', sim.simx_opmode_buffer) accel = sim.simxUnpackFloats(accel) # 3 floats _, orientation_target = sim.simxGetObjectOrientation(clientID, target, -1, sim.simx_opmode_buffer) _, orientation_drone = sim.simxGetObjectOrientation(clientID, drone, -1, sim.simx_opmode_buffer) _, location_drone = sim.simxGetObjectPosition(clientID, drone, -1, sim.simx_opmode_buffer) _, location_target = sim.simxGetObjectPosition(clientID, target, -1, sim.simx_opmode_buffer) _, resolution, image = sim.simxGetVisionSensorImage(clientID, camera, 0, sim.simx_opmode_buffer) _, velocity, _ = sim.simxGetObjectVelocity(clientID, drone, sim.simx_opmode_buffer) _, euler = sim.simxGetObjectOrientation(clientID, drone, target, sim.simx_opmode_buffer) # Bool, 3 floats, _ , 3 floats _, h_detected, h_point, _, h_norm = sim.simxReadProximitySensor(clientID, h_sensor, sim.simx_opmode_buffer)
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) time.sleep(0.03) errorCode, acc = sim.simxGetStringSignal(clientID, 'Acceleration', sim.simx_opmode_buffer) #errorCode, acc = sim.simxGetStringSignal(clientID, 'Gyrometer', sim.simx_opmode_buffer) acc = sim.simxUnpackFloats(acc) # tmp_acc.append(np.sqrt((np.square(acc[0]+)+np.square(acc[1]))/2)) returnCode, position = sim.simxGetObjectPosition( clientID, Body['HeadYaw'], -1, sim.simx_opmode_buffer) if position[2] < 0.4 and position[2] > 0: #fall down print(np.sqrt((np.square(acc[0]) + np.square(acc[1])) / 2), max(acc_record_safe)) sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot) time.sleep(2) sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot) time.sleep(2) acc_record_fall.append( np.sqrt((np.square(acc[0]) + np.square(acc[1])) / 2)) errorCode, acc = sim.simxGetStringSignal( clientID, 'Acceleration', sim.simx_opmode_streaming) returnCode, position = sim.simxGetObjectPosition(
'''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': pos = np.array(walle(state, dim)) if heuristics == 'first_fit': pos = np.array(first_fit(state, dim))
def readObservations(clientID): returnCode, sensorData = sim.simxGetStringSignal(clientID, "points", sim.simx_opmode_streaming) measureData = sim.simxUnpackFloats(sensorData) return measureData
#parsing data to alpha trimmmer filtered_data = alphatrimmer(7, 2, str(norm_distance)) frame_id += 1 if ERRORCODE == vrep.simx_return_ok: img = np.array(image, dtype=np.uint8) img.resize([resolution[1], resolution[0], 3]) vision_sensor_output = cv2.rotate(img, cv2.ROTATE_180) try: parsing_data = frame_buffer(vision_sensor_output) elapsed_time = time.time() - starting_time fps = frame_id / elapsed_time #Unpacking float datas from gyro and accel floatValues1 = vrep.simxUnpackFloats( gyro_out) floatValues2 = vrep.simxUnpackFloats( accel_out) #parsing data to alpha trimmmer CF_out = ComplementaryFilter(floatValues2, floatValues1, eulerAngles, RPY) #parsing data to alpha trimmmer KF_Out = kalman_filter(filtered_data, 0, 0, 0) display_frame = info_frame(parsing_data, filtered_data, fps, KF_Out, CF_out[2], position, position2) cv2.imshow("Image", display_frame) # print("Detection successful") cv2.waitKey(100) except: cv2.imshow("Image", vision_sensor_output)