def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
rad = 1 # meters
self.particle_cloud = []
self.particle_cloud.append(
Particle(xy_theta[0], xy_theta[1], xy_theta[2]))
for i in range(self.n_particles - 1):
# initial facing of the particle
theta = random.random() * 360
# compute params to generate x,y in a circle
other_theta = random.random() * 360
radius = random.random() * rad
# x => straight ahead
x = radius * math.sin(other_theta) + xy_theta[0]
y = radius * math.cos(other_theta) + xy_theta[1]
particle = Particle(x, y, theta)
self.particle_cloud.append(particle)
self.normalize_particles()
self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
# levels of noise to introduce variability
lin_noise = 1
ang_noise = math.pi / 2.0
# if doesn't exist, use odom
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# make a new particle cloud, and then create a bunch of particles at the initial location with some added noise
self.particle_cloud = []
for x in range(self.n_particles):
x = xy_theta[0] + (random_sample() * lin_noise - (lin_noise / 2.0))
y = xy_theta[1] + (random_sample() * lin_noise - (lin_noise / 2.0))
theta = xy_theta[2] + (random_sample() * ang_noise -
(ang_noise / 2.0))
self.particle_cloud.append(Particle(x, y, theta))
# normalize particles because all weights were originall set to 1 on default
self.normalize_particles()
self.update_robot_pose()
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
for particle in self.particle_cloud:
r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2]
d = math.sqrt((delta[0]**2) + (delta[1]**2))
particle.theta += r1 % 360
particle.x += d * math.cos(particle.theta) + normal(0, 0.1)
particle.y += d * math.sin(particle.theta) + normal(0, 0.1)
particle.theta += (delta[2] - r1 + normal(0, 0.1)) % 360
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
print('update_w_odom')
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
for particle in self.particle_cloud:
parc = (math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2]) % 360
particle.x += (math.sqrt((delta[0]**2) + (delta[1]**2)))* math.cos(parc)
particle.y += (math.sqrt((delta[0]**2) + (delta[1]**2))) * math.sin(parc)
particle.theta += delta[2]
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
# TODO: modify particles using delta
# For added difficulty: Implement sample_motion_odometry (Prob Rob p 136)
d = math.sqrt(delta[0]**2 + delta[1]**2) #distance between robot pose and particle pose
r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2] #intial theta to rotate robot by
r2 = delta[2] - r1 #theta between robot orientation after traveling d and final particle orientation
for i in self.particle_cloud: #for each particle transform position
i.theta += r1
i.x += gauss(d * math.cos(i.theta), self.ODOM_ERROR) #add x noise
i.y += gauss(d * math.sin(i.theta), self.ODOM_ERROR) #add y noise
i.theta += r2
def resample_particles(self, particles):
""" Resample the particles according to the new particle weights.
The weights stored with each particle should define the probability that a particular
particle is selected in the resampling step. You may want to make use of the given helper
function draw_random_sample.
"""
print 'Resampling Particles ...'
# make sure the distribution is normalized
particles = normalize_particles(particles)
choices = particles
probabilities = np.array([p.w for p in particles])
most_likely_particle = particles[np.argmax(probabilities)]
# If there are multiple most likelies, don't add new particles
if (np.size(most_likely_particle) == 1):
print 'Adding New Particles ...'
num_to_resample = int(len(particles) / 2)
num_to_reinit = len(particles) - num_to_resample
pose_most_likely = convert_pose_to_xy_and_theta(
most_likely_particle.as_pose())
reinitialized_particles = self.initialize_particle_cloud(
pose_most_likely, num_to_reinit, most_likely_particle.w * 0.9)
else:
print 'Number of most likely particles: ' + str(
np.size(most_likely_particle))
num_to_resample = len(particles)
reinitialized_particles = []
new_particles = draw_random_sample(choices, probabilities,
num_to_resample)
return normalize_particles(new_particles + reinitialized_particles)
def update_initial_pose(self, msg):
print("Update Initial Pose")
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def update_particles_with_odom(self,msg):
print("Update Particles with Odom")
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
#R eh o raio feito a partir de um pitagoras com o X e Y da variacao Delta
r = math.sqrt(delta[0]**2 + delta[1]**2)
#Funcao para conseguir um valor entre -pi e pi e subtrair o antigo valor de theta. (Achei um pouco confusa, )
phi = math.atan2(delta[1], delta[0])-old_odom_xy_theta[2]
for particle in self.particle_cloud:
particle.x += r*math.cos(phi+particle.theta)
particle.y += r*math.sin(phi+particle.theta)
particle.theta += delta[2]
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
# TODO create particles
# Criando particula
print("Inicializacao da Cloud de Particulas")
#Valor auxiliar para nao dar erro na hora de criacao das particulas. Irrelevante
valor_aux = 0.3
for i in range (self.n_particles):
self.particle_cloud.append(Particle(0, 0, 0, valor_aux))
# Randomizar particulas em volta do robo.
for i in self.particle_cloud:
i.x = xy_theta[0]+ random.uniform(-1,1)
i.y = xy_theta[1]+ random.uniform(-1,1)
i.theta = xy_theta[2]+ random.uniform(-45,45)
#Normalizar as particulas e dar update na posicao do robo
self.normalize_particles()
self.update_robot_pose()
print(xy_theta)
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
# Modify particles using delta
for particle in self.particle_cloud:
particle.x -= delta[0]
particle.y -= delta[1]
particle.theta += delta[2]
def initialize_particle_cloud(self, xy_theta=None):
print 'Cria o set inicial de particulas'
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
x, y, theta = xy_theta
# Altere este parametro para aumentar a circunferencia do filtro de particulas
# Na VM ate 1 e suportado
rad = 0.5
self.particle_cloud = []
self.particle_cloud.append(
Particle(xy_theta[0], xy_theta[1], xy_theta[2]))
# print 'particle_values_W', self.particle_cloud[0].w
# print 'particle_values_X', self.particle_cloud[0].x
# print 'particle_values_Y', self.particle_cloud[0].y
# print 'particle_values_THETA', self.particle_cloud[0].theta
for i in range(self.n_particles - 1):
# initial facing of the particle
theta = random.random() * 360
# compute params to generate x,y in a circle
other_theta = random.random() * 360
radius = random.random() * rad
# x => straight ahead
x = radius * math.sin(other_theta) + xy_theta[0]
y = radius * math.cos(other_theta) + xy_theta[1]
particle = Particle(x, y, theta)
self.particle_cloud.append(particle)
self.normalize_particles()
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
print "delta: ", delta
x, y = delta[0], delta[1]
r = math.sqrt(x**2 + y**2)
theta = np.arctan2(float(y), float(x))
phi = theta - old_odom_xy_theta[2]
for particle in self.particle_cloud:
particle.x += r * math.cos(phi + particle.theta)
particle.y += r * math.sin(phi + particle.theta)
particle.theta += delta[2]
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
delta = {'x': new_odom_xy_theta[0] - self.current_odom_xy_theta[0],
'y': new_odom_xy_theta[1] - self.current_odom_xy_theta[1],
'theta': new_odom_xy_theta[2] - self.current_odom_xy_theta[2]}
delta['r'] = math.sqrt(delta['x']**2 + delta['y']**2)
delta['rot'] = angle_diff(math.atan2(delta['y'],delta['x']), old_odom_xy_theta[2])
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
for p in self.particle_cloud:
p.x += delta['r']*math.cos(delta['rot'] + p.theta)
p.y += delta['r']*math.sin(delta['rot'] + p.theta)
p.theta += delta['theta']
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
rad = 1 # meters
self.particle_cloud = []
self.particle_cloud.append(Particle(xy_theta[0], xy_theta[1], xy_theta[2]))
for i in range(self.n_particles-1):
# initial facing of the particle
theta = random.random() * 360
# compute params to generate x,y in a circle
other_theta = random.random() * 360
radius = random.random() * rad
# x => straight ahead
x = radius * math.sin(other_theta) + xy_theta[0]
y = radius * math.cos(other_theta) + xy_theta[1]
particle = Particle(x,y,theta)
self.particle_cloud.append(particle)
self.normalize_particles()
self.update_robot_pose()
def initialize_particle_cloud(self,
xy_theta=None,
num_particles=None,
weight=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
print 'Initializing Particle Cloud...'
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if num_particles == None:
num_particles = self.num_particles
if weight == None:
weight = 1
xs, ys, thetas = gen_random_particle_positions(
xy_theta, (0.25, 0.25, math.pi / 7), num_particles)
particle_cloud = [
Particle(x, y, theta, weight)
for (x, y, theta) in zip(xs, ys, thetas)
]
return particle_cloud
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, 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(self.base_frame, self.odom_frame,
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
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not (self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.d_thresh
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.d_thresh
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles(
) # 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
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.particle_cloud = normalize_particles(
self.initialize_particle_cloud(xy_theta))
self.update_robot_pose(self.particle_cloud)
self.fix_map_to_odom_transform(msg)
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,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(self.base_frame,self.odom_frame,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
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
'''
É AQUI!!!!
'''
self.update_particles_with_odom(msg) # update based on odometry - FAZER
self.update_particles_with_laser(msg) # update based on laser scan - FAZER
self.update_robot_pose() # update robot's pose
""" abaixo """
self.resample_particles() # resample particles to focus on areas of high density - FAZER
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
# TODO create particles
#zcw what the heck is rviz widget for selecting a guest. a guest to the starting pose of the robot
self.normalize_particles()
self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.get_particles_with_noise(xy_theta)
self.normalize_particles()
self.robot_pose = self.odom_pose.pose
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
# TODO create particles
self.normalize_particles()
self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# TODO create particles
self.particle_cloud = self.initial_list_builder(xy_theta)
self.normalize_particles()
self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
for i in range(self.n_particles):
self.particle_cloud.append(Particle(x=xy_theta[0]+gauss(0,0.25),y=xy_theta[1]+gauss(0,0.25),theta=xy_theta[2]+gauss(0,0.25)))
self.normalize_particles()
self.update_robot_pose()
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
print(new_odom_xy_theta)
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
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])
for p in self.particle_cloud:
p.x += delta[0]
p.y += delta[1]
p.theta += delta[2]
self.current_odom_xy_theta = new_odom_xy_theta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return # ????
# TODO: modify particles using delta
for p in self.particle_cloud:
r = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2]
d = math.sqrt((delta[0] ** 2) + (delta[1] ** 2))
p.theta += r % 360
p.x += d * math.cos(p.theta) + normal(0, .1)
p.y += d * math.sin(p.theta) + normal(0, .1)
p.theta += (delta[2] - r + normal(0, .1)) % 360
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
self.particle_cloud.append(Particle(xy_theta[0],xy_theta[1],xy_theta[2]))
# Initialize particle cloud with a decent amount of noise
for i in range (0,self.number_of_particles):
self.particle_cloud.append(Particle(xy_theta[0]+np.random.randn()*.5,xy_theta[1]+np.random.randn()*.5,xy_theta[2]+np.random.randn()*.5))
self.normalize_particles()
self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
# TODO create particles
self.particle_cloud.append(Particle(x=0,y=0,theta=0))
# for i in range(self.n_particles):
# self.particle_cloud.append(Particle(x=normal(0, .1), y=normal(0, .1), theta=normal(0, 1),w=1/float(self.n_particles)))
self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# Creating particle cloud
self.particle_cloud=[Particle(x=gauss(xy_theta[0], .3), y= gauss(xy_theta[1], .3),
theta=gauss(xy_theta[2], .2)) for n in range (0, self.n_particles)]
self.normalize_particles()
self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# Create random array of particles
self.particle_cloud = [
Particle(*arr)
for arr in np.random.normal(xy_theta, self.sd_xy_theta, (
self.n_particles, len(xy_theta)))
]
self.normalize_particles()
self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
#one particle at origin
#self.particle_cloud.append(Particle(0,0,0))
# TODO create particles
for i in range(self.n_particles):
self.particle_cloud.append(Particle(randn(), randn(),randn())) #add robot location guess to each number
self.normalize_particles()
self.update_robot_pose()
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# TODO create particles
self.particle_cloud = []
for i in range(self.n_particles): #create particles with noise; within range +/- 0.3 of each aspect of pose
x_gauss = gauss(xy_theta[0], 0.3)
y_gauss = gauss(xy_theta[1], 0.3)
theta_gauss = gauss(xy_theta[2], 0.3)
#add particle to cloud
self.particle_cloud.append(Particle(x_gauss, y_gauss, theta_gauss))
self.normalize_particles()
self.update_robot_pose()
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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 = .3 # level of noise put into particles after update from odom to introduce variability
# updates the particles based on r1, d, and r2. For more information on this, consult the website
for particle in self.particle_cloud:
# calculates r1, d, and r2
r1 = np.arctan2(float(delta[1]), float(
delta[0])) - old_odom_xy_theta[2]
d = np.sqrt(np.square(delta[0]) + np.square(delta[1]))
r2 = delta[2] - r1
# updates the particles with the above variables, while also adding in some noise
particle.theta = particle.theta + r1 * (
random_sample() * odom_noise + (1 - odom_noise / 2.0))
particle.x = particle.x + d * np.cos(
particle.theta) * (random_sample() * odom_noise +
(1 - odom_noise / 2.0))
particle.y = particle.y + d * np.sin(
particle.theta) * (random_sample() * odom_noise +
(1 - odom_noise / 2.0))
particle.theta = particle.theta + r2 * (
random_sample() * odom_noise + (1 - odom_noise / 2.0))
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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],
angle_diff(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
for i, particle in enumerate(self.particle_cloud):
# TODO: Change odometry uncertainty to be ROS param
# Calculate the angle difference between the old odometry position
# and the old particle position. Then create a rotation matrix between
# the two angles
rotationmatrix = self.make_rotation_matrix(particle.theta - old_odom_xy_theta[2])
# rotate the motion vector, add the result to the particle
rotated_delta = np.dot(rotationmatrix, delta[:2])
linear_randomness = np.random.normal(1, 0.2)
angular_randomness = np.random.uniform(particle.turn_multiplier, 0.3)
particle.x += rotated_delta[0] * linear_randomness
particle.y += rotated_delta[1] * linear_randomness
particle.theta += delta[2] * angular_randomness
# Make sure the particle's angle doesn't wrap
particle.theta = angle_diff(particle.theta, 0)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta is None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
linear_variance = 0.5 # meters
angular_variance = 4
xs = np.random.normal(xy_theta[0], linear_variance, size=self.n_particles)
ys = np.random.normal(xy_theta[1], linear_variance, size=self.n_particles)
thetas = np.random.vonmises(xy_theta[2], angular_variance, size=self.n_particles)
self.particle_cloud = [Particle(x=xs[i], y=ys[i], theta=thetas[i]) for i in xrange(self.n_particles)]
self.normalize_particles()
self.update_robot_pose()
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
x, y, theta = xy_theta
self.particle_cloud = [
Particle(
float(normal(x, self.initial_position_deviation)),
float(normal(y, self.initial_position_deviation)),
float(normal(theta, self.initial_angle_deviation)),
) for n in xrange(self.n_particles)
]
self.normalize_particles()
self.update_robot_pose()
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
# print "Odom particle updating"
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
# Determine angular rotation and displacement from odom history
r1 = math.atan2(delta[1], delta[0]) - old_odom_xy_theta[2]
d = math.sqrt((delta[0] ** 2) + (delta[1] ** 2))
r2 = delta[2] - r1
# Update particle cloud
for particle in self.particle_cloud:
particle.theta = particle.theta + r1 # Initial rotation
odom_x = math.cos(particle.theta) * d # Add displacement
odom_y = math.sin(particle.theta) * d
particle.x = particle.x + odom_x
particle.y = particle.y + odom_y
particle.theta = particle.theta + r2 # Second rotation
# noise it up some:
self.update_particles_with_noise()
def update_particles_with_odom(self, msg):
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# print 'new_odom_xy_theta', new_odom_xy_theta
# Pega a posicao da odom (x,y,tehta)
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
# print 'delta', delta
else:
self.current_odom_xy_theta = new_odom_xy_theta
return
for particle in self.particle_cloud:
d = math.sqrt((delta[0]**2) + (delta[1]**2))
# print 'particle_theta_1', particle.theta
particle.x += d * (math.cos(particle.theta) + normal(0, 0.01))
particle.y += d * (math.sin(particle.theta) + normal(0, 0.01))
particle.theta = self.current_odom_xy_theta[2] #+ normal(0,0.05)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
(x, y, theta) = xy_theta
# Create N particles centered around initial guess
for i in range(self.n_particles):
sample_x = x + randn() * self.std_x
sample_y = y + randn() * self.std_y
sample_theta = theta + uniform() * (2 * np.pi)
self.particle_cloud.append(
Particle(sample_x, sample_y, sample_theta))
self.normalize_particles()
self.update_robot_pose()
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
# TODONE: modify particles using delta
destination_distance = math.hypot(
delta[0], delta[1]) # distance between old and new odom
# Angle to move by in order to change the heading to new theta
destination_heading_theta = self.current_odom_xy_theta[2] - math.atan2(
delta[1], delta[0])
# TODO: Parallelize this with a library
for particle in self.particle_cloud:
# Cosine of the particle heading - destination heading gives us the x component
particle.x += destination_distance * math.cos(
particle.theta - destination_heading_theta)
# Sine of the particle heading - destination heading gives us the y component
particle.y += destination_distance * math.sin(
particle.theta - destination_heading_theta)
particle.theta += delta[2]
def update_particles_with_odom(self, noise):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
noise: a number between 0 and 1 which describes how much noise we will
add to the movements
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
d_x, d_y, d_theta = delta
# move each particle the same distance in their coordinate frame
new_particles = []
for p in self.particle_cloud:
angle_between_frames = old_odom_xy_theta[2] - p.theta
res = rotate(angle_between_frames, d_x, d_y)
new_d_x = res[0]
new_d_y = res[1]
rands = random_sample(3)
pos = np.array([p.x + new_d_x, p.y + new_d_y, p.theta + d_theta])
pos_noisy = pos * (1 + noise * (rands - 0.5))
new_particle = Particle.from_numpy(np.append(pos_noisy, p.w))
new_particles.append(new_particle)
self.particle_cloud = new_particles
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
dx, dy, dtheta = delta
ds = math.hypot(dx, dy)
rotationOne = math.atan2(dy, dx) - self.current_odom_xy_theta[2]
rotationTwo = dtheta - rotationOne
for particle in self.particle_cloud:
# Rotate particle to face its destination coordinate
particle.rotate(rotationOne)
# Advance the particle forward in its coordinate frame
particle.x += ds * math.cos(particle.theta)
particle.y += ds * math.sin(particle.theta)
# Rotate particle to face its final heading
particle.rotate(rotationTwo)
def update_particles_with_odom(self, msg):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# compute the change in x,y,theta since our last update
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
temp = []
# Use trigonometry to update particles based on new odometry pose
for particle in self.particle_cloud:
psy = math.atan2(delta[1],delta[0])-old_odom_xy_theta[2]
intermediate_theta = particle.theta + psy
# Calculate radius based on change in x and y
r = math.sqrt(delta[0]**2 + delta[1]**2)
# Update x and y based on radius and new angle
new_x = particle.x + r*math.cos(intermediate_theta) + np.random.randn()*0.1
new_y = particle.y + r*math.sin(intermediate_theta) + np.random.randn()*0.1
# Add change in angle to old angle
new_theta = delta[2]+particle.theta + np.random.randn()*0.1
temp.append(Particle(new_x,new_y,new_theta))
self.particle_cloud = temp
def update_initial_pose(self, msg):
xy_theta = convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
def scan_received(self, msg):
# print msg
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not (self.initialized):
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(
self.base_frame, 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(self.base_frame, self.odom_frame,
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
# print 'msg.header.frame_id', msg.header.frame_id
# calculate pose of laser relative ot the robot base
p = PoseStamped(
header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
# listener.getLatestCommonTime("/base_link",object_pose_in.header.frame_id)
# p = PoseStamped(header=Header(stamp=msg.header.stamp,
p = PoseStamped(
header=Header(
stamp=self.tf_listener.getLatestCommonTime(
self.base_frame, self.map_frame),
# p = PoseStamped(header=Header(stamp=rospy.Time.now(),
frame_id=self.base_frame),
pose=Pose())
# p_aux = PoseStamped(header=Header(stamp=self.tf_listener.getLatestCommonTime("/base_link","/map"),
p_aux = PoseStamped(
header=Header(
stamp=self.tf_listener.getLatestCommonTime(
self.odom_frame, self.map_frame),
# p_aux = PoseStamped(header=Header(stamp=rospy.Time.now(),
frame_id=self.odom_frame),
pose=Pose())
odom_aux = self.tf_listener.transformPose(self.map_frame, p_aux)
odom_aux_xy_theta = convert_pose_to_xy_and_theta(odom_aux.pose)
# print 'odom_aux_xy_theta', odom_aux_xy_theta
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# print 'self.odom_pose', self.odom_pose
# (trans, root) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
# self.odom_pose = trans
# print trans, root
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
# new_odom_xy_theta = convert_pose_to_xy_and_theta(self.laser_pose.pose)
xy_theta_aux = (new_odom_xy_theta[0] + odom_aux_xy_theta[0],
new_odom_xy_theta[1] + odom_aux_xy_theta[1],
new_odom_xy_theta[2])
self.xy_theta_aux = xy_theta_aux
if not (self.particle_cloud):
self.initialize_particle_cloud(xy_theta_aux)
self.current_odom_xy_theta = new_odom_xy_theta
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) >
self.linear_mov
or math.fabs(new_odom_xy_theta[1] -
self.current_odom_xy_theta[1]) > self.linear_mov
or math.fabs(new_odom_xy_theta[2] -
self.current_odom_xy_theta[2]) > self.angular_mov):
self.update_particles_with_odom(msg)
self.update_particles_with_laser(msg)
self.resample_particles()
self.publish_particles(msg)
