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':
