def validate_localization_transition_update(fname="validation_run.p",
show_plot=True):
validation_run = pickle.load(open(fname, "rb"))
EKF = Localization_EKF(validation_run["states"][0][1], NoiseParams["P0"],
NoiseParams["Q"], MapParams,
validation_run["tf_base_to_camera"],
NoiseParams["g"])
open_loop_states = [EKF.x]
for i in range(len(validation_run["controls"]) - 1):
u = validation_run["controls"][i][1]
dt = validation_run["controls"][
i + 1][0] - validation_run["controls"][i][0]
EKF.transition_update(u, dt)
open_loop_states.append(EKF.x)
ground_truth_x = [s[0] for t, s in validation_run["states"]]
ground_truth_y = [s[1] for t, s in validation_run["states"]]
open_loop_x = [s[0] for s in open_loop_states]
open_loop_y = [s[1] for s in open_loop_states]
plt.plot(ground_truth_x,
ground_truth_y,
label="ground truth",
color="black")
plt.plot(open_loop_x, open_loop_y, label="open loop", color="green")
plt.axis("equal")
if show_plot:
plt.legend(loc=0)
plt.show()
def validate_localization_transition_model(fname="validation_run.p"):
validation_run = pickle.load(open(fname, "rb"))
EKF = Localization_EKF(validation_run["states"][0][1], NoiseParams["P0"],
NoiseParams["Q"], MapParams,
validation_run["tf_base_to_camera"],
NoiseParams["g"])
for i, tc in enumerate(validation_run["controls"]):
timestamp, control = tc
g, Gx, Gu = EKF.transition_model(control, .1)
g_ref, Gx_ref, Gu_ref = validation_run["transition_model_validation"][
i]
if norm(g - g_ref) + norm(Gx - Gx_ref) + norm(Gu - Gu_ref) > 1e-2:
print "At state x = {0} with u = {1} and dt = {2} got Localization_EKF.transition_model output:\n".format(
EKF.x, control, .1)
print g
print Gx
print Gu
print "\nvs. the expected values\n"
print g_ref
print Gx_ref
print Gu_ref
return False
print "Localization_EKF.transition_model seems to be correct"
return True
def validate_localization_map_line_to_predicted_measurement(
fname="validation_run.p"):
validation_run = pickle.load(open(fname, "rb"))
EKF = Localization_EKF(validation_run["states"][0][1], NoiseParams["P0"],
NoiseParams["Q"], MapParams,
validation_run["tf_base_to_camera"],
NoiseParams["g"])
for j in range(EKF.map_lines.shape[1]):
h, Hx = EKF.map_line_to_predicted_measurement(EKF.map_lines[:, j])
h_ref, Hx_ref = validation_run[
"map_line_to_predicted_measurement_validation"][j]
if norm(h - h_ref) + norm(Hx - Hx_ref) > 1e-3:
print "At state x = {0} with m = {1} got Localization_EKF.map_line_to_predicted_measurement_validation output:\n".format(
EKF.x, EKF.map_lines[:, j])
print h
print Hx
print "\nvs. the expected values\n"
print h_ref
print Hx_ref
return False
print "Localization_EKF.map_line_to_predicted_measurement seems to be correct"
return True
def validate_localization_associate_measurements(fname = "validation_run.p"):
if not validate_localization_transition_model(fname) or not validate_localization_map_line_to_predicted_measurement(fname):
print "Validation of associate_measurements cannot proceed."
return False
validation_run = pickle.load(open(fname, "rb"))
EKF = Localization_EKF(validation_run["states"][0][1], NoiseParams["P0"], NoiseParams["Q"],
MapParams, validation_run["tf_base_to_camera"], NoiseParams["g"])
for i in range(len(validation_run["scans"])):
EKF.x, EKF.P = validation_run["associate_measurements_validation_inputs"][i]
alpha, r, C_AR, _, _ = ExtractLines(validation_run["scans"][i][1],
validation_run["scans"][i][2],
LineExtractionParams,
NoiseParams["var_theta"],
NoiseParams["var_rho"])
Z = np.vstack((alpha, r))
v_list, R_list, H_list = EKF.associate_measurements(Z, C_AR)
v_list_ref, R_list_ref, H_list_ref = validation_run["associate_measurements_validation"][i]
if len(v_list) != len(v_list_ref) or len(R_list) != len(R_list_ref) or len(H_list) != len(H_list_ref):
print "You may have an error in Localization_EKF.associate_measurements."
return False
permutation = [np.argmin([norm(R - R_ref) for R_ref in R_list_ref]) for R in R_list]
v_error = sum([norm(v_list[j] - v_list_ref[k]) for j, k in enumerate(permutation)])
R_error = sum([norm(R_list[j] - R_list_ref[k]) for j, k in enumerate(permutation)])
H_error = sum([norm(H_list[j] - H_list_ref[k]) for j, k in enumerate(permutation)])
if v_error + R_error + H_error > 1e-3:
print "You may have an error in Localization_EKF.associate_measurements."
return False
print "Localization_EKF.associate_measurements seems to be correct"
return True
def run(self):
rate = rospy.Rate(100)
while not self.latest_pose:
rate.sleep()
x0 = np.array([self.latest_pose.position.x,
self.latest_pose.position.y,
get_yaw_from_quaternion(self.latest_pose.orientation)])
self.EKF_time = self.latest_pose_time
self.EKF = Localization_EKF(x0, NoiseParams["P0"], NoiseParams["Q"],
MapParams, self.base_to_camera, NoiseParams["g"])
self.OLC = Localization_EKF(x0, NoiseParams["P0"], NoiseParams["Q"],
MapParams, self.base_to_camera, NoiseParams["g"])
while True:
if not self.scans:
rate.sleep()
continue
while self.controls and self.controls[0][0] <= self.scans[0][0]:
next_timestep, next_control = self.controls.popleft()
if next_timestep < self.EKF_time: # guard against time weirdness (msgs out of order)
continue
self.EKF.transition_update(self.current_control,
next_timestep.to_time() - self.EKF_time.to_time())
self.OLC.transition_update(self.current_control,
next_timestep.to_time() - self.EKF_time.to_time())
self.EKF_time, self.current_control = next_timestep, next_control
self.tfBroadcaster.sendTransform(create_transform_msg(
(self.EKF.x[0], self.EKF.x[1], 0),
tf.transformations.quaternion_from_euler(0, 0, self.EKF.x[2]),
"EKF", "world", self.EKF_time)
)
self.tfBroadcaster.sendTransform(create_transform_msg(
(self.OLC.x[0], self.OLC.x[1], 0),
tf.transformations.quaternion_from_euler(0, 0, self.OLC.x[2]),
"open_loop", "world", self.EKF_time)
)
scan_time, theta, rho = self.scans.popleft()
if scan_time < self.EKF_time:
continue
self.EKF.transition_update(self.current_control,
scan_time.to_time() - self.EKF_time.to_time())
self.OLC.transition_update(self.current_control,
scan_time.to_time() - self.EKF_time.to_time())
self.EKF_time = scan_time
alpha, r, C_AR, _, _ = ExtractLines(theta, rho,
LineExtractionParams,
NoiseParams["var_theta"],
NoiseParams["var_rho"])
Z = np.vstack((alpha, r))
self.EKF.measurement_update(Z, C_AR)
while len(self.scans) > 1: # keep only the last element in the queue, if we're falling behind
self.scans.popleft()
def validate_localization_EKF(fname="validation_run.p", show_plot=True):
validate_localization_transition_update(fname, False)
validation_run = pickle.load(open(fname, "rb"))
EKF = Localization_EKF(validation_run["states"][0][1], NoiseParams["P0"],
NoiseParams["Q"], MapParams,
validation_run["tf_base_to_camera"],
NoiseParams["g"])
EKF_states = [EKF.x]
scan_states = []
scan_idx = 0
for i in range(len(validation_run["controls"]) - 1):
u = validation_run["controls"][i][1]
t1 = validation_run["controls"][i + 1][0]
t0 = validation_run["controls"][i][0]
while scan_idx < len(validation_run["scans"]
) and validation_run["scans"][scan_idx][0] < t1:
EKF.transition_update(u, validation_run["scans"][scan_idx][0] - t0)
t0 = validation_run["scans"][scan_idx][0]
alpha, r, C_AR, _, _ = ExtractLines(
validation_run["scans"][scan_idx][1],
validation_run["scans"][scan_idx][2], LineExtractionParams,
NoiseParams["var_theta"], NoiseParams["var_rho"])
Z = np.vstack((alpha, r))
EKF.measurement_update(Z, C_AR)
scan_states.append(EKF.x)
scan_idx = scan_idx + 1
EKF.transition_update(u, t1 - t0)
EKF_states.append(EKF.x)
EKF_x = [s[0] for s in EKF_states]
EKF_y = [s[1] for s in EKF_states]
plt.plot(EKF_x, EKF_y, label="EKF (known map)", color="red")
scan_x = [s[0] for s in scan_states]
scan_y = [s[1] for s in scan_states]
plt.scatter(scan_x,
scan_y,
marker="x",
label="measurement update",
color="blue")
if show_plot:
plt.legend(loc=0)
plt.show()
class LocalizationVisualizer:
def __init__(self):
rospy.init_node('turtlebot_localization')
## Use simulation time (i.e. get time from rostopic /clock)
rospy.set_param('use_sim_time', 'true')
rate = rospy.Rate(10)
while rospy.Time.now() == rospy.Time(0):
rate.sleep()
## Get transformation of camera frame with respect to the base frame
self.tfBuffer = tf2_ros.Buffer()
self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
while True:
try:
# notably camera_link and not camera_depth_frame below, not sure why
self.raw_base_to_camera = self.tfBuffer.lookup_transform(
"base_footprint", "base_scan", rospy.Time()).transform
break
except tf2_ros.LookupException:
rate.sleep()
rotation = self.raw_base_to_camera.rotation
translation = self.raw_base_to_camera.translation
tf_theta = get_yaw_from_quaternion(rotation)
self.base_to_camera = [translation.x, translation.y, tf_theta]
## Colocate the `ground_truth` and `base_footprint` frames for visualization purposes
tf2_ros.StaticTransformBroadcaster().sendTransform(
create_transform_msg((0, 0, 0), (0, 0, 0, 1), "ground_truth",
"base_footprint"))
## Initial state for EKF
self.EKF = None
self.EKF_time = None
self.current_control = np.zeros(2)
self.latest_pose = None
self.latest_pose_time = None
self.controls = deque()
self.scans = deque()
## Set up publishers and subscribers
self.tfBroadcaster = tf2_ros.TransformBroadcaster()
rospy.Subscriber('/scan', LaserScan, self.scan_callback)
rospy.Subscriber('/cmd_vel', Twist, self.control_callback)
rospy.Subscriber('/gazebo/model_states', ModelStates,
self.state_callback)
self.ground_truth_ct = 0
def scan_callback(self, msg):
if self.EKF:
N = len(msg.ranges)
######## ADDED ARTIFICAL NOISE ################
noise = np.random.normal(0, 0.2, N)
self.scans.append((msg.header.stamp,
np.array([
i * msg.angle_increment + msg.angle_min
for i in range(len(msg.ranges))
]), np.array(msg.ranges) + noise))
def control_callback(self, msg):
if self.EKF:
self.controls.append(
(rospy.Time.now(), np.array([msg.linear.x, msg.angular.z])))
def state_callback(self, msg):
self.ground_truth_ct = self.ground_truth_ct + 1
# `rostopic hz /gazebo/model_states` = 1000; let's broadcast the transform at 20Hz to reduce lag
if self.ground_truth_ct % 50 == 0:
self.latest_pose_time = rospy.Time.now()
self.latest_pose = msg.pose[msg.name.index("turtlebot3_burger")]
self.tfBroadcaster.sendTransform(
create_transform_msg(
(self.latest_pose.position.x, self.latest_pose.position.y,
0), (self.latest_pose.orientation.x,
self.latest_pose.orientation.y,
self.latest_pose.orientation.z,
self.latest_pose.orientation.w), "base_footprint",
"world", self.latest_pose_time))
def run(self):
rate = rospy.Rate(100)
while not self.latest_pose:
rate.sleep()
x0 = np.array([
self.latest_pose.position.x, self.latest_pose.position.y,
get_yaw_from_quaternion(self.latest_pose.orientation)
])
self.EKF_time = self.latest_pose_time
self.EKF = Localization_EKF(x0, NoiseParams["P0"], NoiseParams["Q"],
MapParams, self.base_to_camera,
NoiseParams["g"])
self.OLC = Localization_EKF(x0, NoiseParams["P0"], NoiseParams["Q"],
MapParams, self.base_to_camera,
NoiseParams["g"])
while True:
if not self.scans:
rate.sleep()
continue
while self.controls and self.controls[0][0] <= self.scans[0][0]:
next_timestep, next_control = self.controls.popleft()
if next_timestep < self.EKF_time: # guard against time weirdness (msgs out of order)
continue
self.EKF.transition_update(
self.current_control,
next_timestep.to_time() - self.EKF_time.to_time())
self.OLC.transition_update(
self.current_control,
next_timestep.to_time() - self.EKF_time.to_time())
self.EKF_time, self.current_control = next_timestep, next_control
self.tfBroadcaster.sendTransform(
create_transform_msg(
(self.EKF.x[0], self.EKF.x[1], 0),
tf.transformations.quaternion_from_euler(
0, 0,
self.EKF.x[2]), "EKF", "world", self.EKF_time))
self.tfBroadcaster.sendTransform(
create_transform_msg(
(self.OLC.x[0], self.OLC.x[1], 0),
tf.transformations.quaternion_from_euler(
0, 0, self.OLC.x[2]), "open_loop", "world",
self.EKF_time))
scan_time, theta, rho = self.scans.popleft()
if scan_time < self.EKF_time:
continue
self.EKF.transition_update(
self.current_control,
scan_time.to_time() - self.EKF_time.to_time())
self.OLC.transition_update(
self.current_control,
scan_time.to_time() - self.EKF_time.to_time())
self.EKF_time = scan_time
alpha, r, C_AR, _, _ = ExtractLines(theta, rho,
LineExtractionParams,
NoiseParams["var_theta"],
NoiseParams["var_rho"])
Z = np.vstack((alpha, r))
self.EKF.measurement_update(Z, C_AR)
while len(
self.scans
) > 1: # keep only the last element in the queue, if we're falling behind
self.scans.popleft()
