def validate_SLAM_EKF(fname="validation_run.p"): validate_localization_transition_update(fname, False) validation_run = pickle.load(open(fname, "rb")) x0_pose = validation_run["states"][0][1] P0_pose = NoiseParams["P0"] N_map_lines = MapParams.shape[1] x0_map = MapParams.T.flatten() x0_map[4:] = x0_map[4:] + np.vstack(( NoiseParams["std_alpha"] * np.random.randn(N_map_lines - 2), # first two lines fixed NoiseParams["std_r"] * np.random.randn(N_map_lines - 2))).T.flatten() P0_map = np.diag( np.concatenate( (np.zeros(4), np.array([ [NoiseParams["std_alpha"]**2 for i in range(N_map_lines - 2)], [NoiseParams["std_r"]**2 for i in range(N_map_lines - 2)] ]).T.flatten()))) EKF = SLAM_EKF(np.concatenate((x0_pose, x0_map)), scipy.linalg.block_diag(P0_pose, P0_map), NoiseParams["Q"], 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 (noisy map)", color="orange") 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") plt.legend(loc=0) plt.show()
class EKF_SLAM_Visualizer: def __init__(self): rospy.init_node('turtlebot_mapping') ## 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() N_map_lines = CityParams.shape[1] self.x0_map = CityParams.T.flatten() self.x0_map[4:] = self.x0_map[4:] + np.vstack(( NoiseParams["std_alpha"] * np.random.randn(N_map_lines - 2), # first two lines fixed NoiseParams["std_r"] * np.random.randn(N_map_lines - 2))).T.flatten() self.P0_map = np.diag( np.concatenate( (np.zeros(4), np.array([[ NoiseParams["std_alpha"]**2 for i in range(N_map_lines - 2) ], [NoiseParams["std_r"]**2 for i in range(N_map_lines - 2)]]).T.flatten()))) ## 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 self.ground_truth_map_pub = rospy.Publisher("ground_truth_map", Marker, queue_size=10) self.ground_truth_map_marker = Marker() self.ground_truth_map_marker.header.frame_id = "/world" self.ground_truth_map_marker.header.stamp = rospy.Time.now() self.ground_truth_map_marker.ns = "ground_truth" self.ground_truth_map_marker.type = 5 # line list self.ground_truth_map_marker.pose.orientation.w = 1.0 self.ground_truth_map_marker.scale.x = .025 self.ground_truth_map_marker.scale.y = .025 self.ground_truth_map_marker.scale.z = .025 self.ground_truth_map_marker.color.r = 0.0 self.ground_truth_map_marker.color.g = 1.0 self.ground_truth_map_marker.color.b = 0.0 self.ground_truth_map_marker.color.a = 1.0 self.ground_truth_map_marker.lifetime = rospy.Duration(1000) self.ground_truth_map_marker.points = sum( [[Point(p1[0], p1[1], 0), Point(p2[0], p2[1], 0)] for p1, p2 in CITY], []) self.EKF_map_pub = rospy.Publisher("EKF_map", Marker, queue_size=10) self.EKF_map_marker = deepcopy(self.ground_truth_map_marker) self.EKF_map_marker.color.r = 1.0 self.EKF_map_marker.color.g = 0.0 self.EKF_map_marker.color.b = 0.0 self.EKF_map_marker.color.a = 1.0 # self.lane_lines_map_pub = rospy.Publisher("lane_lines_map", Marker, queue_size=10) # self.lane_lines_map_marker = Marker() # self.lane_lines_map_marker.header.frame_id = "/world" # self.lane_lines_map_marker.header.stamp = rospy.Time.now() # self.lane_lines_map_marker.ns = "lane_lines" # self.lane_lines_map_marker.type = 5 # line list # self.lane_lines_map_marker.pose.orientation.w = 1.0 # self.lane_lines_map_marker.scale.x = .025 # self.lane_lines_map_marker.scale.y = .025 # self.lane_lines_map_marker.scale.z = .025 # self.lane_lines_map_marker.color.r = 1.0 # self.lane_lines_map_marker.color.g = 0.0 # self.lane_lines_map_marker.color.b = 0.0 # self.lane_lines_map_marker.color.a = 1.0 # self.lane_lines_map_marker.lifetime = rospy.Duration(1000) # self.lane_lines_map_marker.points = sum([[Point(p1[0], p1[1], 0), # Point(p2[0], p2[1], 0)] for p1, p2 in LANE_LINES_DASHED], []) def scan_callback(self, msg): if self.EKF: 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))) def control_callback(self, msg): if self.EKF: self.controls.append( (rospy.Time.now(), np.array([msg.linear.x, msg.angular.z]) + .1 * np.random.randn(2))) 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_pose = np.array([ self.latest_pose.position.x, self.latest_pose.position.y, get_yaw_from_quaternion(self.latest_pose.orientation) ]) P0_pose = NoiseParams["P0"] self.EKF_time = self.latest_pose_time self.EKF = SLAM_EKF(np.concatenate((x0_pose, self.x0_map)), scipy.linalg.block_diag(P0_pose, self.P0_map), NoiseParams["Q"], self.base_to_camera, 2 * NoiseParams["g"]) self.OLC = SLAM_EKF(np.concatenate((x0_pose, self.x0_map)), scipy.linalg.block_diag(P0_pose, self.P0_map), NoiseParams["Q"], self.base_to_camera, 2 * NoiseParams["g"]) while True: if not self.scans: rate.sleep() continue self.EKF_map_marker.points = [] for j in range((self.EKF.x.size - 3) / 2): p1, p2 = line_endpoints_from_alpha_and_r( self.EKF.x[3 + 2 * j], self.EKF.x[3 + 2 * j + 1]) self.EKF_map_marker.points.extend( [Point(p1[0], p1[1], 0), Point(p2[0], p2[1], 0)]) self.ground_truth_map_pub.publish(self.ground_truth_map_marker) self.EKF_map_pub.publish(self.EKF_map_marker) 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 run(self): rate = rospy.Rate(100) while not self.latest_pose: rate.sleep() x0_pose = np.array([ self.latest_pose.position.x, self.latest_pose.position.y, get_yaw_from_quaternion(self.latest_pose.orientation) ]) P0_pose = NoiseParams["P0"] self.EKF_time = self.latest_pose_time self.EKF = SLAM_EKF(np.concatenate((x0_pose, self.x0_map)), scipy.linalg.block_diag(P0_pose, self.P0_map), NoiseParams["Q"], self.base_to_camera, 2 * NoiseParams["g"]) self.OLC = SLAM_EKF(np.concatenate((x0_pose, self.x0_map)), scipy.linalg.block_diag(P0_pose, self.P0_map), NoiseParams["Q"], self.base_to_camera, 2 * NoiseParams["g"]) while True: if not self.scans: rate.sleep() continue self.EKF_map_marker.points = [] for j in range((self.EKF.x.size - 3) / 2): p1, p2 = line_endpoints_from_alpha_and_r( self.EKF.x[3 + 2 * j], self.EKF.x[3 + 2 * j + 1]) self.EKF_map_marker.points.extend( [Point(p1[0], p1[1], 0), Point(p2[0], p2[1], 0)]) self.ground_truth_map_pub.publish(self.ground_truth_map_marker) self.EKF_map_pub.publish(self.EKF_map_marker) 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()