Error logging

autograder/solutions_go_here.py

@@ -12,18 +12,18 @@ def answer_to_1i():
     """
     Return your answer to 1i in a python list, [x, y, theta]
     """
-    return [0, 0, 0]
+    return [0.2232,-0.0134,0.0524]
 
 def answer_to_1ii():
     """
     Return your answer to 1ii in a python list, [x, y, theta]
     """
-    return [0, 0, 0]
+    return [3.1232, 4.1866, 1.0996]
 
 def answer_to_2():
     """
     Return your answers to 2 in a python list for the values z=0,3,5,8,10
     Each value should be a float
     """
-    return [0, 0, 0, 0, 0]
+    return [0.032, 0.0234, 0.0179, 0.0890, 0.7034]
 

params.yaml

@@ -22,4 +22,7 @@ odom_topic: "/odom"
 particle_filter_frame: "/base_link_pf"
 
 # Deterministic flag; set to true to remove all added noise
-deterministic: true
+deterministic: false
+
+# Lidar Scale
+lidar_scale_to_map_scale: 1.0

src/localization/motion_model.py

@@ -1,13 +1,10 @@
+import rospy
+import numpy as np
 class MotionModel:
 
     def __init__(self):
-
         ####################################
-        # TODO
-        # Do any precomputation for the motion
-        # model here.
-
-        pass
+        self.deterministic = rospy.get_param("~deterministic", True)
 
         ####################################
 
