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)
