def __init__(self):
self.map_s = MapStorage(debug=True)
# Create default marker for display of swarm / debug
point_marker = Marker()
# point_marker.type = Marker.ARROW
point_marker.type = Marker.SPHERE
point_marker.action = Marker.ADD
# point_marker.scale.x = 2.5
# point_marker.scale.y = 0.35
# point_marker.scale.z = 0.35
point_marker.scale.x = 0.2
point_marker.scale.y = 0.2
point_marker.scale.z = 0.2
point_marker.color.r = 1.0
point_marker.color.g = 0.0
point_marker.color.b = 0.0
point_marker.color.a = 1.0
self.mark_p = MarkerPlacer("rviz_map_test", "map", 1000, point_marker)
self.x = 0.0
self.y = 0.0
self.start_x = self.map_s.min_x_pos+0.05
self.start_y = self.map_s.min_y_pos+0.05
self.end_x = self.map_s.max_x_pos
self.end_y = self.map_s.max_y_pos
self.delta_dist = self.map_s.resolution
class MapTest(object):
"""docstring for MapTest"""
def __init__(self):
self.map_s = MapStorage(debug=True)
# Create default marker for display of swarm / debug
point_marker = Marker()
# point_marker.type = Marker.ARROW
point_marker.type = Marker.SPHERE
point_marker.action = Marker.ADD
# point_marker.scale.x = 2.5
# point_marker.scale.y = 0.35
# point_marker.scale.z = 0.35
point_marker.scale.x = 0.2
point_marker.scale.y = 0.2
point_marker.scale.z = 0.2
point_marker.color.r = 1.0
point_marker.color.g = 0.0
point_marker.color.b = 0.0
point_marker.color.a = 1.0
self.mark_p = MarkerPlacer("rviz_map_test", "map", 1000, point_marker)
self.x = 0.0
self.y = 0.0
self.start_x = self.map_s.min_x_pos+0.05
self.start_y = self.map_s.min_y_pos+0.05
self.end_x = self.map_s.max_x_pos
self.end_y = self.map_s.max_y_pos
self.delta_dist = self.map_s.resolution
def run(self):
self.x = self.start_x
self.y = self.start_y
while self.x < self.end_x:
while self.y < self.end_y:
# Check if spot is occupied
if self.map_s.xy_pos_is_occupied(self.x, self.y):
# If spot is occupied place particle
p = Point(self.x, self.y, 0.0)
o = Quaternion()
self.mark_p.place_marker([p], [o])
# Update new y
self.y += 1.0*self.delta_dist
# Sleep a tiny bit
time.sleep(0.0001)
self.y = self.start_y
self.x += 5.0*self.delta_dist
def initialize_map(self):
# Load a map, down sample it and expand it
self.m_s = MapStorage()
# Down sample in order to reduce graph size
self.m_s.downsample_map_by_half()
self.m_s.downsample_map_by_half()
# Expand in order to avoid collisions
# Previously expanded twice (inside down sampling) => 0.1 res -> 0.4 res
# Three more expansions will leave us with an object previously taking up a
# size of 0.4 m to take up ~4*0.4 -> 1.6. The robot size is 1 metre, so
# this leaves a space of 0.6 for errors before collisions occur.
# This is *MUCH* needed - anything else leads to a boatload of collisions
self.m_s.expand_map()
self.m_s.expand_map()
self.m_s.expand_map()
self.m_s.expand_map()
def approx_float_dist(pos0, pos1):
dx = float(pos0[0]) - float(pos1[0])
dy = float(pos0[1]) - float(pos1[1])
return dx**2.0 + dy**2.0
def dist(pos0, pos1):
dx = float(pos0[0]) - float(pos1[0])
dy = float(pos0[1]) - float(pos1[1])
return math.sqrt(dx**2.0 + dy**2.0)
if __name__ == "__main__":
rospy.init_node("astar_tester")
t = Timer("MapStorage")
map = MapStorage()
map.downsample_map_by_half()
map.downsample_map_by_half()
map.expand_map()
map.expand_map()
map.expand_map()
t.print_delta()
def_marker = Marker()
def_marker.type = Marker.SPHERE
def_marker.action = Marker.ADD
def_marker.scale.x = 0.10
def_marker.scale.y = 0.10
def_marker.scale.z = 0.10
def_marker.color.r = 0.0
def_marker.color.g = 1.0
def_marker.color.b = 0.0
class Navigator(object):
"""docstring for Navigator"""
def __init__(self, start_pos, target_list=[]):
"""
Various parameters we use to make decisions on
"""
# The start position we want to hit before we follow targets
self.start_pos = start_pos
# List of targets to follow
self.target_list = target_list
# nav_queue is used to contain a list of lists of coordinates to follow
self.nav_queue = list()
# The distance required of closeness to current coordinate for assuming it to be reached
self.coord_dist_cutoff = 0.6
# What the current intermediate goal is
self.curr_target = (0.0, 0.0)
# Gains for calculating linear and angular speed
# ref: A Stable Target-Tracking Control for Unicycle Mobile Robots Lee, S. et al.
# Linear velocity depends only on K1, angular depends on K1 and K2
self.K1_default = 1.5
self.K2_default = -5.0
self.K1_slow = 0.5
self.K2_slow = -5.0
self.K1 = self.K1_default
self.K2 = self.K2_default
# Set up a markerarray to show the planned path to take
self.planned_markers = MarkerPlacer("rviz_planned_path", "map", 4000)
# Make the marker size a bit bigger so that they are easier to see
self.planned_markers.set_scale(0.12, 0.12, 0.12)
# Set them to be spheres
self.planned_markers.set_type(Marker.SPHERE)
# Pick a random colour since we might have multiple planned paths and want
# to differentiate between them
self.planned_markers.random_color()
# Set up a markerarray to show the actual path taken
self.taken_markers = MarkerPlacer("rviz_taken_path", "map", 4000)
# Make the marker size a bit bigger so that they are easier to see
self.taken_markers.set_scale(0.12, 0.12, 0.12)
# Set them to be spheres
self.taken_markers.set_type(Marker.SPHERE)
# Pick a random colour since we might want to follow multiple paths and
# want to differentiate between them
self.taken_markers.random_color()
# Set up a markerarray to place markers when the goal is reached
self.goal_markers = MarkerPlacer("rviz_goals", "map", 40)
# Make the marker size a bit bigger so that they are easier to see
self.goal_markers.set_scale(0.10, 0.10, 1.35)
# Set them to be spheres
self.goal_markers.set_type(Marker.CYLINDER)
# Set colour to pink
self.goal_markers.set_color(1.0, 0.0, 1.0, 1.0)
# Add a marker for the assumed pose that the navigator is acting on
self.pose_marker = MarkerPlacer("/rviz_nav_pose", "/map")
self.pose_marker.set_color(1.0, 1.0, 0.0, 1.0)
self.pose_marker.set_scale(1.25, 0.25, 0.25)
# Set up a container for current pose of robot. This pose is the fusion of
# data received from the particle filter and odometry updates.
# We store yaw in a separate variable since this will reduce the number of
# transformations is required from going quat -> yaw -> quat since we will
# use the yaw directly for navigation purposes
self.pose = Pose()
self.yaw = 0.0
# Subscribe to the real pose of the robot in order to drop markers along the followed path
self.real_pose = PoseStamped()
rospy.Subscriber("/real_pose", PoseStamped, self.get_real_pose)
# Set up a subscriber to particle filter pose
self.particle_pose = PoseStamped()
rospy.Subscriber("/particle_pose", PoseStamped, self.get_particle_pose)
# At this point we kind-of want to wait until we have a particle update
# before we start throwing odometry updates at the uninitialised position.
rospy.wait_for_message("/particle_pose", Odometry)
# Set up a subscriber for (noisy) odometry updates
# First wait for an Odometry message in order to get a time stamp which
# will be used to update the robots position from the odometry data
rospy.wait_for_message("/odom", Odometry)
self.last_odom_time = rospy.get_time()
# Then create a container for the odometry and subscribe to the topic.
self.odom = Odometry()
rospy.Subscriber("/odom", Odometry, self.get_odom)
# Initialize the map that will be used for calculating paths and overall navigation
self.m_s = None
self.initialize_map()
# Find path from robot position to start position. For now we're doing
# this in the navigator, but should perhaps be outsources to an external
# module later since it can take a very long time.
self.xy_path = list()
self.xy_pos_path = list()
self.get_path_to_start()
# Add the start pos since the resolution of the planned path is not perfect
self.xy_pos_path.append(start_pos)
# Place markers for the current path to show that we know where we are going (hopefully!)
self.mark_current_path()
# At this point we got: Robot pose (fusion of particle + odom) and a list
# of coordinates we want to follow towards a goal. This should be enough
# navigate!
# Set up a publisher for sending velocity commands to the robot
self.vel = Twist()
self.vel_publisher = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# Pop off the first item in list as current goal
self.curr_target = self.xy_pos_path.pop(0)
def run(self):
# Here we need to act on the information previously received regarding
# robot pose and list of coordinates to follow.
# Using these parameters we have to figure out:
# 1) When we are close enough to a coordinate to knock it off the list
# 1.1) Is the target the goal? If so -> What to do?
# 2) What the next coordinate is
# 3) The angle towards the next coordinate
# 4) The angular and linear speed we need to set in order to hit the coordinate
# Find closeness to current coordinate
curr_dist = dist((self.pose.position.x, self.pose.position.y), self.curr_target)
if curr_dist < self.coord_dist_cutoff:
# Drop off a marker to show that we have been here
self.taken_markers.place_marker([copy.deepcopy(self.real_pose.pose.position)],
[copy.deepcopy(self.real_pose.pose.orientation)])
# Check if we have arrived at the last point in the list (the ultimate goal)
if len(self.xy_pos_path) == 0:
if curr_dist < 0.2:
# Poop goal marker
self.goal_markers.place_marker([self.real_pose.pose.position],
[self.real_pose.pose.orientation])
# Check if there are more targets to go to
if len(self.target_list) > 0:
# Change the colour for the planned path and path taken to differentiate
# it from the previous path
self.taken_markers.random_color()
self.planned_markers.random_color()
# Find a new target and path
self.xy_path = self.get_next_path()
# Convert the path to xy_pos format
self.convert_current_path()
# Mark it
self.mark_current_path()
# if no more targets go to sleep
else:
rospy.sleep()
# If we have more coordinates to visit pop off the next one!
else:
self.curr_target = self.xy_pos_path.pop(0)
curr_dist = dist((self.pose.position.x, self.pose.position.y), self.curr_target)
# Adjust the speed depending on how close we are to the current goal
if len(self.xy_pos_path) < 5:
self.K1 = self.K1_slow
self.K2 = self.K2_slow
else:
self.K1 = self.K1_default
self.K2 = self.K2_default
# Figure out the angle we want to travel in to reach the current target
# Assumption: This is given by atan2 between the target and the robot?
# This makes sense as it would transpose the origin to the centre of the
# robot, leaving the calculation to a "virtual" coordinate system around
# the robot.
theta = math.atan2(self.curr_target[1] - self.pose.position.y,
self.curr_target[0] - self.pose.position.x)
# Find the difference in theta that should go towards zero when the
# correct heading is reached
d_theta = theta - self.yaw
d_theta = self.normalize(d_theta)
# Find a new set of linear and angular speeds to publish
lin_speed = self.new_speed(curr_dist, d_theta)
ang_speed = self.new_angular_speed(d_theta)
self.vel.linear.x = lin_speed
self.vel.angular.z = ang_speed
self.vel_publisher.publish(self.vel)
def new_speed(self, radius, target_theta):
"""
ref:
A Stable Target-Tracking Control for Unicycle Mobile Robots
Lee, S. et al.
"""
return self.K1 * radius * math.cos(target_theta)
def new_angular_speed(self, target_theta):
"""
ref:
A Stable Target-Tracking Control for Unicycle Mobile Robots
Lee, S. et al.
"""
factor_1 = (-self.K1) * math.sin(target_theta) * math.cos(target_theta)
factor_2 = self.K2 * target_theta
return factor_1 - factor_2
def get_real_pose(self, pose):
self.real_pose = pose
def get_particle_pose(self, particle_pose):
prev_particle_pose = self.particle_pose
self.particle_pose = particle_pose
# Set up a check to see if the new pose is out of whack - Might happen on
# random occasions. If it is we do not want to update the assumed pose but
# rather continue using the estimated pose from previous particle filter +
# odometry updates
# TODO(geir): Implement the check somehow
if True:
self.pose.position = self.particle_pose.pose.position
self.pose.orientation = self.particle_pose.pose.orientation
pitch, roll, yaw = tf.transformations.euler_from_quaternion([self.pose.orientation.x,
self.pose.orientation.y,
self.pose.orientation.z,
self.pose.orientation.w])
self.yaw = yaw
def get_odom(self, odom):
self.odom = odom
# Update current pose with odom data!
# First get the current time and time since last update
time = rospy.get_time()
d_time = time-self.last_odom_time
self.last_odom_time = time
# Get the linear and angular velocities
lin_vel = self.odom.twist.twist.linear.x
ang_vel = self.odom.twist.twist.angular.z
# Calculate change in yaw, x and y
d_theta = ang_vel * d_time
d_x = (lin_vel * math.cos(self.yaw))*d_time
d_y = (lin_vel * math.sin(self.yaw))*d_time
# Update position
self.yaw += d_theta
self.pose.position.x += d_x
self.pose.position.y += d_y
q = tf.transformations.quaternion_from_euler(0.0, 0.0, self.yaw)
self.pose.orientation = Quaternion(*q)
# Publish the marker for this pose
self.pose_marker.place_marker([self.pose.position], [self.pose.orientation])
def initialize_map(self):
# Load a map, down sample it and expand it
self.m_s = MapStorage()
# Down sample in order to reduce graph size
self.m_s.downsample_map_by_half()
self.m_s.downsample_map_by_half()
# Expand in order to avoid collisions
# Previously expanded twice (inside down sampling) => 0.1 res -> 0.4 res
# Three more expansions will leave us with an object previously taking up a
# size of 0.4 m to take up ~4*0.4 -> 1.6. The robot size is 1 metre, so
# this leaves a space of 0.6 for errors before collisions occur.
# This is *MUCH* needed - anything else leads to a boatload of collisions
self.m_s.expand_map()
self.m_s.expand_map()
self.m_s.expand_map()
self.m_s.expand_map()
def get_next_path(self):
# Here we want to find the item in the list which is the closest to the
# current position of the robot.
closest = 999999.9
closest_index = 0
for i in range(len(self.target_list)):
d = dist((self.pose.position.x, self.pose.position.y),
(self.target_list[i][0], self.target_list[i][1]))
if d < closest:
closest = d
closest_index = i
# Get the target that is closest
pos_t = self.target_list.pop(closest_index)
# Find the x,y values for robot position and target
x_t, y_t = self.m_s.xy_from_xy_pos(pos_t[0], pos_t[1])
x_r, y_r = self.m_s.xy_from_xy_pos(self.pose.position.x, self.pose.position.y)
# Search for path
path = astar_search(self.m_s, (x_r, y_r), (x_t, y_t))
return path
def get_path_to_start(self):
# Convert x_pos,y_pos for self.pose and self.start_pos to x,y for use with A* search
x_r, y_r = self.m_s.xy_from_xy_pos(self.pose.position.x, self.pose.position.y)
x_s, y_s = self.m_s.xy_from_xy_pos(self.start_pos[0], self.start_pos[1])
# Find a path from current position to start position
self.xy_path = astar_search(self.m_s, (x_r, y_r), (x_s, y_s))
# Convert the path in to real coordinates
self.convert_current_path()
def convert_current_path(self):
# The path is now in (x,y) format so we want to convert this to a list in
# (x_pos,y_pos) format in order to use it for navigation
self.xy_pos_path = list()
for p in self.xy_path:
x_pos, y_pos = self.m_s.xy_pos_from_xy(p[0], p[1])
self.xy_pos_path.append((x_pos, y_pos))
def mark_current_path(self):
o = Quaternion() # Need for interface
o_list = list()
p_list = list()
for pos in self.xy_pos_path:
o_list.append(o)
p_list.append(Point(pos[0], pos[1], 0.0))
self.planned_markers.place_marker(p_list, o_list)
def normalize(self, theta):
while theta <= math.pi:
theta += 2.0*math.pi
while theta > math.pi:
theta -= 2.0*math.pi
return theta
def __init__(self, odom_rate, sens_median, sens_std_dev, vel_uniform_dist,
num_particles, base_scan, start_pos=None, debug=False):
# Odom rate is used for re-positioning of particles and assumed pose after applying filter
self.odom_rate = odom_rate
# Need sensory uncertainty distribution for calculating particle beam probabilities
self.sens_median = sens_median
self.sens_std_dev = sens_std_dev
# Need odometry uncertainty for calculating movement probabilities
self.vel_uniform_dist = vel_uniform_dist
# Want to keep track of how many particles we are going to have in the swarm
self.num_particles = num_particles
# If there is a starting position for the robot we want to make use of it
# in order to instantly converge on that spot
self.start_pos = start_pos
# If we want debug messages, let us know!
self.debug = debug
# Pre-calculate factors for calculating beam unertainty since these are
# done millions of times per filter (Well... up to 50 times per beam per
# particle, 30 beams or so, say 1000 particles -> 1.5 million times)
self.laser_prob_f_1 = (1 / (math.sqrt(2.0*math.pi*(self.sens_std_dev**2.0))))
self.laser_prob_f_2 = -0.5 / (self.sens_std_dev**2.0)
# Set up map storage facilities
self.map_s = MapStorage(self.debug)
# Set up a publisher for publishing the (assumed) position of the robot
# found using the particle filter
self.particle_p = ParticlePose("map")
# Set up a transform listener and broadcaster for utility use
self.tf = tf.TransformListener()
# Pre-allocate odometry buffers for use when calculating the movement
# of particles in the swarm
self.lin_vel_buffer = list()
self.ang_vel_buffer = list()
# Receive odometry
self.odom = Odometry()
rospy.Subscriber("odom", Odometry, self.get_odom)
# Receive laser scan
self.scan = LaserScan()
rospy.Subscriber(base_scan, LaserScan, self.get_scan)
# Initialize particle swarm
self.particles = list()
for index in range(self.num_particles):
self.particles.append(copy.deepcopy(Particle()))
self.initiate_particles()
# Sleep for a bit to allow data and transforms to be received
rospy.sleep(0.2)
# Create default marker for display of swarm / debug
point_marker = Marker()
"""
Settings for particle markers with pose
"""
point_marker.type = Marker.ARROW
point_marker.action = Marker.ADD
point_marker.scale.x = 1.25
point_marker.scale.y = 0.25
point_marker.scale.z = 0.25
point_marker.color.r = 0.0
point_marker.color.g = 1.0
point_marker.color.b = 0.0
point_marker.color.a = 1.0
"""
Settings for beam tracing particles
point_marker.type = Marker.SPHERE
point_marker.action = Marker.ADD
point_marker.scale.x = 0.05
point_marker.scale.y = 0.05
point_marker.scale.z = 0.05
point_marker.color.r = 0.0
point_marker.color.g = 1.0
point_marker.color.b = 0.0
point_marker.color.a = 1.0
"""
self.mark_p = MarkerPlacer("rviz_particles", "map", 1000, point_marker)
class ParticleFilter(object):
"""docstring for ParticleFilter"""
def __init__(self, odom_rate, sens_median, sens_std_dev, vel_uniform_dist,
num_particles, base_scan, start_pos=None, debug=False):
# Odom rate is used for re-positioning of particles and assumed pose after applying filter
self.odom_rate = odom_rate
# Need sensory uncertainty distribution for calculating particle beam probabilities
self.sens_median = sens_median
self.sens_std_dev = sens_std_dev
# Need odometry uncertainty for calculating movement probabilities
self.vel_uniform_dist = vel_uniform_dist
# Want to keep track of how many particles we are going to have in the swarm
self.num_particles = num_particles
# If there is a starting position for the robot we want to make use of it
# in order to instantly converge on that spot
self.start_pos = start_pos
# If we want debug messages, let us know!
self.debug = debug
# Pre-calculate factors for calculating beam unertainty since these are
# done millions of times per filter (Well... up to 50 times per beam per
# particle, 30 beams or so, say 1000 particles -> 1.5 million times)
self.laser_prob_f_1 = (1 / (math.sqrt(2.0*math.pi*(self.sens_std_dev**2.0))))
self.laser_prob_f_2 = -0.5 / (self.sens_std_dev**2.0)
# Set up map storage facilities
self.map_s = MapStorage(self.debug)
# Set up a publisher for publishing the (assumed) position of the robot
# found using the particle filter
self.particle_p = ParticlePose("map")
# Set up a transform listener and broadcaster for utility use
self.tf = tf.TransformListener()
# Pre-allocate odometry buffers for use when calculating the movement
# of particles in the swarm
self.lin_vel_buffer = list()
self.ang_vel_buffer = list()
# Receive odometry
self.odom = Odometry()
rospy.Subscriber("odom", Odometry, self.get_odom)
# Receive laser scan
self.scan = LaserScan()
rospy.Subscriber(base_scan, LaserScan, self.get_scan)
# Initialize particle swarm
self.particles = list()
for index in range(self.num_particles):
self.particles.append(copy.deepcopy(Particle()))
self.initiate_particles()
# Sleep for a bit to allow data and transforms to be received
rospy.sleep(0.2)
# Create default marker for display of swarm / debug
point_marker = Marker()
"""
Settings for particle markers with pose
"""
point_marker.type = Marker.ARROW
point_marker.action = Marker.ADD
point_marker.scale.x = 1.25
point_marker.scale.y = 0.25
point_marker.scale.z = 0.25
point_marker.color.r = 0.0
point_marker.color.g = 1.0
point_marker.color.b = 0.0
point_marker.color.a = 1.0
"""
Settings for beam tracing particles
point_marker.type = Marker.SPHERE
point_marker.action = Marker.ADD
point_marker.scale.x = 0.05
point_marker.scale.y = 0.05
point_marker.scale.z = 0.05
point_marker.color.r = 0.0
point_marker.color.g = 1.0
point_marker.color.b = 0.0
point_marker.color.a = 1.0
"""
self.mark_p = MarkerPlacer("rviz_particles", "map", 1000, point_marker)
def get_odom(self, odom):
self.odom = odom
# Buffer odom data for use when updating particles
self.lin_vel_buffer.append(self.odom.twist.twist.linear.x)
self.ang_vel_buffer.append(self.odom.twist.twist.angular.z)
if self.debug:
# print(self.odom)
pass
def get_scan(self, scan):
self.scan = scan
if self.debug:
# print(self.scan)
pass
def initiate_particles(self):
# If we have a starting position, use that
if self.start_pos is not None:
for particle in self.particles:
particle.p[0] = self.start_pos[0]
particle.p[1] = self.start_pos[1]
particle.o = self.start_pos[2]
# If we do not spread the particles out throughout the map
else:
min_x = self.map_s.min_x_pos
max_x = self.map_s.max_x_pos
min_y = self.map_s.min_y_pos
max_y = self.map_s.max_y_pos
for particle in self.particles:
particle.randomize(min_x, max_x, min_y, max_y)
def move_particles(self):
# Copy and zero odom buffers
lin_vel_list = self.lin_vel_buffer
self.lin_vel_buffer = list()
ang_vel_list = self.ang_vel_buffer
self.ang_vel_buffer = list()
# Store the most recent scan for use with filter after movement is
# complete
self.odom_synced_scan = copy.deepcopy(self.scan)
# For each particle change its position and rotation according to each
# buffered data input with a uniform distribution
for particle in self.particles:
for i in range(len(lin_vel_list)):
lin_vel = lin_vel_list[i] + \
np.random.uniform(-self.vel_uniform_dist,
self.vel_uniform_dist)
ang_vel = ang_vel_list[i] + \
np.random.uniform(-self.vel_uniform_dist,
self.vel_uniform_dist)
d_theta = ang_vel / self.odom_rate
d_x = (lin_vel * math.cos(particle.o))/self.odom_rate
d_y = (lin_vel * math.sin(particle.o))/self.odom_rate
particle.p[0] += d_x
particle.p[1] += d_y
particle.o += d_theta
def place_markers(self):
pos_list = list()
or_list = list()
for index in range(len(self.particles)):
# print("placing markers index: " + str(index))
pos_list.append(Point(self.particles[index].p[0], self.particles[index].p[1], 0.0))
quat = quaternion_from_euler(0.0, 0.0, self.particles[index].o)
or_list.append(Quaternion(quat[0], quat[1], quat[2], quat[3]))
self.mark_p.place_marker(pos_list, or_list)
def sensor_probability(self, meas, exp):
f_2 = math.exp(self.laser_prob_f_2 * (meas-exp)**2.0)
prob = self.laser_prob_f_1 * f_2
return prob
def apply_filter(self):
# Copy all scan parameters so that they do not change while we are
# looping through all the parameters since this takes a while
d_theta = self.odom_synced_scan.angle_increment
scan_min_angle = self.odom_synced_scan.angle_min
scan_max_angle = self.odom_synced_scan.angle_max
scan_max_range = self.odom_synced_scan.range_max
ranges = self.odom_synced_scan.ranges
map_res = self.map_s.resolution
# Make an empty list for storing the probability for each particle
probability_list = list()
max_probability = 0
# For all particles:
min_x = self.map_s.min_x_pos
max_x = self.map_s.max_x_pos
min_y = self.map_s.min_y_pos
max_y = self.map_s.max_y_pos
for i in range(len(self.particles)):
# Check if the particle is in a valid position. If it is not
# regenerate the position.
x_r_m = self.particles[i].p[0]
y_r_m = self.particles[i].p[1]
while self.map_s.xy_pos_is_occupied(x_r_m, y_r_m):
self.particles[i].randomize(min_x, max_x, min_y, max_y)
x_r_m = self.particles[i].p[0]
y_r_m = self.particles[i].p[1]
# Get P(x_r_m,y_r_m,theta_r_m) (robot in map frame) from stored
# particle
point_r_m = PointStamped()
point_r_m.point.x = self.particles[i].p[0]
point_r_m.point.y = self.particles[i].p[1]
point_r_m.point.z = 0.0
point_r_m.header.frame_id = "base_link"
point_r_m.header.stamp = self.tf.getLatestCommonTime("base_link", "base_laser_link")
# Transform in to L(x_L_m,y_L_m,theta_L_m) (laser in map frame)
# using /base_link to /base_laser_link (need a transform listener!)
point_l_m = self.tf.transformPoint("base_laser_link", point_r_m)
x_l_m = point_l_m.point.x
y_l_m = point_l_m.point.y
# The orientation of the laser scanner in the map frame is the
# same as the robot, so we are taking a shortcut here
theta_l_m = self.particles[i].o
# Publish a transform that will be used to translate from a point
# in the laser frame of reference to the laser in map frame of
# reference.
theta_b_l = scan_min_angle
theta_b_l_max = scan_max_angle
beam_index = 0
probability = 0
"""
Debug to trace beam!
pos_list = list()
or_list = list()
"""
# For each beam angle theta_b_L (beam in laser frame of reference)
while theta_b_l < theta_b_l_max + d_theta/10.0:
# Find check angle theta_b_m =
# theta_b_L + theta_L_m (beam in map frame of reference)
theta_b_m = theta_l_m + theta_b_l
x_slope = math.cos(theta_b_m)*float(map_res)
y_slope = math.sin(theta_b_m)*float(map_res)
# For each N deltaDistance, N = 1 ->
# N = maxDistance/resolution, deltaDistance = map resolution
for N in range(1, int(scan_max_range/map_res)):
# Check if x_b_m =
# x_L_m + x_b_L =
# x_L_m + N*dDist*cos(theta_L_m + theta_b_L) and
# y_b_m =
# y_L_m + y_b_L =
# y_L_m + N*dDist*sin(theta_L_m + theta_b_L) is occupied.
x_b_m = x_l_m + float(N)*x_slope
y_b_m = y_l_m + float(N)*y_slope
"""
Debug to trace beam!
pos_list.append(Point(x_b_m, y_b_m, 0.0))
or_list.append(Quaternion(0.0, 0.0, 0.0, 0.0))
"""
# If occupied calculate expected distance between them and
# exit loop
if self.map_s.xy_pos_is_occupied(x_b_m, y_b_m):
exp_dist = self.dist(x_l_m, y_l_m, x_b_m, y_b_m)
break
# If no intersection was detected use max range as expected
# distance
# (PS: I was amazed this was possible - an else clause if the
# break in the loop was not executed!)
else:
exp_dist = scan_max_range
# Calculate probability for this beam angle
measured_range = np.random.normal(ranges[beam_index], self.sens_std_dev)
exp_dist = exp_dist
"""
measured_range = Decimal(ranges[beam_index])
exp_dist = Decimal(exp_dist)
"""
probability_beam = self.sensor_probability(measured_range, exp_dist)
# Add to previous calculated probabilities
probability += probability_beam
# Increment theta of beam and index of beam
theta_b_l += 2.0*d_theta
beam_index += 2
"""
Debug to trace beam
self.mark_p.place_marker(pos_list, or_list)
"""
# Add probability for particle to list
probability_list.append(probability)
# Check if the beam probability is the "best", if it is store the
# position as the assumed particle swarm position
if probability > max_probability:
max_probability = probability
self.avg_x = x_r_m
self.avg_y = y_r_m
self.avg_theta = theta_l_m
# print(probability_list)
# Do hat-search using the weighting in probability list and add found
# particles to new list
new_list = self.hat_search(probability_list)
# Assign new list to be used for next iteration
self.particles = new_list
def dist(self, x0, y0, x1, y1):
d_x = abs(x0-x1)
d_y = abs(y0-y1)
return math.sqrt(d_x**2.0 + d_y**2.0)
def hat_search(self, prob_list):
new_part_list = list()
sum_x = 0.0
sum_y = 0.0
sum_theta = 0.0
sum_count = 0
total = sum(prob_list)
# Ensure that we have at least one value with probability larger than 0
if total >= 0:
end_hat = len(self.particles) - len(self.particles)/20
for i in range(end_hat):
rnd_num = sp.random.uniform(0.0, total)
pick_index = 0
picking = True
while picking:
rnd_num -= prob_list[pick_index]
if rnd_num <= 0:
# Add x, y, theta to sum
"""
sum_x += self.particles[pick_index].p[0]
sum_y += self.particles[pick_index].p[1]
sum_theta += self.particles[pick_index].o
sum_count += 1
"""
new_part = Particle([self.particles[pick_index].p[0],
self.particles[pick_index].p[1]],
self.particles[pick_index].o)
new_part_list.append(new_part)
picking = False
else:
pick_index += 1
min_x = self.map_s.min_x_pos
max_x = self.map_s.max_x_pos
min_y = self.map_s.min_y_pos
max_y = self.map_s.max_y_pos
for i in range(end_hat, len(self.particles)):
new_part = Particle()
new_part.randomize(min_x, max_x, min_y, max_y)
new_part_list.append(copy.deepcopy(new_part))
# If the total for some reason ended up being all zeroes let us just
# use the old list, but also need to do the centre of mass calculation
# for publishing centre off mass
else:
new_part_list = self.particles
for i in range(self.particles):
# Add x, y, theta to sum
sum_x += self.particles[i].p[0]
sum_y += self.particles[i].p[1]
sum_theta += self.particles[i].o
sum_count += 1
"""
self.avg_x = sum_x/sum_count
self.avg_y = sum_y/sum_count
self.avg_theta = sum_theta/sum_count
"""
print("total sum: " + str(total))
return new_part_list
def run(self):
# First we want to update the particle locations using the most recent
# odometry data
self.move_particles()
# Then apply filter to produce a list of new and higher probability
# particles
self.apply_filter()
# Place a selection of markers in rviz to illustrate the particle cloud
# self.mark_p.clear_markers()
# self.place_markers()
# Update the position with most recent odometry updates received since
# particle filter was started
self.update_particle_pose()
# Publish the centre off mass position and orientation for particles
avg_rot = quaternion_from_euler(0.0, 0.0, self.avg_theta)
self.particle_p.publish(Point(self.avg_x, self.avg_y, 0.0), Quaternion(*avg_rot))
def update_particle_pose(self):
lin_vel_list = self.lin_vel_buffer
ang_vel_list = self.ang_vel_buffer
for i in range(len(lin_vel_list)):
lin_vel = lin_vel_list[i] + \
np.random.uniform(-self.vel_uniform_dist,
self.vel_uniform_dist)
ang_vel = ang_vel_list[i] + \
np.random.uniform(-self.vel_uniform_dist,
self.vel_uniform_dist)
d_theta = ang_vel / self.odom_rate
d_x = (lin_vel * math.cos(self.avg_theta))/self.odom_rate
d_y = (lin_vel * math.sin(self.avg_theta))/self.odom_rate
self.avg_x += d_x
self.avg_y += d_y
self.avg_theta += d_theta