@@ -31,8 +28,29 @@ def evaluate(self, particles, odometry):
         """
 
         ####################################
-        # TODO
 
-        raise NotImplementedError
+        # number of particles
+        n = len(particles)
+
+        # extract columns from particles
+        x_old = particles[:,0]
+        y_old = particles[:,1]
+        theta_old = particles[:,2]
+
+        c = np.cos(theta_old) # column of all cos's of thetas
+        s = np.sin(theta_old) # column of all sin's of thetas
+
+        # calculate delta column vectors explicitly instead of using rotation matrices
+        d_x = np.reshape( odometry[0]*c - odometry[1]*s ,(n,1))
+        d_y = np.reshape( odometry[0]*s + odometry[1]*c ,(n,1))
 
+        # make column vector of d_theta
+        d_thetas = np.tile(odometry[2], (n,1))
+
+        # combine all the delta column vectors into an n*3 matrix
+        # np.stack defaults to an n*3*1 matrix for whatever reason
+        delta = np.reshape( np.stack([d_x, d_y,d_thetas], axis=1), (n,3) )
+
+        return particles + delta
+
         ####################################

src/localization/particle_filter.py

@@ -4,17 +4,41 @@
 from sensor_model import SensorModel
 from motion_model import MotionModel
 
-from sensor_msgs.msg import LaserScan
+import numpy as np
+import traceback
+import tf
+
+from sensor_msgs.msg import  LaserScan
 from nav_msgs.msg import Odometry
 from geometry_msgs.msg import PoseWithCovarianceStamped
+from geometry_msgs.msg import Pose
+import scipy
+from scipy.stats import circmean
+from geometry_msgs.msg import PoseArray
 
 
 class ParticleFilter:
+    # Jank as hell thread safety
+    lidar_lock = True
+    odom_lock = True
+    odom_prev_time = 0
+
+
+    # TODO: tune! This is the noise that sets the starting cloud
+    init_noise = [2., 2., np.pi]
+    # TODO: tune! This is the noise that shifts our points around when processing odometry particle
+    particle_noise = [0.5,0.5,0.2]
+    odom_noise = [0.5,0.5,0.2]
+
 
     def __init__(self):
         # Get parameters
-        self.particle_filter_frame = \
-                rospy.get_param("~particle_filter_frame")
+        self.particle_filter_frame = rospy.get_param("~particle_filter_frame")
+        self.num_particles = rospy.get_param("~num_particles")
+        self.num_beams_per_particle = rospy.get_param("~num_beams_per_particle")
+        self.scan_field_of_view = rospy.get_param("~scan_field_of_view")
+        self.scan_theta_discretization = rospy.get_param("~scan_theta_discretization")
+        self.deterministic = rospy.get_param("~deterministic")
 
         # Initialize publishers/subscribers
         #
@@ -28,10 +52,10 @@ def __init__(self):
         scan_topic = rospy.get_param("~scan_topic", "/scan")
         odom_topic = rospy.get_param("~odom_topic", "/odom")
         self.laser_sub = rospy.Subscriber(scan_topic, LaserScan,
-                                          YOUR_LIDAR_CALLBACK, # TODO: Fill this in
+                                          self.lidar_callback,
                                           queue_size=1)
         self.odom_sub  = rospy.Subscriber(odom_topic, Odometry,
-                                          YOUR_ODOM_CALLBACK, # TODO: Fill this in
+                                          self.odom_callback,
                                           queue_size=1)
 
         #  *Important Note #2:* You must respond to pose
@@ -40,7 +64,7 @@ def __init__(self):
         #     "Pose Estimate" feature in RViz, which publishes to
         #     /initialpose.
         self.pose_sub  = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped,
-                                          YOUR_POSE_INITIALIZATION_CALLBACK, # TODO: Fill this in
+                                          self.pose_init_callback,
                                           queue_size=1)
 
         #  *Important Note #3:* You must publish your pose estimate to
@@ -50,11 +74,17 @@ def __init__(self):
         #     odometry you publish here should be with respect to the
         #     "/map" frame.
         self.odom_pub  = rospy.Publisher("/pf/pose/odom", Odometry, queue_size = 1)
+        self.cloud_pub  = rospy.Publisher("/pf/pose/cloud", PoseArray, queue_size = 1)
+        self.listener = tf.TransformListener()
 
         # Initialize the models
         self.motion_model = MotionModel()
         self.sensor_model = SensorModel()
 
+        self.odom_prev_time = rospy.get_time()
+        self.particles = np.zeros([self.num_particles,3])
+        self.lidar_lock = False
+        self.odom_lock = False
         # Implement the MCL algorithm
         # using the sensor model and the motion model
         #
@@ -64,9 +94,180 @@ def __init__(self):
         #
         # Publish a transformation frame between the map
         # and the particle_filter_frame.
+        self.s = "/home/racecar/error_log_localization.csv"
+
+        with open(self.s, "w") as self.error_log:
+            self.error_log.write("")
+        self.error_log = open(self.s, "a")
+
+    def lidar_callback(self, data):
+
+        # an instance of the odom function is already running, wait for it to finish
+        while self.odom_lock:
+            rospy.sleep(0.001)
+
+        # an instance of the lidar function is already running, don't evaluate this lidar message
+        if self.lidar_lock:
+            return
+
+        # sensor model is not initialized
+        if not self.sensor_model.map_set:
+            return
+
+        # claim the lidar lock. No other processes will be created until this lock is released
+        self.lidar_lock = True
+
+        try:
+            # downsample lidar data to num_beams_per_particle 
+            thetas = np.linspace(-self.scan_field_of_view/2., self.scan_field_of_view/2., self.num_beams_per_particle)
+
+            # retrieve ranges at desired angles
+            angle_range = data.angle_max - data.angle_min
+            n = len(data.ranges)
+
+            theta_indices = ((thetas - data.angle_min) * n / angle_range).astype(int)
+            observation = np.array(data.ranges)[theta_indices]
+
+            # compute probabilities
+            probabilities = self.sensor_model.evaluate(self.particles, observation)
+
+            # normalize
+            probabilities = probabilities / np.sum(probabilities)   
+
+            # resample point cloud
+            particle_indices = np.random.choice(self.num_particles, p=probabilities, size=self.num_particles).astype(int)
+            self.particles = self.particles[particle_indices]
+
+        except Exception as e:
+            rospy.logwarn("lidar_callback threw an exception")
+            rospy.logwarn(e)
+            traceback.print_exc()
+        finally:
+            # release the lidar_lock, so other lidar_callback processes can be created
+            self.lidar_lock = False
+
+    def odom_callback(self, data):
+        # someone else is using the particle array right now, don't touch that data
+        if self.lidar_lock or self.odom_lock:
+            return
+
+        self.odom_lock = True
+        try:
+            # find delta time
+            time = rospy.get_time()
+            dt = time - self.odom_prev_time
+            self.odom_prev_time = time
+
+            # get odom
+            odom = np.array([data.twist.twist.linear.x, data.twist.twist.linear.y, data.twist.twist.angular.z]) * dt
+            odom[0] += np.random.normal()*self.odom_noise[0]
+            odom[1] += np.random.normal()*self.odom_noise[1]
+            odom[2] += np.random.normal()*self.odom_noise[2]
+            # update the point cloud
+            self.particles = self.motion_model.evaluate(self.particles, odom)
+
+            # add noise
+            if not self.deterministic:
+                self.particles += self.generate_noise(self.particle_noise)
+
+            # publish results
+            self.publish_poses()
+        except Exception as e:
+            rospy.logwarn("odom_callback threw an exception")
+            rospy.logwarn(e)
+            traceback.print_exc()
+        finally:
+            self.odom_lock = False
+
+    def log_error(self):
+        self.listener.waitForTransform("base_link", "map", rospy.Time(), rospy.Duration(4.0))
+        t = self.listener.getLatestCommonTime("base_link", "map")
+        exp_position, exp_quaternion = self.listener.lookupTransform("base_link", "map", t)
+        exp_angle = tf.transformations.euler_from_quaternion(exp_quaternion)[2]
+        act_position = [self.avg_x, self.avg_y, 0]
+        act_angle = self.avg_theta
+        x_offset = act_position[0]-exp_position[0]
+        y_offset = act_position[1]-exp_position[1]
+        theta_offset = act_angle-exp_angle
+        self.error_log.write(str(rospy.get_rostime())+",")
+        self.error_log.write(str(x_offset)+",")
+        self.error_log.write(str(y_offset)+",")
+        self.error_log.write(str(theta_offset)+","+"\n")
+
+    def pose_init_callback(self, data):
+        # Pull position from data
+        pose = data.pose.pose
+        q = pose.orientation
+
+        theta = tf.transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
+        center_particle = [pose.position.x, pose.position.y, theta]
+
+        # Pull covariance from data
+        # covariance = data.pose.covariance
+        # c_x = covariance[0]      +1
+        # c_y = covariance[1]      +1
+        # c_theta = covariance[5]  +1
+
+        # Combine to set particle array
+        noise = self.generate_noise(self.init_noise)
+        self.particles = noise + center_particle
+
+    def generate_noise(self, weights):
+        noise = np.random.normal(size=[self.num_particles, 3])
+        return noise * weights
+
+    def publish_poses(self):
+        # publish the average point
+        self.avg_x = np.mean(self.particles[:,0])
+        self.avg_y = np.mean(self.particles[:,1])
+        self.avg_theta = scipy.stats.circmean(self.particles[:,2])
+
+        msg = Odometry()
+        msg.header.stamp = rospy.get_rostime()
+        msg.header.frame_id = "map"
+        # msg.child_frame_id = ""
+        msg.pose.pose.position.x = self.avg_x
+        msg.pose.pose.position.y = self.avg_y
+        msg.pose.pose.position.z = 0
+        quat = tf.transformations.quaternion_from_euler(0,0,self.avg_theta)
+        msg.pose.pose.orientation.x = quat[0]
+        msg.pose.pose.orientation.y = quat[1]
+        msg.pose.pose.orientation.z = quat[2]
+        msg.pose.pose.orientation.w = quat[3]
+        self.odom_pub.publish(msg)
+        self.log_error()
+
+        # Publish transform
+        br = tf.TransformBroadcaster()
+        br.sendTransform((self.avg_x, self.avg_y, 0),
+            tf.transformations.quaternion_from_euler(0, 0, self.avg_theta),
+            rospy.Time.now(),
+            "base_link_pf",
+            "map")
+
+        # Publish all the particles
+        msg = PoseArray()
+
+        msg.header.stamp = rospy.get_rostime()
+        msg.header.frame_id = "map"
+        # msg.child_frame_id = ""
+        msg.poses = []
+        for i in range(self.num_particles):
+            msg.poses.append( Pose() )
+            msg.poses[i].position.x = self.particles[i][0]
+            msg.poses[i].position.y = self.particles[i][1]
+            quat = tf.transformations.quaternion_from_euler(0, 0, self.particles[i][2])
+            msg.poses[i].orientation.x = quat[0]
+            msg.poses[i].orientation.y = quat[1]
+            msg.poses[i].orientation.z = quat[2]
+            msg.poses[i].orientation.w = quat[3]
+        self.cloud_pub.publish(msg)
 
 
 if __name__ == "__main__":
     rospy.init_node("particle_filter")
     pf = ParticleFilter()
+    # rospy.sleep(3)
     rospy.spin()
+    # while not rospy.is_shutdown():
+    #     rospy.sleep(0.5)