def listener():
global data
global veh, fea
data = DataCollect.DataCollect(2, 1)
p1 = np.matrix('1.01; 0.1')
p2 = np.matrix('2.9; 1.9')
cov1 = np.matrix('0.5 0; 0 0.5')
cov2 = np.matrix('0.5 0; 0 0.5')
init_veh_distr = [
Distribution.Distribution(p1, cov1),
Distribution.Distribution(p2, cov2)
]
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 = 200
var_pos2 = 200
var_init = [[var_pos1, var_pos1], [var_pos2, var_pos2]]
veh = algorithm.init_veh(2, 1, init_veh_distr, var_init)
fea = algorithm.init_feat(1, 2, init_feat_distr)
#print "Feature in listner" + str(fea)
rospy.init_node('listener', anonymous=True)
# rospy.Subscriber("position2", Pos, callback, queue_size=100)
# spin() simply keeps python from exiting until this node is stopped
#print "inside listener"
pos = message_filters.Subscriber('position', Pos)
pos2 = message_filters.Subscriber('position2', Pos)
ts = message_filters.ApproximateTimeSynchronizer([pos, pos2], 2, 1)
ts.registerCallback(callback)
rospy.spin()
error_mean_distance = [None for i in range(n_v)]
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')
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(p2, cov2)
]
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]),
