From a563b78484e6d7407bc4900a69825bdd1fdd920b Mon Sep 17 00:00:00 2001 From: Jonah Arthur Kaye Date: Tue, 13 Apr 2021 17:24:00 -0500 Subject: [PATCH 01/10] test --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 8ccad58..0e3e4ff 100644 --- a/README.md +++ b/README.md @@ -1 +1,5 @@ # particle_filter_localization_project + + +test + From d9c1be65ea8910008239cb6633f16f5bdb61755d Mon Sep 17 00:00:00 2001 From: yoonsjeong Date: Tue, 13 Apr 2021 17:48:08 -0500 Subject: [PATCH 02/10] added readme --- README.md | 42 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 0e3e4ff..65235dd 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,45 @@ # particle_filter_localization_project +## Implementation Plan -test +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. From 29d23139d03b9d974ecf424ce99536c1f2a8a71f Mon Sep 17 00:00:00 2001 From: Jonah Arthur Kaye Date: Sun, 18 Apr 2021 17:22:11 -0500 Subject: [PATCH 03/10] d --- scripts/.vscode/c_cpp_properties.json | 27 +++++++++++++++++++++++++++ scripts/.vscode/settings.json | 6 ++++++ scripts/particle_filter.py | 0 3 files changed, 33 insertions(+) create mode 100644 scripts/.vscode/c_cpp_properties.json create mode 100644 scripts/.vscode/settings.json mode change 100644 => 100755 scripts/particle_filter.py diff --git a/scripts/.vscode/c_cpp_properties.json b/scripts/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..a7c39ca --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/scripts/.vscode/settings.json b/scripts/.vscode/settings.json new file mode 100644 index 0000000..71a9cf6 --- /dev/null +++ b/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" + ] +} \ No newline at end of file diff --git a/scripts/particle_filter.py b/scripts/particle_filter.py old mode 100644 new mode 100755 From 24b0b9ffe9d8f6470cf6c3845b476f972805caee Mon Sep 17 00:00:00 2001 From: Yoon Jeong Date: Sun, 18 Apr 2021 17:22:19 -0500 Subject: [PATCH 04/10] added likelihood field --- scripts/likelihood_field.py | 100 ++++++++++++++++++++++++++++++++++++ 1 file changed, 100 insertions(+) create mode 100644 scripts/likelihood_field.py diff --git a/scripts/likelihood_field.py b/scripts/likelihood_field.py new file mode 100644 index 0000000..2341024 --- /dev/null +++ b/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') From b50f20f3cd9c4260886a61140b761ba22d2d13c6 Mon Sep 17 00:00:00 2001 From: Jonah Arthur Kaye Date: Sun, 18 Apr 2021 18:16:32 -0500 Subject: [PATCH 05/10] update --- scripts/particle_filter.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/scripts/particle_filter.py b/scripts/particle_filter.py index 53a5325..ff8b01d 100755 --- a/scripts/particle_filter.py +++ b/scripts/particle_filter.py @@ -260,6 +260,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__": From a9b703f515d7137749c3571286a72cff38f93911 Mon Sep 17 00:00:00 2001 From: Jonah Arthur Kaye Date: Sun, 18 Apr 2021 18:56:03 -0500 Subject: [PATCH 06/10] update particle weights --- scripts/particle_filter.py | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/scripts/particle_filter.py b/scripts/particle_filter.py index ff8b01d..4a89fe2 100755 --- a/scripts/particle_filter.py +++ b/scripts/particle_filter.py @@ -248,10 +248,24 @@ def update_estimated_robot_pose(self): 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): From 4bd14833df8acd0fea0e316aa82f8b8c11f66f13 Mon Sep 17 00:00:00 2001 From: Jonah Arthur Kaye Date: Sun, 18 Apr 2021 18:58:02 -0500 Subject: [PATCH 07/10] normalize_particles --- scripts/particle_filter.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/scripts/particle_filter.py b/scripts/particle_filter.py index 4a89fe2..bb17e64 100755 --- a/scripts/particle_filter.py +++ b/scripts/particle_filter.py @@ -133,10 +133,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): From ca1dd99d97e83229db26572dde2d0645ec44a986 Mon Sep 17 00:00:00 2001 From: Jonah Arthur Kaye Date: Sun, 18 Apr 2021 19:29:06 -0500 Subject: [PATCH 08/10] put stuff down in all the functions except initialize particles --- scripts/particle_filter.py | 41 +++++++++++++++++++++++++++++++------- 1 file changed, 34 insertions(+), 7 deletions(-) diff --git a/scripts/particle_filter.py b/scripts/particle_filter.py index bb17e64..3e2be7d 100755 --- a/scripts/particle_filter.py +++ b/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: @@ -124,8 +130,6 @@ def get_map(self, data): def initialize_particle_cloud(self): # TODO - - self.normalize_particles() self.publish_particle_cloud() @@ -164,8 +168,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): @@ -243,13 +249,34 @@ 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): - cardinal_directions_idxs = [0, 45 , 90, , 135, 180, 225, 270, 315] + 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: From e9b7870930eea35d16aed2223c6964bf4353eec5 Mon Sep 17 00:00:00 2001 From: yoonsjeong Date: Mon, 19 Apr 2021 12:47:42 -0500 Subject: [PATCH 09/10] wrote init particle cloud func --- scripts/particle_filter.py | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/scripts/particle_filter.py b/scripts/particle_filter.py index 53a5325..1999653 100755 --- a/scripts/particle_filter.py +++ b/scripts/particle_filter.py @@ -120,12 +120,29 @@ 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: + theta = random.uniform(0, np.pi) + xy.append(theta) + self.particle_cloud.append(xy) + self.normalize_particles() self.publish_particle_cloud() From 0d48855310f2b3c8cb81a6f440487a674e1a60c9 Mon Sep 17 00:00:00 2001 From: yoonsjeong Date: Mon, 19 Apr 2021 13:14:20 -0500 Subject: [PATCH 10/10] added pose --- scripts/particle_filter.py | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/scripts/particle_filter.py b/scripts/particle_filter.py index 1999653..31c2d70 100755 --- a/scripts/particle_filter.py +++ b/scripts/particle_filter.py @@ -128,7 +128,7 @@ def get_valid_coords(self): for i in range(res * width): for j in range(res * height): if grid[i][j] == 100: - valid_coords.append([i, j]) + valid_coords.append((i, j)) return valid_coords def initialize_particle_cloud(self): @@ -139,10 +139,24 @@ def initialize_particle_cloud(self): self.particle_cloud = [] for xy in cloud_xy: + pos_x, pos_y = xy theta = random.uniform(0, np.pi) - xy.append(theta) - self.particle_cloud.append(xy) - + 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() self.publish_particle_cloud()