def add_noise(std_pos, std_yaw):
poses = []
for p in self.particlecloud.poses: # Iterate over all poses
new_pose = Pose()
while not self.is_valid_position(
new_pose.position
): # Repeat until point is valid (unoccupied)
new_pose.position.x = gauss(
p.position.x, std_pos) # Add noise to x-coordinate
new_pose.position.y = gauss(
p.position.y, std_pos) # Add noise to y-coordinate
rotate_amounts = vonmises(0.0, 1 / std_yaw**2)
new_pose.orientation = rotateQuaternion(
p.orientation, rotate_amounts)
poses.append(new_pose)
self.particlecloud.poses = poses
def generate_gaussian_pose(initial_pose):
p = Pose()
p.position.x = gauss(initial_pose.pose.pose.position.x,
self.POSITION_STANDARD_DEVIATION)
p.position.y = gauss(initial_pose.pose.pose.position.y,
self.POSITION_STANDARD_DEVIATION)
p.position.z = 0.0
# Convert initial orientation from quaternion into euler representation and get yaw angle
# initial_yaw = tf.transformations.euler_from_quaternion(orientation_to_vec(initial_pose.pose.pose.orientation))[2]
# rand_yaw = vonmises(initial_yaw, 1.0 / self.ORIENTATION_STANDARD_DEVIATION ** 2) # Generate random yaw angle
# vector = tf.transformations.quaternion_from_euler(0.0, 0.0, rand_yaw) # Convert yaw angle into quaternion representation
# p.orientation = vec_to_orientation(vector)
# heading = getHeading(initial_pose.pose.pose.orientation)
rotate_amounts = vonmises(
0.0, 1 / self.ORIENTATION_STANDARD_DEVIATION**2)
p.orientation = rotateQuaternion(
initial_pose.pose.pose.orientation, rotate_amounts)
return p
def test_VonMises_range(self):
# Make sure generated random variables are in [-pi, pi].
# Regression test for ticket #986.
for mu in np.linspace(-7., 7., 5):
r = random.vonmises(mu, 1, 50)
assert_(np.all(r > -np.pi) and np.all(r <= np.pi))