SHARE
TWEET

Untitled

a guest Nov 16th, 2019 92 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2. import rospy
  3. import tf
  4. import tf.transformations as tr
  5. from std_msgs.msg import String, Header, ColorRGBA
  6. from nav_msgs.msg import OccupancyGrid, MapMetaData, Odometry
  7. from geometry_msgs.msg import Twist, PoseStamped, Point
  8. from sensor_msgs.msg import LaserScan
  9. from visualization_msgs.msg import Marker, MarkerArray
  10. from math import sqrt, cos, sin, pi, atan2
  11. from threading import Thread, Lock
  12. from math import pi, log, exp
  13. import random
  14. import numpy as np
  15. import sys
  16. import pickle
  17.  
  18.  
  19. class Particle(object):
  20.     def __init__(self, id, x,y, theta):
  21.         self.x = x
  22.         self.y = y
  23.         self.id = id
  24.         self.theta = theta
  25.  
  26. class ParticleFilter(object):
  27.     def __init__(self, num_particles, occ_grid_map, xmin, xmax, ymin, ymax,
  28.                  laser_min_range, laser_max_range, laser_min_angle, laser_max_angle,
  29.                  dynamics_translation_noise_std_dev,
  30.                  dynamics_orientation_noise_std_dev,
  31.                  beam_range_measurement_noise_std_dev):
  32.  
  33.         self.num_particles = num_particles
  34.         self.ogm = occ_grid_map
  35.         self.grid_map = np.array(self.ogm.data, dtype='int8')
  36.         self.grid_map = self.grid_map.reshape((self.ogm.info.height, self.ogm.info.width))
  37.         self.grid_bin = (self.grid_map == 0).astype('uint8')  # Cell is True iff probability of being occupied is zero
  38.  
  39.         # Workspace boundaries
  40.         self.xmax = xmax
  41.         self.xmin = xmin
  42.         self.ymin = ymin
  43.         self.ymax = ymax
  44.  
  45.         self.laser_max_angle = laser_max_angle
  46.         self.laser_min_angle = laser_min_angle
  47.         self.laser_max_range = laser_max_range
  48.         self.laser_min_range = laser_min_range
  49.  
  50.         # Std deviation of noise affecting translation in the dynamics model for particles
  51.         self.dynamics_translation_noise_std_dev = dynamics_translation_noise_std_dev
  52.  
  53.         # Std deviation of noise affecting orientation in the dynamics model for particles
  54.         self.dynamics_orientation_noise_std_dev = dynamics_orientation_noise_std_dev
  55.  
  56.         # Std deviation of noise affecting measured range from the laser measurement model
  57.         self.beam_range_measurement_noise_std_dev = beam_range_measurement_noise_std_dev
  58.  
  59.         # Number of laser beams to simulate when predicting what a
  60.         # particle's measurement is going to be
  61.         self.eval_beams = 32
  62.  
  63.         # Previous odometry measurement of the robot
  64.         self.last_robot_odom = None
  65.  
  66.         # Current odometry measurement of the robot
  67.         self.robot_odom = None
  68.  
  69.         # Relative motion since the last time particles were updated
  70.         self.dx = 0
  71.         self.dy = 0
  72.         self.dyaw = 0
  73.  
  74.         self.particles = []
  75.         self.weights = []
  76.  
  77.     def sigmoid(self, x):
  78.         """Numerically-stable sigmoid function."""
  79.         if x >= 0:
  80.             z = exp(-x)
  81.             return 1 / (1 + z)
  82.         else:
  83.             # if x is less than zero then z will be small, denom can't be
  84.             # zero because it's 1+z.
  85.             z = exp(x)
  86.             return z / (1 + z)
  87.  
  88.     def get_random_free_state(self):
  89.         while True:
  90.             # Note: we initialize particles closer to the robot's initial
  91.             # position in order to make the initialization easier
  92.             xrand = np.random.uniform(self.xmin*0.2, self.xmax*0.2)
  93.             yrand = np.random.uniform(self.ymin*0.2, self.ymax*0.2)
  94.             row, col = self.metric_to_grid_coords(xrand, yrand)
  95.             if self.grid_bin[row, col]:
  96.                 theta = np.random.uniform(0, 2*pi)
  97.                 return xrand, yrand, theta
  98.  
  99.     def init_particles(self):
  100.         """Initializes particles uniformly randomly with map frame coordinates,
  101.         within the boundaries set by xmin,xmax, ymin,ymax"""
  102.         for i in xrange(self.num_particles):
  103.             xrand, yrand, theta = self.get_random_free_state()
  104.             # Note: same orientation as the initial orientation of the robot
  105.             # to make initialization easier
  106.             self.particles.append(Particle(i, xrand, yrand, 0))
  107.  
  108.     def handle_observation(self, laser_scan, dt):
  109.         """Does prediction, weight update, and resampling."""
  110.  
  111.         # TODO: for every particle
  112.         # 1) Predict its relative motion since the last time an observation was received using
  113.         # predict_particle_odometry().
  114.         # 2) Compute the squared norm of the difference between the particle's predicted laser scan
  115.         # and the actual laser scan
  116.  
  117.         errors = []
  118.         for particle in self.particles:
  119.             self.predict_particle_odometry(particle)
  120.             error = self.get_prediction_error_squared(laser_scan, particle)
  121.             errors.append(error)
  122.  
  123.         # TODO: exponentiate the prediction errors you computed above
  124.         # using numerical stability tricks such as
  125.         # http://timvieira.github.io/blog/post/2014/02/11/exp-normalize-trick/
  126.         # if you think it is necessary
  127.  
  128.         self.weights = [exp(-error) for error in errors]
  129.         sigmoid_weight = [self.sigmoid(error) for error in errors]
  130.  
  131.         # Do resampling. Depending on how you implement it you might
  132.         # or might not need to normalize your weights by their sum, so
  133.         # they are treated as probabilities
  134.  
  135.         N_eff = 0
  136.     N_thresh = 100
  137.         new_particles = []
  138.  
  139.         N_eff = sum([1 / (weight ** 2) for weight in sigmoid_weight])
  140.  
  141.         # Deprivation: only replacing if threshold achieved
  142.         if N_eff > N_thresh:
  143.             self.resample(new_particles)
  144.             self.particles = new_particles
  145.  
  146.     def divide_up(self, id, particle, num, particle_list):
  147.         for i in xrange(int(num)):
  148.             xrand = np.random.uniform(particle.x*-0.5, particle.x*0.5)
  149.             yrand = np.random.uniform(particle.y*-0.5, particle.y*0.5)
  150.             theta = np.random.uniform(particle.theta*-0.5, particle.theta*0.5)
  151.             particle_list.append(Particle(id, xrand, yrand, theta))
  152.             id += 1
  153.  
  154.     def resample(self, new_particles):
  155.  
  156.         # TODO: sample particle i with probability that
  157.         # is proportional to its weight w_i. Sampling
  158.         # can be done with repetition/replacement, so
  159.         # you can sample the same particle more than once.
  160.  
  161.     # following method number 1 from lecture slides to resample
  162.         sample_u = np.random.uniform(0,1)
  163.         index = int(sample_u * (self.num_particles - 1))
  164.         beta = 0.0
  165.  
  166.     # handle empty case, set all equal to not crash
  167.         if self.weights == []:
  168.             self.weights = [1] * self.num_particles
  169.  
  170.         max_w = max(self.weights)
  171.         for particle in self.particles:
  172.             beta += np.random.uniform(0,1) * 2.0 * max_w
  173.             while beta > self.weights[index]:
  174.                 beta -= self.weights[index]
  175.                 index = (index + 1) % self.num_particles
  176.  
  177.             particle = self.particles[index]
  178.             new_particles.append(Particle(particle.id, particle.x, particle.y, particle.theta))
  179.  
  180.  
  181.     def simulate_laser_scan_for_particle(self, x, y, yaw_in_map, angles, min_range, max_range):
  182.         """If the robot was at the given particle, what would its laser scan
  183.         be (in the known map)? Returns the predicted laser ranges if a particle with state (x,y,yaw_in_map)
  184.         is to scan along relative angles in angles."""
  185.         # for every relative angle in angles
  186.         # 1. The absolute angle based on the robot's orientation is computed
  187.         # 2. Ray tracing from (x,y) along the abosulte angle using step size range_step is done
  188.         #    (a) If the currently examined point is within the bounds of the workspace
  189.         #        stop if it meets an obstacle or if it reaches max_range
  190.         #    (b) If the currently examined point is outside the bounds of the workspace
  191.         #        stop if it reaches max_range
  192.         # 3. The computed collection of ranges corresponding to the given angles is returned
  193.  
  194.         ranges = []
  195.         range_step = self.ogm.info.resolution
  196.  
  197.         for angle in angles:
  198.         # absolute angle
  199.             theta = yaw_in_map + angle
  200.  
  201.         # ray trace
  202.             r = min_range
  203.             while r <= max_range:
  204.  
  205.                 xm = x + r*cos(theta)
  206.                 ym = y + r*sin(theta)
  207.  
  208.         # check conditions
  209.                 if xm > self.xmax or xm < self.xmin:
  210.                     break
  211.  
  212.                 if ym > self.ymax or ym < self.ymin:
  213.                     break
  214.  
  215.                 row, col = self.metric_to_grid_coords(xm, ym)
  216.                 free = self.grid_bin[row, col].all()
  217.                 if not free:
  218.                     break
  219.  
  220.                 r += range_step
  221.  
  222.             ranges.append(r)
  223.  
  224.         return ranges
  225.  
  226.  
  227.     def subsample_laser_scan(self, laser_scan_msg):
  228.         """Subsamples a set number of beams (self.eval_beams) from the incoming actual laser scan. It also
  229.         converts the Inf range measurements into max_range range measurements, in order to be able to
  230.         compute a difference."""
  231.  
  232.         # To convert the laser points from the husky_1/base_laser frame, whose z-axis points downwards
  233.         # to the same frame pointing upwards
  234.  
  235.         N = len(laser_scan_msg.ranges)
  236.         ranges_in_upwards_baselaser_frame = laser_scan_msg.ranges
  237.         angles_in_baselaser_frame = [(laser_scan_msg.angle_max - laser_scan_msg.angle_min)*float(i)/N + laser_scan_msg.angle_min for i in xrange(N)]
  238.  
  239.         step = N/self.eval_beams
  240.         angles_in_upwards_baselaser_frame = angles_in_baselaser_frame[::step]
  241.         ranges_in_upwards_baselaser_frame = ranges_in_upwards_baselaser_frame[::-step]
  242.  
  243.         assert (len(ranges_in_upwards_baselaser_frame) == len(angles_in_upwards_baselaser_frame))
  244.  
  245.         actual_ranges = []
  246.         for r in ranges_in_upwards_baselaser_frame:
  247.             if r >= self.laser_min_range and r <= self.laser_max_range:
  248.                 actual_ranges.append(r)
  249.  
  250.             if r < self.laser_min_range:
  251.                 actual_ranges.append(self.laser_min_range)
  252.  
  253.             if r > self.laser_max_range:
  254.                 actual_ranges.append(self.laser_max_range)
  255.  
  256.  
  257.         return actual_ranges, angles_in_upwards_baselaser_frame
  258.  
  259.     def get_prediction_error_squared(self, laser_scan_msg, particle):
  260.         """
  261.         This function evaluates the squared norm of the difference/error between the
  262.         scan in laser_scan_msg and the one that was predicted by the given particle.
  263.  
  264.         Assume that the bearing of each beam relative to the robot's orientation has zero noise,
  265.         so the only noise in the measurement comes from the range of each beam and is
  266.         distributed as N(0, beam_range_measurement_std_dev^2)
  267.         """
  268.  
  269.         # If the particle is out of the bounds of the workspace
  270.         # give it a large error
  271.         if particle.x < self.xmin or particle.x > self.xmax:
  272.             return 300
  273.  
  274.         if particle.y < self.ymin or particle.y > self.ymax:
  275.             return 300
  276.  
  277.         # If the particle falls inside an obstacle
  278.         # give it a large error
  279.         row, col = self.metric_to_grid_coords(particle.x, particle.y)
  280.         if row >= 201 or col >=201:
  281.             return 300
  282.  
  283.         if not self.grid_bin[row, col]:
  284.             return 300
  285.  
  286.         assert (self.laser_min_range >= 0)
  287.         assert (self.laser_max_range > 0)
  288.  
  289.         # TODO: subsample the recived actual laser scan using the
  290.         # subsample_laser_scan method above
  291.         [real_ranges, angles] = self.subsample_laser_scan(laser_scan_msg)
  292.  
  293.         # TODO: simulate a laser scan using one of the methods of this class
  294.         simulated_ranges = self.simulate_laser_scan_for_particle(particle.x, particle.y, particle.theta, angles, self.laser_min_range, self.laser_max_range)
  295.  
  296.         # TODO: compute the difference bwteen predicted ranges and actual ranges
  297.         # Take the squared norm of that difference
  298.  
  299.         diff = [real_range - simulated_range for real_range, simulated_range in zip(real_ranges, simulated_ranges)]
  300.  
  301.         norm_error = 0
  302.         norm_error = np.linalg.norm(diff)
  303.  
  304.  
  305.         return norm_error**2
  306.  
  307.     def handle_odometry(self, robot_odom):
  308.         """Compute the relative motion of the robot from the previous odometry measurement
  309.         to the current odometry measurement."""
  310.         self.last_robot_odom = self.robot_odom
  311.         self.robot_odom = robot_odom
  312.  
  313.         if self.last_robot_odom:
  314.  
  315.             p_map_currbaselink = np.array([self.robot_odom.pose.pose.position.x,
  316.                                            self.robot_odom.pose.pose.position.y,
  317.                                            self.robot_odom.pose.pose.position.z])
  318.  
  319.             p_map_lastbaselink = np.array([self.last_robot_odom.pose.pose.position.x,
  320.                                            self.last_robot_odom.pose.pose.position.y,
  321.                                            self.last_robot_odom.pose.pose.position.z])
  322.  
  323.             q_map_lastbaselink = np.array([self.last_robot_odom.pose.pose.orientation.x,
  324.                                            self.last_robot_odom.pose.pose.orientation.y,
  325.                                            self.last_robot_odom.pose.pose.orientation.z,
  326.                                            self.last_robot_odom.pose.pose.orientation.w])
  327.  
  328.             q_map_currbaselink = np.array([self.robot_odom.pose.pose.orientation.x,
  329.                                            self.robot_odom.pose.pose.orientation.y,
  330.                                            self.robot_odom.pose.pose.orientation.z,
  331.                                            self.robot_odom.pose.pose.orientation.w])
  332.  
  333.             R_map_lastbaselink = tr.quaternion_matrix(q_map_lastbaselink)[0:3,0:3]
  334.  
  335.             p_lastbaselink_currbaselink = R_map_lastbaselink.transpose().dot(p_map_currbaselink - p_map_lastbaselink)
  336.             q_lastbaselink_currbaselink = tr.quaternion_multiply(tr.quaternion_inverse(q_map_lastbaselink), q_map_currbaselink)
  337.  
  338.             _, _, yaw_diff = tr.euler_from_quaternion(q_lastbaselink_currbaselink)
  339.  
  340.             self.dyaw += yaw_diff
  341.             self.dx += p_lastbaselink_currbaselink[0]
  342.             self.dy += p_lastbaselink_currbaselink[1]
  343.  
  344.  
  345.     def predict_particle_odometry(self, particle):
  346.         """
  347.         Where will the particle go after time dt passes?
  348.         This function modifies the particle's state by simulating the effects
  349.         of the given control forward in time.
  350.  
  351.         Assume Dubins dynamics with variable forward velocity for the Husky.
  352.         """
  353.  
  354.         nx = random.gauss(0, self.dynamics_translation_noise_std_dev)
  355.         ny = random.gauss(0, self.dynamics_translation_noise_std_dev)
  356.         ntheta = random.gauss(0, self.dynamics_orientation_noise_std_dev)
  357.  
  358.         v = sqrt(self.dx**2 + self.dy**2)
  359.  
  360.         # Don't let the particle propagation be dominated by noise
  361.         if abs(v) < 1e-10 and abs(self.dyaw) < 1e-5:
  362.             return
  363.  
  364.         particle.x += v * cos(particle.theta) + nx
  365.         particle.y += v * sin(particle.theta) + ny
  366.         particle.theta += self.dyaw + ntheta
  367.  
  368.  
  369.     def metric_to_grid_coords(self, x, y):
  370.         """Converts metric coordinates to occupancy grid coordinates"""
  371.  
  372.         gx = (x - self.ogm.info.origin.position.x) / self.ogm.info.resolution
  373.         gy = (y - self.ogm.info.origin.position.y) / self.ogm.info.resolution
  374.         row = min(max(int(gy), 0), self.ogm.info.height)
  375.         col = min(max(int(gx), 0), self.ogm.info.width)
  376.         return (row, col)
  377.  
  378. class MonteCarloLocalization(object):
  379.  
  380.     def __init__(self, num_particles, xmin, xmax, ymin, ymax):
  381.         rospy.init_node('monte_carlo_localization', anonymous=True)
  382.         self.map_file = rospy.get_param("~map_file")
  383.  
  384.         dynamics_translation_noise_std_dev   = rospy.get_param("~dynamics_translation_noise_std_dev")
  385.         dynamics_orientation_noise_std_dev   = rospy.get_param("~dynamics_orientation_noise_std_dev")
  386.         beam_range_measurement_noise_std_dev = rospy.get_param("~beam_range_measurement_noise_std_dev")
  387.  
  388.         pkl_file = open(self.map_file, 'rb')
  389.         self.ogm = pickle.load(pkl_file)
  390.         pkl_file.close()
  391.  
  392.         self.q_baselink_baselaser = np.array([1.0, 0, 0, 0])
  393.         self.R_baselink_baselaser = tr.quaternion_matrix(self.q_baselink_baselaser)[0:3,0:3]
  394.         self.p_baselink_baselaser = np.array([0.337, 0.0, 0.308])
  395.  
  396.         self.pf = ParticleFilter(num_particles, self.ogm, xmin, xmax, ymin, ymax, 0, 0, 0, 0,
  397.                                  dynamics_translation_noise_std_dev,
  398.                                  dynamics_orientation_noise_std_dev,
  399.                                  beam_range_measurement_noise_std_dev)
  400.  
  401.         self.pf.init_particles()
  402.         self.last_scan = None
  403.         self.mutex = Lock()
  404.  
  405.         self.laser_points_marker_pub = rospy.Publisher('/husky_1/debug/laser_points', Marker, queue_size=1)
  406.         self.particles_pub = rospy.Publisher('/husky_1/particle_filter/particles', MarkerArray, queue_size=1)
  407.         self.odom_sub = rospy.Subscriber('/husky_1/odometry/ground_truth', Odometry, self.odometry_callback, queue_size=1)
  408.         self.laser_sub = rospy.Subscriber('/husky_1/scan', LaserScan, self.laser_scan_callback, queue_size=1)
  409.  
  410.     def odometry_callback(self, msg):
  411.         self.mutex.acquire()
  412.         self.pf.handle_odometry(msg)
  413.         self.mutex.release()
  414.  
  415.     def get_2d_laser_points_marker(self, timestamp, frame_id, pts_in_map, marker_id, rgba):
  416.         msg = Marker()
  417.         msg.header.stamp = timestamp
  418.         msg.header.frame_id = frame_id
  419.         msg.ns = 'laser_points'
  420.         msg.id = marker_id
  421.         msg.type = 6
  422.         msg.action = 0
  423.         msg.points = [Point(pt[0], pt[1], pt[2]) for pt in pts_in_map]
  424.         msg.colors = [rgba for pt in pts_in_map]
  425.  
  426.         for pt in pts_in_map:
  427.             assert((not np.isnan(pt).any()) and np.isfinite(pt).all())
  428.  
  429.         msg.scale.x = 0.1
  430.         msg.scale.y = 0.1
  431.         msg.scale.z = 0.1
  432.         return msg
  433.  
  434.  
  435.     def laser_scan_callback(self, msg):
  436.         self.pf.laser_min_angle = msg.angle_min
  437.         self.pf.laser_max_angle = msg.angle_max
  438.         self.pf.laser_min_range = msg.range_min
  439.         self.pf.laser_max_range = msg.range_max
  440.  
  441.         dt_since_last_scan = 0
  442.  
  443.         if self.last_scan:
  444.             dt_since_last_scan = (msg.header.stamp - self.last_scan.header.stamp).to_sec()
  445.  
  446.         self.mutex.acquire()
  447.         self.publish_laser_pts(msg)
  448.         self.pf.handle_observation(msg, dt_since_last_scan)
  449.  
  450.         self.pf.dx = 0
  451.         self.pf.dy = 0
  452.         self.pf.dyaw = 0
  453.  
  454.         self.mutex.release()
  455.         self.last_scan = msg
  456.  
  457.     def publish_laser_pts(self, msg):
  458.         """Publishes the currently received laser scan points from the robot, after we subsampled
  459.         them in order to comparse them with the expected laser scan from each particle."""
  460.         if self.pf.robot_odom is None:
  461.             return
  462.  
  463.         subsampled_ranges, subsampled_angles = self.pf.subsample_laser_scan(msg)
  464.  
  465.         N = len(subsampled_ranges)
  466.         x = self.pf.robot_odom.pose.pose.position.x
  467.         y = self.pf.robot_odom.pose.pose.position.y
  468.         _, _ , yaw_in_map = tr.euler_from_quaternion(np.array([self.pf.robot_odom.pose.pose.orientation.x,
  469.                                                                self.pf.robot_odom.pose.pose.orientation.y,
  470.                                                                self.pf.robot_odom.pose.pose.orientation.z,
  471.                                                                self.pf.robot_odom.pose.pose.orientation.w]))
  472.  
  473.         pts_in_map = [ (x + r*cos(theta + yaw_in_map),
  474.                         y + r*sin(theta + yaw_in_map),
  475.                         0.3) for r,theta in zip(subsampled_ranges, subsampled_angles)]
  476.  
  477.         lpmarker = self.get_2d_laser_points_marker(msg.header.stamp, 'map', pts_in_map, 30000, ColorRGBA(1.0, 0.0, 0, 1.0))
  478.         self.laser_points_marker_pub.publish(lpmarker)
  479.  
  480.  
  481.     def get_particle_marker(self, timestamp, particle, marker_id):
  482.         """Returns an rviz marker that visualizes a single particle"""
  483.         msg = Marker()
  484.         msg.header.stamp = timestamp
  485.         msg.header.frame_id = 'map'
  486.         msg.ns = 'particles'
  487.         msg.id = marker_id
  488.         msg.type = 0  # arrow
  489.         msg.action = 0 # add/modify
  490.         msg.lifetime = rospy.Duration(1)
  491.  
  492.         yaw_in_map = particle.theta
  493.         vx = cos(yaw_in_map)
  494.         vy = sin(yaw_in_map)
  495.  
  496.         msg.color = ColorRGBA(0, 1.0, 0, 1.0)
  497.  
  498.         msg.points.append(Point(particle.x, particle.y, 0.2))
  499.         msg.points.append(Point(particle.x + 0.3*vx, particle.y + 0.3*vy, 0.2))
  500.  
  501.         msg.scale.x = 0.05
  502.         msg.scale.y = 0.15
  503.         msg.scale.z = 0.1
  504.         return msg
  505.  
  506.     def publish_particle_markers(self):
  507.         """ Publishes the particles of the particle filter in rviz"""
  508.         ma = MarkerArray()
  509.         ts = rospy.Time.now()
  510.         for i in xrange(len(self.pf.particles)):
  511.             ma.markers.append(self.get_particle_marker(ts, self.pf.particles[i], i))
  512.  
  513.         self.particles_pub.publish(ma)
  514.  
  515.     def run(self):
  516.         rate = rospy.Rate(20)
  517.         while not rospy.is_shutdown():
  518.             self.publish_particle_markers()
  519.             rate.sleep()
  520.  
  521.  
  522. if __name__ == '__main__':
  523.     num_particles = 100
  524.  
  525.     # Workspace boundaries in meters
  526.     xmin = -20
  527.     xmax = 20
  528.     ymin = -20
  529.     ymax = 20
  530.  
  531.     mcl = MonteCarloLocalization(num_particles, xmin, xmax, ymin, ymax)
  532.     mcl.run()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top