Init func

README.md

@@ -1 +1,45 @@
 # particle_filter_localization_project
+
+## Implementation Plan
+
+1. The names of your team members
+Jonah Kaye, Yoon Jeong
+
+1. How you will initialize your particle cloud (initialize_particle_cloud)?
+
+    **Implementation:** We will generate uniformly random points and put it into an array called `self.particle_cloud` using the Python "random" library. We will use a for loop to do this. We will use as many random points as possible without compromising on efficiency. 
+
+    **Test:** We will do an eye check by printing the variable (using "echo") to make sure that it appears random and is within the bounds of what we have set. We can print the length of the array to ensure that it has the number of initial points we expect.
+1. How you will update the position of the particles will be updated based on the movements of the robot (update_particles_with_motion_model)?
+
+    **Implementation:** We will subscribe to the odometer rostopic and add the distance travelled to the particles in the particle cloud, and will also add Gaussian noise by using the Python random library. We will use a for loop to iterate through the particle cloud.
+
+    **Test:** We will instruct our robot to go a certain distance, and as above, echo our particle cloud array variable and do an eyeball check to see that the points have been updated.
+1. How you will compute the importance weights of each particle after receiving the robot's laser scan data?(update_particle_weights_with_measurement_model)?
+
+    **Implementation:** We will subscribe to the scan rostopic and convert the ms.ranges into the z_t(m) value. Then, we will use the measurement weight equation from class, taking the sum of 1/abs(Z_t(i) - Z_t(m)) and then we will put it into an ordered weight list that is stored in the class.
+
+    **Test:** We will take an example, having instructed our robot to go only a certain distance, and then check by hand that the robot calculations are correct. 
+
+1. How you will normalize the particles' importance weights (normalize_particles) and resample the particles (resample_particles)?
+
+    **Implementation:** To normalize the weights of the particle, we will sum up all the particle weights and then take the inverse, and then multiply each weight by that inverse. We will use a for loop to iterate through the particle weights. There is a python function in the "random" library called `choices` that will allow us to select weights to resample with the appropriate probabilities.
+
+    **Test:** To test that normalization worked properly, we can check to see that the list sums up to 1. To see if the resampling is working properly we can see if the visualization we create is converging upon certain peaks and removing a good number of particles per time step.
+1. How you will update the estimated pose of the robot (update_estimated_robot_pose)?
+
+    **Implementation:** We will take the average of all the points that have been resampled in order to find the estimated pose.
+
+    **Test:** To test this, we will put it into rviz and do an eyeball check to see that the pose approximates the cloud of particles.
+1. How you will incorporate noise into your particle filter localization?
+
+    **Implementation:** We will be incorporating noise by using the normal function available from the Python random library.
+
+    **Test:** To test this, we will see if rviz produces a cloud of particles rather than slowly converging on individual points.
+
+1. A brief timeline sketching out when you would like to have accomplished each of the components listed above.
+
+**By this weekend:** We want to be fully finished with `initialize_particle_cloud` (Yoon) and `update_particles_with_motion_model` (Jonah). We want to have started `update_particle_weights_with_measurement_model` (Yoon) and `normalize_particles` (Jonah) and `resample_particles` (Jonah).
+**By next Wednesday:** We want to be finished with writing all the coding aspects and be working on debugging.
+This timeline may change as we work on it.
+

scripts/.vscode/c_cpp_properties.json

@@ -0,0 +1,27 @@
+{
+    "configurations": [
+        {
+            "browse": {
+                "databaseFilename": "",
+                "limitSymbolsToIncludedHeaders": true
+            },
+            "includePath": [
+                "/home/jonahkaye/catkin_ws/devel/include/**",
+                "/opt/ros/noetic/include/**",
+                "/home/jonahkaye/catkin_ws/src/open_manipulator_dependencies/roboticsgroup_gazebo_plugins/include/**",
+                "/home/jonahkaye/catkin_ws/src/turtlebot3_simulations/turtlebot3_fake/include/**",
+                "/home/jonahkaye/catkin_ws/src/turtlebot3_simulations/turtlebot3_gazebo/include/**",
+                "/home/jonahkaye/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_bringup/include/**",
+                "/home/jonahkaye/catkin_ws/src/turtlebot3_manipulation/turtlebot3_manipulation_gui/include/**",
+                "/home/jonahkaye/catkin_ws/src/turtlebot3/turtlebot3_slam/include/**",
+                "/usr/include/**"
+            ],
+            "name": "ROS",
+            "intelliSenseMode": "gcc-x64",
+            "compilerPath": "/usr/bin/clang",
+            "cStandard": "c11",
+            "cppStandard": "c++14"
+        }
+    ],
+    "version": 4
+}

scripts/.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/jonahkaye/catkin_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

scripts/likelihood_field.py

