]
q = 0.00001
# q = 1
var_init = [q, q]
v = algorithm.init_veh(n_v, n_f, init_veh_distr, init_feat_distr, var_init)
meas = me.Measurement(n_v, n_f, t_s, True)
data = DataCollect.DataCollect(n_v, n_f)
data.init(n_v, n_f, init_veh_distr, init_feat_distr[:n_f])
v = algorithm.init_veh(n_v, n_f, init_veh_distr, init_feat_distr[:n_f],
var_init)
# meas.meas_fea[0][1][:, 10:25] = None
algorithm.main_loop(v, meas, 1, t_s, data)
fontprop = FontProperties()
fontprop.set_size('small')
#
# plt.figure(1, figsize=(9, 5))
# err1 = plt.subplot(211)
# plt.plot(error[0][0, :], label='Square error in x')
# plt.plot(error[0][1, :], label='Square error in y')
# plt.plot(error_mean_distance[0], label='Mean distance error')
# plt.title('The RMSE for vehicle 1')
# plt.xlabel('Time')
# plt.ylabel('RMSE [m]')
# plt.subplots_adjust(hspace=0.36)
# err1.legend(ncol=3, prop=fontprop)
# err1.set_ylim([err1.get_ylim()[0], err1.get_ylim()[0] + (err1.get_ylim()[1] - err1.get_ylim()[0])*1.15])
for n_mp in range(1, rangeNmp):
data = [None for i in range(runs)]
meas = [None for i in range(runs)]
error_square = [[None for i in range(runs)] for j in range(n_v)]
error_distance = [[None for i in range(runs)] for j in range(n_v)]
error = [None for i in range(n_v)]
error_mean_distance = [None for i in range(n_v)]
for run in range(runs):
meas[run] = me.Measurement(n_v, n_f, 1)
data[run] = DataCollect.DataCollect(n_v, n_f)
data[run].init(n_v, n_f, init_veh_distr, init_feat_distr[:n_f])
v = algorithm.init_veh(n_v, n_f, init_veh_distr, init_feat_distr[:n_f], var_init)
print('Nmp = '+str(n_mp))
algorithm.main_loop(v, meas[run], n_mp, 1, data[run])
for k in range(n_v):
error_square[k][run] = np.power(data[run].get_veh_belief(k)[:2, :] - meas[run].mean_veh[k], 2)
error_distance[k][run] = np.sum(error_square[k][run], axis = 0)
for l in range(n_v):
error[l] = np.mean(error_square[l], axis = 0)
error_mean_distance[l] = np.mean(error_distance[l], axis = 0)
collectMSE[l][n_mp-1] = np.mean(error_mean_distance[l][7:], axis=0)
print collectMSE
fontprop = FontProperties()
fontprop.set_size('small')
plt.figure(1, figsize=(9, 5))
init_feat_distr = [
Distribution.Distribution(p3, cov3),
Distribution.Distribution(p4, cov4),
Distribution.Distribution(p5, cov5),
Distribution.Distribution(p6, cov6),
Distribution.Distribution(p7, cov7)
]
q = 1
var_init = [q, q]
data = DataCollect.DataCollect(n_v, n_f)
data.init(n_v, n_f, init_veh_distr, init_feat_distr[:n_f])
v = algorithm.init_veh(n_v, n_f, init_veh_distr, init_feat_distr[:n_f],
var_init)
algorithm.main_loop(v, meas, N_mp, 1, data)
data.save_data('ICP' + str(n_v) + '_' + str(n_f) + name + '_' + str(N_mp) +
'.pickle')
fontprop = FontProperties()
fontprop.set_size('small')
pos_est0 = data.get_veh_belief(0)
pos_updt0 = data.get_updt_veh_belief(0)
pos_pred0 = data.get_pred_veh_belief(0)
pos_est1 = data.get_veh_belief(1)
pos_updt1 = data.get_updt_veh_belief(1)
pos_pred1 = data.get_pred_veh_belief(1)
# plt.figure(2)
# plt.plot(np.concatenate(([None], np.array(meas[0].mean_veh[0][1]).flatten()), axis=0), 'b',
def icp():
global count, Gps_x0, Gps_y0, Gps_x1, Gps_y1, veh, fea, data
if icp_bool:
msg2 = ''' ( ICP Activated ) '''
if not icp_bool:
msg2 = ''' ( ICP De-Activated ) '''
print "-------------------------------- ****** Run " + str(count) + str(
msg2) + " ****** --------------------------------"
if count == 0:
n_v = 2
n_f = 1
p1 = np.matrix([[Gps_x0], [Gps_y0], [0], [0]])
p2 = np.matrix([[Gps_x1], [Gps_y1], [0], [0]])
cov1 = np.matrix('10 0 0 0; ' '0 10 0 0; ' '0 0 1 0; ' '0 0 0 1')
cov2 = np.matrix('10 0 0 0; ' '0 10 0 0; ' '0 0 1 0; ' '0 0 0 1')
init_veh_distr = [
Distribution.Distribution(p1, cov1),
Distribution.Distribution(p2, cov2)
]
p3 = np.matrix('1.7; 0.5')
cov3 = np.matrix('10 0; 0 10')
init_feat_distr = [Distribution.Distribution(p3, cov3)]
q0 = 1.0e-2
q1 = 1.0e-2
var_init = [q0, q1]
data = DataCollect.DataCollect(n_v, n_f)
data.init(n_v, n_f, init_veh_distr, init_feat_distr)
veh = algorithm.init_veh(n_v, n_f, init_veh_distr, init_feat_distr,
var_init)
count = count + 1
start_time = rospy.get_time()
if (Gps_x0 and Gps_x1 and Gps_y0 and Gps_y1) is not None:
mean_v = [
np.matrix([[Gps_x0], [Gps_y0]]),
np.matrix([[Gps_x1], [Gps_y1]])
]
mean_f = [[
np.matrix([[float(feat11_x) / 1000], [float(feat11_y) / 1000]]),
np.matrix([[float(feat21_x) / 1000], [float(feat21_y) / 1000]])
]]
#data = DataCollect.DataCollect(2, 1)
meas = me.MeasurementMixer(2, 1, mean_v, mean_f)
print "Feature 1 Measurement X = " + str(float(
meas.meas_fea[0][0][0])) + " , Y = " + str(
float(meas.meas_fea[0][0][1]))
print "Feature 2 Measurement X = " + str(float(
meas.meas_fea[0][1][0])) + " , Y = " + str(
float(meas.meas_fea[0][1][1]))
print "Vehicle 1 Measurement X = " + str(float(
meas.meas_veh[0][0])) + " , Y = " + str(float(meas.meas_veh[0][1]))
print "Vehicle 2 Measurement X = " + str(float(
meas.meas_veh[1][0])) + " , Y = " + str(float(meas.meas_veh[1][1]))
#print v
#print "Feature in callback" + str(fea)
for v in veh:
v.update_time_step(T_s)
algorithm.main_loop(veh, meas, 2, 1, data)
actual1 = data.get_veh_belief(0)
actual2 = data.get_veh_belief(1)
last_x0 = actual1[0][-1]
last_y0 = actual1[1][-1]
last_x1 = actual2[0][-1]
last_y1 = actual2[1][-1]
x_coord0 = float(meas.meas_veh[0][0])
y_coord0 = float(meas.meas_veh[0][1])
x_coord1 = float(meas.meas_veh[1][0])
y_coord1 = float(meas.meas_veh[1][1])
print 'Vehicle 1 ICP X = ' + str(last_x0) + ', Y = ' + str(last_y0)
print 'Vehicle 2 ICP X = ' + str(last_x1) + ', Y = ' + str(last_y1)
global T_s
stop_time = rospy.get_time()
# T_s = stop_time - start_time
print T_s, "Sampling Time"
print str(count) + "Runs"
vehicle0 = Vehicle()
vehicle0.heading = angle0
vehicle0.header.stamp = rospy.get_rostime()
vehicle0.x_meas = x_coord0
vehicle0.y_meas = y_coord0
vehicle0.x_icp = last_x0
vehicle0.y_icp = last_y0
vehicle0.x_actual_meas = Gps_x0
vehicle0.y_actual_meas = Gps_y0
vehicle1 = Vehicle()
vehicle1.heading = angle1
vehicle1.header.stamp = rospy.get_rostime()
vehicle1.x_meas = x_coord1
vehicle1.y_meas = y_coord1
vehicle1.x_icp = last_x1
vehicle1.y_icp = last_y1
vehicle1.x_actual_meas = Gps_x1
vehicle1.y_actual_meas = Gps_y1
pub0 = rospy.Publisher("Robot0/config", Vehicle, queue_size=10)
pub1 = rospy.Publisher("Robot1/config", Vehicle, queue_size=10)
pub0.publish(vehicle0)
pub1.publish(vehicle1)
print "Heading Angle Robot 0: " + str(angle0)
print "Heading Angle Robot 1: " + str(angle1)
Distribution.Distribution(init_veh_distr2mean, cov2)
]
init_feat_distr = []
for f in range(n_f):
init_feat_distr_mean = np.reshape(
np.random.multivariate_normal(np.array(p3).flatten(), cov3, 1),
[2, 1])
init_feat_distr.append(
Distribution.Distribution(init_feat_distr_mean, cov3))
meas[run] = me.Measurement(n_v, n_f, 1)
dataICP[run] = DataCollect.DataCollect(n_v, n_f)
dataICP[run].init(n_v, n_f, init_veh_distr, init_feat_distr[:n_f])
v = algorithm.init_veh(n_v, n_f, init_veh_distr, init_feat_distr[:n_f],
var_init)
algorithm.main_loop(v, meas[run], N_mp, 1, dataICP[run])
for k in range(n_v):
error_square[k][run] = np.power(
dataICP[run].get_veh_belief(k)[:2, :] - meas[run].mean_veh[k],
2)
error_distance[k][run] = np.sum(error_square[k][run], axis=0)
for l in range(n_v):
error[l] = np.sqrt(np.mean(error_square[l], axis=0))
error_mean_norm[l] = np.sqrt(np.mean(error_distance[l], axis=0))
with open(
'/home/tomek/TheTitans/ICP/Pickles/errorICP' + str(n_v) + '_' +
str(n_f) + '_' + str(meas[0].vehicle(0, 0).get_cov()[0, 0]) + '_' +
str(meas[0].vehicle(1, 0).get_cov()[0, 0]) + '_' +
str(meas[0].feature(0, 0, 0).get_cov()[0, 0]) + '_' + str(runs) +
'_' + str(N_mp) + '.pickle', 'wb') as output:
p3 = np.matrix('0.5; 0.5')
cov3 = np.matrix('5 0; 0 5')
init_feat_distr = [Distribution.Distribution(p3, cov3)]
data.init(2, 1, init_veh_distr, init_feat_distr)
var_pos1 = 2
var_pos2 = 2
var_init = [[var_pos1, var_pos1], [var_pos2, var_pos2]]
while count < 1000:
meas = me.Measurement(2, 1)
v = algorithm.init_veh(2, 1, init_veh_distr, var_init)
f = algorithm.init_feat(1, 2, init_feat_distr)
algorithm.main_loop(v, f, meas, 7, 1, data)
++count
x_pos_est0, y_pos_est0 = data.get_veh_belief(0)
x_pos_pred0, y_pos_pred0 = data.get_pred_veh_belief(0)
x_pos_est1, y_pos_est1 = data.get_veh_belief(1)
x_pos_pred1, y_pos_pred1 = data.get_pred_veh_belief(1)
error0 = [
Kalman.rmse(data.get_veh_belief(0)[0][1:], meas.mean_veh[0][0]),
Kalman.rmse(data.get_veh_belief(0)[1][1:], meas.mean_veh[0][1])
]
errorP0 = [
np.sqrt(v[0].pos_belief.get_cov()[0, 0]),
np.sqrt(v[0].pos_belief.get_cov()[1, 1])
]
def callback(pos, pos2):
global count
count = count + 1
start_time = rospy.get_time()
#print "Inside Callback"
srv1 = 'get_coord0'
srv2 = 'get_coord1'
#Gps_x1, Gps_y1, Gps_x2, Gps_y2 = 0
rospy.wait_for_service(srv1)
try:
get_coords1 = rospy.ServiceProxy(srv1, GetCoord)
f = get_coords1(1)
tmp_pos1 = f.data.data
error_count = 0
while error_count < 10:
if np.size(tmp_pos1) != 3:
Gps_x1 = tmp_pos1[0]
Gps_y1 = tmp_pos1[1]
error_count = 11
else:
error_count += 1
Gps_x1 = None
Gps_y1 = None
except rospy.ServiceException as exc1:
print("Service1 did not process request: " + str(exc1))
rospy.wait_for_service(srv2)
error_count = 0
try:
get_coords2 = rospy.ServiceProxy(srv2, GetCoord)
f = get_coords2(1)
tmp_pos2 = f.data.data
while error_count < 10:
if np.size(tmp_pos2) != 3:
Gps_x2 = tmp_pos2[0]
Gps_y2 = tmp_pos2[1]
error_count = 11
else:
error_count += 1
Gps_x2 = None
Gps_y2 = None
except rospy.ServiceException as exc2:
print("Service2 did not process request: " + str(exc2))
if (Gps_x1 and Gps_x2 and Gps_y1 and Gps_y2) is not None:
mean_v = [
np.matrix([[Gps_x1], [Gps_y1]]),
np.matrix([[Gps_x2], [Gps_y2]])
]
mean_f = [[
np.matrix([[float(pos.x1) / 1000], [float(pos.y1) / 1000]]),
np.matrix([[float(pos2.x1) / 1000], [float(pos2.y1) / 1000]])
]]
#data = DataCollect.DataCollect(2, 1)
T_s = 0.1
meas = me.MeasurementMixer(2, 1, mean_v, mean_f)
print "Feature 1 Measurement X = " + str(float(
meas.meas_fea[0][0][0])) + " , Y = " + str(
float(meas.meas_fea[0][0][1]))
print "Feature 2 Measurement X = " + str(float(
meas.meas_fea[0][1][1])) + " , Y = " + str(
float(meas.meas_fea[0][1][1]))
print "Vehicle 1 Measurement X = " + str(float(
meas.meas_veh[0][0])) + " , Y = " + str(float(meas.meas_veh[0][1]))
print "Vehicle 2 Measurement X = " + str(float(
meas.meas_veh[1][0])) + " , Y = " + str(float(meas.meas_veh[1][1]))
#print v
#print "Feature in callback" + str(fea)
algorithm.main_loop(veh, fea, meas, 7, 1, data)
actual1 = data.get_veh_belief(0)
actual2 = data.get_veh_belief(1)
last_x = actual1[0][-1]
last_y = actual1[1][-1]
last_x2 = actual2[0][-1]
last_y2 = actual2[1][-1]
print 'Vehicle 1 ICP X = ' + str(last_x) + ', Y = ' + str(last_y)
print 'Vehicle 2 ICP X = ' + str(last_x2) + ', Y = ' + str(last_y2)
global T_s
stop_time = rospy.get_time()
T_s = stop_time - start_time
print T_s, "Sampling Time"
print str(count) + "Runs"
