class RunRobot(object):
''' Represents all of the sensor and robot model related operations'''
def __init__(self):
self.transform_helper = TFHelper()
self.odom_sub = rospy.Subscriber('odom', Odometry, self.odom_callback)
self.odom_pos = None
self.odom_ori = None
self.odom_header = None
self.robot_xyyaw_pose = None
def odom_callback(self, msg):
self.odom_header = msg.header
self.odom_pos = msg.pose.pose.position
self.odom_ori = msg.pose.pose.orientation
def laserCallback(self, msg):
''' Represents all of the logic for handling laser messages'''
# Set ranges for front, back, left, and right
front_left = msg.ranges[0:90]
back_left = msg.ranges[90:180]
back_right = msg.ranges[180:270]
front_right = msg.ranges[270:360]
laser_diff = 0 #set difference between quadrants = 0
# Calculate difference between left front and back to determine where wall is on left side
for front_left, back_left in zip(front_left, reversed(back_left)):
if front_left == 0.0 or back_left == 0.0:
continue
laser_diff += front_left - back_left
# Calculate diff between right front and back to determine where wall is on right side
for front_right, back_right in zip(front_right, reversed(back_right)):
if front_right == 0.0 or back_right == 0.0:
continue
laser_diff += back_right - front_right
def robot_position(self):
'''Represents the position of the robot as a x, y, yaw tuple'''
pose = self.transform_helper.convert_translation_rotation_to_pose(self.odom_pos, self.odom_ori)
self.robot_xyyaw_pose = self.transform.helper.convert_pose_to_xy_and_theta(pose)
return self.robot_xyyaw_pose
def get_pose(self):
translation = [self.x, self.y, 0]
rotation = TFHelper.convert_theta_to_quaternion(self.theta)
return TFHelper.convert_translation_rotation_to_pose(
translation, rotation)
class RobotLocalizer(object):
'''
The class that represents a Particle Filter ROS Node
'''
def __init__(self):
print('Initializing')
self.initialized = False # make sure we don't perform updates before everything is setup
rospy.init_node('localizer')
self.pf = ParticleFilter()
self.base_frame = "base_link" # Robot base frame
self.map_frame = "map" # Map coord frame
self.odom_frame = "odom" # Odom coord frame
self.scan_topic = "scan" # Laser scan topic
self.linear_threshold = 0.1 # the amount of linear movement before performing an update
self.angular_threshold = math.pi / 10 # the amount of angular movement before performing an update
self.max_dist = 2.0 # maximum penalty to assess in the likelihood field model
self.odom_pose = PoseStamped()
self.robot_pose = Pose()
self.robot_pose = Pose()
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.process_scan)
# init pf
# subscribers and publisher
self.odom_sub = rospy.Subscriber("/odom", Odometry,
self.odom_particle_updater)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.pose_updater)
# enable listening for and broadcasting coord transforms
self.tf_listener = TransformListener()
self.tf_broadcaster = TransformBroadcaster()
self.occupancy_field = OccupancyField()
self.transform_helper = TFHelper()
self.current_odom_xy_theta = []
print("initialization complete")
self.initialized = True
def robot_pose_updater(self):
''' Update the estimate of the robot's pose given the updated particles by computing the mean pose'''
self.pf.particle_normalizer()
# Calculate avg particle position based on pose
mean_particle = Particle(0, 0, 0, 0)
mean_particle_theta_x = 0
mean_particle_theta_y = 0
for particle in self.pf.particle_cloud:
mean_particle.x += particle.x * particle.w
mean_particle.y += particle.y * particle.w
# Using trig to calculate angle (@Paul I hate Trigonometry!)
distance_vector = np.sqrt(
np.square(particle.y) + np.square(particle.x))
mean_particle_theta_x += distance_vector * np.cos(
particle.theta) * particle.w
mean_particle_theta_y += distance_vector * np.sin(
particle.theta) * particle.w
mean_particle.theta = np.arctan2(float(mean_particle_theta_y),
float(mean_particle_theta_x))
self.robot_pose = mean_particle.particle_to_pose()
# Get the laser messages
def laserCallback(self, msg):
self.laserCallback = msg
def odom_particle_updater(self, msg):
''' Updates particles based on new odom pose using a delta value for x,y,theta'''
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
# compute the change (delta) in x,y,theta
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
odom_noise = .25 # noise level
'''
updates the particles based on angle1, dist, and angle2.
angle1: Angle by which the robot rotates to face new xy position
dist: Distance to move forward to xy position
angle2: Angle by which the robot rotates to face final direction
'''
for particle in self.pf.particle_cloud:
# calculates angle1, d, and angle2
angle1 = np.arctan2(float(delta[1]), float(
delta[0])) - old_odom_xy_theta[2]
dist = np.sqrt(np.square(delta[0]) + np.square(delta[1]))
angle2 = delta[2] - angle1
# updates the particles with the above variables, while also adding in some noise
#This is the part of class where Paul moved and we all updated based on that movement
particle.theta = particle.theta + angle1 * (
random_sample() * odom_noise + (1 - odom_noise / 2.0))
particle.x = particle.x + dist * np.cos(
particle.theta) * (random_sample() * odom_noise +
(1 - odom_noise / 2.0))
particle.y = particle.y + dist * np.sin(
particle.theta) * (random_sample() * odom_noise +
(1 - odom_noise / 2.0))
particle.theta = particle.theta + angle2 * (
random_sample() * odom_noise + (1 - odom_noise / 2.0))
def particle_resampler(self):
'''Resample the particles according to the new particle weights.'''
# make sure the distribution is normalized
self.pf.particle_normalizer()
particle_list = []
weights = []
num_samples = len(self.pf.particle_cloud)
for particle in self.pf.particle_cloud:
particle_list.append(particle)
weights.append(particle.w)
self.pf.particle_cloud = self.draw_random_sample(
particle_list, weights, num_samples)
def draw_random_sample(self, choices, probabilities, n):
""" Return a random sample of n elements from the set choices with the specified probabilities
choices: the values to sample from represented as a list
probabilities: the probability of selecting each element in choices represented as a list
n: the number of samples
"""
# sets up an index list for the chosen particles, and makes bins for the probabilities
values = np.array(range(len(choices)))
probs = np.array(probabilities)
bins = np.add.accumulate(probs)
inds = values[np.digitize(
random_sample(n), bins
)] # chooses the new particles based on the probabilities of the old ones
samples = []
for i in inds:
samples.append(
deepcopy(choices[int(i)])
) # makes the new particle cloud based on the chosen particles
return samples
def laser_particle_updater(self, msg):
'''Updates the particle weights in response to the scan contained in the msg'''
# Find the total error for each particle based on 36 laser measurements taken from the Neato's actual position
for particle in self.pf.particle_cloud:
error = []
for theta in range(0, 360, 10):
rad = np.radians(theta)
err = self.occupancy_field.get_closest_obstacle_distance(
particle.x +
msg.ranges[theta] * np.cos(particle.theta + rad),
particle.y +
msg.ranges[theta] * np.sin(particle.theta + rad))
if (
math.isnan(err)
): # if the get_closest_obstacle_distance method finds that a point is out of bounds, then the particle can't ever be it
particle.w = 0
break
if (
sum(error) == 0
): # if the particle is basically a perfect match, then we make the particle almost always enter the next iteration through resampling
particle.w = 1.0
else:
particle.w = 1.0 / sum(
error
) # the errors are inverted such that large errors become small and small errors become large
def pose_updater(self, msg):
''' Restart particle filter based on updated pose '''
xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
msg.pose.pose)
#self.fix_map_to_odom_transform(msg)
self.pf.particle_cloud_init(xy_theta)
self.fix_map_to_odom_transform(msg)
print("particle cloud initialized")
def process_scan(self, msg):
'''Handling laser data to update our understanding of the robot based on laser and odometry'''
if not (self.initialized):
print("first if not")
# wait for initialization to complete
return
if not (self.tf_listener.canTransform('base_link', msg.header.frame_id,
msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not (self.tf_listener.canTransform('base_link', 'odom',
msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
pose = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, pose)
# find out where the robot thinks it is based on its odometry
pose = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, pose)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = self.transform_helper.convert_pose_to_xy_and_theta(
self.odom_pose.pose)
if not (self.pf.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.pf.particle_cloud_init()
# cache the last odometry pose so we can only update our particle filter if we move more than self.linear_threshold or self.angular_threshold
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
print("Trying to initialize!")
self.fix_map_to_odom_transform(msg)
print("Initialized finally!")
self.pf.particle_publisher(msg)
else:
# we have moved far enough to do an update!
#self.odom_particle_updater(msg) # update based on odometry
#print("map!")
#self.laser_particle_updater(msg) # update based on laser scan
#self.robot_pose_updater() # update robot's pose
self.particle_resampler(
) # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(
msg
) # update map to odom transform now that we have new particles
self.pf.particle_publisher(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = \
self.transform_helper.convert_pose_inverse_transform(self.robot_pose)
pose = PoseStamped(
pose=self.transform_helper.convert_translation_rotation_to_pose(
translation, rotation),
header=Header(stamp=msg.header.stamp, frame_id=self.base_frame))
self.tf_listener.waitForTransform(self.base_frame,
self.odom_frame, msg.header.stamp,
rospy.Duration(1.0))
self.odom_to_map = self.tf_listener.transformPose(
self.odom_frame, pose)
(self.translation,
self.rotation) = self.transform_helper.convert_pose_inverse_transform(
self.odom_to_map.pose)
def send_last_map_to_odom_transform(self):
if not (hasattr(self, 'translation') and hasattr(self, 'rotation')):
print("sup dude")
return
self.tf_broadcaster.sendTransform(self.translation, self.rotation,
rospy.get_rostime(), 'odom', 'map')