@@ -0,0 +1,100 @@
+""" An implementation of an likelihood field for particle filter use """
+""" Written by Paul Ruvolo for his Comp Robo 2020 course """
+
+import rospy
+
+from nav_msgs.srv import GetMap
+import numpy as np
+from sklearn.neighbors import NearestNeighbors
+
+
+class LikelihoodField(object):
+    """ Stores an likelihood field for an input map.  An likelihood field returns
+        the distance to the closest obstacle for any coordinate in the map
+        Attributes:
+            map: the map to localize against (nav_msgs/OccupancyGrid)
+            closest_occ: the distance for each entry in the OccupancyGrid to
+            the closest obstacle
+    """
+
+    def __init__(self):
+        # grab the map from the map server
+        rospy.wait_for_service("static_map")
+        static_map = rospy.ServiceProxy("static_map", GetMap)
+        self.map = static_map().map
+
+        # The coordinates of each grid cell in the map
+        X = np.zeros((self.map.info.width*self.map.info.height, 2))
+
+        # while we're at it let's count the number of occupied cells
+        total_occupied = 0
+        curr = 0
+        for i in range(self.map.info.width):
+            for j in range(self.map.info.height):
+                # occupancy grids are stored in row major order
+                ind = i + j*self.map.info.width
+                if self.map.data[ind] > 0:
+                    total_occupied += 1
+                X[curr, 0] = float(i)
+                X[curr, 1] = float(j)
+                curr += 1
+
+        # The coordinates of each occupied grid cell in the map
+        occupied = np.zeros((total_occupied, 2))
+        curr = 0
+        for i in range(self.map.info.width):
+            for j in range(self.map.info.height):
+                # occupancy grids are stored in row major order
+                ind = i + j*self.map.info.width
+                if self.map.data[ind] > 0:
+                    occupied[curr, 0] = float(i)
+                    occupied[curr, 1] = float(j)
+                    curr += 1
+        # use super fast scikit learn nearest neighbor algorithm
+        nbrs = NearestNeighbors(n_neighbors=1,
+                                algorithm="ball_tree").fit(occupied)
+        distances, indices = nbrs.kneighbors(X)
+
+        self.closest_occ = np.zeros((self.map.info.width, self.map.info.height))
+        curr = 0
+        for i in range(self.map.info.width):
+            for j in range(self.map.info.height):
+                self.closest_occ[i, j] = \
+                    distances[curr][0]*self.map.info.resolution
+                curr += 1
+        self.occupied = occupied
+
+    def get_obstacle_bounding_box(self):
+        """
+        Returns: the upper and lower bounds of x and y such that the resultant
+        bounding box contains all of the obstacles in the map.  The format of
+        the return value is ((x_lower, x_upper), (y_lower, y_upper))
+        """
+        lower_bounds = self.occupied.min(axis=0)
+        upper_bounds = self.occupied.max(axis=0)
+        r = self.map.info.resolution
+        return ((lower_bounds[0]*r + self.map.info.origin.position.x,
+                 upper_bounds[0]*r + self.map.info.origin.position.x),
+                (lower_bounds[1]*r + self.map.info.origin.position.y,
+                 upper_bounds[1]*r + self.map.info.origin.position.y))
+
+    def get_closest_obstacle_distance(self, x, y):
+        """ Compute the closest obstacle to the specified (x,y) coordinate in
+            the map.  If the (x,y) coordinate is out of the map boundaries, nan
+            will be returned. """
+        x_coord = (x - self.map.info.origin.position.x)/self.map.info.resolution
+        y_coord = (y - self.map.info.origin.position.y)/self.map.info.resolution
+        if type(x) is np.ndarray:
+            x_coord = x_coord.astype(np.int)
+            y_coord = y_coord.astype(np.int)
+        else:
+            x_coord = int(x_coord)
+            y_coord = int(y_coord)
+
+        is_valid = (x_coord >= 0) & (y_coord >= 0) & (x_coord < self.map.info.width) & (y_coord < self.map.info.height)
+        if type(x) is np.ndarray:
+            distances = np.float('nan')*np.ones(x_coord.shape)
+            distances[is_valid] = self.closest_occ[x_coord[is_valid], y_coord[is_valid]]
+            return distances
+        else:
+            return self.closest_occ[x_coord, y_coord] if is_valid else float('nan')

scripts/particle_filter.py

