def do_long_range_message(config, robot1, robot2, motion1, motion2):
"""Potentially transfer long range messages between robots.
Depending on whether the robots are within thresh distance, they will
exchange long range measurements. The long range measurements are sent in
the form of LongRangeMeasurement objects.
Args:
config: Configuration file.
robot1: First Robot object.
robot2: Second Robot object.
motion1: First RobotMotion object.
motion2: Second RobotMotion object.
"""
# If within long range, exchange long range sensor measurements.
if dist(motion1.pos, motion2.pos) < config['long_thresh']:
# Get message data from each robot.
message1to2_data = robot1.get_long_range_message()
message2to1_data = robot2.get_long_range_message()
# Create the sensor model, which will be used to make all measurements.
sm = SensorModel(config)
# Compute the pairwise sensor measurements between each robot.
num_sensors = len(config['sensor_parameters'])
robot1_measurements = [
sm.get_measurement(motion1, i, motion2) for i in range(num_sensors)
]
robot2_measurements = [
sm.get_measurement(motion2, i, motion1) for i in range(num_sensors)
]
logging.debug("Robot 1 Measurements: %s", robot1_measurements)
logging.debug("Robot 2 Measurements: %s", robot2_measurements)
# Stuff the data into the LongRangeMessages
message1to2 = LongRangeMessage(message1to2_data, robot2_measurements)
message2to1 = LongRangeMessage(message2to1_data, robot1_measurements)
# And then transmit both of the messages.
# Do it this way to ensure that receiving a message doesn't
# modify some state inside the robot.
robot1.receive_long_range_message(message2to1)
robot2.receive_long_range_message(message1to2)
# indicate whether these two robots communicated
return True
return False
def test_hyperparameter():
np.random.seed(10008)
map_obj = MapReader('../data/map/wean.dat')
occupancy_map = map_obj.get_map()
# generate a random particle, then sent out beams from that location
# indices = np.where(occupancy_map.flatten() == 0)[0]
# ind = np.random.choice(indices, 1)[0]
# y, x = ind // w, ind % w
# theta = -np.pi / 2
# angle = np.pi * (40 / 180)
X = init_particles_freespace(1, occupancy_map)
sensor = SensorModel(occupancy_map)
X = X.reshape(1, -1)
z_t_star = sensor.ray_casting(X[:, :3], num_beams=10)
print(z_t_star)
print(z_t_star.max())
z = np.arange(sensor._max_range + 2).astype(np.float)
p_hit, p_short, p_max, p_rand = sensor.estimate_density(z, z_t_star[0][5])
# plot(1, p_hit)
# plot(2, p_short)
# plot(3, p_max)
# plot(4, p_rand)
# w_hit = 3 # 99 / 2 / 2.5 / 4 # 1.
# w_short = 0.05 # 2 * 198 / 4 / 2.5 / 4 # 1
# w_max = 0.1 # 49 / 2.5 / 4 # 0.5
# w_rand = 10 # 990 / 4 # 5
# self._z_hit = 99 / 2 / 2.5 / 4 # 1.
# self._z_short = 198 / 4 // 2.5 / 4 # 1
# self._z_max = 49 / 2.5 / 4 # 0.5
# self._z_rand = 990 / 4 # 5
# w_hit = 1.
# w_short = 0.1
# w_max = 0.5
# w_rand = 10
w_hit = 1000 # 99 / 2 / 2.5 / 4 # 1.
w_short = 0.01 # 2 * 198 / 4 / 2.5 / 4 # 1
w_max = 0.03 # 49 / 4 / 4 # 0.5
w_rand = 12500
p = sensor._z_hit * p_hit + sensor._z_short * p_short + sensor._z_max * p_max + sensor._z_rand * p_rand
plot(5, p)
plt.show()
def __init__(self, init_p_occ=0.8, min_count=5):
self.tmap_bin = None
self.tmap = None
self.tmap_aug = None
self.pmap = None
self.map_properties = {}
self.tmap_loaded = False
self.gplane_loaded = False
self.gp_plane_map = None
self.gp_emap = None
self.gp_emap_prop = {}
self.gp_ec = 0
# gamma model
self.gamma_model_loaded = False
self.gamma_rbin_ids = []
self.gamma_avgs = []
self.gamma_stds = []
self.count = []
# intensity spectrum of ground
self.ground_spectrum = {}
self.min_count = min_count
# probability model variables
self.init_p_occ = init_p_occ
# saftey factor - the largest this should be is 1 - init_p_occ
self.saftey_value = 1 - self.init_p_occ
# sensor model
self.sensor_model = SensorModel()
def __init__(self, map_file='../data/map/wean.dat'):
self.map = Map(map_file)
#self.map.display_gaussian([], 'Gaussian Blurred Map')
self._number_particles = 1000
self.particles = list()
self._particle_probabilities = []
for _ in range(0, self._number_particles):
print 'Initializing particle', _
particle = Particle(self.map)
#particle.x = 4080 + np.random.normal(scale=35)
#particle.y = 3980 + np.random.normal(scale=15)
#particle.theta = math.pi + 0.1 + np.random.normal(scale=.1)
self.particles.append(particle)
self._particle_probabilities.append(1)
self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1)
self._sensor_model = SensorModel(self.map)
self._ranges = []
def __init__(self):
# Get parameters
self.particle_filter_frame = \
rospy.get_param("~particle_filter_frame")
# Initialize the models
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
class MyTestCase(unittest.TestCase):
def setUp(self):
self.sensor_model = SensorModel(map)
self.particle = Particle(map)
self.particle.x = 250
self.particle.y = 250
self.particle.theta = 0
def test_plot(self):
self.sensor_model.plot_model()
def test_laser_state(self):
(x, y, theta) = _laser_state(self.particle)
self.assertEqual(x, 275)
self.assertEqual(y, 250)
self.assertEqual(theta, 0)
self.particle.theta = -math.pi / 2
(x, y, theta) = _laser_state(self.particle)
self.assertEqual(x, 250)
self.assertEqual(y, 225)
self.assertEqual(theta, -math.pi / 2)
def test_ray_cast(self):
laser_state = _laser_state(self.particle)
right = self.sensor_model._ray_cast(laser_state, -math.pi / 2)
self.assertAlmostEqual(right, 250, places=0) # should hit edge of map
straight = self.sensor_model._ray_cast(laser_state, 0)
self.assertAlmostEqual(
straight, 225,
places=0) # should hit wall in front, minus 25 for laser offset
left = self.sensor_model._ray_cast(laser_state, math.pi / 2)
self.assertAlmostEqual(left, 750,
places=0) # should hit edge of map on left
left_diagonal = self.sensor_model._ray_cast(laser_state, math.pi / 4)
self.assertAlmostEqual(left_diagonal,
225 / math.cos(math.pi / 4),
places=0) # should hit wall off to left
def test__get_probability(self):
log_probabilities = []
particle = Particle(map)
particle.x = 250
particle.y = 500
particle.theta = 0
for _ in range(0, 10):
x = 200 + 10 * _
ranges = []
for i in range(-90, 91):
side = abs(500.0 / (math.sin(i * math.pi / 180) + 0.000001))
wall = abs((500.0 - x - 25) /
(math.cos(i * math.pi / 180) + 0.0000001))
ranges.append(min(side, wall))
log_probabilities.append(
self.sensor_model.get_probability(particle, ranges))
most_likely_reading = max(enumerate(log_probabilities),
key=lambda x: x[1])[0]
self.assertEqual(most_likely_reading, 5)
def test_raycasting_vectorize():
np.random.seed(10008)
map_obj = MapReader('../data/map/wean.dat')
occupancy_map = map_obj.get_map()
# generate a random particle, then sent out beams from that location
h, w = occupancy_map.shape
indices = np.where(occupancy_map.flatten() == 0)[0]
ind = np.random.choice(indices, 1)[0]
y, x = ind // w, ind % w
theta = np.pi / 2
X = np.array([[x, y, theta]])
X = np.repeat(X, 2, axis=0)
X[:, :2] *= 10
num_beams = 180
sensor = SensorModel(occupancy_map)
z_t_star = sensor.ray_casting(X, num_beams=num_beams)
x0, y0 = X[0, :2]
angle = np.arange(num_beams) * (np.pi / num_beams)
angle = theta + angle - np.pi / 2
x1 = x0 + z_t_star * np.cos(angle)
y1 = y0 - z_t_star * np.sin(angle)
x0, y0 = x0 / 10, y0 / 10
x1, y1 = x1 / 10, y1 / 10
# plot figure
fig = plt.figure()
plt.imshow(occupancy_map)
plt.scatter(x0, y0, c='red')
plt.scatter(x1, y1, c='yellow')
print(f'(x0, y0): ({x0}, {y0}), (x1, y1): ({x1}, {y1})')
# plt.plot((x0, x1), (y0, y1), color='yellow')
plt.show()
print(z_t_star)
class MyTestCase(unittest.TestCase):
def setUp(self):
self.sensor_model = SensorModel(map)
self.particle = Particle(map)
self.particle.x = 250
self.particle.y = 250
self.particle.theta = 0
def test_plot(self):
self.sensor_model.plot_model()
def test_laser_state(self):
(x, y, theta) = _laser_state(self.particle)
self.assertEqual(x, 275)
self.assertEqual(y, 250)
self.assertEqual(theta, 0)
self.particle.theta = -math.pi/2
(x, y, theta) = _laser_state(self.particle)
self.assertEqual(x, 250)
self.assertEqual(y, 225)
self.assertEqual(theta, -math.pi/2)
def test_ray_cast(self):
laser_state = _laser_state(self.particle)
right = self.sensor_model._ray_cast(laser_state, -math.pi/2)
self.assertAlmostEqual(right, 250, places=0) # should hit edge of map
straight = self.sensor_model._ray_cast(laser_state, 0)
self.assertAlmostEqual(straight, 225, places=0) # should hit wall in front, minus 25 for laser offset
left = self.sensor_model._ray_cast(laser_state, math.pi/2)
self.assertAlmostEqual(left, 750, places=0) # should hit edge of map on left
left_diagonal = self.sensor_model._ray_cast(laser_state, math.pi/4)
self.assertAlmostEqual(left_diagonal, 225/math.cos(math.pi/4), places=0) # should hit wall off to left
def test__get_probability(self):
log_probabilities = []
particle = Particle(map)
particle.x = 250
particle.y = 500
particle.theta = 0
for _ in range(0, 10):
x = 200 + 10*_
ranges = []
for i in range(-90, 91):
side = abs(500.0/(math.sin(i*math.pi/180)+0.000001))
wall = abs((500.0-x-25)/(math.cos(i*math.pi/180)+0.0000001))
ranges.append(min(side, wall))
log_probabilities.append(self.sensor_model.get_probability(particle, ranges))
most_likely_reading = max(enumerate(log_probabilities), key=lambda x: x[1])[0]
self.assertEqual(most_likely_reading, 5)
def __init__(self, map_file='../data/map/wean.dat'):
self.map = Map(map_file)
#self.map.display_gaussian([], 'Gaussian Blurred Map')
self._number_particles = 1000
self.particles = list()
self._particle_probabilities = []
for _ in range(0, self._number_particles):
print 'Initializing particle', _
particle = Particle(self.map)
#particle.x = 4080 + np.random.normal(scale=35)
#particle.y = 3980 + np.random.normal(scale=15)
#particle.theta = math.pi + 0.1 + np.random.normal(scale=.1)
self.particles.append(particle)
self._particle_probabilities.append(1)
self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1)
self._sensor_model = SensorModel(self.map)
self._ranges = []
def __init__(self):
# Get parameters
self.particle_filter_frame = \
rospy.get_param("~particle_filter_frame")
# Initialize publishers/subscribers
#
# *Important Note #1:* It is critical for your particle
# filter to obtain the following topic names from the
# parameters for the autograder to work correctly. Note
# that while the Odometry message contains both a pose and
# a twist component, you will only be provided with the
# twist component, so you should rely only on that
# information, and *not* use the pose component.
scan_topic = rospy.get_param("~scan_topic", "/scan")
odom_topic = rospy.get_param("~odom_topic", "/odom")
self.laser_sub = rospy.Subscriber(scan_topic, LaserScan,
YOUR_LIDAR_CALLBACK, # TODO: Fill this in
queue_size=1)
self.odom_sub = rospy.Subscriber(odom_topic, Odometry,
YOUR_ODOM_CALLBACK, # TODO: Fill this in
queue_size=1)
# *Important Note #2:* You must respond to pose
# initialization requests sent to the /initialpose
# topic. You can test that this works properly using the
# "Pose Estimate" feature in RViz, which publishes to
# /initialpose.
self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped,
YOUR_POSE_INITIALIZATION_CALLBACK, # TODO: Fill this in
queue_size=1)
# *Important Note #3:* You must publish your pose estimate to
# the following topic. In particular, you must use the
# pose field of the Odometry message. You do not need to
# provide the twist part of the Odometry message. The
# odometry you publish here should be with respect to the
# "/map" frame.
self.odom_pub = rospy.Publisher("/pf/pose/odom", Odometry, queue_size = 1)
# Initialize the models
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
def __init__(self):
rospy.init_node('pf')
real_robot = False
# create instances of two helper objects that are provided to you
# as part of the project
self.particle_filter = ParticleFilter()
self.occupancy_field = OccupancyField()
self.TFHelper = TFHelper()
self.sensor_model = sensor_model = SensorModel(
model_noise_rate=0.5,
odometry_noise_rate=0.15,
world_model=self.occupancy_field,
TFHelper=self.TFHelper)
self.position_delta = None # Pose, delta from current to previous odometry reading
self.last_scan = None # list of ranges
self.odom = None # Pose, current odometry reading
self.x_y_spread = 0.3 # Spread constant for x-y initialization of particles
self.z_spread = 0.2 # Spread constant for z initialization of particles
self.n_particles = 150 # number of particles
# pose_listener responds to selection of a new approximate robot
# location (for instance using rviz)
rospy.Subscriber("initialpose", PoseWithCovarianceStamped,
self.update_initial_pose)
rospy.Subscriber("odom", Odometry, self.update_position)
rospy.Subscriber("stable_scan", LaserScan, self.update_scan)
# publisher for the particle cloud for visualizing in rviz.
self.particle_pub = rospy.Publisher("particlecloud",
PoseArray,
queue_size=10)
def setUp(self):
self.sensor_model = SensorModel(map)
self.particle = Particle(map)
self.particle.x = 250
self.particle.y = 250
self.particle.theta = 0
parser.add_argument('--path_to_log', default='../data/log/robotdata1.log')
parser.add_argument('--output', default='results')
parser.add_argument('--num_particles', default=500, type=int)
parser.add_argument('--visualize', action='store_true')
args = parser.parse_args()
src_path_map = args.path_to_map
src_path_log = args.path_to_log
os.makedirs(args.output, exist_ok=True)
map_obj = MapReader(src_path_map)
occupancy_map = map_obj.get_map()
logfile = open(src_path_log, 'r')
motion_model = MotionModel()
sensor_model = SensorModel(occupancy_map)
resampler = Resampling()
map_model = ReadTable()
ray_map = map_model.return_Map()
print(ray_map.shape)
# ray_map = 0
bool_occ_map = (occupancy_map < 0.35) & (occupancy_map >= 0)
num_particles = args.num_particles
initial_num = num_particles
# X_bar = init_particles_random(num_particles, occupancy_map)
X_bar = init_particles_freespace(num_particles, occupancy_map)
# initialize mesh map module
print('Load mesh map and initialize map module...')
map_module = MapModule(map_poses, map_file)
# initialize particles
print('Monte Carlo localization initializing...')
map_size, road_coords = gen_coords_given_poses(map_poses)
if config['pose_tracking']:
particles = init_particles_pose_tracking(numParticles,
poses[start_idx])
else:
particles = init_particles_given_coords(numParticles, road_coords)
# initialize sensor model
sensor_model = SensorModel(map_module, scan_folder, config['range_image'])
if config['range_image']['render_instanced']:
update_weights = sensor_model.update_weights_instanced
else:
update_weights = sensor_model.update_weights
# generate odom commands
commands = gen_commands(poses)
# initialize a visualizer
if visualize:
plt.ion()
visualizer = Visualizer(map_size,
poses,
map_poses,
grid_res=grid_res,
parser.add_argument('--adaptive',
action='store_true',
help='Adaptively select particles or not')
args = parser.parse_args()
src_path_map = args.path_to_map
src_path_log = args.path_to_log
os.makedirs(args.output, exist_ok=True)
map_obj = MapReader(src_path_map)
occupancy_map = map_obj.get_map()
with open(src_path_log, 'r') as f:
logfile = f.readlines()
motion_model = MotionModel()
sensor_model = SensorModel(occupancy_map)
resampler = Resampling()
num_particles = args.num_particles
X_bar = init_particles_freespace(num_particles, occupancy_map)
num_beams = 10 if args.debug else 18
"""
Monte Carlo Localization Algorithm : Main Loop
"""
if args.visualize:
visualize_map(occupancy_map)
visualize_timestep(X_bar, 0, args.output)
first_time_idx = True
total_time, cnt, time_id = 0., 0., 0
from readtable import ReadTable
from matplotlib import pyplot as plt
from matplotlib import figure as fig
import time
src_path_map = '../data/map/wean.dat'
src_path_log = '../data/log/robotdata1.log'
map_obj = MapReader(src_path_map)
occupancy_map = map_obj.get_map()
bool_occ_map = (occupancy_map < 0.35) & (occupancy_map >= 0)
print(bool_occ_map[500][400])
logfile = open(src_path_log, 'r')
motion_model = MotionModel()
sensor_model = SensorModel(occupancy_map)
resampler = Resampling()
def visualize_ray_cast(x_t1, bool_occ_map, tstep, output_path):
walk_stride = 8
angle_stride = 10
x, y, theta = x_t1
laser_x_t1 = x + 25 * math.cos(theta)
laser_y_t1 = y + 25 * math.sin(theta)
xout = laser_x_t1 / 10
yout = laser_y_t1 / 10
for deg in range(-90, 90, angle_stride):
laser_theta = (deg + theta * 180 / math.pi) * (math.pi / 180)
calc_distance, x_end, y_end = ray_cast_visual(laser_x_t1, laser_y_t1,
laser_theta, walk_stride)
def init_world_once(self, do_test=True):
# create a blank world, PRM style
num_nodes = self.config["num_nodes"]
connection_radius = self.config["connection_radius"]
environment_size = self.config["environment_size"]
self.sensor_range = self.config["sensor_range"]
# vertices
self.vertices_surface = []
self.vertices = []
count = 0
for vertex_idx in xrange(num_nodes):
x = random.uniform(0, environment_size[0])
y = random.uniform(0, environment_size[1])
for z in [-10, self.surface_level]:
v = Vertex(x, y, z, count)
count += 1
self.vertices.append(v)
if z == self.surface_level:
self.vertices_surface.append(True)
else:
self.vertices_surface.append(False)
# edges, stored as a matrix indexed as [vertex_start, vertex_end]
self.num_nodes = len(
self.vertices
) #doubled the input num_nodes in creating two layers of vertices instead of one
self.edge_matrix = [None] * self.num_nodes
self.edge_adjacency_idx_lists = [None] * self.num_nodes
self.edge_adjacency_edge_lists = [None] * self.num_nodes
for vertex_start_idx in xrange(self.num_nodes):
self.edge_matrix[vertex_start_idx] = [None] * self.num_nodes
self.edge_adjacency_idx_lists[vertex_start_idx] = []
self.edge_adjacency_edge_lists[vertex_start_idx] = []
for vertex_end_idx in xrange(self.num_nodes):
cost = distance(self.vertices[vertex_start_idx],
self.vertices[vertex_end_idx])
if vertex_start_idx != vertex_end_idx and cost <= connection_radius and not (
self.vertices_surface[vertex_start_idx]
and self.vertices_surface[vertex_end_idx]):
exists = True
else:
exists = False
edge = Edge(vertex_start_idx, vertex_end_idx, cost, exists)
self.edge_matrix[vertex_start_idx][vertex_end_idx] = edge
if exists:
self.edge_adjacency_idx_lists[vertex_start_idx].append(
vertex_end_idx)
self.edge_adjacency_edge_lists[vertex_start_idx].append(
edge)
if do_test:
self.test_indices()
#self.vertex_target_idx = self.create_target_idx() #single robot
# Define prior distribution
self.num_classes = self.config["num_classes"]
self.prob_of_class0 = self.config["prob_of_class0"]
self.prob_of_other_classes = (1 - self.prob_of_class0) / (
self.num_classes - 1)
self.prior = np.zeros(self.num_classes) #p_y0, p_y1, ...
for i in range(self.num_classes):
if i == 0:
self.prior[i] = self.prob_of_class0
else:
self.prior[i] = self.prob_of_other_classes
self.randomize_targets()
self.original_classes_y = copy.copy(self.classes_y)
# Define single random drop-off location (on surface) per world
temp = random.randint(0, len(self.vertices) - 1)
while self.vertices_surface[temp] != True:
temp = random.randint(0, len(self.vertices) - 1)
self.drop_off_idx = temp
'''
# Set all class 2 vertices to is_armed = True
self.is_armed_array = np.zeros(self.num_nodes, dtype=bool) #default, False for all
for v in xrange(len(self.classes_y)):
if self.classes_y[v] == 2: #it is a mine
self.is_armed_array[v] = True
'''
'''
# Vertex classes
# Each vertex has a number between 0 and n, which represents the class of the vertex
num_v_y0 = int(self.num_nodes*self.prob_of_class0)
num_v_other = self.num_nodes - num_v_y0
num_v_each = num_v_other/(self.num_classes-1)
classes_y = []
classes_y.extend([0]*num_v_y0) # add the non-target vertex classes
for i in range(1,self.num_classes):
classes_y.extend([i]*num_v_each) # add the target vertex classes
# G round truth vertex classes
random.shuffle(classes_y) #shuffle the list to randomize location of targets
'''
self.comms_range = self.config["comms_range"]
self.vertices_in_comms_range = self.generateCommsRangeVertices()
# Setup sensor model
self.sensor_model = SensorModel(self.config, self.num_nodes, self)
class ParticleFilter(object):
"""
This is the main particle filter object.
"""
def __init__(self, map_file='../data/map/wean.dat'):
self.map = Map(map_file)
#self.map.display_gaussian([], 'Gaussian Blurred Map')
self._number_particles = 1000
self.particles = list()
self._particle_probabilities = []
for _ in range(0, self._number_particles):
print 'Initializing particle', _
particle = Particle(self.map)
#particle.x = 4080 + np.random.normal(scale=35)
#particle.y = 3980 + np.random.normal(scale=15)
#particle.theta = math.pi + 0.1 + np.random.normal(scale=.1)
self.particles.append(particle)
self._particle_probabilities.append(1)
self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1)
self._sensor_model = SensorModel(self.map)
self._ranges = []
def log(self, file_handle):
line = list()
for particle in self.particles:
loc = str(particle.x) + ',' + str(particle.y)
line.append(loc)
file_handle.write(';'.join(line))
file_handle.write('\n')
def display(self):
self.map.display(self.particles, ranges=self._ranges)
def update(self, line):
"""
Update step.
Reading is a single reading (i.e. line) from the log file
Could be either an odometry or laser reading
"""
measurement_type = line[0] # O = odometry, L = laser scan
measurements = np.fromstring(line[2:], sep=' ')
odometry = measurements[0:3]
time_stamp = measurements[-1] # last variable
for particle in self.particles:
self._motion_model.update(particle, odometry, time_stamp)
if measurement_type == "L":
odometry_laser = measurements[3:6] # coordinates of the laser in standard odometry frame
self._ranges = measurements[6:-1] # exclude last variable, the time stamp
self._particle_probabilities = list() # unnormalized sensor model probabilities of the particles
for particle in self.particles:
self._particle_probabilities.append(self._sensor_model.get_probability(particle, self._ranges))
argsorted_probabilities = np.argsort(-np.asarray(self._particle_probabilities))
self.particles[argsorted_probabilities[0]].debug = True
self.particles[argsorted_probabilities[1]].debug = True
self.particles[argsorted_probabilities[2]].debug = True
def _resample(self):
"""
Resamples the particles given unnormalized particle probabilites
"""
particle_probabilities = np.asarray(self._particle_probabilities)/sum(self._particle_probabilities) # normalize
indices = np.random.choice(range(0, self._number_particles), size=self._number_particles, replace=True,
p=particle_probabilities)
indices.sort()
previous_index = -1
new_particles = list()
for index in indices:
if index != previous_index:
new_particles.append(self.particles[index])
else:
new_particles.append(deepcopy(self.particles[index]))
previous_index = index
self.particles = new_particles
def __init__(
self,
publish_tf,
n_particles,
n_viz_particles,
odometry_topic,
motor_state_topic,
servo_state_topic,
scan_topic,
laser_ray_step,
exclude_max_range_rays,
max_range_meters,
speed_to_erpm_offset,
speed_to_erpm_gain,
steering_angle_to_servo_offset,
steering_angle_to_servo_gain,
car_length,
):
"""
Initializes the particle filter
publish_tf: Whether or not to publish the tf. Should be false in sim, true on real robot
n_particles: The number of particles
n_viz_particles: The number of particles to visualize
odometry_topic: The topic containing odometry information
motor_state_topic: The topic containing motor state information
servo_state_topic: The topic containing servo state information
scan_topic: The topic containing laser scans
laser_ray_step: Step for downsampling laser scans
exclude_max_range_rays: Whether to exclude rays that are beyond the max range
max_range_meters: The max range of the laser
speed_to_erpm_offset: Offset conversion param from rpm to speed
speed_to_erpm_gain: Gain conversion param from rpm to speed
steering_angle_to_servo_offset: Offset conversion param from servo position to steering angle
steering_angle_to_servo_gain: Gain conversion param from servo position to steering angle
car_length: The length of the car
"""
self.PUBLISH_TF = publish_tf
# The number of particles in this implementation, the total number of particles is constant.
self.N_PARTICLES = n_particles
self.N_VIZ_PARTICLES = n_viz_particles # The number of particles to visualize
# Cached list of particle indices
self.particle_indices = np.arange(self.N_PARTICLES)
# Numpy matrix of dimension N_PARTICLES x 3
self.particles = np.zeros((self.N_PARTICLES, 3))
# Numpy matrix containing weight for each particle
self.weights = np.ones(self.N_PARTICLES) / float(self.N_PARTICLES)
# A lock used to prevent concurrency issues. You do not need to worry about this
self.state_lock = Lock()
self.tfl = tf.TransformListener(
) # Transforms points between coordinate frames
# Get the map
map_msg = rospy.wait_for_message(MAP_TOPIC, OccupancyGrid)
self.map_info = map_msg.info # Save info about map for later use
# Create numpy array representing map for later use
array_255 = np.array(map_msg.data).reshape(
(map_msg.info.height, map_msg.info.width))
self.permissible_region = np.zeros_like(array_255, dtype=bool)
# Numpy array of dimension (map_msg.info.height, map_msg.info.width), with values 0: not permissible, 1: permissible
self.permissible_region[array_255 == 0] = 1
# Globally initialize the particles
# Publish particle filter state
# Used to create a tf between the map and the laser for visualization
self.pub_tf = tf.TransformBroadcaster()
# Publishes the expected pose
self.pose_pub = rospy.Publisher(PUBLISH_PREFIX + "/inferred_pose",
PoseStamped,
queue_size=1)
# Publishes a subsample of the particles
self.particle_pub = rospy.Publisher(PUBLISH_PREFIX + "/particles",
PoseArray,
queue_size=1)
# Publishes the most recent laser scan
self.pub_laser = rospy.Publisher(PUBLISH_PREFIX + "/scan",
LaserScan,
queue_size=1)
# Publishes the path of the car
self.pub_odom = rospy.Publisher(PUBLISH_PREFIX + "/odom",
Odometry,
queue_size=1)
rospy.sleep(1.0)
self.initialize_global()
# An object used for resampling
self.resampler = ReSampler(self.particles, self.weights,
self.state_lock)
# An object used for applying sensor model
self.sensor_model = SensorModel(
scan_topic,
laser_ray_step,
exclude_max_range_rays,
max_range_meters,
map_msg,
self.particles,
self.weights,
car_length,
self.state_lock,
)
# An object used for applying kinematic motion model
self.motion_model = KinematicMotionModel(
motor_state_topic,
servo_state_topic,
speed_to_erpm_offset,
speed_to_erpm_gain,
steering_angle_to_servo_offset,
steering_angle_to_servo_gain,
car_length,
self.particles,
self.state_lock,
)
self.permissible_x, self.permissible_y = np.where(
self.permissible_region == 1)
# Parameters/flags/vars for global localization
self.global_localize = False
self.global_suspend = False
self.ents = None
self.ents_sum = 0.0
self.noisy_cnt = 0
# number of regions to partition. Simulation: 25, Real: 5.
self.NUM_REGIONS = 25
# number of updates for regional localization. Simulation 5, Real: 3.
self.REGIONAL_ROUNDS = 5
self.regions = []
self.click_mode = True
self.debug_mode = False
if self.debug_mode:
self.global_localize = True # True when doing global localization
# precompute regions. Each region is represented by arrays of x, y indices on the map
region_size = len(self.permissible_x) / self.NUM_REGIONS
idx = np.argsort(self.permissible_y) # column-major
_px, _py = self.permissible_x[idx], self.permissible_y[idx]
for i in range(self.NUM_REGIONS):
self.regions.append((
_px[i * region_size:(i + 1) * region_size],
_py[i * region_size:(i + 1) * region_size],
))
# Subscribe to the '/initialpose' topic. Publised by RVIZ. See clicked_pose_cb function in this file for more info
# three different reactions to click on rviz
self.click_sub = rospy.Subscriber(
"/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose_cb,
queue_size=1,
)
self.init_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.reinit_cb,
queue_size=1)
rospy.wait_for_message(scan_topic, LaserScan)
print("Initialization complete")
class UWBDataFusion(object):
def __init__(self, init_p_occ=0.8, min_count=5):
self.tmap_bin = None
self.tmap = None
self.tmap_aug = None
self.pmap = None
self.map_properties = {}
self.tmap_loaded = False
self.gplane_loaded = False
self.gp_plane_map = None
self.gp_emap = None
self.gp_emap_prop = {}
self.gp_ec = 0
# gamma model
self.gamma_model_loaded = False
self.gamma_rbin_ids = []
self.gamma_avgs = []
self.gamma_stds = []
self.count = []
# intensity spectrum of ground
self.ground_spectrum = {}
self.min_count = min_count
# probability model variables
self.init_p_occ = init_p_occ
# saftey factor - the largest this should be is 1 - init_p_occ
self.saftey_value = 1 - self.init_p_occ
# sensor model
self.sensor_model = SensorModel()
def set_tmap(self, tmap, map_properties):
'''
Set the traversability map
Input: Tmap, npfloat array ti is [0-1] negative value indicates unknown ti
'''
self.map_properties = map_properties
# initial traversability map
self.tmap = tmap
# determine the occupancy map - binary mask where ti value is 1
self.tmap_bin = (tmap == 1)*1
# init probability of occupancy map
self.pmap = self.tmap_bin*self.init_p_occ
# init tmap_aug
self.agument_tmap()
# set the initilised flag
self.tmap_loaded = True
def set_gplane(self, gplane, gplane_emap, gp_emap_prop, gp_ec):
'''
Input:
-gplane GroundPlane object
-gplane_emap numpy elevation map, in cm
-gp_emap_prop dictionary containing the map map_properties
-gp_ec the error code for the elevation map in m; max_z value + 1
'''
self.gp_plane_map = gplane
self.gp_emap = gplane_emap
self.gp_emap_prop = gp_emap_prop
self.gp_ec = gp_ec
self.gplane_loaded = True
assert('cell_size' in gp_emap_prop)
def update_pocc(self, uwb_mask_pcd, uwb_scan ):
if self.init() == False:
return
# update ground spectrum
self.determine_ground_spec(uwb_mask_pcd)
# evaluate measurement likelyhood
self.sensor_model.prob_sen(uwb_mask_pcd, self.ground_spectrum, uwb_scan)
# update pocc
self.bayesian_update(uwb_mask_pcd)
def determine_ground_spec(self, uwb_mask_pcd, radar_range_res=0.3):
'''
Determine the ground spectrum vector for the current ground
Input
-uwb_pcd, pointcloud representation of where the uwb_beamwidth is, relative to the map frame
Returns: Dictionary, contianing the ground intensity values indexed by the range bin
Note not all range bins will have valid intensity values, if unknown -1 -1 will
be present
'''
if self.init() == False:
return 0
ranges = uwb_mask_pcd.channels[0].values
# angle scalar for the pdd
angle_scalars = self._evaluate_angle_scalar(uwb_mask_pcd)
# determine the sensor vector
s_vec = self._evalaute_sensor_vector(uwb_mask_pcd)
# get the incident angle mask
angle_mask = self._evaluate_incident_angle(uwb_mask_pcd, s_vec)
# determine the incident angles and weights for each range gate
angle_weights_gated = {}
# range gate the angles and determine the expected ground return for each range bin
for i, angle in enumerate(angle_mask):
if i > 0:
bin_index = int(m.floor(ranges[i]/radar_range_res))
if bin_index in angle_weights_gated:
angle_weights_gated[bin_index].append((angle, angle_scalars[i]))
else:
angle_weights_gated[bin_index] = [(angle, angle_scalars[i])]
# for each range bin evaluate the expected ground return [iavg, imax, imin]
# if unknown -1
intensity_spectrum = {}
for bin_index, angles_weights in angle_weights_gated.iteritems():
# only request intensity if there are valid incident angles
intensity_spectrum[bin_index] = self.expected_ground_return(angles_weights, bin_index)
self.ground_spectrum = intensity_spectrum
return 1
def bayesian_update(self, uwb_pcd):
points = uwb_pcd.points
ps_occ = uwb_pcd.channels[9].values
ps_empty = uwb_pcd.channels[10].values
region = uwb_pcd.channels[5].values
for i, point in enumerate(points):
if i > 0:
if region[i] != -1 and ps_occ[i] != -1 and ps_empty[i] != -1:
[row_index, col_index] = cartesian_2_index(point.x, point.y, self.map_properties)
pri = self.pmap[row_index, col_index]
# bayesian update
pocc = (ps_occ[i]*pri)/(ps_occ[i]*pri + ps_empty[i]*(1-pri))
assert(pocc <= 1)
self.pmap[row_index, col_index] = pocc
# update tmap
self.agument_tmap()
def agument_tmap(self):
# apply a saftey factor to evaluate the safe probability of occupancy map
pocc_sf = self.pmap + self.saftey_value
# apply piece wise function to saturate the probabilities
procc_sf = np.piecewise(pocc_sf, [pocc_sf <= 1, pocc_sf > 1], [lambda pocc_sf:pocc_sf , 1])
# calculate the augmented traversability map
self.tmap_aug = self.tmap*procc_sf
def init(self):
return self.tmap_loaded == True and self.gplane_loaded == True and self.gamma_model_loaded == True
def update_ground_model(self, rbin_ids, gamma_avgs, gamma_stds, counts):
'''
Update the ground gamma model. A gamma value will only be present if there has been at least one update to
a range bin
'''
self.gamma_model_loaded = True
self.gamma_rbin_ids = rbin_ids
self.gamma_avgs = gamma_avgs
self.gamma_stds = gamma_stds
self.count = counts
def _evalaute_sensor_vector(self, uwb_fov_mask):
'''
Evaluates the senor vector, which is a vector from the sensor orging to a point on the ground
which is projected from the x,y positions of the UWB_FOV_Mask to the ground plane evaluate using 1x1m grids.
Given UWB_MASK format sensor_msgs.Pointcloud, encoded with meta data such as state.
evaluate the sensing vector and update the range channel of the UWB_MASK to the true_range
'''
points = uwb_fov_mask.points
sensor_pos = points[0]
S = np.array([sensor_pos.x, sensor_pos.y, sensor_pos.z])
zero_vector = np.array([0.0,0.0,0.0])
s_list = []
s_list.append(zero_vector)
for i, point in enumerate(points):
if i > 0:
# project the mask point into the ground using the higher resolution elevation map
x_loc = point.x
y_loc = point.y
[row_index, col_index] = cartesian_2_index(x_loc, y_loc, self.gp_emap_prop)
# determine the z projection of the mask point in meters
z_proj = self.gp_emap[row_index, col_index]/100.0
# check z value of cell is known
if z_proj == self.gp_ec:
s_list.append(zero_vector)
else:
assert(z_proj != self.gp_ec)
# therefore the point on the ground is
G = np.array([x_loc, y_loc, z_proj])
# determine the sensing vector - originates from ground
# vector from ground point to sensor origin
v_gs = S - G
s_list.append(v_gs)
return s_list
def _evaluate_incident_angle(self, uwb_fov_mask, s_list):
'''
Evaluates the incident angle between the sensing vector and the ground
Input: sensing vector, list of numpy vectors
the first sensing vector is a zero vector, othe rest of the data vectors
that orginate from the ground and travel to the radar position , unless the elevation value
is unknown
'''
zero_vector = np.array([0.0,0.0,0.0])
points = uwb_fov_mask.points
i_angles = []
# to align the angles vector with all the points
i_angles.append(-1)
for i, point in enumerate(points):
if i > 0:
sensor_vector = s_list[i]
if (sensor_vector != zero_vector).all():
x_loc = point.x
y_loc = point.y
normal_vector = self.gp_plane_map.get_normal(x_loc, y_loc)
# check normal vector is known
if (normal_vector == zero_vector).all():
i_angles.append(-1)
else:
aspect_angle = angle_between_vectors(normal_vector, sensor_vector)
incident_angle = m.fabs(90 - aspect_angle)
i_angles.append(incident_angle)
else:
i_angles.append(-1)
return i_angles
def _evaluate_angle_scalar(self, uwb_fov_mask, uwb_beamwidth=40):
'''
Evaluates a scalar that compensates for the intensity variation in the UWB radar beam pattern
'''
RAD2DEG = 180.0/m.pi
points = uwb_fov_mask.points
azi_channel = uwb_fov_mask.channels[1].values
angle_scalars = []
# place holder scalar for the sensor origin point
angle_scalars.append(0)
for i, point in enumerate(points):
if i > 0:
# get azimuth position in degrees
azimuth = azi_channel[i]*RAD2DEG
a_scalar = inverse_parabola(azimuth, uwb_beamwidth)
angle_scalars.append(a_scalar)
return angle_scalars
@staticmethod
def eval_incident_angle_sum(angle_weights):
'''
input: angle_weights, a list of tupples containing an incident angle and an assoicated weight
angle, in degrees -1 if the incident angles is unknown
returns: sum of the incident angle multiplied by its weight or -1 if there are invalid anlges in
the list of angle_weights
'''
incident_angle_sum = 0
DEG2RAD = m.pi/180.0
for i, angle_weight in enumerate(angle_weights):
angle, weight = angle_weight[0]*DEG2RAD, angle_weight[1]
if angle_weight[0] == -1:
return -1
assert(weight > 0)
incident_angle_sum = incident_angle_sum + m.sin(angle)*weight
assert(m.sin(angle)*weight > 0)
return incident_angle_sum
def expected_ground_return(self, angle_weights, range_bin_index):
'''
input: angle_weights, a list of tupples containing an incident angle and an assoicated weight
angle, in degrees -1 if the incident angles is unknown
'''
incident_angle_sum = self.eval_incident_angle_sum(angle_weights)
# check the current mask does not lie in an unknown region
if incident_angle_sum == -1:
return [-1, -1]
max_index = len(self.count) - 1
index_offset = self.gamma_rbin_ids[0]
# index used to acess the gamma vectors which are aligned to particular range bins
index = range_bin_index - index_offset
# check there are measurements are in a valid range bin
if index > max_index or index < 0:
return [-1, -1]
else:
if self.count[index] >= self.min_count:
mean_gamma = self.gamma_avgs[index]
std_gamma = self.gamma_stds[index]
ground_return = mean_gamma*incident_angle_sum
ground_return_max = (mean_gamma + std_gamma)*incident_angle_sum
return [ground_return, ground_return_max]
else:
return [-1, -1]
config = {
'sensor_sigma': 0.1,
'sensor_parameters': [
{
'delta': [1, 0],
'orientation': 0,
'fov': 180,
},
{
'delta': [-1, 0],
'orientation': 180,
'fov': 180,
},
]
}
sm = SensorModel(config)
config1 = {
'start': [0, 0],
'sigma_initial': [[0.1, 0],
[0, 0.1]],
'id': 1
}
motion1 = RobotMotion(config1)
motion1.pos = np.array([0, 0])
motion1.th = math.radians(0)
motion2 = RobotMotion(config1)
motion2.pos = np.array([10, 0])
print(sm.get_measurement(motion1, 0, motion2))
class ParticleFilter:
'''
Monte Carlo Localization based on odometry and a laser scanner.
'''
def __init__(self):
# Get parameters
self.particle_filter_frame = rospy.get_param("~particle_filter_frame")
self.MAX_PARTICLES = int(rospy.get_param("~num_particles"))
self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles"))
self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1"))
self.ANGLE_STEP = int(rospy.get_param("~angle_step"))
self.DO_VIZ = bool(rospy.get_param("~do_viz"))
# MCL algorithm
self.iters = 0
self.lidar_initialized = False
self.odom_initialized = False
self.map_initialized = False
self.last_odom_pose = None # last received odom pose
self.last_stamp = None
self.laser_angles = None
self.downsampled_angles = None
self.state_lock = Lock()
# paritcles
self.inferred_pose = None
self.particles = np.zeros((self.MAX_PARTICLES, 3))
self.particle_indices = np.arange(self.MAX_PARTICLES)
self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES)
# Initialize the models
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
# initialize the state
self.smoothing = Utils.CircularArray(10)
self.timer = Utils.Timer(10)
self.initialize_global()
# map
self.permissible_region = None
self.SHOW_FINE_TIMING = False
# these topics are for visualization
self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose",
PoseStamped,
queue_size=1)
self.particle_pub = rospy.Publisher("/pf/viz/particles",
PoseArray,
queue_size=1)
self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan",
LaserScan,
queue_size=1)
self.path_pub = rospy.Publisher('/pf/viz/path', Path, queue_size=1)
self.path = Path()
if self.PUBLISH_ODOM:
self.odom_pub = rospy.Publisher("/pf/pose/odom",
Odometry,
queue_size=1)
# these topics are for coordinate space things
self.pub_tf = tf.TransformBroadcaster()
# these topics are to receive data from the racecar
self.laser_sub = rospy.Subscriber(rospy.get_param(
"~scan_topic", "/scan"),
LaserScan,
self.callback_lidar,
queue_size=1)
self.odom_sub = rospy.Subscriber(rospy.get_param(
"~odom_topic", "/odom"),
Odometry,
self.callback_odom,
queue_size=1)
self.pose_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose,
queue_size=1)
self.click_sub = rospy.Subscriber("/clicked_point",
PointStamped,
self.clicked_pose,
queue_size=1)
print "Finished initializing, waiting on messages..."
def initialize_global(self):
'''
Spread the particle distribution over the permissible region of the state space.
'''
print("GLOBAL INITIALIZATION")
# randomize over grid coordinate space
self.state_lock.acquire()
print('Waiting for map..')
while not self.sensor_model.map_set:
continue
self.map_initialized = True
self.permissible_region = self.sensor_model.permissible_region
self.map = self.sensor_model.map
self.map_info = self.sensor_model.map_info
permissible_x, permissible_y = np.where(self.permissible_region == 1)
indices = np.random.randint(0,
len(permissible_x),
size=self.MAX_PARTICLES)
permissible_states = np.zeros((self.MAX_PARTICLES, 3))
permissible_states[:, 0] = permissible_y[indices]
permissible_states[:, 1] = permissible_x[indices]
permissible_states[:, 2] = np.random.random(
self.MAX_PARTICLES) * np.pi * 2.0
Utils.map_to_world(permissible_states, self.map_info)
self.particles = permissible_states
self.weights[:] = 1.0 / self.MAX_PARTICLES
self.state_lock.release()
def initialize_particles_pose(self, pose):
'''
Initialize particles in the general region of the provided pose.
'''
print "SETTING POSE"
print pose
self.state_lock.acquire()
self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES)
self.particles[:, 0] = pose.position.x + np.random.normal(
loc=0.0, scale=0.5, size=self.MAX_PARTICLES)
self.particles[:, 1] = pose.position.y + np.random.normal(
loc=0.0, scale=0.5, size=self.MAX_PARTICLES)
self.particles[:, 2] = Utils.quaternion_to_angle(
pose.orientation) + np.random.normal(
loc=0.0, scale=0.4, size=self.MAX_PARTICLES)
self.state_lock.release()
def clicked_pose(self, msg):
'''
Receive pose messages from RViz and initialize the particle distribution in response.
'''
print('clicked_pose')
if isinstance(msg, PointStamped):
self.initialize_particles_pose(msg.pose.pose)
#self.initialize_global()
elif isinstance(msg, PoseWithCovarianceStamped):
self.initialize_particles_pose(msg.pose.pose)
def publish_tf(self, pose, stamp=None):
""" Publish a tf for the car. This tells ROS where the car is with respect to the map. """
if stamp == None:
stamp = rospy.Time.now()
# this may cause issues with the TF tree. If so, see the below code.
self.pub_tf.sendTransform(
(pose[0], pose[1], 0),
tf.transformations.quaternion_from_euler(0, 0, pose[2]), stamp,
"/laser", "/map")
# also publish odometry to facilitate getting the localization pose
if self.PUBLISH_ODOM:
odom = Odometry()
odom.header = Utils.make_header("/map", stamp)
odom.pose.pose.position.x = pose[0]
odom.pose.pose.position.y = pose[1]
odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2])
self.odom_pub.publish(odom)
"""
Our particle filter provides estimates for the "laser" frame
since that is where our laser range estimates are measure from. Thus,
We want to publish a "map" -> "laser" transform.
However, the car's position is measured with respect to the "base_link"
frame (it is the root of the TF tree). Thus, we should actually define
a "map" -> "base_link" transform as to not break the TF tree.
"""
# Get map -> laser transform.
map_laser_pos = np.array((pose[0], pose[1], 0))
map_laser_rotation = np.array(
tf.transformations.quaternion_from_euler(0, 0, pose[2]))
# Apply laser -> base_link transform to map -> laser transform
# This gives a map -> base_link transform
laser_base_link_offset = (0.265, 0, 0)
map_laser_pos -= np.dot(
tf.transformations.quaternion_matrix(
tf.transformations.unit_vector(map_laser_rotation))[:3, :3],
laser_base_link_offset).T
# Publish transform
self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp,
self.particle_filter_frame, "/map")
def publish_particles(self, particles):
pa = PoseArray()
pa.header = Utils.make_header("map")
pa.poses = Utils.particles_to_poses(particles)
self.particle_pub.publish(pa)
def publish_scan(self, angles, ranges):
ls = LaserScan()
ls.header = Utils.make_header("laser", stamp=self.last_stamp)
ls.angle_min = np.min(angles)
ls.angle_max = np.max(angles)
ls.angle_increment = np.abs(angles[0] - angles[1])
ls.range_min = 0
ls.range_max = np.max(ranges)
ls.ranges = ranges
self.pub_fake_scan.publish(ls)
def visualize(self):
'''
Publish various visualization messages.
'''
if not self.DO_VIZ:
return
if self.pose_pub.get_num_connections() > 0 and isinstance(
self.inferred_pose, np.ndarray):
ps = PoseStamped()
ps.header = Utils.make_header("map")
ps.pose.position.x = self.inferred_pose[0]
ps.pose.position.y = self.inferred_pose[1]
ps.pose.orientation = Utils.angle_to_quaternion(
self.inferred_pose[2])
self.pose_pub.publish(ps)
if self.iters % 10 == 0:
self.path.header = ps.header
self.path.poses.append(ps)
self.path_pub.publish(self.path)
if self.particle_pub.get_num_connections() > 0:
if self.MAX_PARTICLES > self.MAX_VIZ_PARTICLES:
# randomly downsample particles
proposal_indices = np.random.choice(self.particle_indices,
self.MAX_VIZ_PARTICLES,
p=self.weights)
# proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES)
self.publish_particles(self.particles[proposal_indices, :])
else:
self.publish_particles(self.particles)
if self.pub_fake_scan.get_num_connections() > 0 and isinstance(
self.sensor_model.particle_scans, np.ndarray):
# generate the scan from the point of view of the inferred position for visualization
inferred_scans = self.sensor_model.get_scans(
self.inferred_pose[None, :])
self.publish_scan(self.downsampled_angles, inferred_scans[0, :])
def callback_lidar(self, msg):
'''
Initializes reused buffers, and stores the relevant laser scanner data for later use.
'''
if not isinstance(self.laser_angles, np.ndarray):
print "...Received first LiDAR message"
self.laser_angles = np.linspace(msg.angle_min, msg.angle_max,
len(msg.ranges))
self.downsampled_angles = np.copy(
self.laser_angles[0::self.ANGLE_STEP]).astype(np.float32)
#self.viz_queries = np.zeros((self.downsampled_angles.shape[0],3), dtype=np.float32)
#self.viz_ranges = np.zeros(self.downsampled_angles.shape[0], dtype=np.float32)
#print self.downsampled_angles.shape[0]
self.downsampled_ranges = np.array(msg.ranges[::self.ANGLE_STEP])
self.lidar_initialized = True
def callback_odom(self, msg):
'''
Store deltas between consecutive odometry messages in the coordinate space of the car.
Odometry data is accumulated via dead reckoning, so it is very inaccurate on its own.
'''
position = np.array(
[msg.pose.pose.position.x, msg.pose.pose.position.y])
orientation = Utils.quaternion_to_angle(msg.pose.pose.orientation)
pose = np.array([position[0], position[1], orientation])
if isinstance(self.last_odom_pose, np.ndarray):
rot = Utils.rotation_matrix(-self.last_odom_pose[2])
delta = np.array([position - self.last_odom_pose[0:2]]).transpose()
local_delta = (rot * delta).transpose()
# changes in x,y,theta in local coordinate system of the car
self.odometry_data = np.array([
local_delta[0, 0], local_delta[0, 1],
orientation - self.last_odom_pose[2]
])
self.last_odom_pose = pose
self.last_stamp = msg.header.stamp
self.odom_initialized = True
else:
print "...Received first Odometry message"
self.last_odom_pose = pose
# this topic is slower than lidar, so update every time we receive a message
self.update()
def expected_pose(self, particles):
# returns the expected value of the pose given the particle distribution
return np.dot(particles.transpose(), self.weights)
def MCL(self, odom, scans):
# Implement the MCL algorithm
# using the sensor model and the motion model
#
# Make sure you include some way to initialize
# your particles, ideally with some sort
# of interactive interface in rviz
proposal_indices = np.random.choice(self.particle_indices,
self.MAX_PARTICLES,
p=self.weights)
proposal_distribution = self.particles[proposal_indices, :]
if self.SHOW_FINE_TIMING:
t_propose = time.time()
#print("resample",self.expected_pose(proposal_distribution))
# compute the motion model to update the proposal distribution
proposal_distribution = self.motion_model.evaluate(
proposal_distribution, odom)
if self.SHOW_FINE_TIMING:
t_motion = time.time()
#print("motion model:", self.expected_pose(proposal_distribution))
# compute the sensor model
self.weights = self.sensor_model.evaluate(proposal_distribution, scans)
if self.SHOW_FINE_TIMING:
t_sensor = time.time()
# normalize importance weights
self.weights /= np.sum(self.weights)
#print("sensor model:", self.expected_pose(proposal_distribution))
if self.SHOW_FINE_TIMING:
t_norm = time.time()
t_total = (t_norm - t) / 100.0
if self.SHOW_FINE_TIMING and self.iters % 10 == 0:
print "MCL: propose: ", np.round((t_propose-t)/t_total, 2), "motion:", np.round((t_motion-t_propose)/t_total, 2), \
"sensor:", np.round((t_sensor-t_motion)/t_total, 2), "norm:", np.round((t_norm-t_sensor)/t_total, 2)
# save the particles
self.particles = proposal_distribution
#print(self.expected_pose(self.particles))
# Publish a transformation frame between the map
# and the particle_filter_frame.
def update(self):
'''
Apply the MCL function to update particle filter state.
Ensures the state is correctly initialized, and acquires the state lock before proceeding.
'''
if self.lidar_initialized and self.odom_initialized and self.map_initialized:
if self.state_lock.locked():
print("Concurrency error avoided")
else:
self.state_lock.acquire()
self.timer.tick()
self.iters += 1
t1 = time.time()
scans = np.copy(self.downsampled_ranges).astype(np.float32)
odom = np.copy(self.odometry_data)
self.odometry_data = np.zeros(3)
# run the MCL update algorithm
self.MCL(odom, scans)
# compute the expected value of the robot pose
self.inferred_pose = self.expected_pose(self.particles)
self.state_lock.release()
t2 = time.time()
# publish transformation frame based on inferred pose
self.publish_tf(self.inferred_pose, self.last_stamp)
# this is for tracking particle filter speed
ips = 1.0 / (t2 - t1)
self.smoothing.append(ips)
if self.iters % 10 == 0:
print "iters per sec:", int(
self.timer.fps()), " possible:", int(
self.smoothing.mean())
self.visualize()
class ParticleFilter(object):
"""
This is the main particle filter object.
"""
def __init__(self, map_file='../data/map/wean.dat'):
self.map = Map(map_file)
#self.map.display_gaussian([], 'Gaussian Blurred Map')
self._number_particles = 1000
self.particles = list()
self._particle_probabilities = []
for _ in range(0, self._number_particles):
print 'Initializing particle', _
particle = Particle(self.map)
#particle.x = 4080 + np.random.normal(scale=35)
#particle.y = 3980 + np.random.normal(scale=15)
#particle.theta = math.pi + 0.1 + np.random.normal(scale=.1)
self.particles.append(particle)
self._particle_probabilities.append(1)
self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1)
self._sensor_model = SensorModel(self.map)
self._ranges = []
def log(self, file_handle):
line = list()
for particle in self.particles:
loc = str(particle.x) + ',' + str(particle.y)
line.append(loc)
file_handle.write(';'.join(line))
file_handle.write('\n')
def display(self):
self.map.display(self.particles, ranges=self._ranges)
def update(self, line):
"""
Update step.
Reading is a single reading (i.e. line) from the log file
Could be either an odometry or laser reading
"""
measurement_type = line[0] # O = odometry, L = laser scan
measurements = np.fromstring(line[2:], sep=' ')
odometry = measurements[0:3]
time_stamp = measurements[-1] # last variable
for particle in self.particles:
self._motion_model.update(particle, odometry, time_stamp)
if measurement_type == "L":
odometry_laser = measurements[
3:6] # coordinates of the laser in standard odometry frame
self._ranges = measurements[
6:-1] # exclude last variable, the time stamp
self._particle_probabilities = list(
) # unnormalized sensor model probabilities of the particles
for particle in self.particles:
self._particle_probabilities.append(
self._sensor_model.get_probability(particle, self._ranges))
argsorted_probabilities = np.argsort(
-np.asarray(self._particle_probabilities))
self.particles[argsorted_probabilities[0]].debug = True
self.particles[argsorted_probabilities[1]].debug = True
self.particles[argsorted_probabilities[2]].debug = True
def _resample(self):
"""
Resamples the particles given unnormalized particle probabilites
"""
particle_probabilities = np.asarray(
self._particle_probabilities) / sum(
self._particle_probabilities) # normalize
indices = np.random.choice(range(0, self._number_particles),
size=self._number_particles,
replace=True,
p=particle_probabilities)
indices.sort()
previous_index = -1
new_particles = list()
for index in indices:
if index != previous_index:
new_particles.append(self.particles[index])
else:
new_particles.append(deepcopy(self.particles[index]))
previous_index = index
self.particles = new_particles
def __init__(self):
# using locks so threads can't access self.particles at the same time (avoiding race condition)
self.lock = Lock()
self.lock.acquire()
# Get parameters
self.particle_filter_frame = \
rospy.get_param("~particle_filter_frame") # should be '/base_link_pf' in sim and '/base_link' on car
# Initialize the models
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
# Topics
self.odom_topic = rospy.get_param("~odom_topic")
self.scan_topic = rospy.get_param("~scan_topic")
self.particle_marker_topic = '/particle_marker'
self.click_topic = '/move_base_simple/goal'
self.pose_array_topic = '/pose_array'
self.inferred_pose_topic = '/inferred_pose'
self.error_x_topic = '/error_x'
self.error_y_topic = '/error_y'
self.error_t_topic = '/error_t'
# Subscribers and Publishers
rospy.Subscriber(
self.odom_topic, Odometry,
self.odom_callback) # '/odom' for sim and 'vesc/odom' for car
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_callback)
self.tf_broadcaster = TransformBroadcaster()
# Listening to clicks in rviz and publishing particle markers to rviz
rospy.Subscriber(self.click_topic, PoseStamped, self.click_callback)
self.particle_marker_pub = rospy.Publisher(self.particle_marker_topic,
Marker,
queue_size=1)
self.pose_array_pub = rospy.Publisher(self.pose_array_topic,
PoseArray,
queue_size=1)
self.inferred_pose_pub = rospy.Publisher(self.inferred_pose_topic,
Marker,
queue_size=1)
self.error_x_pub = rospy.Publisher(self.error_x_topic,
Float32,
queue_size=1)
self.error_y_pub = rospy.Publisher(self.error_y_topic,
Float32,
queue_size=1)
self.error_t_pub = rospy.Publisher(self.error_t_topic,
Float32,
queue_size=1)
self.rate = rospy.Rate(30) # should be greater than 20 Hz
# Implement the MCL algorithm
# using the sensor model and the motion model
#
# Make sure you include some way to initialize
# your particles, ideally with some sort
# of interactive interface in rviz
#
# Publish a transformation frame between the map
# and the particle_filter_frame.
# Initializing particles and weights
self.num_particles = rospy.get_param('~num_particles')
# currently intializing all particles as origin
self.particles = np.zeros((self.num_particles, 3))
# all particles are initially equally likely
self.weights = np.full((self.num_particles), 1. / self.num_particles)
# for error calculations
self.groundtruth = [0, 0, 0]
# send drive commands to test in sim
self.drive_pub = rospy.Publisher('/drive',
AckermannDriveStamped,
queue_size=1)
self.drive_msg = AckermannDriveStamped()
self.drive_msg.drive.steering_angle = 0
self.drive_msg.drive.speed = 1
if self.SIM == False: # don't toggle on driving if working on real robot
self.DRIVE = False
self.publish_transform()
self.lock.release()
class ParticleFilter:
# toggle robot driving in a circle in the simulator
DRIVING = True
# set to True if working in the simulator, else False
SIM = True
"""
*** IF WORKING ON THE REAL ROBOT: ***
- in params.yaml:
- change particle_filter_frame from 'base_link_pf' to 'base_link'
- change odom_topic from '/odom' to '/vesc/odom'
- in localize.launch:
- uncomment out region that initializes map server to use stata basement map
- use SIM = False and DRIVING = False
"""
def __init__(self):
# using locks so threads can't access self.particles at the same time (avoiding race condition)
self.lock = Lock()
self.lock.acquire()
# Get parameters
self.particle_filter_frame = \
rospy.get_param("~particle_filter_frame") # should be '/base_link_pf' in sim and '/base_link' on car
# Initialize the models
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
# Topics
self.odom_topic = rospy.get_param("~odom_topic")
self.scan_topic = rospy.get_param("~scan_topic")
self.particle_marker_topic = '/particle_marker'
self.click_topic = '/move_base_simple/goal'
self.pose_array_topic = '/pose_array'
self.inferred_pose_topic = '/inferred_pose'
self.error_x_topic = '/error_x'
self.error_y_topic = '/error_y'
self.error_t_topic = '/error_t'
# Subscribers and Publishers
rospy.Subscriber(
self.odom_topic, Odometry,
self.odom_callback) # '/odom' for sim and 'vesc/odom' for car
rospy.Subscriber(self.scan_topic, LaserScan, self.scan_callback)
self.tf_broadcaster = TransformBroadcaster()
# Listening to clicks in rviz and publishing particle markers to rviz
rospy.Subscriber(self.click_topic, PoseStamped, self.click_callback)
self.particle_marker_pub = rospy.Publisher(self.particle_marker_topic,
Marker,
queue_size=1)
self.pose_array_pub = rospy.Publisher(self.pose_array_topic,
PoseArray,
queue_size=1)
self.inferred_pose_pub = rospy.Publisher(self.inferred_pose_topic,
Marker,
queue_size=1)
self.error_x_pub = rospy.Publisher(self.error_x_topic,
Float32,
queue_size=1)
self.error_y_pub = rospy.Publisher(self.error_y_topic,
Float32,
queue_size=1)
self.error_t_pub = rospy.Publisher(self.error_t_topic,
Float32,
queue_size=1)
self.rate = rospy.Rate(30) # should be greater than 20 Hz
# Implement the MCL algorithm
# using the sensor model and the motion model
#
# Make sure you include some way to initialize
# your particles, ideally with some sort
# of interactive interface in rviz
#
# Publish a transformation frame between the map
# and the particle_filter_frame.
# Initializing particles and weights
self.num_particles = rospy.get_param('~num_particles')
# currently intializing all particles as origin
self.particles = np.zeros((self.num_particles, 3))
# all particles are initially equally likely
self.weights = np.full((self.num_particles), 1. / self.num_particles)
# for error calculations
self.groundtruth = [0, 0, 0]
# send drive commands to test in sim
self.drive_pub = rospy.Publisher('/drive',
AckermannDriveStamped,
queue_size=1)
self.drive_msg = AckermannDriveStamped()
self.drive_msg.drive.steering_angle = 0
self.drive_msg.drive.speed = 1
if self.SIM == False: # don't toggle on driving if working on real robot
self.DRIVE = False
self.publish_transform()
self.lock.release()
def odom_callback(self, odom_data):
'''
whenever you get odometry data, use the motion model to update the particle positions
determine the "average" particle pose and publish that transform
:param odom_data: odometry data (type - Odometry)
:return: no return, but updates self.particles positions and publishes transform
'''
self.lock.acquire()
#start = time()
x = odom_data.pose.pose.position.x
y = odom_data.pose.pose.position.y
q_w = odom_data.pose.pose.orientation.w
q_z = odom_data.pose.pose.orientation.z
euler = transformations.euler_from_quaternion(
np.array([0, 0, q_z, q_w]))
self.groundtruth = [x, y, euler]
self.particles = self.motion_model.evaluate(
self.particles,
self.motion_model.get_delta_x(odom_data),
noise=0.2)
self.publish_transform()
#end = time()
#print('odom_callback time:', end-start)
self.lock.release()
def scan_callback(self, scan_data):
'''
whenever you get sensor data, use the sensor model to compute the particle probabilities. Then resample the particles based on these probabilities
determine the "average" particle pose and publish that transform
:param scan_data: scan data (type - LaserScan)
:return: no return, but updates self.particles by resampling particles and publishes transform
'''
self.lock.acquire()
probabilities = self.sensor_model.evaluate(self.particles,
scan_data,
downsample=20,
sim=self.SIM)
# if map wasn't initialized in sensor_model, probabilites=None
if isinstance(probabilities, type(None)):
self.lock.release()
rospy.loginfo('no probabilities to use in scan_callback')
return
smooth_p = np.power(probabilities, [1 / 3.] * len(probabilities))
self.weights = smooth_p / np.sum(
smooth_p) # normalize weights so they sum to 1
# resample particles based on weights
self.particles = self.resample_particles()
self.publish_transform()
self.lock.release()
def resample_particles(self):
'''
reseample particles based on their probability weights
allow repeats of particles; to not allow, use replace=False
'''
# choose with indices in particles array to keep using probabilities as weights
indices = np.random.choice(self.num_particles,
self.num_particles,
p=self.weights)
# return particles with chosen indices
return self.particles[indices]
def publish_transform(self):
'''
anytime the particles are updated, determine the 'average' particle pose and broadcast that transform
:return: no return, but broadcast transform between 'map' frame and self.particle_filter_frame
'''
x_avg, y_avg, theta_avg = self.avg_pose()
q = transformations.quaternion_from_euler(0, 0, theta_avg)
# broadcast transformation
self.tf_broadcaster.sendTransform(
(x_avg, y_avg, 0), q, rospy.Time.now(), self.particle_filter_frame,
'map')
self.publish_pose_array()
#self.publish_particle_markers()
# Driving in the simulator
if self.DRIVING:
self.drive_pub.publish(self.drive_msg)
# publish inferred position in rviz as arrow
if self.inferred_pose_pub.get_num_connections > 0:
a = Marker()
a.header.frame_id = 'map'
a.type = Marker.ARROW
a.color = ColorRGBA(0, 1, 1, 1)
a.scale.x = 0.5
a.scale.y = 0.1
a.scale.z = 0.1
q = transformations.quaternion_from_euler(0, 0, theta_avg)
quat = Quaternion()
quat.x = q[0]
quat.y = q[1]
quat.z = q[2]
quat.w = q[3]
a.pose.position.x = x_avg
a.pose.position.y = y_avg
a.pose.orientation = quat
self.inferred_pose_pub.publish(a)
# for error plots
try:
x, y, theta = self.groundtruth
x_error = x_avg - x
y_error = y_avg - y
t_error = theta_avg - theta[2]
self.error_x_pub.publish(x_error)
self.error_y_pub.publish(y_error)
self.error_t_pub.publish(t_error)
except Exception as e:
rospy.loginfo(e)
def avg_pose(self):
'''
determine the average particle pose
** have to use mean of circular quantities for theta
** distirbution might be multi modal
:param: no params, but uses instance variable self.particles
:return: average particle pose as [x, y, theta]
'''
x = np.mean(self.particles[:, 0])
y = np.mean(self.particles[:, 1])
# circular mean is angle between average x component and y component of the angles data
theta = np.arctan2(np.mean(np.sin(self.particles[:, 2])),
np.mean(np.cos(self.particles[:, 2])))
return [x, y, theta]
def click_callback(self, pose_data):
'''
use click in rviz to initialize particles
:param pose_data: pose of rviz click (type - PoseStamped)
use 2D Nav Goal to guess car location
:return: no return, but updates self.particles
'''
self.lock.acquire()
x = pose_data.pose.position.x
y = pose_data.pose.position.y
q = pose_data.pose.orientation
theta = transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])[2]
# random points from normal distribution centered at [x, y, theta]
self.particles = np.random.normal([x, y, theta], 1,
(self.num_particles, 3))
self.weights = np.full((self.num_particles), 1. / self.num_particles)
self.publish_pose_array()
#self.publish_particle_markers()
self.lock.release()
def publish_particle_markers(self):
'''
publish particles as markers in rviz
'''
# if something is listening for marker publications
if self.particle_marker_pub.get_num_connections > 0:
m = Marker()
m.header.frame_id = 'map'
m.type = Marker.POINTS
#m.color = ColorRGBA(1, 0, 0, 1)
m.scale.x = 0.2
m.scale.y = 0.2
m.scale.z = 0.2
for i in range(len(self.particles)):
p = self.particles[i]
w = self.weights[i]
m.points.append(Point(p[0], p[1], 0))
m.colors.append(self.prob_to_color(w))
self.particle_marker_pub.publish(m)
def prob_to_color(self, prob):
'''
convert a probability to a color
red is high probability
orange
yellow
green
cyan
light blue
blue
purple
magenta
black is low
:param prob: probability between 0 and 1
:return: ColorRBGA corresponding to prob
'''
m = max(self.weights) # max weight
if prob > m * 0.9:
return ColorRGBA(1, 0, 0, 1)
if prob > m * 0.8:
return ColorRGBA(1, 0.5, 0, 1)
if prob > m * 0.7:
return ColorRGBA(1, 1, 0, 1)
if prob > m * 0.6:
return ColorRGBA(0, 1, 0, 1)
if prob > m * 0.5:
return ColorRGBA(0, 1, 1, 1)
if prob > m * 0.4:
return ColorRGBA(0, 0.5, 1, 1)
if prob > m * 0.3:
return ColorRGBA(0, 0, 1, 1)
if prob > m * 0.2:
return ColorRGBA(0.5, 0, 1, 1)
if prob > m * 0.1:
return ColorRGBA(1, 0, 1, 1)
else:
return ColorRGBA(0, 0, 0, 1)
def publish_pose_array(self):
'''
publish arrows in rviz for to visualize particles
'''
particles = np.copy(self.particles)
if self.pose_array_topic > 0:
pa = PoseArray()
pa.header.frame_id = 'map'
for p in particles[:20]:
pose = Pose()
pose.position = Point(p[0], p[1], 0)
q = transformations.quaternion_from_euler(0, 0, p[2])
quat = Quaternion()
quat.x = q[0]
quat.y = q[1]
quat.z = q[2]
quat.w = q[3]
pose.orientation = quat
pa.poses.append(pose)
self.pose_array_pub.publish(pa)
def setUp(self):
self.sensor_model = SensorModel(map)
self.particle = Particle(map)
self.particle.x = 250
self.particle.y = 250
self.particle.theta = 0
def __init__(self):
# Get parameters
self.particle_filter_frame = rospy.get_param("~particle_filter_frame")
self.MAX_PARTICLES = int(rospy.get_param("~num_particles"))
self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles"))
self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1"))
self.ANGLE_STEP = int(rospy.get_param("~angle_step"))
self.DO_VIZ = bool(rospy.get_param("~do_viz"))
# MCL algorithm
self.iters = 0
self.lidar_initialized = False
self.odom_initialized = False
self.map_initialized = False
self.last_odom_pose = None # last received odom pose
self.last_stamp = None
self.laser_angles = None
self.downsampled_angles = None
self.state_lock = Lock()
# paritcles
self.inferred_pose = None
self.particles = np.zeros((self.MAX_PARTICLES, 3))
self.particle_indices = np.arange(self.MAX_PARTICLES)
self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES)
# Initialize the models
self.motion_model = MotionModel()
self.sensor_model = SensorModel()
# initialize the state
self.smoothing = Utils.CircularArray(10)
self.timer = Utils.Timer(10)
self.initialize_global()
# map
self.permissible_region = None
self.SHOW_FINE_TIMING = False
# these topics are for visualization
self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose",
PoseStamped,
queue_size=1)
self.particle_pub = rospy.Publisher("/pf/viz/particles",
PoseArray,
queue_size=1)
self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan",
LaserScan,
queue_size=1)
self.path_pub = rospy.Publisher('/pf/viz/path', Path, queue_size=1)
self.path = Path()
if self.PUBLISH_ODOM:
self.odom_pub = rospy.Publisher("/pf/pose/odom",
Odometry,
queue_size=1)
# these topics are for coordinate space things
self.pub_tf = tf.TransformBroadcaster()
# these topics are to receive data from the racecar
self.laser_sub = rospy.Subscriber(rospy.get_param(
"~scan_topic", "/scan"),
LaserScan,
self.callback_lidar,
queue_size=1)
self.odom_sub = rospy.Subscriber(rospy.get_param(
"~odom_topic", "/odom"),
Odometry,
self.callback_odom,
queue_size=1)
self.pose_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose,
queue_size=1)
self.click_sub = rospy.Subscriber("/clicked_point",
PointStamped,
self.clicked_pose,
queue_size=1)
print "Finished initializing, waiting on messages..."
class World():
CLASS_NONTARGET = 0
CLASS_WILDLIFE = 1
CLASS_MINE = 2
CLASS_BENIGN = 3
def __init__(self, config):
# Create a world
self.config = config
self.surface_level = 0
self.fully_set_seed = rospy.get_param('~fully_set_seed')
def init_world(self, seed1, do_test=True):
self.seed = seed1
random.seed(seed1) # for repeatable trials
if self.fully_set_seed: # True for plotting not training
np.random.seed(seed1 + 1) # for repeatable trials
print('init_world seed1 test: random.randint(25,50)',
random.randint(25, 50))
self.init_world_once(do_test)
while not self.is_connected():
print("World graph is disconnected. Reinitializing...")
self.init_world_once(do_test)
def init_world_once(self, do_test=True):
# create a blank world, PRM style
num_nodes = self.config["num_nodes"]
connection_radius = self.config["connection_radius"]
environment_size = self.config["environment_size"]
self.sensor_range = self.config["sensor_range"]
# vertices
self.vertices_surface = []
self.vertices = []
count = 0
for vertex_idx in xrange(num_nodes):
x = random.uniform(0, environment_size[0])
y = random.uniform(0, environment_size[1])
for z in [-10, self.surface_level]:
v = Vertex(x, y, z, count)
count += 1
self.vertices.append(v)
if z == self.surface_level:
self.vertices_surface.append(True)
else:
self.vertices_surface.append(False)
# edges, stored as a matrix indexed as [vertex_start, vertex_end]
self.num_nodes = len(
self.vertices
) #doubled the input num_nodes in creating two layers of vertices instead of one
self.edge_matrix = [None] * self.num_nodes
self.edge_adjacency_idx_lists = [None] * self.num_nodes
self.edge_adjacency_edge_lists = [None] * self.num_nodes
for vertex_start_idx in xrange(self.num_nodes):
self.edge_matrix[vertex_start_idx] = [None] * self.num_nodes
self.edge_adjacency_idx_lists[vertex_start_idx] = []
self.edge_adjacency_edge_lists[vertex_start_idx] = []
for vertex_end_idx in xrange(self.num_nodes):
cost = distance(self.vertices[vertex_start_idx],
self.vertices[vertex_end_idx])
if vertex_start_idx != vertex_end_idx and cost <= connection_radius and not (
self.vertices_surface[vertex_start_idx]
and self.vertices_surface[vertex_end_idx]):
exists = True
else:
exists = False
edge = Edge(vertex_start_idx, vertex_end_idx, cost, exists)
self.edge_matrix[vertex_start_idx][vertex_end_idx] = edge
if exists:
self.edge_adjacency_idx_lists[vertex_start_idx].append(
vertex_end_idx)
self.edge_adjacency_edge_lists[vertex_start_idx].append(
edge)
if do_test:
self.test_indices()
#self.vertex_target_idx = self.create_target_idx() #single robot
# Define prior distribution
self.num_classes = self.config["num_classes"]
self.prob_of_class0 = self.config["prob_of_class0"]
self.prob_of_other_classes = (1 - self.prob_of_class0) / (
self.num_classes - 1)
self.prior = np.zeros(self.num_classes) #p_y0, p_y1, ...
for i in range(self.num_classes):
if i == 0:
self.prior[i] = self.prob_of_class0
else:
self.prior[i] = self.prob_of_other_classes
self.randomize_targets()
self.original_classes_y = copy.copy(self.classes_y)
# Define single random drop-off location (on surface) per world
temp = random.randint(0, len(self.vertices) - 1)
while self.vertices_surface[temp] != True:
temp = random.randint(0, len(self.vertices) - 1)
self.drop_off_idx = temp
'''
# Set all class 2 vertices to is_armed = True
self.is_armed_array = np.zeros(self.num_nodes, dtype=bool) #default, False for all
for v in xrange(len(self.classes_y)):
if self.classes_y[v] == 2: #it is a mine
self.is_armed_array[v] = True
'''
'''
# Vertex classes
# Each vertex has a number between 0 and n, which represents the class of the vertex
num_v_y0 = int(self.num_nodes*self.prob_of_class0)
num_v_other = self.num_nodes - num_v_y0
num_v_each = num_v_other/(self.num_classes-1)
classes_y = []
classes_y.extend([0]*num_v_y0) # add the non-target vertex classes
for i in range(1,self.num_classes):
classes_y.extend([i]*num_v_each) # add the target vertex classes
# G round truth vertex classes
random.shuffle(classes_y) #shuffle the list to randomize location of targets
'''
self.comms_range = self.config["comms_range"]
self.vertices_in_comms_range = self.generateCommsRangeVertices()
# Setup sensor model
self.sensor_model = SensorModel(self.config, self.num_nodes, self)
'''
#old likelihood function we have replaced
# it did not account for beyond sensor range -> 0
def robot_env_observations(self, vertex_robot, vertex_target):
# need robot location, robot sensor model, and the actual target location
# Do we want to have a field of view or assume 360 degree awareness for now?
# Do we really need to get closer to a target ever once we are in range?
# Where do we check if we have already sensed the particular target?
# wouldn't want to just keep circling back to the same target and reporting it
sensor_range = self.config["sensor_range"]
distance_to_target = distance(vertex_robot, vertex_target)
if distance_to_target <= sensor_range:
prob_true_pos = 0.95 - 0.01*distance_to_target
#prob_false_pos = 0.2 - 0.01*distance_to_target
# True positive
if random.random() <= prob_true_pos:
robot_senses_target = True
return vertex_target
else: # False negative
robot_senses_target = False
else:
# False positive
if random.random() <= prob_false_pos:
robot_senses_target = True
# for vertex in range pick a random one
vertex_near_robot =
return vertex_near_robot # near defined as within sensor_range
else: # True negative
robot_senses_target = False
if robot_senses_target:
return True
else:
return False
'''
def is_open_set_empty(self, open_set):
for i in open_set:
if i == True:
return False
return True
def is_connected(self):
num_vertices = len(self.vertices)
open_set = [False] * num_vertices
closed_set = [False] * num_vertices
open_set[
0] = True # Adding a vertex to open set, as our starting point
while not self.is_open_set_empty(open_set):
# find a vertex in open_set
v_current = open_set.index(True)
# remove it from the open set
open_set[v_current] = False
# add it to the closed set
closed_set[v_current] = True
# get the set of neighbours
neighbours = self.edge_adjacency_edge_lists[v_current]
# expand neighbouring nodes
for e in neighbours:
v_next = e.vertex_end_idx
if not closed_set[v_next] == True:
# If not in closed set, add to open set
open_set[v_next] = True
if False in closed_set: # you haven't visited at least one vertex
return False
return True
def reset_world(self):
print('original_classes_y', self.original_classes_y)
self.classes_y = copy.copy(self.original_classes_y)
print('self.classes_y after reset', self.classes_y)
def reset_seed(self):
# DO NOT USE DURING RUN OF ALG
# For use when plotting so identical trees have same random numbers
random.seed(self.seed) # for repeatable trials
if self.fully_set_seed: #Safety check: This should always be true when this function is called because its true in plot_results.launch
np.random.seed(self.seed + 1) # for repeatable trials
def target_inclusion_test(self):
target_inclusion_test_list = np.zeros(
self.num_classes -
1) #minus 1 for the void of target class (i.e. 0)
for vertex_class in self.classes_y:
if vertex_class == 1:
target_inclusion_test_list[0] = 1
elif vertex_class == 2:
target_inclusion_test_list[1] = 1
elif vertex_class == 3:
target_inclusion_test_list[2] = 1
if 0 in target_inclusion_test_list: #one target type not included, so we should randomize again
return False
return True
def randomize_targets(self):
# Randomize targets and ensure that all three target types are present
#print('world.randomize_targets test, seed test: random.randint(25,50)', random.randint(25,50))
#test_count = 0
#print('world.randomize_targets test NUM NODES', self.num_nodes)
#print('world.randomize_targets test len(self.vertices)',len(self.vertices))
# Vertex classes - ground truth
all_targets_included = False
while not all_targets_included:
#test_count+= 1
self.classes_y = np.array([])
for i in range(self.num_nodes):
#print('world.randomize_targets in-loop test, seed test: random.randint(25,50)', random.randint(25,50))
self.classes_y = np.append(
self.classes_y,
np.random.choice(a=len(self.prior), p=self.prior))
all_targets_included = self.target_inclusion_test()
#print('world.randomize_targets TEST_COUNT', test_count)
#print("classes_y",self.classes_y.shape)
#self.original_classes = self.classes_y
'''
def inclusive_randomize_targets(self):
while not self.target_inclusion_test():
self.randomize_targets()
'''
def disarm_target(self, vertex_idx, scorer):
if self.classes_y[vertex_idx] == World.CLASS_MINE:
self.classes_y[
vertex_idx] = World.CLASS_NONTARGET # Now is class 0 because target removed
scorer.action_reward(
vertex_idx, World.CLASS_MINE
) # Generate score #use reward_action_to_target from scorer
return True
return False
def pickup_target(self, vertex_idx, scorer):
if self.classes_y[vertex_idx] == World.CLASS_BENIGN:
self.classes_y[
vertex_idx] = World.CLASS_NONTARGET # Now is class 0 because target removed
scorer.action_reward(vertex_idx,
World.CLASS_BENIGN) # Generate score
return True
return False
def dropoff_target(self, vertex_idx, scorer, state):
if self.drop_off_idx == vertex_idx: # Now is class 0 because target removed
scorer.dropoff_reward(
state.picked_up_target_count) # Generate score
return True
return False
def report_target(self, vertex_idx, scorer, is_at_surface,
is_in_comms): # Check with Graeme DONE
#if classes_y[vertex_idx] == World.CLASS_WILDLIFE:
response = scorer.submit_target(vertex_idx, World.CLASS_WILDLIFE,
is_at_surface,
is_in_comms) # Generate score
print('reported something', response)
if response == scorer.RESPONSE_CORRECT:
print('report is correct')
self.classes_y[
vertex_idx] = World.CLASS_NONTARGET # Now is class 0 because target removed
return response
def generateCommsRangeVertices(self):
idx_list = []
for vertex in self.vertices:
if distance_to_base(
vertex
) < self.comms_range and vertex.position.z > self.surface_level - 0.0001:
idx_list.append(vertex.vertex_idx)
return idx_list #list of indices of vertices in comms range (and at surface)
def robot_env_observations(self, vertex_robot_idx):
#use for single target case
#likelihoods = self.sensor_model.all_likelihoods(vertex_robot_idx, self.vertex_target_idx)
#return a single observation, z based on the probability distribution
#return np.random.choice(a=len(self.vertices)+1, p=likelihoods)
v_in_range = np.array([], dtype=int)
z_array = np.array([], dtype=int)
for v in xrange(self.num_nodes):
if self.sensor_model.distances[vertex_robot_idx][
v] < self.sensor_range:
likelihoods = self.sensor_model.all_likelihoods(
self.classes_y[v])
v_in_range = np.append(v_in_range, v)
z_array = np.append(
z_array, np.random.choice(a=self.num_classes,
p=likelihoods))
return z_array, v_in_range
def create_target_idx(self):
#pick random vertex
random_vertex_idx = random.randrange(len(self.vertices))
#random_vertex = self.vertices[random_vertex_idx]
return random_vertex_idx
def pickle_graph(self):
graph = GraphPickle(self.vertices, self.edge_matrix)
return pickle.dumps(graph)
def test_indices(self):
# sanity check that graph was constructed correctly
rospy.loginfo("running graph test")
num_nodes = len(self.vertices)
if num_nodes <= 0:
rospy.logerr("empty graph created")
for vertex_idx in xrange(num_nodes):
if self.vertices[vertex_idx].vertex_idx != vertex_idx:
rospy.logerr("vertex index test failed")
for vertex_start_idx in xrange(num_nodes):
for vertex_end_idx in xrange(num_nodes):
#print(self.edge_matrix[vertex_start_idx][vertex_end_idx].vertex_start_idx,vertex_start_idx)
if self.edge_matrix[vertex_start_idx][
vertex_end_idx].vertex_start_idx != vertex_start_idx:
rospy.logerr("edge index start test failed")
if self.edge_matrix[vertex_start_idx][
vertex_end_idx].vertex_end_idx != vertex_end_idx:
rospy.logerr("edge index end test failed")
if self.edge_matrix[vertex_start_idx][vertex_end_idx].exists:
if vertex_end_idx not in self.edge_adjacency_idx_lists[
vertex_start_idx]:
rospy.logerr("edge_adjacency_idx_lists test failed")
def get_edges_out(self, vertex_idx):
return self.edge_matrix[vertex_idx]
def plot_world(self, ax, target_belief):
rospy.loginfo("plotting world")
num_nodes = len(self.vertices)
# plot possible edges
for vertex_start_idx in xrange(num_nodes):
for vertex_end_idx in xrange(num_nodes):
if self.edge_matrix[vertex_start_idx][
vertex_end_idx].potentially_exists:
vertex_start = self.vertices[vertex_start_idx]
vertex_end = self.vertices[vertex_end_idx]
ax.plot([vertex_start.position.x, vertex_end.position.x],
[vertex_start.position.y, vertex_end.position.y],
':k',
zorder=1)
# plot actual edges
for vertex_start_idx in xrange(num_nodes):
for vertex_end_idx in xrange(num_nodes):
if self.edge_matrix[vertex_start_idx][vertex_end_idx].exists:
vertex_start = self.vertices[vertex_start_idx]
vertex_end = self.vertices[vertex_end_idx]
ax.plot([vertex_start.position.x, vertex_end.position.x],
[vertex_start.position.y, vertex_end.position.y],
'-r',
zorder=10)
# plot vertices
xs = []
ys = []
size_list = []
for vertex_idx in xrange(num_nodes):
xs.append(self.vertices[vertex_idx].position.x)
ys.append(self.vertices[vertex_idx].position.y)
#p = target_belief.prob_dist[vertex_idx] # Single target case
#sp = 2+10*p
#size_list.append(sp)
#ax.scatter(xs, ys, s=5, zorder=20)
# print(size_list)
self.h_scatter_plot = ax.scatter(xs, ys, s=size_list, zorder=20)
'''
# Plot goal location as blue star
pos_target = self.vertices[self.vertex_target_idx].position
ax.plot(pos_target.x, pos_target.y, 'b*', markersize=20, zorder=10)
'''
# Plot targets as blue stars
for v in xrange(len(self.classes_y)):
if self.classes_y[
v]: #it's not 0, hence is a target of some kind (1,2,or 3)
pos_target = self.vertices[v].position
ax.plot(pos_target.x,
pos_target.y,
'b*',
markersize=20,
zorder=10)
# labels, axis etc
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_xlim(0, self.config["environment_size"][0])
ax.set_ylim(0, self.config["environment_size"][1])
ax.grid(True)
ax.set_aspect('equal', 'box')
def plot_world_update(self, ax, target_belief):
# Check with Graeme: This is for size and color changes, and is not updated to account for prob_dist[v] being a list
# Should I use this for anything?
num_nodes = len(self.vertices)
size_list = []
colors_list = []
max_p = 0
for vertex_idx in xrange(num_nodes):
p = target_belief.prob_dist[vertex_idx]
if p >= max_p:
max_p = p
for vertex_idx in xrange(num_nodes):
p = target_belief.prob_dist[vertex_idx]
sp = 1 + 100 * (p / max_p)
size_list.append(sp)
p /= max_p
c = [0, p, 1 - p]
colors_list.append(c)
# size_list_round = [round(p, 1) for p in size_list]
# print(size_list_round)
self.h_scatter_plot.set_sizes(size_list)
self.h_scatter_plot.set_color(colors_list)
class ParticleFilter:
"""
Implements particle filtering for estimating the state of the robot car
"""
def __init__(
self,
publish_tf,
n_particles,
n_viz_particles,
odometry_topic,
motor_state_topic,
servo_state_topic,
scan_topic,
laser_ray_step,
exclude_max_range_rays,
max_range_meters,
speed_to_erpm_offset,
speed_to_erpm_gain,
steering_angle_to_servo_offset,
steering_angle_to_servo_gain,
car_length,
):
"""
Initializes the particle filter
publish_tf: Whether or not to publish the tf. Should be false in sim, true on real robot
n_particles: The number of particles
n_viz_particles: The number of particles to visualize
odometry_topic: The topic containing odometry information
motor_state_topic: The topic containing motor state information
servo_state_topic: The topic containing servo state information
scan_topic: The topic containing laser scans
laser_ray_step: Step for downsampling laser scans
exclude_max_range_rays: Whether to exclude rays that are beyond the max range
max_range_meters: The max range of the laser
speed_to_erpm_offset: Offset conversion param from rpm to speed
speed_to_erpm_gain: Gain conversion param from rpm to speed
steering_angle_to_servo_offset: Offset conversion param from servo position to steering angle
steering_angle_to_servo_gain: Gain conversion param from servo position to steering angle
car_length: The length of the car
"""
self.PUBLISH_TF = publish_tf
# The number of particles in this implementation, the total number of particles is constant.
self.N_PARTICLES = n_particles
self.N_VIZ_PARTICLES = n_viz_particles # The number of particles to visualize
# Cached list of particle indices
self.particle_indices = np.arange(self.N_PARTICLES)
# Numpy matrix of dimension N_PARTICLES x 3
self.particles = np.zeros((self.N_PARTICLES, 3))
# Numpy matrix containing weight for each particle
self.weights = np.ones(self.N_PARTICLES) / float(self.N_PARTICLES)
# A lock used to prevent concurrency issues. You do not need to worry about this
self.state_lock = Lock()
self.tfl = tf.TransformListener(
) # Transforms points between coordinate frames
# Get the map
map_msg = rospy.wait_for_message(MAP_TOPIC, OccupancyGrid)
self.map_info = map_msg.info # Save info about map for later use
# Create numpy array representing map for later use
array_255 = np.array(map_msg.data).reshape(
(map_msg.info.height, map_msg.info.width))
self.permissible_region = np.zeros_like(array_255, dtype=bool)
# Numpy array of dimension (map_msg.info.height, map_msg.info.width), with values 0: not permissible, 1: permissible
self.permissible_region[array_255 == 0] = 1
# Globally initialize the particles
# Publish particle filter state
# Used to create a tf between the map and the laser for visualization
self.pub_tf = tf.TransformBroadcaster()
# Publishes the expected pose
self.pose_pub = rospy.Publisher(PUBLISH_PREFIX + "/inferred_pose",
PoseStamped,
queue_size=1)
# Publishes a subsample of the particles
self.particle_pub = rospy.Publisher(PUBLISH_PREFIX + "/particles",
PoseArray,
queue_size=1)
# Publishes the most recent laser scan
self.pub_laser = rospy.Publisher(PUBLISH_PREFIX + "/scan",
LaserScan,
queue_size=1)
# Publishes the path of the car
self.pub_odom = rospy.Publisher(PUBLISH_PREFIX + "/odom",
Odometry,
queue_size=1)
rospy.sleep(1.0)
self.initialize_global()
# An object used for resampling
self.resampler = ReSampler(self.particles, self.weights,
self.state_lock)
# An object used for applying sensor model
self.sensor_model = SensorModel(
scan_topic,
laser_ray_step,
exclude_max_range_rays,
max_range_meters,
map_msg,
self.particles,
self.weights,
car_length,
self.state_lock,
)
# An object used for applying kinematic motion model
self.motion_model = KinematicMotionModel(
motor_state_topic,
servo_state_topic,
speed_to_erpm_offset,
speed_to_erpm_gain,
steering_angle_to_servo_offset,
steering_angle_to_servo_gain,
car_length,
self.particles,
self.state_lock,
)
self.permissible_x, self.permissible_y = np.where(
self.permissible_region == 1)
# Parameters/flags/vars for global localization
self.global_localize = False
self.global_suspend = False
self.ents = None
self.ents_sum = 0.0
self.noisy_cnt = 0
# number of regions to partition. Simulation: 25, Real: 5.
self.NUM_REGIONS = 25
# number of updates for regional localization. Simulation 5, Real: 3.
self.REGIONAL_ROUNDS = 5
self.regions = []
self.click_mode = True
self.debug_mode = False
if self.debug_mode:
self.global_localize = True # True when doing global localization
# precompute regions. Each region is represented by arrays of x, y indices on the map
region_size = len(self.permissible_x) / self.NUM_REGIONS
idx = np.argsort(self.permissible_y) # column-major
_px, _py = self.permissible_x[idx], self.permissible_y[idx]
for i in range(self.NUM_REGIONS):
self.regions.append((
_px[i * region_size:(i + 1) * region_size],
_py[i * region_size:(i + 1) * region_size],
))
# Subscribe to the '/initialpose' topic. Publised by RVIZ. See clicked_pose_cb function in this file for more info
# three different reactions to click on rviz
self.click_sub = rospy.Subscriber(
"/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose_cb,
queue_size=1,
)
self.init_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.reinit_cb,
queue_size=1)
rospy.wait_for_message(scan_topic, LaserScan)
print("Initialization complete")
def initialize_global(self):
"""
Initialize the particles to cover the map
"""
self.state_lock.acquire()
# Get in-bounds locations
permissible_x, permissible_y = np.where(self.permissible_region == 1)
# The number of particles at each location, each with different rotation
angle_step = 4
# The sample interval for permissible states
permissible_step = angle_step * len(
permissible_x) / self.particles.shape[0]
# Indices of permissible states to use
indices = np.arange(0, len(permissible_x),
permissible_step)[:(self.particles.shape[0] /
angle_step)]
# Proxy for the new particles
permissible_states = np.zeros((self.particles.shape[0], 3))
# Loop through permissible states, each iteration drawing particles with
# different rotation
for i in range(angle_step):
idx_start = i * (self.particles.shape[0] / angle_step)
idx_end = (i + 1) * (self.particles.shape[0] / angle_step)
permissible_states[idx_start:idx_end, 0] = permissible_y[indices]
permissible_states[idx_start:idx_end, 1] = permissible_x[indices]
permissible_states[idx_start:idx_end,
2] = i * (2 * np.pi / angle_step)
# Transform permissible states to be w.r.t world
utils.map_to_world(permissible_states, self.map_info)
# Reset particles and weights
self.particles[:, :] = permissible_states[:, :]
self.weights[:] = 1.0 / self.particles.shape[0]
# self.publish_particles(self.particles)
self.state_lock.release()
def reinit_cb(self, msg):
if self.debug_mode:
self.global_localize = True
def publish_tf(self, pose, stamp=None):
"""
Publish a tf between the laser and the map
This is necessary in order to visualize the laser scan within the map
pose: The pose of the laser w.r.t the map
stamp: The time at which this pose was calculated, defaults to None - resulting
in using the time at which this function was called as the stamp
"""
if stamp is None:
stamp = rospy.Time.now()
try:
# Lookup the offset between laser and odom
delta_off, delta_rot = self.tfl.lookupTransform(
"/laser_link", "/odom", rospy.Time(0))
# Transform offset to be w.r.t the map
off_x = delta_off[0] * np.cos(pose[2]) - delta_off[1] * np.sin(
pose[2])
off_y = delta_off[0] * np.sin(pose[2]) + delta_off[1] * np.cos(
pose[2])
# Broadcast the tf
self.pub_tf.sendTransform(
(pose[0] + off_x, pose[1] + off_y, 0.0),
quaternion_from_euler(
0, 0, pose[2] + euler_from_quaternion(delta_rot)[2]),
stamp,
"/odom",
"/map",
)
except (tf.LookupException
) as e: # Will occur if odom frame does not exist
print(e)
print("failed to find odom")
def expected_pose(self):
"""
Uses cosine and sine averaging to more accurately compute average theta
To get one combined value use the dot product of position and weight vectors
https://en.wikipedia.org/wiki/Mean_of_circular_quantities
returns: np array of the expected pose given the current particles and weights
"""
cosines = np.cos(self.particles[:, 2])
sines = np.sin(self.particles[:, 2])
theta = np.arctan2(np.dot(sines, self.weights),
np.dot(cosines, self.weights))
position = np.dot(self.particles[:, 0:2].transpose(), self.weights)
position[0] += (car_length / 2) * np.cos(theta)
position[1] += (car_length / 2) * np.sin(theta)
return np.array((position[0], position[1], theta), dtype=np.float)
def clicked_pose_cb(self, msg):
"""
Reinitialize particles and weights according to the received initial pose
Applies Gaussian noise to each particle's pose
HINT: use Utils.quaternion_to_angle()
Remember to use vectorized computation!
msg: '/initialpose' topic. RVIZ publishes a message to this topic when you specify an initial pose using its GUI
returns: nothing
"""
if self.click_mode:
self.state_lock.acquire()
pose = msg.pose.pose
print("SETTING POSE")
VAR_X = 0.001
VAR_Y = 0.001
VAR_THETA = 0.001
quat = pose.orientation
theta = utils.quaternion_to_angle(quat)
x = pose.position.x
y = pose.position.y
self.particles[:, 0] = np.random.normal(x, VAR_X,
self.particles.shape[0])
self.particles[:, 1] = np.random.normal(y, VAR_Y,
self.particles.shape[0])
self.particles[:, 2] = np.random.normal(theta, VAR_THETA,
self.particles.shape[0])
self.weights.fill(1 / self.N_PARTICLES)
self.state_lock.release()
def visualize(self):
"""
Visualize the current state of the filter
(1) Publishes a tf between the map and the laser. Necessary for visualizing the laser scan in the map
(2) Publishes the most recent laser measurement. Note that the frame_id of this message should be '/laser'
(3) Publishes a PoseStamped message indicating the expected pose of the car
(4) Publishes a subsample of the particles (use self.N_VIZ_PARTICLES).
Sample so that particles with higher weights are more likely to be sampled.
"""
self.state_lock.acquire()
self.inferred_pose = self.expected_pose()
if isinstance(self.inferred_pose, np.ndarray):
if self.PUBLISH_TF:
self.publish_tf(self.inferred_pose)
ps = PoseStamped()
ps.header = utils.make_header("map")
ps.pose.position.x = self.inferred_pose[0]
ps.pose.position.y = self.inferred_pose[1]
ps.pose.orientation = utils.angle_to_quaternion(
self.inferred_pose[2])
if self.pose_pub.get_num_connections() > 0:
self.pose_pub.publish(ps)
if self.pub_odom.get_num_connections() > 0:
odom = Odometry()
odom.header = ps.header
odom.pose.pose = ps.pose
self.pub_odom.publish(odom)
if self.particle_pub.get_num_connections() > 0:
if self.particles.shape[0] > self.N_VIZ_PARTICLES:
# randomly downsample particles
proposal_indices = np.random.choice(self.particle_indices,
self.N_VIZ_PARTICLES,
p=self.weights)
self.publish_particles(self.particles[proposal_indices, :])
else:
self.publish_particles(self.particles)
if self.pub_laser.get_num_connections() > 0 and isinstance(
self.sensor_model.last_laser, LaserScan):
self.sensor_model.last_laser.header.frame_id = "/laser"
self.sensor_model.last_laser.header.stamp = rospy.Time.now()
self.pub_laser.publish(self.sensor_model.last_laser)
self.state_lock.release()
def publish_particles(self, particles):
"""
Helper function for publishing a pose array of particles
particles: To particles to publish
"""
pa = PoseArray()
pa.header = utils.make_header("map")
pa.poses = utils.particles_to_poses(particles)
self.particle_pub.publish(pa)
def set_particles(self, region):
self.state_lock.acquire()
# Get in-bounds locations
permissible_x, permissible_y = region
assert len(permissible_x) >= self.particles.shape[0]
# The number of particles at each location, each with different rotation
angle_step = 4
# The sample interval for permissible states
permissible_step = angle_step * len(
permissible_x) / self.particles.shape[0]
# Indices of permissible states to use
indices = np.arange(0, len(permissible_x),
permissible_step)[:(self.particles.shape[0] /
angle_step)]
# Proxy for the new particles
permissible_states = np.zeros((self.particles.shape[0], 3))
# Loop through permissible states, each iteration drawing particles with
# different rotation
for i in range(angle_step):
idx_start = i * (self.particles.shape[0] / angle_step)
idx_end = (i + 1) * (self.particles.shape[0] / angle_step)
permissible_states[idx_start:idx_end, 0] = permissible_y[indices]
permissible_states[idx_start:idx_end, 1] = permissible_x[indices]
permissible_states[idx_start:idx_end,
2] = i * (2 * np.pi / angle_step)
# Transform permissible states to be w.r.t world
utils.map_to_world(permissible_states, self.map_info)
# Reset particles and weights
self.particles[:, :] = permissible_states[:, :]
self.weights[:] = 1.0 / self.particles.shape[0]
self.publish_particles(self.particles)
self.state_lock.release()
def global_localization(self):
self.sensor_model.reset_confidence()
candidate_num = self.particles.shape[0] / self.NUM_REGIONS
regional_particles = []
regional_weights = []
for i in range(self.NUM_REGIONS):
self.set_particles(self.regions[i])
cnt_updates = 0
while cnt_updates < self.REGIONAL_ROUNDS: # each cluster update
if self.sensor_model.do_resample: # if weights updated by sensor model
self.resampler.resample_low_variance()
self.visualize()
self.sensor_model.do_resample = False
cnt_updates += 1
self.state_lock.acquire()
candidate_indices = np.argsort(self.weights)[-candidate_num:]
candidates = self.particles[candidate_indices]
candidates_weights = self.weights[candidate_indices]
regional_particles.append(candidates.copy()) # save the particles
regional_weights.append(
candidates_weights) # save the particles' weights
self.state_lock.release()
self.state_lock.acquire()
self.particles[:] = np.concatenate(regional_particles)
self.weights[:] = np.concatenate(regional_weights)
self.weights /= self.weights.sum()
self.global_localize = False
self.global_suspend = True
self.sensor_model.do_confidence_update = True
self.state_lock.release()
def suspend_update(self):
self.state_lock.acquire()
ent = -((self.weights * np.log2(self.weights)).sum())
print("entropy ==", ent)
self.ents_sum += ent
self.ents.put(ent)
if self.ents.qsize() > 10:
self.ents_sum -= self.ents.get()
self.state_lock.release()
if self.ents_sum / 10 >= 8:
self.global_suspend = False
elif 2.0 < ent < 3.0:
self.resampler.resample_low_variance()
parser.add_argument('--output', default='results')
parser.add_argument('--num_particles', default=3000, type=int)
parser.add_argument('--visualize', action='store_false')
parser.add_argument('--path_to_raycast_map', default='raycast_map.npy')
args = parser.parse_args()
src_path_map = args.path_to_map
src_path_log = args.path_to_log
os.makedirs(args.output, exist_ok=True)
map_obj = MapReader(src_path_map)
occupancy_map = map_obj.get_map()
logfile = open(src_path_log, 'r')
motion_model = MotionModel()
sensor_model = SensorModel(occupancy_map)
resampler = Resampling()
num_particles = args.num_particles
# X_bar = init_particles_random(num_particles, occupancy_map)
X_bar = init_particles_freespace(num_particles, occupancy_map)
"""
Monte Carlo Localization Algorithm : Main Loop
"""
if not os.path.exists(args.path_to_raycast_map):
print("Start pre-computing the ray cast map")
raycast_map = sensor_model.precompute_raycast()
np.save(args.path_to_raycast_map, raycast_map)
print('Pre-compute of ray casting done!')
else:
raycast_map = np.load(args.path_to_raycast_map)