From c30ece58c50f4e7b877cba46872bc5c43f6ba591 Mon Sep 17 00:00:00 2001 From: HaonanYin Date: Thu, 9 Nov 2017 14:12:04 -0500 Subject: [PATCH] For test --- corobot_localization/src/particle.py | 128 ++++++++++++++++++++++++++ corobot_localization/src/pf_haonan.py | 98 ++++++++++++++++++++ 2 files changed, 226 insertions(+) create mode 100644 corobot_localization/src/particle.py create mode 100644 corobot_localization/src/pf_haonan.py diff --git a/corobot_localization/src/particle.py b/corobot_localization/src/particle.py new file mode 100644 index 0000000..d2c73e6 --- /dev/null +++ b/corobot_localization/src/particle.py @@ -0,0 +1,128 @@ +import random +import math +from bresenhem import bresenhem +from kinect_loc import bres_condition + + +class Particle: + slots = ("x_pos", "y_pos", "orientation", "probability") + + def __init__(self, x_exact, y_exact, orient_exact, mu, x_sigma, y_sigma, orientation_sigma): + self.x_pos = x_exact + round(random.gauss(mu, x_sigma), 4) * random.randint(-1, 1) + self.y_pos = y_exact + round(random.gauss(mu, y_sigma), 4) * random.randint(-1, 1) + self.orientation = orient_exact + round(random.gauss(mu, orientation_sigma), 4) + self.probability = 1 / 500 + + def predict_update(self, trv_dist, new_orient, mu, dist_sigma, orient_sigma): + est_trv_dist = trv_dist + round(random.gauss(mu, dist_sigma), 4) * random.randint(-1, 1) + est_orient = new_orient + round(random.gauss(mu, orient_sigma), 4) * random.randint(-1, 1) + self.x_pos = math.cos(self.orientation + new_orient) * est_trv_dist + self.x_pos + self.y_pos = math.sin(self.orientation + new_orient) * est_trv_dist + self.y_pos + self.orientation = round(math.fmod(self.orientation + est_orient), (2 * math.pi)) + + def loc_check(self, map): + resolution = map.info.resolution + pixel_x = int(self.x_pos / resolution) + pixel_y = int(self.y_pos / resolution) + i = pixel_x + (map.info.height - pixel_y - 1) * map.info.width + occ = map.data[i] + if occ > 50: + return False + else: + return True + + def probability_update(self, map, laser_scan): + starting = laser_scan.angle_min + ending = laser_scan.angle_max + increment = laser_scan.angle_increment * 10 + + # Calculated the expected Laser Scan results using bresenhem algorithm + + # Do calculations w.r.t the images pixels + res = map.resolution + ht = map.info.height + + # should convert robot pose into kinect pose (offset backward ~9 cm) first + x_coord = self.x_pos / res + y_coord = ht - (self.y_pos / res) + starting_scan = self.orientation + starting + ending_scan = self.orientation + ending + + current_scan = starting_scan + + scan = [] + + # note theta is in original (right-handed) coords + # to follow the scan order, we will increment in this coord system + # but then negate the theta (or 2pi-theta) to do the image testing + while current_scan <= ending_scan: + scan_range = laser_scan.range_max / res + target_x = x_coord + scan_range * math.cos(current_scan) + target_y = y_coord + scan_range * math.sin(current_scan) + + if x_coord < 0: + x_coord = 0 + elif x_coord > map.info.width: + x_coord = map.info.width + else: + x_coord = int(x_coord) + + if y_coord < 0: + y_coord = 0 + elif y_coord > map.info.height: + y_coord = map.info.height + else: + y_coord = int(y_coord) + + if target_x < 0: + target_x = 0 + elif target_x > map.info.width: + target_x = map.info.width + else: + target_x = int(target_x) + + if target_y < 0: + target_y = 0 + elif target_y > map.info.height: + target_y = map.info.height + else: + target_y = int(target_y) + + distance = bresenhem(x_coord, y_coord, target_x, target_y, bres_condition) + + if distance[2] is True: + actual_dist = math.sqrt((distance[0] - x_coord) ** 2 + (distance[1] - y_coord) ** 2) * res + scan.append(actual_dist) + else: + scan.append(laser_scan.range_max) + + current_scan += increment + + # Compare expected and real data + dist_diff = [] + scan_idx = 0 + while scan_idx < len(laser_scan.ranges): + if laser_scan.ranges[scan_idx] < laser_scan.range_min\ + or laser_scan.ranges[scan_idx] > laser_scan.range_max: + dist_diff.append(1) + else: + dist_diff.append(laser_scan.ranges[scan_idx] / scan[scan_idx]) + scan_idx += 10 + + dist_idx = 0 + dist_exp = 0 + while dist_idx < len(dist_diff): + dist_exp += dist_diff[dist_idx] + dist_idx += 1 + dist_exp = dist_exp / len(dist_diff) + + if dist_exp <= 0.8: + self.probability = 0.4 + elif dist_exp < 0.95: + self.probability = (4/3) * dist_exp - (2 /3) + elif dist_exp <= 1.05: + self.probability = 0.6 + elif dist_exp <= 1.2: + self.probability = (-8 / 3) * dist_exp + 3.4 + else: + self.probability = 0.2 \ No newline at end of file diff --git a/corobot_localization/src/pf_haonan.py b/corobot_localization/src/pf_haonan.py new file mode 100644 index 0000000..9b8c0c4 --- /dev/null +++ b/corobot_localization/src/pf_haonan.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python +import roslib +import math + +import rospy +import particle + +from corobot_common.msg import Pose +from nav_msgs.msg import Odometry +from sensor_msgs.msg import LaserScan +from ekf import EKF +from corobot_qrcode_python import CSVReader +from corobot_common.srv import GetCoMap + + +def pf_initialize(pose): + # Get pose information + x_real = pose.x + y_real = pose.y + orientation = pose.theta + covariance = tuple(pose.cov.flat) + + particle_count = 0 + particles = [] + # Initialize 500 objects of particle + while particle_count < 500: + particles[particle_count] = Particle(x_real, y_real, orientation, 0, + covariance[0], covariance[4], covariance[8]) + particle_count += 1 + + +def prediction(odom): + odom_delta = ekf.get_odom_delta(odom) + if odom_delta is not None: + tuple_odom = odom_delta.T.tolist()[0] + else: + tuple_odom = tuple(0, 0, 0) + delta_x = tuple_odom[0] + delta_y = tuple_odom[1] + delta_theta = tuple_odom[2] + trv_dist = math.sqrt((delta_x ** 2) + (delta_y ** 2)) + + particle_count = 0 + mu = 0 + trv_sigma = 0.05 + orient_sigma = (0.5 / 360) * math.pi * 2 + for each_particle in particles: + each_particle.predict_update(trv_dist, delta_theta, 0, trv_sigma, orient_sigma) + if each_particle.loc_check(map) is False: + each_particle.probability = 0 + particle_count += 1 + + +def update_model(scan): + particle_count = 0 + while particle_count < len(particles): + if particles[particle_count].probability == 0: + particles.pop(particle_count) + else: + particles[particle_count].probability_update(scan, map) + if particles[particle_count].probability <= 0.2: + particles.pop(particle_count) + else: + particle_count += 1 + + sorted_particles = sorted(particles, key=lambda particle: particle.probability, reverse=True) + prtcle_idx = 0 + while len(particles) < 500: + particles.append(sorted_particles[prtcle_idx]) + prtcle_idx += 1 + + +def main(): + global pose_pub, ekf, particles, map + rospy.wait_for_service('get_map') + map_srv = rospy.ServiceProxy('get_map', GetCoMap) + map = map_srv().map + pose = Pose() + roslib.load_manifest("corobot_localization") + map_reader = CSVReader + rospy.init_node("localization") + ekf = EKF() + pose_pub = rospy.Publisher("pose", Pose) + # rospy.Subscriber("odom", Odometry, odom_callback) + # rospy.Subscriber("qrcode_pose", Pose, qrcode_callback) + rospy.Subscriber("qrcode_pose", Pose, pf_initialize) + rospy.Subscriber("odom", Odometry, prediction) + rospy.Subscriber("scan", LaserScan, update_model) + rospy.spin() + + pf_initialize() + + +if __name__ == "__main__": + try: + main() + except rospy.ROSInterruptException: + pass