@@ -38,8 +38,14 @@ def draw_random_sample():
     We recommend that you fill in this function using random_sample.
     """
     # TODO
-    return
+    weights = []
+    positions = []
+    for p in self.particle_cloud:
+        weights.append(p.w)
+        positions.append(p.pose)
 
+    randomList = random.choices(sampleList, cum_weights=(5, 15, 35, 65, 100), k=5)
+    return randomList
 
 class Particle:
 
@@ -120,11 +126,41 @@ def get_map(self, data):
 
         self.map = data
 
+    def get_valid_coords(self):
+        res, width, height = self.map.info.resolution, self.map.info.width, self.map.info.height 
+        grid = self.map.data
+
+        valid_coords = []
+        for i in range(res * width):
+            for j in range(res * height):
+                if grid[i][j] == 100:
+                    valid_coords.append((i, j))
+        return valid_coords
 
     def initialize_particle_cloud(self):
 
-        # TODO
-
+        sample_xy = self.get_valid_coords()
+        cloud_xy = random.sample(sample_xy, self.num_particles)
+
+        self.particle_cloud = []
+        for xy in cloud_xy:
+            pos_x, pos_y = xy
+            theta = random.uniform(0, np.pi)
+            or_x, or_y, or_z, or_w = quaternion_from_euler(0.0, 0.0, theta)
+
+            pose = Pose()
+            pose.position.x = pos_x
+            pose.position.y = pos_y
+            pose.position.z = 0
+
+            pose.orientation = Quaternion()
+            pose.orientation.x = or_x
+            pose.orientation.y = or_y
+            pose.orientation.z = or_z
+            pose.orientation.w = or_w
+
+            particle = Particle(pose, 1.0)
+            self.particle_cloud.append(particle)
 
         self.normalize_particles()
 
@@ -133,10 +169,11 @@ def initialize_particle_cloud(self):
 
     def normalize_particles(self):
         # make all the particle weights sum to 1.0
-
-        # TODO
-
-
+        sum = 0
+        for p in self.particle_cloud:
+            sum = sum + p.w
+        for p in self.particle_cloud:
+            p.w = p.w / sum
 
     def publish_particle_cloud(self):
 
@@ -163,8 +200,10 @@ def publish_estimated_robot_pose(self):
 
     def resample_particles(self):
 
-        # TODO
-
+        randomList = random_sample()
+        for p in range(len(self.particle_cloud)):
+            self.particle_cloud[p].pose = randomList[p]
+            self.particle_cloud[p].w = 1
 
 
     def robot_scan_received(self, data):
@@ -242,16 +281,51 @@ def robot_scan_received(self, data):
     def update_estimated_robot_pose(self):
         # based on the particles within the particle cloud, update the robot pose estimate
 
-        # TODO
-
+        x_sum = 0 
+        y_sum = 0
+        theta_sum
+
+        for p in self.particle_cloud:
+            x_sum = x_sum + p.pose.position.x
+            y_sum = y_sum + p.pose.position.y
+            theta_sum = theta_sum + euler_from_quaternion(p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w)
+
+        x_aver = x_sum / len(self.particle_cloud)
+        y_aver = y_sum / len(self.particle_cloud)
+        theta_aver = theta_sum / len(self.particle_cloud)
+
+        p = Pose()
+        p.position.x = x_aver
+        p.position.y = y_aver
+        p.position.z = 0
+        q = quaternion_from_euler(0.0, 0.0, theta_aver)
+        p.orientation.x = q[0]
+        p.orientation.y = q[1]
+        p.orientation.z = q[2]
+        p.orientation.w = q[3]
+        self.robot_estimate = p 
 
 
     def update_particle_weights_with_measurement_model(self, data):
 
-        # TODO
-
-
-
+        cardinal_directions_idxs = [0, 45 , 90, 135, 180, 225, 270, 315]
+        for p in self.particle_cloud:
+            q = 1
+            for idx in cardinal_directions_idxs:
+                # starting if condition
+                ztk = data.ranges[idx]
+                if ztk > 3.5: continue
+                # boilerplate vars
+                particle_x, particle_y = p.pose.position.x, p.pose.position.y
+                quat_in = [p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w]
+                theta_z = euler_from_quaternion(quat_in)[2]
+                theta_z += idx * np.pi / 180 # add curr cardinal direction (in rads)
+                # algorithm
+                x_ztk = particle_x + ztk * np.cos(theta_z)
+                y_ztk = particle_y + ztk * np.sin(theta_z)
+                dist = self.likelihood_field.get_closest_obstacle_distance(x_ztk, y_ztk)
+                q = q * compute_prob_zero_centered_gaussian(dist, sd=0.1) # recommended SD
+        p.w = q   
 
     def update_particles_with_motion_model(self):
 
@@ -260,6 +334,14 @@ def update_particles_with_motion_model(self):
 
         # TODO
 
+        for p in self.particle_cloud:
+            p.pose.position.x = p.pose.position.x + (self.curr_x - self.old_x)
+            p.pose.position.y = p.pose.posiiton.y + (self.curr_y - self.old_y)
+            q = quaternion_from_euler(0.0, 0.0, self.curr_yaw - self.old_yaw)
+            p.pose.orientation.x = p.pose.orientation.x + q[0]
+            p.pose.orientation.y = p.pose.orientation.y + q[1]
+            p.pose.orientation.z = p.pose.orientation.z + q[2]
+            p.pose.orientation.w = p.pose.orientation.w + q[3]
 
 
 if __name__=="__main__":