def mapCallBack(self, message):
# 2
self.map = Map(message)
self.width = self.map.width
self.height = self.map.height
sig = self.config['laser_sigma_hit']
coordinates = []
queryPts = []
rospy.sleep(1)
# 3
for col in range(0, self.width):
for row in range(0, self.height):
x, y = self.map.cell_position(row, col)
cellVal = self.map.get_cell(x, y)
if (cellVal > 0.5):
coordinates.append([x, y])
self.kdt = KDTree(coordinates)
for col in range(0, self.width):
for row in range(0, self.height):
dist, ind = self.kdt.query([col, row], k=1)
gauss = (1 /
(sig * m.sqrt(2 * m.pi))) * ((m.e)**-(((dist)**2) /
(2 * (sig**2))))
self.map.set_cell(col, row, gauss)
self.likely_pub.publish(self.map.to_message())
def handle_map_data(self, data): if( self.handle_map_first_called == 1 ): self.my_map = Map(data) self.true_map = Map(data) self.my_map_width = self.my_map.width self.my_map_height = self.my_map.height self.handle_map_first_called = 0
def handle_mapserver(self, resp):
self.map = Map(resp)
self.likelihood_map = Map(resp)
self.width = self.map.width
self.height = self.map.height
self.initialize_particles()
self.update_likelihood()
self.move()
def handle_map_message(self, message):
if (self.initialized == False):
self.resolution = message.info.resolution
self.origin_x = message.info.origin.position.x
self.origin_y = message.info.origin.position.y
self.width = message.info.width
self.height = message.info.height
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
self.mapOri = Map(message)
self.mapLH = Map(message)
self.initialized = True
def handle_map_message(self, message):
if(self.initialized == False):
self.resolution = message.info.resolution
self.origin_x = message.info.origin.position.x
self.origin_y = message.info.origin.position.y
self.width = message.info.width
self.height = message.info.height
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
self.mapOri = Map(message)
self.mapLH = Map(message)
self.initialized = True
def __init__(self, mapfile, radius=15,
delta_step=10, max_linear_vel=20, max_steer_angle=1.):
self.radius = radius
# Obtain the boundary limits.
# Check if file exists.
self.m = Map(mapfile, laser_max_range=4, downsample_factor=1)
self.map = self.m.occupancy_grid / 255.
self.xlimit = [0, np.shape(self.map)[0]-1]
self.ylimit = [0, np.shape(self.map)[1]-1]
self.delta_step = delta_step # Number of steps in simulation rollout
self.max_linear_vel = max_linear_vel
self.max_steer_angle = max_steer_angle
def mapInit(self, mapData):
if not self.isMapInit:
self.isMapInit = True
self.baseMap = Map(mapData)
self.likelihood_field = Map(mapData)
lsh = self.config['laser_sigma_hit']
width = self.baseMap.width
height = self.baseMap.height
occPoints = list()
points = list()
unoccPoints = list()
for r in range(height):
for c in range(width):
x, y = self.baseMap.cell_position(r, c)
if self.baseMap.get_cell(x, y) == 1:
occPoints.append((x,y))
else:
unoccPoints.append((x,y))
points.append((x,y))
self.occPointsSet = set(occPoints)
occPoints = np.array(occPoints)
points = np.array(points)
self.occPoints = occPoints
self.points = points
self.unoccPoints = unoccPoints
self.tree = KDTree(occPoints)
distance, index = self.tree.query(points, k = 1)
print "Points ", len(points), " should match ", width*height
for d, i, p in zip(distance, index, points):
x, y = p
self.likelihood_field.set_cell(x, y, likeProb(d, lsh))
self.likelihoodFieldPub.publish(self.likelihood_field.to_message())
self.particleInit()
def run(self, steps):
"""execute the mission"""
# make sure there are drones available
if not hasattr(self, "drones"):
raise RuntimeError("drone variable has not been initialized.")
if len(self.drones) <= 0:
raise RuntimeError(
"drone variable is empty. Please add drones to the mission planner."
)
# get the map width and height (for the signal estimator)
map = self.drones[0].get_internal_map()
map_width = map.get_width()
map_height = map.get_height()
# intialize the flight path variable to support real-time flight path visualization
# support multiple drone
drone_identifier_list = []
for drone in self.drones:
drone_identifier_list.append(drone.get_identifier())
# pass the drone identifier list and the internal map associated with drone[0]
# assume all drones has the same associated internal map
# the drone internal map is for getting the width and height of the map to plot the dynamic chart
self.flight_path = Path(drone_identifier_list,
self.drones[0].get_internal_map())
# when step is 0, all drones are at the initial location
for drone in self.drones:
self.flight_path.add_coords(
drone.get_identifier(), drone.get_current_coord(
)) #drone.get_x_cor(), drone.get_y_cor())
# when step is 0, show message as "initializing"
self.flight_path.add_message("initializing...")
# set the total number of steps
total_steps = steps
# start the step with 1 (first execution)
steps = 1
while steps <= total_steps:
# intialize response_data to a dictionary
# re-initialize everytime the loop runs
# deprecated
response_data = dict()
# initialize the shared_flight_data object to store flight parameters
# generated by drones and ATC
shared_flight_data = Shared_Flight_Data()
##### execution time #####
for drone in self.drones:
# execute the next step of the drone movement
drone.next_step()
# emit response data after each execution
# note that it is crucial to make identifiers unique to appropriately receive the response data
# response_data = {"Ego": ... "Enemy": ...}
# deprecated
response_data[
drone.get_identifier()] = drone.emit_response_data()
# emit the flight data of the drone after each execution
shared_flight_data.add(drone.emit_flight_data())
# debug
# print(str(shared_flight_data))
##### /execution time #####
# turn on or off the signal estimation
estimated_signal = None
is_signal_estimation = True
if is_signal_estimation == True:
##### signal estimation #####
# time span for the signal estimator
LOOKAHEAD_TIME_SPAN = 5
# note that this will solely be used for one ego drone one enemy drone situation
# obtain the current signal data (include every drone)
signal_data = dict()
for drone in self.drones:
signal_data[drone.get_id()] = {
"current_coord": drone.get_current_coord(),
"init_coord": drone.get_init_coord()
}
# encapsulated signal data into Signal_Element
current_signal_element = Signal_Element(steps, signal_data)
# initialize signal estimator
signal_estimator = Signal_Estimator(current_signal_element)
# create identical maps for drones
est_internal_map = Map(map_width, map_height)
est_ego_drone = EgoDrone(est_internal_map)
est_enemy_drone = EnemyDrone(est_internal_map)
# set the initial positions for teh ego drone and the enemy drone
# find the initial location for the drones
# ! no work around due to how flight mission/path is encoded
# to-do: change how flight mission is encoded
# ego_init_x = current_signal_element.get_signal_data_by_id_key(est_ego_drone.identifier, "init_coords")["x_cor"]
# ego_init_y = current_signal_element.get_signal_data_by_id_key(est_ego_drone.identifier, "init_coords")["y_cor"]
# enemy_init_x = current_signal_element.get_signal_data_by_id_key(est_enemy_drone.identifier, "init_coords")["x_cor"]
# enemy_init_y = current_signal_element.get_signal_data_by_id_key(est_enemy_drone.identifier, "init_coords")["y_cor"]
ego_init_coord = current_signal_element.get_signal_data_by_id_key(
est_ego_drone.identifier, "init_coord")
enemy_init_coord = current_signal_element.get_signal_data_by_id_key(
est_enemy_drone.identifier, "init_coord")
ego_current_coord = current_signal_element.get_signal_data_by_id_key(
est_ego_drone.identifier, "current_coord")
enemy_current_coord = current_signal_element.get_signal_data_by_id_key(
est_enemy_drone.identifier, "current_coord")
est_ego_drone.set_init_coord(ego_init_coord)
est_enemy_drone.set_init_coord(enemy_init_coord)
est_ego_drone.set_current_coord(ego_current_coord)
est_enemy_drone.set_current_coord(enemy_current_coord)
signal_estimator.add_drone(est_ego_drone)
signal_estimator.add_drone(est_enemy_drone)
# estimate the entirety of the mission, given the lookahead time
signal_estimator.fixed_mission_estimate(LOOKAHEAD_TIME_SPAN)
# maybe change the time_span
estimated_signal = signal_estimator.get_signal_estimation()
print("step = " + str(steps) + " estimated signal: " +
str(estimated_signal))
# signal_data
# have obtained all necessary data from the drones (such as current coordinates)
# current_signal_element = Signal_Element(0, {"Ego": {"current_coord": Coord(0, 20), "init_coord": Coord(0, 20)}, "Enemy": {"current_coord": Coord(0, 5), "init_coord": Coord(0, 5)}})
# signal_estimator = Signal_Estimator(current_signal_element)
##### /signal estimation #####
##### data processing #####
# write the step data to the response_data
# create flight data for air traffic control
# pass messages to the shared flight data (by using ATC_Flight_Data object)
ATC_message = "normal"
ATC_flight_data = ATC_Flight_Data("ATC", {
"step": steps,
"message": ATC_message
})
# add the flight broadcaster to the
shared_flight_data.add(ATC_flight_data)
# deprecated
response_data["step"] = steps
# decrement the step
steps += 1
# write the robustness score to the map
# need to be modified and deprecated
self.write_robustness_score_to_map(response_data)
# record the path, and update it in real time
# self.flight_path.add_coords_by_response_data(response_data)
self.flight_path.add_coords_by_shared_flight_data(
shared_flight_data)
self.flight_path.add_estimated_signal(estimated_signal)
# added warning messages
self.flight_path.add_message_by_shared_flight_data(
shared_flight_data)
##### /data processing #####
##### post-execution time #####
# broadcast the response data back to the drones
for drone in self.drones:
# drone.receive_response_data(response_data)
drone.receive_shared_flight_data(shared_flight_data)
def create_internal_map(self, width, height):
self.internal_map = Map(width, height)
class Mission_Planner:
"""plan the execute the flight mission
Attributes:
drones: A list of Drone objects involved in the simulation
"""
def __init__(self):
self.drones = []
def run(self, steps):
"""execute the mission"""
# make sure there are drones available
if not hasattr(self, "drones"):
raise RuntimeError("drone variable has not been initialized.")
if len(self.drones) <= 0:
raise RuntimeError(
"drone variable is empty. Please add drones to the mission planner."
)
# get the map width and height (for the signal estimator)
map = self.drones[0].get_internal_map()
map_width = map.get_width()
map_height = map.get_height()
# intialize the flight path variable to support real-time flight path visualization
# support multiple drone
drone_identifier_list = []
for drone in self.drones:
drone_identifier_list.append(drone.get_identifier())
# pass the drone identifier list and the internal map associated with drone[0]
# assume all drones has the same associated internal map
# the drone internal map is for getting the width and height of the map to plot the dynamic chart
self.flight_path = Path(drone_identifier_list,
self.drones[0].get_internal_map())
# when step is 0, all drones are at the initial location
for drone in self.drones:
self.flight_path.add_coords(
drone.get_identifier(), drone.get_current_coord(
)) #drone.get_x_cor(), drone.get_y_cor())
# when step is 0, show message as "initializing"
self.flight_path.add_message("initializing...")
# set the total number of steps
total_steps = steps
# start the step with 1 (first execution)
steps = 1
while steps <= total_steps:
# intialize response_data to a dictionary
# re-initialize everytime the loop runs
# deprecated
response_data = dict()
# initialize the shared_flight_data object to store flight parameters
# generated by drones and ATC
shared_flight_data = Shared_Flight_Data()
##### execution time #####
for drone in self.drones:
# execute the next step of the drone movement
drone.next_step()
# emit response data after each execution
# note that it is crucial to make identifiers unique to appropriately receive the response data
# response_data = {"Ego": ... "Enemy": ...}
# deprecated
response_data[
drone.get_identifier()] = drone.emit_response_data()
# emit the flight data of the drone after each execution
shared_flight_data.add(drone.emit_flight_data())
# debug
# print(str(shared_flight_data))
##### /execution time #####
# turn on or off the signal estimation
estimated_signal = None
is_signal_estimation = True
if is_signal_estimation == True:
##### signal estimation #####
# time span for the signal estimator
LOOKAHEAD_TIME_SPAN = 5
# note that this will solely be used for one ego drone one enemy drone situation
# obtain the current signal data (include every drone)
signal_data = dict()
for drone in self.drones:
signal_data[drone.get_id()] = {
"current_coord": drone.get_current_coord(),
"init_coord": drone.get_init_coord()
}
# encapsulated signal data into Signal_Element
current_signal_element = Signal_Element(steps, signal_data)
# initialize signal estimator
signal_estimator = Signal_Estimator(current_signal_element)
# create identical maps for drones
est_internal_map = Map(map_width, map_height)
est_ego_drone = EgoDrone(est_internal_map)
est_enemy_drone = EnemyDrone(est_internal_map)
# set the initial positions for teh ego drone and the enemy drone
# find the initial location for the drones
# ! no work around due to how flight mission/path is encoded
# to-do: change how flight mission is encoded
# ego_init_x = current_signal_element.get_signal_data_by_id_key(est_ego_drone.identifier, "init_coords")["x_cor"]
# ego_init_y = current_signal_element.get_signal_data_by_id_key(est_ego_drone.identifier, "init_coords")["y_cor"]
# enemy_init_x = current_signal_element.get_signal_data_by_id_key(est_enemy_drone.identifier, "init_coords")["x_cor"]
# enemy_init_y = current_signal_element.get_signal_data_by_id_key(est_enemy_drone.identifier, "init_coords")["y_cor"]
ego_init_coord = current_signal_element.get_signal_data_by_id_key(
est_ego_drone.identifier, "init_coord")
enemy_init_coord = current_signal_element.get_signal_data_by_id_key(
est_enemy_drone.identifier, "init_coord")
ego_current_coord = current_signal_element.get_signal_data_by_id_key(
est_ego_drone.identifier, "current_coord")
enemy_current_coord = current_signal_element.get_signal_data_by_id_key(
est_enemy_drone.identifier, "current_coord")
est_ego_drone.set_init_coord(ego_init_coord)
est_enemy_drone.set_init_coord(enemy_init_coord)
est_ego_drone.set_current_coord(ego_current_coord)
est_enemy_drone.set_current_coord(enemy_current_coord)
signal_estimator.add_drone(est_ego_drone)
signal_estimator.add_drone(est_enemy_drone)
# estimate the entirety of the mission, given the lookahead time
signal_estimator.fixed_mission_estimate(LOOKAHEAD_TIME_SPAN)
# maybe change the time_span
estimated_signal = signal_estimator.get_signal_estimation()
print("step = " + str(steps) + " estimated signal: " +
str(estimated_signal))
# signal_data
# have obtained all necessary data from the drones (such as current coordinates)
# current_signal_element = Signal_Element(0, {"Ego": {"current_coord": Coord(0, 20), "init_coord": Coord(0, 20)}, "Enemy": {"current_coord": Coord(0, 5), "init_coord": Coord(0, 5)}})
# signal_estimator = Signal_Estimator(current_signal_element)
##### /signal estimation #####
##### data processing #####
# write the step data to the response_data
# create flight data for air traffic control
# pass messages to the shared flight data (by using ATC_Flight_Data object)
ATC_message = "normal"
ATC_flight_data = ATC_Flight_Data("ATC", {
"step": steps,
"message": ATC_message
})
# add the flight broadcaster to the
shared_flight_data.add(ATC_flight_data)
# deprecated
response_data["step"] = steps
# decrement the step
steps += 1
# write the robustness score to the map
# need to be modified and deprecated
self.write_robustness_score_to_map(response_data)
# record the path, and update it in real time
# self.flight_path.add_coords_by_response_data(response_data)
self.flight_path.add_coords_by_shared_flight_data(
shared_flight_data)
self.flight_path.add_estimated_signal(estimated_signal)
# added warning messages
self.flight_path.add_message_by_shared_flight_data(
shared_flight_data)
##### /data processing #####
##### post-execution time #####
# broadcast the response data back to the drones
for drone in self.drones:
# drone.receive_response_data(response_data)
drone.receive_shared_flight_data(shared_flight_data)
##### /post-execution time #####
# self.flight_path.display_coords()
def get_flight_path(self):
if hasattr(self, "flight_path"):
return self.flight_path
else:
raise RuntimeError(
"flight_path variable has not been initialized.")
def write_robustness_score_to_map(self, response_data):
# write the robustness score back to the map
ego_map_cell = response_data["Ego"]["current_map_cell"]
enemy_map_cell = response_data["Enemy"]["current_map_cell"]
self.compute_dynamic_robustness(ego_map_cell, enemy_map_cell)
def add_drone(self, new_drone):
# add new drones to the MissionPlanner
self.drones.append(new_drone)
# round the float value to the nearest integer value
def round(self, float_value):
return int(float_value + 0.5)
def create_internal_map(self, width, height):
self.internal_map = Map(width, height)
def get_internal_map(self):
if hasattr(self, "internal_map"):
return self.internal_map
else:
raise RuntimeError("map variable has not been initialized.")
# compute the static robustness of the map based on obstacles and boundaries
# right now boundaries only
# compute it for all cells
def compute_static_robustness(self):
# set the minimum safety distance to the boundary
min_safety_distance_boundary = 3.0
# parameter for robustness score penalization
alpha = 32
# compute min and max robustness for scaling
# maybe adjust the min/max robustness score to the actual min/max robustness score in the future
# robustness_min = -1 * min_safety_distance_boundary
# robustness_max = max(self.internal_map.get_width(), self.internal_map.get_height()) / 2 - 1
# set the boundary of the map
top_boundary_y = 0
left_boundary_x = 0
right_boundary_x = self.internal_map.get_width() - 1
bottom_boundary_y = self.internal_map.get_height() - 1
# store the raw robustness values for analyzing min and max robustness
raw_robustness_value_list = []
# first robustness loop. Calculate the raw robustness score
for map_cell in self.get_internal_map().get_flattened_internal_map():
x_cor = map_cell.get_x_cor()
y_cor = map_cell.get_y_cor()
distance_to_top = y_cor - top_boundary_y
distance_to_bottom = bottom_boundary_y - y_cor
distance_to_left = x_cor - left_boundary_x
distance_to_right = right_boundary_x - x_cor
min_distance_to_boundary = min(distance_to_top, distance_to_bottom,
distance_to_left, distance_to_right)
raw_robustness_value = min_distance_to_boundary - min_safety_distance_boundary
raw_robustness_value_list.append(raw_robustness_value)
# store the raw robustness value to the map cell for future analysis
map_cell.set_attribute("static_robustness_score_raw",
raw_robustness_value)
# compute the min and max robustness scores
robustness_min = min(raw_robustness_value_list)
robustness_max = max(raw_robustness_value_list)
# second loop, scale and penalize robustness scores
for map_cell in self.get_internal_map().get_flattened_internal_map():
raw_robustness_value = map_cell.get_attribute(
"static_robustness_score_raw")
# scale the robustness value to a scale of -1 to 1 (inclusive)
scaled_robustness_value = 0
if raw_robustness_value < 0:
scaled_robustness_value = (raw_robustness_value -
robustness_min) / (
-1 * robustness_min) - 1
else:
scaled_robustness_value = raw_robustness_value / robustness_max
# penalize negative robustness scores
penalized_robustness_value = 0
if raw_robustness_value < 0:
penalized_robustness_value = raw_robustness_value - (
alpha**(-raw_robustness_value - 1) / (alpha - 1))
else:
penalized_robustness_value = raw_robustness_value
map_cell.set_attribute("static_robustness_score_scaled",
scaled_robustness_value)
map_cell.set_attribute("static_robustness_score_penalized",
penalized_robustness_value)
# compute the dynamic robustness based on the location of the enemy drone
# compute it for all cells
def compute_dynamic_robustness(self, ego_map_cell, enemy_map_cell):
# alpha = 32
min_safety_distance_enemy_drone = 3.0
x_cor_enemy = enemy_map_cell.x()
y_cor_enemy = enemy_map_cell.y()
x_cor_ego = ego_map_cell.x()
y_cor_ego = ego_map_cell.y()
# robustness_value = 0
# robustness_min = -1 * min_safety_distance_enemy_drone
# robustness_max = max(self.internal_map.get_width(), self.internal_map.get_height()) / 2 - 1
# store the raw robustness values
raw_robustness_value_list = []
# for map_cell in self.get_internal_map().get_flattened_internal_map():
# x_cor = map_cell.get_x_cor()
# y_cor = map_cell.get_y_cor()
distance_to_enemy_drone = math.sqrt((x_cor_ego - x_cor_enemy)**2 +
(y_cor_ego - y_cor_enemy)**2)
raw_robustness_value = distance_to_enemy_drone - min_safety_distance_enemy_drone
# # by default set to empty list []
# # original_dynamic_robustness_score_raw = []
# if (map_cell.has_attribute("dynamic_robustness_score_raw")):
# original_dynamic_robustness_score_raw = map_cell.get_attribute("dynamic_robustness_score_raw")
# store the list of raw robustness scores in the map cell
# original_dynamic_robustness_score_raw.append(raw_robustness_value)
ego_map_cell.set_attribute("dynamic_robustness_score_raw",
raw_robustness_value)
# append the robustness value to the list
# raw_robustness_value_list.append(raw_robustness_value)
# compute the max and min robustness value
# robustness_max = max(raw_robustness_value_list)
# robustness_min = min(raw_robustness_value_list)
# Scale and Penalization
# for map_cell in self.get_internal_map().get_flattened_internal_map():
# # obtain the raw robustness value from the map cell
# raw_robustness_value = map_cell.get_attribute("dynamic_robustness_score_raw")[-1]
# original_dynamic_robustness_score_scaled = []
# if (map_cell.has_attribute("dynamic_robustness_score_scaled")):
# original_dynamic_robustness_score_scaled = map_cell.get_attribute("dynamic_robustness_score_scaled")
# original_dynamic_robustness_score_penalized = []
# if (map_cell.has_attribute("dynamic_robustness_score_penalized")):
# original_dynamic_robustness_score_penalized = map_cell.get_attribute("dynamic_robustness_score_penalized")
# # scale the robustness value
# if raw_robustness_value < 0:
# scaled_robustness_value = (raw_robustness_value - robustness_min) / (-1 * robustness_min) - 1
# else:
# scaled_robustness_value = raw_robustness_value / robustness_max
# # penalize negative robustness scores
# penalized_robustness_value = 0
# if raw_robustness_value < 0:
# penalized_robustness_value = raw_robustness_value - (alpha**(-raw_robustness_value - 1)/(alpha - 1))
# else:
# penalized_robustness_value = raw_robustness_value
# # append the computed values to the corresponding cells
# original_dynamic_robustness_score_scaled.append(scaled_robustness_value)
# original_dynamic_robustness_score_penalized.append(penalized_robustness_value)
# map_cell.set_attribute("dynamic_robustness_score_scaled", original_dynamic_robustness_score_scaled)
# map_cell.set_attribute("dynamic_robustness_score_penalized", original_dynamic_robustness_score_penalized)
# compute the dynamic robustness based on the location of the enemy drone
# compute it for all cells
def compute_dynamic_robustness_comprehensive(self, enemy_map_cell):
alpha = 32
min_safety_distance_enemy_drone = 3.0
x_cor_enemy = enemy_map_cell.get_x_cor()
y_cor_enemy = enemy_map_cell.get_y_cor()
# robustness_value = 0
# robustness_min = -1 * min_safety_distance_enemy_drone
# robustness_max = max(self.internal_map.get_width(), self.internal_map.get_height()) / 2 - 1
# store the raw robustness values
raw_robustness_value_list = []
for map_cell in self.get_internal_map().get_flattened_internal_map():
x_cor = map_cell.get_x_cor()
y_cor = map_cell.get_y_cor()
distance_to_enemy_drone = math.sqrt((x_cor - x_cor_enemy)**2 +
(y_cor - y_cor_enemy)**2)
raw_robustness_value = distance_to_enemy_drone - min_safety_distance_enemy_drone
# by default set to empty list []
original_dynamic_robustness_score_raw = []
if (map_cell.has_attribute("dynamic_robustness_score_raw")):
original_dynamic_robustness_score_raw = map_cell.get_attribute(
"dynamic_robustness_score_raw")
# store the list of raw robustness scores in the map cell
original_dynamic_robustness_score_raw.append(raw_robustness_value)
map_cell.set_attribute("dynamic_robustness_score_raw",
original_dynamic_robustness_score_raw)
# append the robustness value to the list
raw_robustness_value_list.append(raw_robustness_value)
# compute the max and min robustness value
robustness_max = max(raw_robustness_value_list)
robustness_min = min(raw_robustness_value_list)
for map_cell in self.get_internal_map().get_flattened_internal_map():
# obtain the raw robustness value from the map cell
raw_robustness_value = map_cell.get_attribute(
"dynamic_robustness_score_raw")[-1]
original_dynamic_robustness_score_scaled = []
if (map_cell.has_attribute("dynamic_robustness_score_scaled")):
original_dynamic_robustness_score_scaled = map_cell.get_attribute(
"dynamic_robustness_score_scaled")
original_dynamic_robustness_score_penalized = []
if (map_cell.has_attribute("dynamic_robustness_score_penalized")):
original_dynamic_robustness_score_penalized = map_cell.get_attribute(
"dynamic_robustness_score_penalized")
# scale the robustness value
if raw_robustness_value < 0:
scaled_robustness_value = (raw_robustness_value -
robustness_min) / (
-1 * robustness_min) - 1
else:
scaled_robustness_value = raw_robustness_value / robustness_max
# penalize negative robustness scores
penalized_robustness_value = 0
if raw_robustness_value < 0:
penalized_robustness_value = raw_robustness_value - (
alpha**(-raw_robustness_value - 1) / (alpha - 1))
else:
penalized_robustness_value = raw_robustness_value
# append the computed values to the corresponding cells
original_dynamic_robustness_score_scaled.append(
scaled_robustness_value)
original_dynamic_robustness_score_penalized.append(
penalized_robustness_value)
map_cell.set_attribute("dynamic_robustness_score_scaled",
original_dynamic_robustness_score_scaled)
map_cell.set_attribute(
"dynamic_robustness_score_penalized",
original_dynamic_robustness_score_penalized)
# control the output (data) on the heatmap
def compute_output(self):
# # use the static robustness score
# for map_cell in self.get_internal_map().get_flattened_internal_map():
# map_cell.set_output(map_cell.get_attribute("static_robustness_score_scaled"))
# use the dynamic robustness score
for map_cell in self.get_internal_map().get_flattened_internal_map():
if map_cell.has_attribute("dynamic_robustness_score_raw"):
map_cell.set_output(
map_cell.get_attribute("dynamic_robustness_score_raw"))
else:
map_cell.set_output(0)
# fragmented heat map (a particular state in the process)
# use the static robustness score
# for map_cell in self.get_internal_map().get_flattened_internal_map():
# map_cell.set_output(map_cell.get_attribute("dynamic_robustness_score_scaled")[23] * 0.5 + map_cell.get_attribute("static_robustness_score_scaled") * 0.5)
# min robustness (without boundary)
# for map_cell in self.get_internal_map().get_flattened_internal_map():
# scaled
# map_cell.set_output(min(map_cell.get_attribute("dynamic_robustness_score_scaled")))
# penalized
# map_cell.set_output(min(map_cell.get_attribute("dynamic_robustness_score_penalized")))
# min robustness (with boundary)
# for map_cell in self.get_internal_map().get_flattened_internal_map():
# scaled
# map_cell.set_output(min(map_cell.get_attribute("dynamic_robustness_score_scaled")) * 0.5 + map_cell.get_attribute("static_robustness_score_scaled") * 0.5)
# penalized
# map_cell.set_output(min(map_cell.get_attribute("dynamic_robustness_score_penalized")) * 0.5 + map_cell.get_attribute("static_robustness_score_penalized") * 0.5)
# max robustness (without boundary)
# for map_cell in self.get_internal_map().get_flattened_internal_map():
# scaled
# map_cell.set_output(max(map_cell.get_attribute("dynamic_robustness_score_scaled")))
# penalized
# map_cell.set_output(max(map_cell.get_attribute("dynamic_robustness_score_penalized")))
# max robustness (with boundary)
# for map_cell in self.get_internal_map().get_flattened_internal_map():
# scaled
# map_cell.set_output(max(map_cell.get_attribute("dynamic_robustness_score_scaled")) * 0.5 + map_cell.get_attribute("static_robustness_score_scaled") * 0.5)
# penalized
# map_cell.set_output(max(map_cell.get_attribute("dynamic_robustness_score_penalized")) * 0.5 + map_cell.get_attribute("static_robustness_score_penalized") * 0.5)
# average (without boundary)
# for map_cell in self.get_internal_map().get_flattened_internal_map():
# scaled
# map_cell.set_output(statistics.mean(map_cell.get_attribute("dynamic_robustness_score_scaled")))
# map_cell.set_output(statistics.mean(map_cell.get_attribute("dynamic_robustness_score_penalized")))
# average (with boundary)
# for map_cell in self.get_internal_map().get_flattened_internal_map():
# map_cell.set_output(statistics.mean(map_cell.get_attribute("dynamic_robustness_score_penalized")) * 0.5 + map_cell.get_attribute("static_robustness_score_penalized") * 0.5)
# display the heatmap using seaborn library
# use function used in previous program
# How to draw blank heatmaps?
# https://seaborn.pydata.org/generated/seaborn.heatmap.html
def display_heatmap(self):
data = self.get_internal_map().get_output_map()
# use zeros like and ones like to display the data
# mask = np.ones_like(data)
# mask[np.triu_indices_from(mask)] = True
# mask[10][1] = False # Do not display 10, 1
ax = sns.heatmap(data, cmap="Spectral")
# ax = sns.heatmap(data, mask=mask, cmap="Spectral")
# ax = sns.heatmap(data, cmap="coolwarm")
# configure the heatmap interface
plt.tick_params(axis='both',
which='major',
labelsize=10,
labelbottom=False,
bottom=False,
top=False,
labeltop=True,
length=0)
plt.show()
"current_coord": Coord(1, 20),
"init_coord": Coord(1, 20)
},
"Enemy": {
"current_coord": Coord(0, 5),
"init_coord": Coord(0, 5)
}
})
# modified demo
# current_signal_element = Signal_Element(0, {"Ego": {"current_coord": Coord(2, 20), "init_coord": Coord(1, 20)}, "Enemy": {"current_coord": Coord(1, 6), "init_coord": Coord(0, 5)}})
signal_estimator = Signal_Estimator(current_signal_element)
# create identical maps for drones
est_internal_map = Map(21, 21)
est_ego_drone = EgoDrone(est_internal_map)
est_enemy_drone = EnemyDrone(est_internal_map)
# set the initial positions for teh ego drone and the enemy drone
# find the initial location for the drones
# ! no work around due to how flight mission/path is encoded
# to-do: change how flight mission is encoded
# ego_init_x = current_signal_element.get_signal_data_by_id_key(est_ego_drone.identifier, "init_coords")["x_cor"]
# ego_init_y = current_signal_element.get_signal_data_by_id_key(est_ego_drone.identifier, "init_coords")["y_cor"]
# enemy_init_x = current_signal_element.get_signal_data_by_id_key(est_enemy_drone.identifier, "init_coords")["x_cor"]
# enemy_init_y = current_signal_element.get_signal_data_by_id_key(est_enemy_drone.identifier, "init_coords")["y_cor"]
ego_init_coord = current_signal_element.get_signal_data_by_id_key(
class Robot():
def __init__(self):
self.config = read_config()
rospy.init_node('robot')
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
self.move_list = self.config["move_list"]
#Particle update values to use
self.first_sigma_x = self.config["first_move_sigma_x"]
self.first_sigma_y = self.config["first_move_sigma_y"]
self.first_sigma_angle = self.config["first_move_sigma_angle"]
self.resample_sigma_x = self.config["resample_sigma_x"]
self.resample_sigma_y = self.config["resample_sigma_y"]
self.resample_sigma_angle = self.config["resample_sigma_angle"]
self.isFirstMove = True
self.particleList = []
self.posePtr = []
self.tally = 0
laser_sub = rospy.Subscriber(
"/base_scan",
LaserScan,
self.senseCallBack
)
self.likely_pub = rospy.Publisher(
"/likelihood_field",
OccupancyGrid,
queue_size = 10,
latch = True
)
self.pose_pub = rospy.Publisher(
"/particlecloud",
PoseArray,
queue_size = 10
)
self.result_pub = rospy.Publisher(
"/result_update",
Bool,
queue_size = 10
)
self.sim_pub = rospy.Publisher(
"/sim_complete",
Bool,
queue_size = 10
)
map_sub = rospy.Subscriber(
"/map",
OccupancyGrid,
self.mapCallBack
)
rospy.sleep(1)
self.width = self.map.width
self.height = self.map.height
# particle init
i = 0
self.numParticles = self.config["num_particles"]
while(i < self.numParticles):
i += 1
x = random.randint(0, self.width)
y = random.randint(0, self.height)
theta = random.uniform(0, 2*m.pi)
obstacle = self.map.get_cell(x,y)
if(obstacle < 0.5):
pose = get_pose(x,y,theta)
self.pose_array.poses.append(pose)
p = Particle(x,y,theta, 1)
self.particleList.append(p)
else:
i -= 1
self.pose_pub.publish(self.pose_array)
rospy.wait_for_message("/likelihood_field", OccupancyGrid)
self.moveRobot()
self.sim_pub.publish(True)
rospy.is_shutdown()
rospy.spin()
def sigmoid(self, x):
y = (1.0 / (1.0 + m.exp(-x)))
return y
def mapCallBack(self, message):
self.map = Map(message)
self.whocares = Map(message)
self.width = self.map.width
self.height = self.map.height
self.sig = self.config['laser_sigma_hit']
coordinates = []
rospy.sleep(1)
for col in range(0, self.width):
for row in range(0, self.height):
x, y = self.map.cell_position(row, col)
self.cellVal = self.map.get_cell(x,y)
if (self.cellVal > 0.5):
coordinates.append([x,y])
self.kdt = KDTree(coordinates)
for col in range(0, self.width):
for row in range(0, self.height):
dist, ind = self.kdt.query([col,row], k=1)
gauss = (1/(self.sig*m.sqrt(2*m.pi)))*((m.e)**-(((dist)**2)/(2*(self.sig**2))))
self.map.set_cell(col, row, gauss)
self.likely_pub.publish(self.map.to_message())
def senseCallBack(self, laser):
self.lsr = laser
def moveRobot(self):
for t in range(0, len(self.move_list)):
move = self.move_list[t]
#move robot
a = move[0]
d = move[1]
n = move[2]
move_function(a,0)
self.first = True
for s in range(0, n):
move_function(0, d)
self.motion_update(move)
self.first = False
self.isFirstMove = False
self.pose_array.poses = self.posePtr
self.pose_pub.publish(self.pose_array)
self.posePtr = []
self.sensor_update()
self.resample()
self.result_pub.publish(True)
#For one particle
def motion_update(self, mv):
for i in range(0, len(self.particleList)):
part_in = self.particleList[i]
x = part_in.getX()
y = part_in.getY()
theta = part_in.getTheta()
weight = part_in.getWeight()
if (self.isFirstMove):
x = x + random.gauss(0, self.first_sigma_x)
y = y + random.gauss(0, self.first_sigma_y)
theta = theta + random.gauss(0, self.first_sigma_angle)
if (self.first):
theta = theta + m.radians(mv[0])
x = x + mv[1] * m.cos(theta)
y = y + mv[1] * m.sin(theta)
new_pose = get_pose(x,y,theta)
self.posePtr.append(new_pose)
part_in.setX(x)
part_in.setY(y)
part_in.setTheta(theta)
if (m.isnan(self.whocares.get_cell(x,y)) or self.whocares.get_cell(x,y) == 1.0):
part_in.setWeight(0.0)
def sensor_update(self):
z_rand = self.config["laser_z_rand"]
z_hit = self.config["laser_z_hit"]
normalizer = 0
#Laser Scan
for p in range(0, len(self.particleList)):
particle = self.particleList[p]
if(self.map.get_cell(particle.getX(),particle.getY()) == 1):
particle.setWeight(0.0)
ptot = 0
noNAN = 0
for l in range(0,len(self.lsr.ranges)):
scan = self.lsr.ranges[l]
if(scan != self.lsr.range_min and scan != self.lsr.range_max):
angle = self.lsr.angle_min + (self.lsr.angle_increment * l) + particle.getTheta()
x_pos = scan * m.cos(angle) + particle.getX()
y_pos = scan * m.sin(angle) + particle.getY()
lp = self.map.get_cell(x_pos,y_pos)
if (m.isnan(lp)):
noNAN += 1
else:
if (noNAN > (len(self.lsr.ranges)*.75)):
particle.setWeight(0.0)
pz = (z_hit * lp) + z_rand
ptot += (pz**2)
old_weight = particle.getWeight()
new_weight = old_weight * self.sigmoid(ptot)
normalizer += new_weight
new_particle = Particle(particle.getX(),particle.getY(),particle.getTheta(),new_weight)
self.particleList[p] = new_particle
for q in range(0, len(self.particleList)):
particle = self.particleList[q]
weight = particle.getWeight() / normalizer
particle.setWeight(weight)
def resample(self):
good_weights = []
good_particles = []
for w in range(0, len(self.particleList)):
particle = self.particleList[w]
value = self.whocares.get_cell(particle.getX(),particle.getY())
if (not (value == 1.0 or m.isnan(value))):
good_particles.append(self.particleList[w])
good_weights.append(self.particleList[w].getWeight())
# new_particles = np.random.choice(self.particleList, self.numParticles, True, weights)
for p in range(0,len(self.particleList)):
particle = self.particleList[p]
value = self.whocares.get_cell(particle.getX(),particle.getY())
if (value == 1.0 or m.isnan(value)):
dingledangle = np.random.choice(good_particles, 1, True, good_weights)
particle = dingledangle[0]
x = particle.getX() + random.gauss(0,self.resample_sigma_x)
y = particle.getY() + random.gauss(0,self.resample_sigma_y)
theta = particle.getTheta() + random.gauss(0,self.resample_sigma_angle)
new_particle = Particle(x,y,theta,particle.getWeight())
self.particleList[p] = new_particle
class Robot():
def __init__(self):
self.config = read_config()
rospy.init_node("robot")
self.rate = rospy.Rate(1)
self.rate.sleep()
self.result_publisher = rospy.Publisher("/result_update",
Bool,
queue_size=10)
self.complete_publisher = rospy.Publisher("/sim_complete",
Bool,
queue_size=10)
self.poseArray_publisher = rospy.Publisher("/particlecloud",
PoseArray,
queue_size=10)
self.likelihood_publisher = rospy.Publisher("/likelihood_field",
OccupancyGrid,
queue_size=10,
latch=True)
self.map_service = rospy.Subscriber("/map", OccupancyGrid,
self.handle_map_message)
self.laser_service = rospy.Subscriber("/base_scan", LaserScan,
self.handle_laser_message)
self.initialized = False
self.mess = Bool()
self.mess.data = True
self.originX = 0
self.originY = 0
self.width = 0
self.height = 0
self.resolution = 0
self.numPart = self.config['num_particles']
self.laser_sigma_hit = self.config['laser_sigma_hit']
self.laser_z_hit = self.config['laser_z_hit']
self.laser_z_rand = self.config['laser_z_rand']
self.move_list = self.config['move_list']
self.first_move_sigma_x = self.config['first_move_sigma_x']
self.first_move_sigma_y = self.config['first_move_sigma_y']
self.first_move_sigma_angle = self.config['first_move_sigma_angle']
self.resample_sigma_x = self.config['resample_sigma_x']
self.resample_sigma_y = self.config['resample_sigma_y']
self.resample_sigma_angle = self.config['resample_sigma_angle']
self.mapLH = None
self.mapOri = None
self.particleX = []
self.particleY = []
self.particleTheta = []
self.particleWeight = []
self.pose_array = None
self.initializeParticle()
self.move()
rospy.spin()
def get_nearest_obstacle(self, x, y, k=1):
# Check if KDTree is initialized
if not hasattr(self, 'obstacle_tree'):
self._initialize_obstacle_KDTree()
# Query for the given location
dist, ind = self.obstacle_tree.query((x, y), k)
# Transform index to actual locations
obstacles = self.obstacle_tree.get_arrays()[0]
obst = [obstacles[i] for i in ind]
return dist, obst
def handle_map_message(self, message):
if (self.initialized == False):
self.resolution = message.info.resolution
self.origin_x = message.info.origin.position.x
self.origin_y = message.info.origin.position.y
self.width = message.info.width
self.height = message.info.height
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
self.mapOri = Map(message)
self.mapLH = Map(message)
self.initialized = True
def initializeParticle(self):
self.rate.sleep()
for i in range(self.numPart):
x = random.uniform(0, self.width)
y = random.uniform(0, self.height)
while (self.mapOri.get_cell(x, y) != 0):
x = random.uniform(0, self.width)
y = random.uniform(0, self.height)
self.particleX.append(x)
self.particleY.append(y)
self.particleTheta.append(random.uniform(0, 2 * np.pi))
self.particleWeight.append(1 / self.numPart)
self.pose_array.poses.append(
get_pose(self.particleX[i], self.particleY[i],
self.particleTheta[i]))
self.poseArray_publisher.publish(self.pose_array)
obstalces = filter(lambda x: self.mapOri.get_cell(x[0], x[1]) != 0.0,
[(x, y) for x in range(self.width)
for y in range(self.height)])
self.obstacle_tree = KDTree(obstalces, leaf_size=20)
self.mapLH.grid = np.array([[
GaussianPDF(
self.get_nearest_obstacle(x, y)[0][0][0], 0,
self.laser_sigma_hit) for x in range(self.width)
] for y in range(self.height)])
self.likelihood_publisher.publish(self.mapLH.to_message())
def move(self):
first_move = True
for moveStep in self.move_list:
angle = moveStep[0]
dist = moveStep[1]
num = moveStep[2]
print(angle)
print(dist)
print(num)
if (first_move == True):
move_function(angle, 0)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleTheta[i] = self.particleTheta[i] + (
angle / 180.0 * self.angle_max) + random.gauss(
0, self.first_move_sigma_angle)
self.particleX[i] = (
self.particleX[i] +
random.gauss(0, self.first_move_sigma_x))
self.particleY[i] = (
self.particleY[i] +
random.gauss(0, self.first_move_sigma_y))
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i], resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
for manyTimes in range(num):
move_function(0, dist)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleX[i] = self.particleX[i] + random.gauss(
0, self.first_move_sigma_x)
self.particleY[i] = self.particleY[i] + random.gauss(
0, self.first_move_sigma_y)
self.particleTheta[
i] = self.particleTheta[i] + random.gauss(
0, self.first_move_sigma_angle)
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
else:
self.particleX[i] = self.particleX[i] + math.cos(
self.particleTheta[i]) * dist
self.particleY[i] = self.particleY[i] + math.sin(
self.particleTheta[i]) * dist
index = 0
pTot = 1
for z in self.ranges:
laserAngle = self.angle_min + self.angle_incr * index
index += 1
if (z >= self.range_min
and z <= self.range_max):
xz = self.particleX[i] + math.cos(
self.particleTheta[i] + laserAngle) * z
yz = self.particleY[i] + math.sin(
self.particleTheta[i] + laserAngle) * z
prob = self.mapLH.get_cell(xz, yz)
if (math.isnan(prob)):
prob = self.laser_z_rand
else:
prob = self.laser_z_hit * prob + self.laser_z_rand
pTot *= prob
self.particleWeight[i] = self.particleWeight[i] * (
pTot + 0.001)
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(
self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(
self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i],
resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
first_move = False
else:
move_function(angle, 0)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleTheta[i] = self.particleTheta[i] + (
angle / 180.0 * self.angle_max)
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i], resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
for manyTimes in range(num):
move_function(0, dist)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
particle_poss = self.mapLH.get_cell(
self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
else:
self.particleX[i] = (
self.particleX[i] +
math.cos(self.particleTheta[i]) * dist)
self.particleY[i] = (
self.particleY[i] +
math.sin(self.particleTheta[i]) * dist)
index = 0
pTot = 1
for z in self.ranges:
laserAngle = self.angle_min + self.angle_incr * index
index += 1
if (z >= self.range_min
and z <= self.range_max):
xz = self.particleX[i] + math.cos(
self.particleTheta[i] + laserAngle) * z
yz = self.particleY[i] + math.sin(
self.particleTheta[i] + laserAngle) * z
prob = self.mapLH.get_cell(xz, yz)
if (math.isnan(prob)):
prob = self.laser_z_rand
else:
prob = self.laser_z_hit * prob + self.laser_z_rand
pTot *= prob
self.particleWeight[i] = self.particleWeight[i] * (
-pTot + 0.001)
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i] / wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if (re <= resampleProb[j]):
index = j
break
resampleX.append(
self.particleX[index] +
random.gauss(0, self.resample_sigma_x))
resampleY.append(
self.particleY[index] +
random.gauss(0, self.resample_sigma_y))
resampleAngle.append(
self.particleTheta[index] +
random.gauss(0, self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(
get_pose(resampleX[i], resampleY[i],
resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
self.result_publisher.publish(self.mess)
self.complete_publisher.publish(self.mess)
def handle_laser_message(self, message):
self.angle_min = message.angle_min
self.angle_max = message.angle_max
self.angle_incr = message.angle_increment
self.range_min = message.range_min
self.range_max = message.range_max
self.ranges = message.ranges
class Robot():
def __init__(self):
rospy.init_node('robot_node', anonymous = True)
self.map_inited = 0;
self.move_num = 0
self.first_move = True
self.init_map_sub()
self.init_pubs()
rospy.sleep(3) #make sure we create the Map object in init_subs
self.init_config()
random.seed(self.seed)
self.init_particles()
self.publish_particles()
self.init_likelihood_map()
self.start_moves()
if self.move_num >= self.total_moves:
self.handle_shutdown()
rospy.spin()
def handle_shutdown(self):
self.sim_complete.publish(True)
rospy.sleep(3)
rospy.signal_shutdown("Done with Moves")
def init_config(self):
self.config = read_config()
self.num_particles = self.config["num_particles"]
self.map_width = self.myMap.width
self.map_height = self.myMap.height
self.laser_sig_hit = self.config["laser_sigma_hit"]
self.move_list = self.config["move_list"]
self.fm_sigma_x = self.config["first_move_sigma_x"]
self.fm_sigma_y = self.config["first_move_sigma_y"]
self.fm_sigma_ang = self.config["first_move_sigma_angle"]
self.total_moves = len(self.move_list)
self.laser_z_hit = self.config["laser_z_hit"]
self.laser_z_rand = self.config["laser_z_rand"]
self.resamp_sig_x = self.config["resample_sigma_x"]
self.resamp_sig_y = self.config["resample_sigma_y"]
self.resamp_sig_a = self.config["resample_sigma_angle"]
self.turned = False
self.seed = self.config["seed"]
def init_map_sub(self):
self.map_sub = rospy.Subscriber('/map', OccupancyGrid, self.handle_map_reply)
self.scan_sub = rospy.Subscriber('/base_scan', LaserScan, self.handle_scan)
def init_pubs(self):
self.particle_cloud_pub = rospy.Publisher('/particlecloud', PoseArray,
queue_size = 10, latch = True)
self.likelihood_pub = rospy.Publisher('/likelihood_field', OccupancyGrid,
queue_size = 10, latch = True)
self.result_pub = rospy.Publisher('/result_update', Bool,
queue_size = 10)
self.sim_complete = rospy.Publisher('/sim_complete', Bool,
queue_size = 10)
def handle_map_reply(self, grid):
if self.map_inited == 0:
self.myMap = Map(grid)
self.likelihood_map = Map(grid)
self.map_inited = 1
def handle_scan(self, scan):
self.scan = scan
self.num_sensors = (scan.angle_max - scan.angle_min) / scan.angle_increment
self.scan_ls = scan.ranges
self.scan_ang_min = scan.angle_min
def init_likelihood_map(self):
occupied_points = self.get_occupied_points()
near_ls = self.get_nearest_neighbors(occupied_points)
it = 0
for x in range (0, self.map_width):
for y in range (0, self.map_height):
val = self.gaussian_likelihood(near_ls[it][0], 0, self.laser_sig_hit)
self.likelihood_map.set_cell(x, y, val)
it+= 1
self.publish_likelihood_map()
def get_occupied_points(self):
occupied_points = []
for x in range ( 0, self.map_width):
for y in range ( 0, self.map_height):
if self.likelihood_map.get_cell(x, y) == 1:
occupied_points.append( [x , y])
return occupied_points
def publish_likelihood_map(self):
message = self.likelihood_map.to_message()
self.likelihood_pub.publish(message)
def gaussian_likelihood(self, x1, x2, sigma):
#alpha = 1 / ( sigma * math.sqrt( 2 * math.pi ))
ex = math.exp(-(x1 - x2)**2 / 2 * sigma**2 )
res = ex
return res
def get_nearest_neighbors(self, occu_pt):
allpts = []
for x in range (0, self.map_width):
for y in range (0, self.map_height):
allpts.append([x,y])
allpts = np.array(allpts)
x = np.array(occu_pt)
kdt = KDTree(x, leaf_size=30, metric='euclidean')
dist, ind = kdt.query(allpts, k=1, return_distance=True)
return dist
def publish_particles(self):
pose_arr = PoseArray()
pose_arr.header.stamp = rospy.Time.now()
pose_arr.header.frame_id = 'map'
pose_arr.poses = []
for p in self.particle_ls:
pose_arr.poses.append(p.pose)
self.particle_cloud_pub.publish(pose_arr)
def start_moves(self):
move_count = 0
while move_count < self.total_moves:
self.move_robot()
self.publish_particles()
self.turned = False
move_count += 1
self.result_pub.publish(True)
def move_robot(self):
angle, dist, steps = self.get_move()
#turn robot
helper_functions.move_function(angle, 0)
p_theta_mov = math.radians(angle)
for p in self.particle_ls:
if self.first_move:
p_theta_mov = random.gauss(p_theta_mov, self.fm_sigma_ang)
p.set_theta( p.theta+p_theta_mov)
p.update_pose()
#move robot
for n in range (0, steps):
helper_functions.move_function(0, dist)
#move particles?
self.move_particles()
self.publish_particles()
self.reweigh_all_particles()
self.publish_particles()
self.resample_particles()
self.publish_particles()
self.publish_particles()
def get_move(self):
self.current_move = self.move_list[self.move_num]
self.move_num += 1
return self.current_move
def resample_particles(self):
pick_ls = []
for p in self.particle_ls:
p_amount = [copy.deepcopy(p)] * int(round(self.num_particles * p.weight))
pick_ls.append(p_amount)
picked_ls_flat = list(itertools.chain(*pick_ls))
new_ls = []
for i in range (0, self.num_particles):
picked = copy.deepcopy(random.choice(picked_ls_flat))
x = random.gauss(picked.x, self.resamp_sig_x)
y = random.gauss(picked.y, self.resamp_sig_y)
#theta = np.random.normal(picked.theta, self.resamp_sig_a, 1)
theta = random.gauss(picked.theta, self.resamp_sig_a)
resampled_p = Particle(x, y, theta, picked.weight)
#picked.update_pose()
new_ls.append(resampled_p)
self.particle_ls = copy.deepcopy(new_ls)
def reweigh_all_particles(self):
Lp = 0
self.laser_ind = 0
for p in self.particle_ls:
p_tot = 0
num_shit_sensors = 0
for s in self.scan_ls:
laser_angle_local = self.scan_ang_min+ self.scan.angle_increment*self.laser_ind
x = s*math.cos(laser_angle_local+p.theta) + p.x
y = s*math.sin(laser_angle_local+p.theta) + p.y
Lp = self.likelihood_map.get_cell(x, y)
if math.isnan(Lp):
num_shit_sensors += 1
Lp = 0
Pz = self.laser_z_hit*Lp + self.laser_z_rand
p_tot += Pz**3
self.laser_ind += 1
if p.x < 0 or p.y < 0 or p.x >= self.map_width or p.y >= self.map_height:
p.weight = 0
elif self.likelihood_map.get_cell(p.x, p.y) >=0.9:
p.weight = 0
elif num_shit_sensors / self.num_sensors > 0.6:
p.weight = 0
else:
p.weight = p.weight *(1/(1+math.exp(-1*p_tot)))
self.normalize_particles()
def normalize_particles(self):
su = 0
for p in self.particle_ls:
su += p.weight
for p in self.particle_ls:
p.weight = p.weight / su
def move_particles(self):
#TODO: list comprehension moved_list = [
ang, dist, steps = self.current_move
angle_rad = math.radians(ang)
#move every particle
for p in self.particle_ls:
x_bar = dist*math.cos(p.theta)
y_bar = dist*math.sin(p.theta)
if self.first_move:
#add noise
angl, x, y = self.handle_first_particle_move(x_bar, y_bar,p.theta)
p.x = p.x + x
p.y = p.y + y
p.update_pose()
else:
p.x = p.x + x_bar
p.y = p.y + y_bar
p.update_pose()
self.first_move = False
def handle_first_particle_move(self, x_bar, y_bar, angle):
mu = 0
sig_x = self.fm_sigma_x
sig_y = self.fm_sigma_y
sig_ang = self.fm_sigma_ang
theta = random.gauss(angle, sig_ang)
x = random.gauss(x_bar, sig_x)
y = random.gauss(y_bar, sig_y)
return theta, x, y
def distance_moved_in_xy(self, d, n, theta):
return x, y
def gaussian(self, x, x2, sigma):
alpha = 1 / ( sigma * math.sqrt( 2 * math.pi ))
ex = math.exp(-(x - x2)**2 / 2 * sigma**2 )
res = alpha * ex
return res
def init_particles(self):
self.particle_ls = []
for i in range (0, self.num_particles):
r_x = random.uniform(0, self.map_width)
r_y = random.uniform(0, self.map_height)
r_theta = random.uniform(0, 2*math.pi)
p = Particle(r_x, r_y, r_theta, 1./self.num_particles)
self.particle_ls.append(p)
class CarEnvironment(object):
""" Car Environment. Car is represented as a circular robot.
Robot state: [x, y, theta]
"""
def __init__(self, mapfile, radius=15,
delta_step=10, max_linear_vel=20, max_steer_angle=1.):
self.radius = radius
# Obtain the boundary limits.
# Check if file exists.
self.m = Map(mapfile, laser_max_range=4, downsample_factor=1)
self.map = self.m.occupancy_grid / 255.
self.xlimit = [0, np.shape(self.map)[0]-1]
self.ylimit = [0, np.shape(self.map)[1]-1]
self.delta_step = delta_step # Number of steps in simulation rollout
self.max_linear_vel = max_linear_vel
self.max_steer_angle = max_steer_angle
#self.goal = goal
# Check if start and goal are within limits and collision free
# if not self.state_validity_checker(start) or not self.state_validity_checker(goal):
# raise ValueError('Start and Goal state must be within the map limits');
# exit(0)
def sample(self):
# Sample random clear point from map
clear = np.argwhere(self.map == 0)
idx = np.random.choice(len(clear))
xy = clear[idx, :].reshape((2, 1))
theta = np.random.uniform(0,2*np.pi)
return np.concatenate([xy, np.array([[theta]])])
def sample_action(self):
# Sample random direction of motion and random steer angle
linear_vel = (0.5 + 0.5*np.random.rand()) * self.max_linear_vel
if np.random.rand() > 0.5:
linear_vel = -1 * linear_vel
steer_angle = (2*np.random.rand() - 1) * self.max_steer_angle # uniformly distributed
return linear_vel, steer_angle
def simulate_car(self, x_near, x_rand, linear_vel, steer_angle):
""" Simulates a given control from the nearest state on the graph to the random sample.
@param x_near: a [3 x 1] numpy array. Nearest point on the current graph to the random sample
@param x_rand: a [3 x 1] numpy array. Random sample
@param linear_vel: a Python float. Linear velocity of the car (control)
@param steer_angle: a Python float. Steering angle of the car (control)
@return: x_new: a [3 x 1] numpy array
delta_t: a Python float. Time required to execute trajectory
"""
# Set vehicle constants
dt = 0.1 # Step by 0.1 seconds
L = 7.5 # Car length
# Simulate forward from xnear using the controls (linear_vel, steer_angle) to generate the rollout
x = x_near
rollout = [x]
for i in range(self.delta_step):
x_rollout = np.zeros_like(x)
x_rollout[0] = x[0] + linear_vel * np.cos(x[2]) * dt
x_rollout[1] = x[1] + linear_vel * np.sin(x[2]) * dt
x_rollout[2] = x[2] + (linear_vel/L) * np.tan(steer_angle) * dt
x_rollout[2] = x_rollout[2] % (2*np.pi)
x = x_rollout
rollout.append(x_rollout) # maintain history
rollout = np.concatenate(rollout, axis=1) # Shape: [3 x delta_step]
# Find the closest point to x_rand on the rollout
# This is x_new. Discard the rest of the rollout
min_ind = np.argmin(self.compute_distance(rollout, x_rand))
x_new = rollout[:, min_ind].reshape(3,1)
rollout = rollout[:, :min_ind+1] # don't need the rest
delta_t = rollout.shape[1]
# Check for validity of the path
if self.state_validity_checker(rollout):
return x_new, rollout
else:
return None, None
# def step(self, x, action):
# linear_vel = action[0]
# steer_angle = action[1]
# dt = 0.1
# L = 7.5
# x_rollout = np.zeros_like(x)
# x_rollout[0] = x[0] + linear_vel * np.cos(x[2]) * dt
# x_rollout[1] = x[1] + linear_vel * np.sin(x[2]) * dt
# x_rollout[2] = x[2] + (linear_vel/L) * np.tan(steer_angle) * dt
# x_rollout[2] = x_rollout[2] % (2*np.pi)
# return x_rollout
def angular_difference(self, start_config, end_config):
""" Compute angular difference
@param start_config: a [3 x n] numpy array of states
@param end_config: a [3 x 1] numpy array of goal state
"""
th1 = start_config[2,:]; th2 = end_config[2,:]
ang_diff = th1-th2
ang_diff = ((ang_diff + np.pi) % (2*np.pi)) - np.pi
ang_diff = ang_diff*(180/np.pi) # convert to degrees
return ang_diff
def compute_distance(self, start_config, end_config):
""" Distance function: alignment, xy distance, angular difference
- alignment: measure of how far start_config is from line defined by end_config
similar to projection of start_config xy position end_config line
@param start_config: a [3 x n] numpy array of states
@param end_config: a [3 x 1] numpy array of goal state
"""
ang_diff = np.abs(self.angular_difference(start_config, end_config))/180
e_to_s = start_config[:2,:] - end_config[:2,:] # Shape: [2 x n]
euclidean_distance = np.linalg.norm(e_to_s, axis=0) # Shape: [n]
e_to_s = e_to_s / euclidean_distance[None,:]
e_vec = np.array([np.cos(end_config[2,0]), np.sin(end_config[2,0])])
alignment = 1 - np.abs(e_vec.dot(e_to_s)) # Shape: [n]
# alignment is in [0,1], euclidean_distance can be large, ang_diff is between [0,1]
return 50*alignment + euclidean_distance + 50*ang_diff
def goal_criterion(self, config, goal_config, pos_tor = 30, ang_tor = 360, no_angle = False):
""" Return True if config is close enough to goal
@param config: a [3 x 1] numpy array of a state
@param goal_config: a [3 x 1] numpy array of goal state
"""
if no_angle:
return np.linalg.norm(config[:2,:] - goal_config[:2,:]) < pos_tor
if np.linalg.norm(config[:2,:] - goal_config[:2,:]) < pos_tor and \
np.abs(self.angular_difference(config, goal_config)) < ang_tor:
print(f'Goal reached! State: {config[:,0]}, Goal state: {goal_config[:,0]}')
print(f'xy_diff: {np.linalg.norm(config[:2,:] - goal_config[:2,:]):.03f}, '\
f'ang_diff: {np.abs(self.angular_difference(config, goal_config))[0]:.03f}')
return True
else:
return False
def out_of_limits_violation(self, config):
""" Check limit violations
@param config: a [3 x n] numpy array of n states
"""
out_of_limits = np.stack([config[0,:] <= self.radius,
config[0,:] >= (self.xlimit[1] - self.radius),
config[1,:] <= self.radius,
config[1,:] >= (self.ylimit[1] - self.radius)])
return np.any(out_of_limits)
def collision_violation(self, config):
""" Check whether car is in collision with obstacle
@param config: a [3 x n] numpy array of n states
"""
theta = np.linspace(0, 2*np.pi, 50)
xs = self.radius * np.cos(theta) + config[0,:,None] # Shape: [n x 50]
ys = self.radius * np.sin(theta) + config[1,:,None] # Shape: [n x 50]
xys = np.stack([xs,ys],axis=0) # Shape: [2 x n x 50]
cfloor = np.round(xys.reshape(2,-1)).astype("int")
try:
values = self.map[cfloor[0, :], cfloor[1, :]]
except IndexError:
return False
return np.sum(values) > 0
def state_validity_checker(self, config):
""" Check validity of state
@param config: = a [3 x n] numpy array of n states.
"""
valid_position = ~self.collision_violation(config)
valid_limits = ~self.out_of_limits_violation(config)
return valid_position and valid_limits
def edge_validity_checker(self, config1, config2):
assert(config1.shape == (3, 1))
assert(config2.shape == (3, 1))
n = max(self.xlimit[1], self.ylimit[1])
x_vals = np.linspace(config1[0], config2[0], n).reshape(1, n)
y_vals = np.linspace(config1[1], config2[1], n).reshape(1, n)
configs = np.vstack((x_vals, y_vals))
return self.state_validity_checker(configs)
def plot_car(self, config):
""" Plot the car
@param config: a [3 x 1] numpy array
"""
config = config[:,0]
# Plot car as a circle
ax = plt.gca()
circle1 = plt.Circle(config[:2][::-1], self.radius, fill=True, facecolor='w')
circle2 = plt.Circle(config[:2][::-1], self.radius, fill=False, color='k')
car1 = ax.add_artist(circle1)
car2 = ax.add_artist(circle2)
# Now plot a line for the direction of the car
theta = config[2]
ed = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) @ np.array([[self.radius*1.5, 0]]).T;
ed = ed[:,0]
car3 = ax.plot([config[1], config[1]+ed[1]], [config[0], config[0]+ed[0]], 'b-', linewidth=3)
return [car1, car2, car3]
def init_visualizer(self):
""" Initialize visualizer
"""
self.fig = plt.figure()
self.ax1 = self.fig.add_subplot(1, 1, 1)
# Plot img
visit_map = 1 - np.copy(self.map) # black is obstacle, white is free space
self.ax1_img = self.ax1.imshow(visit_map, interpolation="nearest", cmap="gray")
self.car_plot = None
self.particle_plot = None
def draw_obstacles(self, xys, *args, **kwargs):
'''
xys: a numpy array of size Nx2 representing locations of obstacles in the global coordinate system.
'''
ln = self.ax1.plot(xys[:, 0], xys[:, 1], '+', *args, **kwargs)
return ln
def draw_particles(self, particles, *args, **kwargs):
x = particles[:, 0]
y = particles[:, 1]
# delta = 10
# xd = np.cos(particles[:, 2])*delta
# yd = np.sin(particles[:, 2])*delta
particle_plot = self.ax1.scatter(y, x, s = 0.1)
# for i in range(x.shape[0]):
# self.ax1.arrow(y[i], x[i], yd[i], xd[i], color = 'r', head_width = 5, head_length = 5)
self.fig.canvas.draw()
return particle_plot
def visualize_plan(self, plan=None, tree=None, visited=None):
'''
Visualize the final path
@param plan: a [3 x n] numpy array of states
'''
visit_map = 1 - np.copy(self.map) # black is obstacle, white is free space
self.ax1.cla()
self.ax1.imshow(visit_map, interpolation="nearest", cmap="gray")
if tree is not None:
for idx in range(len(tree.vertices)):
if idx == tree.GetRootID():
continue
econfig = tree.vertices[idx]
sconfig = tree.vertices[tree.edges[idx][0]]
x = [sconfig[0], econfig[0]]
y = [sconfig[1], econfig[1]]
self.ax1.plot(y, x, 'r')
ln = None
if plan is not None:
for i in range(np.shape(plan)[1]):
self.plot_car(plan[:,i:i+1])
self.fig.canvas.draw()
pos = np.array([plan[1, i], plan[0, i]]) / 100
heading = plan[2, i]
n_ray = 180
fov = np.deg2rad(360.0)
depth = self.m.get_1d_depth(pos, heading, fov, n_ray, resolution=0.01)
obstacles = depth_to_xy(depth, pos, heading, fov)
if not ln is None:
ln[0].remove()
ln = self.draw_obstacles(obstacles*100, markeredgewidth=1.5)
plt.pause(.025)
self.fig.canvas.draw()
plt.pause(1e-10)
def get_measurement(self, plan):
measurement = []
for i in range(np.shape(plan)[1]):
pos = np.array([plan[1, i], plan[0, i]]) / 100
if plan.shape[0] == 2:
heading = 0.0
else:
heading = plan[2, i]
n_ray = 180
fov = np.deg2rad(360.0)
depth = self.m.get_1d_depth(pos, heading, fov, n_ray, resolution=0.01)
#obstacles = depth_to_xy(depth, pos, heading, fov)
measurement.append(depth)
return np.array(measurement)
def random_walk(self, sim_t = 100):
start = self.sample()
state_traj = [start]
action_traj = []
x = start
for t in range(sim_t):
linear_vel, steer_angle = self.sample_action()
if t % 10 == 0:
action = np.array([linear_vel, steer_angle])
x_next = self.step(x, action)
if self.edge_validity_checker(x, x_next):
state_traj.append(x_next)
action_traj.append(action)
else:
break
x = x_next
state_traj = np.array(state_traj).squeeze()
if state_traj.shape[0] < 10:
return None, None, None
action_traj = np.array(action_traj)
measure_traj = self.get_measurement(state_traj[1:].T)
return state_traj, action_traj, measure_traj
def visualize_traj(self, plan, measurement):
visit_map = 1 - np.copy(self.map) # black is obstacle, white is free space
self.ax1.cla()
self.ax1.imshow(visit_map, interpolation="nearest", cmap="gray")
ln = None
car1, car2, car3 = None, None, None
if plan is not None:
for i in range(np.shape(plan)[1]):
if not ln is None:
ln[0].remove()
car1.remove()
car2.remove()
car3[0].remove()
car1, car2, car3 = self.plot_car(plan[:,i:i+1])
self.fig.canvas.draw()
pos = np.array([plan[1, i], plan[0, i]]) / 100
heading = plan[2, i]
# n_ray = 180
fov = np.deg2rad(360.0)
# depth = self.m.get_1d_depth(pos, heading, fov, n_ray, resolution=0.01)
obstacles = depth_to_xy(measurement[i], pos, heading, fov)
ln = self.draw_obstacles(obstacles*100, markeredgewidth=1.5)
plt.pause(.025)
self.fig.canvas.draw()
plt.pause(1e-10)
def render(self, state = None, particles = None, dt = 0.01):
#self.init_visualizer()
if not self.car_plot is None:
self.car_plot[0].remove()
self.car_plot[1].remove()
self.car_plot[2][0].remove()
if not self.particle_plot is None:
self.particle_plot.remove()
if not state is None:
self.car_plot = self.plot_car(state)
if not particles is None:
self.particle_plot = self.draw_particles(particles.squeeze())
self.car_plot = self.plot_car(self.state)
self.fig.canvas.draw()
plt.pause(dt)
# MPNet steer
def steerTo(self, state, delta):
# import IPython
# IPython.embed()
for ip, tp in enumerate(np.linspace(0, np.pi, 13)):
rot_state_p = state.T + np.dot(np.array([[np.cos(tp), -np.sin(tp)], [np.sin(tp), np.cos(tp)]]), delta.T)
rot_state_p = np.concatenate([rot_state_p, [[0]]], axis = 0)
if self.state_validity_checker(rot_state_p):
break
for im, tm in enumerate(np.linspace(0, -np.pi, 13)):
rot_state_m = state.T + np.dot(np.array([[np.cos(tm), -np.sin(tm)], [np.sin(tm), np.cos(tm)]]), delta.T)
rot_state_m = np.concatenate([rot_state_m, [[0]]], axis = 0)
if self.state_validity_checker(rot_state_m):
break
if ip <= im:
return rot_state_p.T[:, :2]
else:
return rot_state_m.T[:, :2]
# for reinforcement learning
def get_local_goal(self):
dxy = self.global_goal[:2, 0] - self.state[:2, 0]
local_goal = np.dot(np.array([[np.cos(-self.state[2, 0]), -np.sin(-self.state[2, 0])], [np.sin(-self.state[2, 0]), np.cos(-self.state[2, 0])]]), dxy[None].T)
return local_goal.T[0]
def reset(self):
# generate state
self.state = None
while True:
self.state = self.sample()
if self.state_validity_checker(self.state):
break
# generate goal
goal_dist = np.random.random()*10 + 15
while True:
angle = np.random.random() * np.pi * 2
self.global_goal = np.array([[self.state[0, 0] + goal_dist * np.cos(angle), self.state[1, 0] + goal_dist * np.sin(angle), 0.0]]).T
if self.state_validity_checker(self.global_goal):
break
observation = self.get_measurement(self.state)
local_goal = self.get_local_goal()
return observation, local_goal
def get_reward(self, next_state, action):
# reach cost
if self.goal_criterion(next_state, self.global_goal, pos_tor = 1, no_angle = True):
return 500, True # done, cost
# collision cost
if not self.state_validity_checker(next_state):
return -500, True
# goal distant cost
eps = 10
cost = eps * (np.linalg.norm(self.global_goal[:2] - self.state[:2]) - np.linalg.norm(self.global_goal[:2] - next_state[:2]))
# reach cost
cost -= 20
cost -= np.abs(action[1]) * 5
return cost, False
def step(self, action):
dt = 0.1 # Step by 0.1 seconds
L = 7.5 # Car length
next_state = np.zeros_like(self.state)
linear_vel, steer_angle = action[0], action[1]
next_state[0] = self.state[0] + linear_vel * np.cos(self.state[2]) * dt
next_state[1] = self.state[1] + linear_vel * np.sin(self.state[2]) * dt
next_state[2] = self.state[2] + (linear_vel/L) * np.tan(steer_angle) * dt
next_state[2] = next_state[2] % (2*np.pi)
reward, done = self.get_reward(next_state, action)
self.state = next_state
observation = self.get_measurement(self.state)
local_goal = self.get_local_goal()
return observation, local_goal, done, reward
# purely update state
def step_action(self, action):
dt = 0.1 # Step by 0.1 seconds
L = 7.5 # Car length
next_state = np.zeros_like(self.state)
linear_vel, steer_angle = action[0], action[1]
next_state[0] = self.state[0] + linear_vel * np.cos(self.state[2]) * dt
next_state[1] = self.state[1] + linear_vel * np.sin(self.state[2]) * dt
next_state[2] = self.state[2] + (linear_vel/L) * np.tan(steer_angle) * dt
next_state[2] = next_state[2] % (2*np.pi)
self.state = next_state
observation = self.get_measurement(self.state)
return self.state, observation
# for close loop
def draw_start_goal(self, start, goal):
ax = plt.gca()
circle1 = plt.Circle(start[::-1], self.radius, fill=True, facecolor='g')
circle2 = plt.Circle(goal[::-1], self.radius, fill=True, facecolor='r')
ax.add_artist(circle1)
ax.add_artist(circle2)
plt.pause(0.0001)
def setState(self, state):
self.state = state
return self.get_measurement(state)
class Robot():
def __init__(self):
""" This will read the config files and set up the different
listeners and what not
"""
self.config = read_config()
self.moveList = self.config['move_list']
self.particles = list()
self.isMapInit = False
random.seed(self.config['seed'])
np.random.seed(self.config['seed'])
self.baseMap = None
self.likelihood_field = None
self.laserScanFlag = False
self.laserScanData = None
self.robotInit()
rospy.init_node("robot")
self.start()
def robotInit(self):
""" Init Subscribers and Publishers"""
self.mapSub = rospy.Subscriber(
"/map",
OccupancyGrid,
self.mapInit
)
self.laserScanSub = rospy.Subscriber(
"/base_scan",
LaserScan,
self.laserScanUpdate
)
#Do I need latch? TODO
self.particleCloudPub = rospy.Publisher(
"/particlecloud",
PoseArray,
queue_size = 10
)
self.likelihoodFieldPub = rospy.Publisher(
"/likelihood_field",
OccupancyGrid,
queue_size = 10,
latch = True
)
self.resultUpdatePub = rospy.Publisher(
"/result_update",
Bool,
queue_size = 10
)
self.simCompletePub = rospy.Publisher(
"/sim_complete",
Bool,
queue_size = 10
)
def mapInit(self, mapData):
if not self.isMapInit:
self.isMapInit = True
self.baseMap = Map(mapData)
self.likelihood_field = Map(mapData)
lsh = self.config['laser_sigma_hit']
width = self.baseMap.width
height = self.baseMap.height
occPoints = list()
points = list()
unoccPoints = list()
for r in range(height):
for c in range(width):
x, y = self.baseMap.cell_position(r, c)
if self.baseMap.get_cell(x, y) == 1:
occPoints.append((x,y))
else:
unoccPoints.append((x,y))
points.append((x,y))
self.occPointsSet = set(occPoints)
occPoints = np.array(occPoints)
points = np.array(points)
self.occPoints = occPoints
self.points = points
self.unoccPoints = unoccPoints
self.tree = KDTree(occPoints)
distance, index = self.tree.query(points, k = 1)
print "Points ", len(points), " should match ", width*height
for d, i, p in zip(distance, index, points):
x, y = p
self.likelihood_field.set_cell(x, y, likeProb(d, lsh))
self.likelihoodFieldPub.publish(self.likelihood_field.to_message())
self.particleInit()
def particleInit(self):
default_weight = 1.0/(self.config["num_particles"])
numPart = self.config["num_particles"]
randPoints = np.random.choice(range(len(self.unoccPoints)), numPart, replace=True)
randPoints = [self.unoccPoints[i] for i in randPoints]
for x, y in randPoints:
p = Particle(x, y, m.pi*random.random(), default_weight)
self.particles.append(p)
self.publishParticles()
self.move()
def publishParticles(self):
pose_array = self.poseArrayTemplate()
for p in self.particles:
pose_array.poses.append(p.get_pose())
self.particleCloudPub.publish(pose_array)
def laserScanUpdate(self, laserScanMessage):
if not self.laserScanFlag:
self.laserScanFlag = True
self.laserScanData = laserScanMessage
def move(self):
moves = self.config["move_list"]
for i, (a, d, n) in enumerate(moves):
print "Move %d, Turn at Angle %.2f, then move %.2f meters %d times." % (i, a, d, n)
print "First turn at angle %.2f" % a
helper_functions.move_function(a, 0)
self.particles = self.updateMove(self.particles, 0.0, a)
self.publishParticles()
self.particles = self.commit(self.particles)
self.publishParticles()
for __ in xrange(n):
print "Moving forward with distance %d" % d
helper_functions.move_function(0, d)
self.particles = self.updateMove(self.particles, d, 0.0)
self.publishParticles()
self.particles = self.commit(self.particles)
self.publishParticles()
self.resultUpdatePub.publish(True)
self.simCompletePub.publish(True)
rospy.sleep(10)
rospy.signal_shutdown("sim complete")
def updateMove(self, parts, d, t):
p = parts[0]
toret = list()
rad = (m.pi * t)/180.0
for p in parts:
theta = p.theta + rad
x = p.x + d*m.cos(p.theta)
y = p.y + d*m.sin(p.theta)
particle = Particle(x, y, theta, p.weight)
toret.append(particle)
return toret
def reweight(self, parts):
zhit = self.config["laser_z_hit"]
zrand = self.config["laser_z_rand"]
lsd = self.getLaserScanData()
increment = lsd.angle_increment
startAngle = lsd.angle_min
newParts = list()
for p in parts:
x, y, theta, q = p.x, p.y, p.theta, p.weight
iswall = self.baseMap.get_cell(x, y)
if m.isnan(iswall):
q = 0.0
elif iswall == 1.0:
q = 0.0
currincrement = 0.0
for r in lsd.ranges:
if q == 0.0:
break
currx = x + r * m.cos(theta + startAngle + currincrement)
curry = y + r * m.sin(theta + startAngle + currincrement)
prob = self.likelihood_field.get_cell(currx, curry)
if m.isnan(prob):
prob = 0.0
q *= zhit * prob + zrand
q += zrand
currincrement += increment
p.weight = q
newParts.append(p)
return self.normalize(newParts)
def normalize(self, parts):
#ALL WEIGHTS GETTING SO SMALL ITS BECOMING 0
weights = [p.weight * 1000 if not m.isnan(p.weight) else 0.0 for p in parts]
normalizer = sum(weights)
if m.isnan(normalizer):
print "GOFFYY!!!!"
else:
weights = [w/normalizer if not m.isnan(w/normalizer) else 0.0 for w in weights]
#why do this?! you don't use it again...
print "TTTTTT", weights.count(0), len(weights)
normalizer = sum(weights)
for i, __ in enumerate(parts):
parts[i].weight = weights[i]
return parts
def resample(self, parts):
res = list()
weights = [p.weight for p in parts]
print "SUMMM", sum(weights)
randPoints = np.random.choice(parts, self.config["num_particles"], replace=True, p=weights)
for p in randPoints:
noise_x = random.gauss(0, self.config["resample_sigma_x"])
noise_y = random.gauss(0, self.config["resample_sigma_y"])
noise_theta = random.gauss(0, self.config["resample_sigma_angle"])
p.x += noise_x
p.y += noise_y
p.theta += noise_theta
res.append(p)
return res
def commit(self, parts):
return self.resample(self.reweight(parts))
def getLaserScanData(self):
self.laserScanFlag = False
while not self.laserScanFlag:
continue
return self.laserScanData
def poseArrayTemplate(self):
pose_array = PoseArray()
pose_array.header.stamp = rospy.Time.now()
pose_array.header.frame_id = 'map'
pose_array.poses =[]
return pose_array
def start(self):
rospy.sleep(5)
rospy.spin()
def handle_map_reply(self, grid):
if self.map_inited == 0:
self.myMap = Map(grid)
self.likelihood_map = Map(grid)
self.map_inited = 1
class Robot():
def __init__(self):
self.config = read_config()
rospy.init_node("robot")
self.rate = rospy.Rate(1)
self.rate.sleep()
self.result_publisher = rospy.Publisher(
"/result_update",
Bool,
queue_size = 10
)
self.complete_publisher = rospy.Publisher(
"/sim_complete",
Bool,
queue_size = 10
)
self.poseArray_publisher = rospy.Publisher(
"/particlecloud",
PoseArray,
queue_size = 10
)
self.likelihood_publisher = rospy.Publisher(
"/likelihood_field",
OccupancyGrid,
queue_size = 10,
latch = True
)
self.map_service = rospy.Subscriber(
"/map",
OccupancyGrid,
self.handle_map_message
)
self.laser_service = rospy.Subscriber(
"/base_scan",
LaserScan,
self.handle_laser_message
)
self.initialized = False
self.mess = Bool()
self.mess.data = True
self.originX = 0
self.originY = 0
self.width = 0
self.height = 0
self.resolution = 0
self.numPart = self.config['num_particles']
self.laser_sigma_hit = self.config['laser_sigma_hit']
self.laser_z_hit = self.config['laser_z_hit']
self.laser_z_rand = self.config['laser_z_rand']
self.move_list = self.config['move_list']
self.first_move_sigma_x = self.config['first_move_sigma_x']
self.first_move_sigma_y = self.config['first_move_sigma_y']
self.first_move_sigma_angle = self.config['first_move_sigma_angle']
self.resample_sigma_x = self.config['resample_sigma_x']
self.resample_sigma_y = self.config['resample_sigma_y']
self.resample_sigma_angle = self.config['resample_sigma_angle']
self.mapLH = None
self.mapOri = None
self.particleX = []
self.particleY = []
self.particleTheta = []
self.particleWeight = []
self.pose_array = None
self.initializeParticle()
self.move()
rospy.spin()
def get_nearest_obstacle(self, x, y, k=1):
# Check if KDTree is initialized
if not hasattr(self, 'obstacle_tree'):
self._initialize_obstacle_KDTree()
# Query for the given location
dist, ind = self.obstacle_tree.query((x, y), k)
# Transform index to actual locations
obstacles = self.obstacle_tree.get_arrays()[0]
obst = [obstacles[i] for i in ind]
return dist, obst
def handle_map_message(self, message):
if(self.initialized == False):
self.resolution = message.info.resolution
self.origin_x = message.info.origin.position.x
self.origin_y = message.info.origin.position.y
self.width = message.info.width
self.height = message.info.height
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
self.mapOri = Map(message)
self.mapLH = Map(message)
self.initialized = True
def initializeParticle(self):
self.rate.sleep()
for i in range(self.numPart):
x = random.uniform(0,self.width)
y = random.uniform(0,self.height)
while(self.mapOri.get_cell(x,y) != 0):
x = random.uniform(0,self.width)
y = random.uniform(0,self.height)
self.particleX.append(x)
self.particleY.append(y)
self.particleTheta.append(random.uniform(0,2*np.pi))
self.particleWeight.append(1/self.numPart)
self.pose_array.poses.append(get_pose(self.particleX[i],self.particleY[i],self.particleTheta[i]))
self.poseArray_publisher.publish(self.pose_array)
obstalces = filter(lambda x: self.mapOri.get_cell(x[0], x[1]) != 0.0, [(x, y) for x in range(self.width) for y in range(self.height)])
self.obstacle_tree = KDTree(obstalces, leaf_size=20)
self.mapLH.grid = np.array([[GaussianPDF(self.get_nearest_obstacle(x,y)[0][0][0], 0, self.laser_sigma_hit)
for x in range(self.width)]
for y in range(self.height)])
self.likelihood_publisher.publish(self.mapLH.to_message())
def move(self):
first_move = True
for moveStep in self.move_list:
angle = moveStep[0]
dist = moveStep[1]
num = moveStep[2]
print(angle)
print(dist)
print(num)
if(first_move == True):
move_function(angle,0)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleTheta[i] = self.particleTheta[i]+(angle / 180.0 * self.angle_max)+random.gauss(0,self.first_move_sigma_angle)
self.particleX[i] = (self.particleX[i]+random.gauss(0,self.first_move_sigma_x))
self.particleY[i] = (self.particleY[i]+random.gauss(0,self.first_move_sigma_y))
particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i]/wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if(re<=resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x))
resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y))
resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
for manyTimes in range(num):
move_function(0,dist)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleX[i] = self.particleX[i]+random.gauss(0,self.first_move_sigma_x)
self.particleY[i] = self.particleY[i]+random.gauss(0,self.first_move_sigma_y)
self.particleTheta[i] = self.particleTheta[i]+random.gauss(0,self.first_move_sigma_angle)
particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
else:
self.particleX[i] = self.particleX[i]+math.cos(self.particleTheta[i])*dist
self.particleY[i] = self.particleY[i]+math.sin(self.particleTheta[i])*dist
index = 0
pTot = 1
for z in self.ranges:
laserAngle = self.angle_min + self.angle_incr*index
index+=1
if(z>=self.range_min and z<=self.range_max):
xz = self.particleX[i]+math.cos(self.particleTheta[i]+laserAngle)*z
yz = self.particleY[i]+math.sin(self.particleTheta[i]+laserAngle)*z
prob = self.mapLH.get_cell(xz,yz)
if(math.isnan(prob)):
prob = self.laser_z_rand
else:
prob = self.laser_z_hit * prob + self.laser_z_rand
pTot *= prob
self.particleWeight[i] = self.particleWeight[i]*(pTot+0.001)
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i]/wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if(re<=resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x))
resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y))
resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
first_move = False
else:
move_function(angle,0)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
self.particleTheta[i] = self.particleTheta[i]+(angle / 180.0 * self.angle_max)
particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i]/wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if(re<=resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x))
resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y))
resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
for manyTimes in range(num):
move_function(0,dist)
wTot = 0
wSoFar = 0
resampleProb = []
resampleX = []
resampleY = []
resampleAngle = []
resampleWeight = []
for i in range(self.numPart):
particle_poss = self.mapLH.get_cell(self.particleX[i], self.particleY[i])
if math.isnan(particle_poss) or particle_poss == 1.0:
self.particleWeight[i] = 0.0
else:
self.particleX[i] = (self.particleX[i]+math.cos(self.particleTheta[i])*dist)
self.particleY[i] = (self.particleY[i]+math.sin(self.particleTheta[i])*dist)
index = 0
pTot = 1
for z in self.ranges:
laserAngle = self.angle_min + self.angle_incr*index
index+=1
if(z>=self.range_min and z<=self.range_max):
xz = self.particleX[i]+math.cos(self.particleTheta[i]+laserAngle)*z
yz = self.particleY[i]+math.sin(self.particleTheta[i]+laserAngle)*z
prob = self.mapLH.get_cell(xz,yz)
if(math.isnan(prob)):
prob = self.laser_z_rand
else:
prob = self.laser_z_hit * prob + self.laser_z_rand
pTot *= prob
self.particleWeight[i] = self.particleWeight[i]*(-pTot+0.001)
wTot += self.particleWeight[i]
for i in range(self.numPart):
self.particleWeight[i] = self.particleWeight[i]/wTot
wSoFar += self.particleWeight[i]
resampleProb.append(wSoFar)
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.poses = []
for i in range(self.numPart):
re = random.random()
index = -1
for j in range(self.numPart):
if(re<=resampleProb[j]):
index = j
break
resampleX.append(self.particleX[index]+random.gauss(0,self.resample_sigma_x))
resampleY.append(self.particleY[index]+random.gauss(0,self.resample_sigma_y))
resampleAngle.append(self.particleTheta[index]+random.gauss(0,self.resample_sigma_angle))
resampleWeight.append(self.particleWeight[index])
self.pose_array.poses.append(get_pose(resampleX[i],resampleY[i],resampleAngle[i]))
self.particleX = resampleX
self.particleY = resampleY
self.particleTheta = resampleAngle
self.particleWeight = resampleWeight
self.poseArray_publisher.publish(self.pose_array)
self.result_publisher.publish(self.mess)
self.complete_publisher.publish(self.mess)
def handle_laser_message(self,message):
self.angle_min = message.angle_min
self.angle_max = message.angle_max
self.angle_incr = message.angle_increment
self.range_min = message.range_min
self.range_max = message.range_max
self.ranges = message.ranges
class Robot():
def __init__(self):
self.config = read_config()
rospy.init_node('robot')
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
self.moves = self.config["move_list"]
#Particle update values to use
self.isFirstMove = True
self.first_sigma_x = read.config["first_move_sigma_x"]
self.first_sigma_y = read.config["first_move_sigma_y"]
self.first_sigma_angle = read.config["first_move_sigma_angle"]
self.resample_sigma_x = read.config["resample_sigma_x"]
self.resample_sigma_y = read.config["resample_sigma_y"]
self.resample_sigma_angle = read.config["resample_sigma_angle"]
laser_sub = rospy.Subscriber("/base_scan", LaserScan,
self.sensorUpdate)
self.likely_pub = rospy.Publisher("/likelihood_field",
OccupancyGrid,
queue_size=10,
latch=True)
self.pose_pub = rospy.Publisher("/particlecloud",
PoseArray,
queue_size=10)
map_sub = rospy.Subscriber("/map", OccupancyGrid, self.mapCallBack)
rospy.sleep(1)
self.width = self.map.width
self.height = self.map.height
# particle init
for i in range(0, self.config["num_particles"]):
x = random.randint(0, self.width)
y = random.randint(0, self.height)
theta = random.uniform(0, 2 * m.pi)
obstacle = self.map.get_cell(x, y)
if (obstacle < 0.5):
pose = get_pose(x, y, theta)
self.pose_array.poses.append(pose)
else:
i -= 1
rospy.sleep(1)
self.pose_pub.publish(self.pose_array)
self.moveRobot(self.moves)
rospy.spin()
def mapCallBack(self, message):
# 2
self.map = Map(message)
self.width = self.map.width
self.height = self.map.height
sig = self.config['laser_sigma_hit']
coordinates = []
queryPts = []
rospy.sleep(1)
# 3
for col in range(0, self.width):
for row in range(0, self.height):
x, y = self.map.cell_position(row, col)
cellVal = self.map.get_cell(x, y)
if (cellVal > 0.5):
coordinates.append([x, y])
self.kdt = KDTree(coordinates)
for col in range(0, self.width):
for row in range(0, self.height):
dist, ind = self.kdt.query([col, row], k=1)
gauss = (1 /
(sig * m.sqrt(2 * m.pi))) * ((m.e)**-(((dist)**2) /
(2 * (sig**2))))
self.map.set_cell(col, row, gauss)
self.likely_pub.publish(self.map.to_message())
def sensorUpdate(self, laser):
self.laserMsg = laser
def moveRobot(self, motion_cmd):
for i in range(0, len(self.moves)): #4
motion = self.moves[i]
a = motion[0]
d = motion[1]
n = motion[2]
move_function(a, 0)
for t in range(0, n):
move_function(0, d)
updateParticles(a, d)
def updateParticles(a, d):
new_pose_array = []
for j in range(0, self.config["num_particles"]):
particle = self.pose_array.poses[j]
x = particle[0]
y = particle[1]
theta = particle[2]
if (self.isFirstMove):
x = x + random.gauss(0, first_move_sigma_x)
y = y + random.gauss(0, first_move_sigma_y)
theta = theta + random.gauss(0, first_move_sigma_theta)
self.isFirstMove = False
x = x + d * math.cos(a)
y = y + d * math.sin(a)
theta = theta + a
pose = get_pose(x, y, theta)
new_pose_array.append(pose)
self.pose_array.poses = new_pose_array
#Reweight
def motionUpdate(self, motion_cmd, particle):
a = motion_cmd[0]
d = motion_cmd[1]
n = motion_cmd[2]
class Robot():
def __init__(self):
rospy.init_node("robot")
self.config = read_config()
#r.seed(self.config['seed'])
self.num_particles = self.config['num_particles']
self.laser_sigma_hit = self.config['laser_sigma_hit']
self.move_list = self.config['move_list']
self.move_list_length = len(self.move_list)
self.first_move_sigma_x = self.config['first_move_sigma_x']
self.first_move_sigma_y = self.config['first_move_sigma_y']
self.first_move_sigma_angle = self.config['first_move_sigma_angle']
self.laser_z_hit = self.config['laser_z_hit']
self.laser_z_rand = self.config['laser_z_rand']
self.resample_sigma_x = self.config['resample_sigma_x']
self.resample_sigma_y = self.config['resample_sigma_y']
self.resample_sigma_theta = self.config['resample_sigma_angle']
self.handle_map_first_called = 1
self.move_made = 0
self.map_data_sub = rospy.Subscriber(
"/map",
OccupancyGrid,
self.handle_map_data
)
self.base_scan_data = rospy.Subscriber(
"/base_scan",
LaserScan,
self.handle_base_scan_data
)
self.particle_pose_pub = rospy.Publisher(
"/particlecloud",
PoseArray,
queue_size = 10
)
self.likelihood_pub = rospy.Publisher(
"/likelihood_field",
OccupancyGrid,
queue_size = 1,
latch = True
)
self.result_update_pub = rospy.Publisher(
"/result_update",
Bool,
queue_size = 10
)
self.sim_complete_pub = rospy.Publisher(
"/sim_complete",
Bool,
queue_size = 10
)
self.scan_data_avai = 0
self.sensor_loop()
def sensor_loop(self):
while not rospy.is_shutdown():
if ( self.handle_map_first_called == 0):
#print "went in while(true) loop"
self.create_particles()
self.construct_field()
#print "finished construct field"
# publish OccupancyMap
grid_mssg = self.my_map.to_message()
rospy.sleep(1)
self.likelihood_pub.publish(grid_mssg)
self.make_all_moves()
break
rospy.sleep(1)
self.sim_complete_pub.publish(True)
rospy.sleep(1)
rospy.signal_shutdown(Robot)
def handle_base_scan_data (self, data):
self.scan_info = data
self.scan_data_avai = 1
def process_scan_data (self):
while( self.scan_data_avai == 0 ):
rospy.sleep(0.1)
#print "process data"
#self.scan_data_avai = 0
self.scan_data = self.scan_info
for i in range (self.num_particles):
if( np.isnan(self.particle_array[i].x) or np.isnan(self.particle_array[i].y) or np.isinf(self.particle_array[i].x) or np.isinf(self.particle_array[i].y) ):
continue
cell_prob1 = self.my_map.get_cell(self.particle_array[i].x, self.particle_array[i].y)
cell_prob2 = self.true_map.get_cell(self.particle_array[i].x, self.particle_array[i].y)
if (np.isnan(cell_prob1) or cell_prob2 == 1.0):
self.particle_array[i].weight = 0.0
continue
pz_array = []
for j in range (10):
angle = self.particle_array[i].theta + (self.scan_data.angle_min + (self.scan_data.angle_increment * j))
x = self.particle_array[i].x + (self.scan_data.ranges[j] * np.cos(angle))
y = self.particle_array[i].y + (self.scan_data.ranges[j] * np.sin(angle))
if( np.isnan(x) or np.isnan(y) or np.isinf(x) or np.isinf(y) ):
#lp = 0
pz = 0
else:
lp = self.my_map.get_cell( x, y )
if( np.isnan(lp) ):
pz = 0
else:
pz = (self.laser_z_hit * lp) + self.laser_z_rand
# if( np.isnan(lp) ):
# #lp = 0
# pz = 0
#pz = (self.laser_z_hit * lp) + self.laser_z_rand
pz_array.append(pz)
p_tot = 0
for a in range (len(pz_array)):
value_cubed = math.pow(pz_array[a],3)
if(np.isnan(value_cubed)):
continue
else:
p_tot = p_tot + value_cubed
#sigmoid function for calculating weight
self.particle_array[i].weight = self.particle_array[i].weight * (1.0/(1.0+math.pow(math.e, -1*p_tot)))
self.normalize_weight()
def create_particles(self):
self.particle_array = []
self.pose_array = PoseArray()
self.pose_array.header.stamp = rospy.Time.now()
self.pose_array.header.frame_id = 'map'
self.pose_array.poses = []
for i in range (self.num_particles):
# x and y are coordinates
col = r.randint(0, self.my_map_width)
row = r.randint(0, self.my_map_height)
x, y = self.my_map.cell_position (row,col)
theta = math.radians(r.random() * 360)
pose = get_pose (x,y,theta)
particle = Particle()
particle.x = x
particle.y = y
particle.theta = theta
particle.pose = pose
particle.weight = (1.0/self.num_particles)
self.particle_array.append(particle)
self.pose_array.poses.append(pose)
rospy.sleep(1)
self.particle_pose_pub.publish(self.pose_array)
def handle_map_data(self, data):
if( self.handle_map_first_called == 1 ):
self.my_map = Map(data)
self.true_map = Map(data)
self.my_map_width = self.my_map.width
self.my_map_height = self.my_map.height
self.handle_map_first_called = 0
#print "handle map is called"
def make_all_moves(self):
self.num_moves = len(self.move_list)
for i in range (self.num_moves):
#print "make_move"
self.make_move()
rospy.sleep(1)
self.result_update_pub.publish(True)
self.move_made = self.move_made + 1
def make_move(self):
i = self.move_made
#add noise to x, y, theta when first move
move_function(self.move_list[i][0], 0)
for j in range(len(self.particle_array)):
self.particle_array[j].theta += math.radians(self.move_list[i][0])
self.particle_array[i].theta = self.particle_array[i].theta % (math.radians(360))
for a in range(self.move_list[i][2]):
move_function(0, self.move_list[i][1])
self.particle_update(i)
if(i == 0):
self.particle_add_noise_first_move()
self.process_scan_data()
self.resampling_particle()
def normalize_weight(self):
total = 0
for k in range (self.num_particles):
total += self.particle_array[k].weight
for j in range (self.num_particles):
self.particle_array[j].weight /= total
total = 0
for m in range (self.num_particles):
total += self.particle_array[m].weight
#print "total should be 1: ", total
def resampling_particle(self):
#print "resampling"
self.weight_array = []
new_array = []
for i in range (self.num_particles):
self.weight_array.append(self.particle_array[i].weight)
for j in range (self.num_particles):
particle = np.random.choice(self.particle_array, None, True, self.weight_array)
new_particle = Particle()
new_particle.x = particle.x
new_particle.y = particle.y
new_particle.theta = particle.theta
new_particle.weight = particle.weight
new_particle.pose = particle.pose
new_x = self.add_resample_noise(new_particle.x, self.resample_sigma_x)
new_y = self.add_resample_noise(new_particle.y, self.resample_sigma_y)
new_theta = self.add_resample_noise(new_particle.theta, self.resample_sigma_theta)
new_pose = get_pose(new_x, new_y, new_theta)
new_particle.x = new_x
new_particle.y = new_y
new_particle.theta = new_theta
new_particle.pose = new_pose
new_array.append(new_particle)
for m in range (self.num_particles):
self.particle_array[m] = new_array[m]
self.pose_array.poses[m] = new_array[m].pose
#self.normalize_weight()
# publish the pose array
rospy.sleep(1)
self.particle_pose_pub.publish(self.pose_array)
def particle_update (self, i):
for a in range (self.num_particles):
update_x = self.particle_array[a].x + self.move_list[i][1] * np.cos(self.particle_array[a].theta)
update_y = self.particle_array[a].y + self.move_list[i][1] * np.sin(self.particle_array[a].theta)
self.particle_array[a].x = update_x
self.particle_array[a].y = update_y
self.particle_array[a].pose = get_pose(update_x, update_y, self.particle_array[a].theta)
def particle_add_noise_first_move(self):
for a in range (self.num_particles):
self.particle_array[a].x = self.add_first_move_noise(self.particle_array[a].x, self.first_move_sigma_x)
self.particle_array[a].y = self.add_first_move_noise(self.particle_array[a].y, self.first_move_sigma_y)
self.particle_array[a].theta = self.add_first_move_noise(self.particle_array[a].theta, self.first_move_sigma_angle)
self.particle_array[a].pose = get_pose(self.particle_array[a].x, self.particle_array[a].y, self.particle_array[a].theta)
def add_first_move_noise(self, coordinate, sd):
# noise = r.gauss(0, sd)*100.)/100.
noise = r.gauss(0, sd)
added_noise = coordinate + noise
return added_noise
def add_resample_noise(self, coordinate, sd):
# noise = math.ceil(r.gauss(0, sd) * 100.) /100.
noise = r.gauss(0, sd)
added_noise = coordinate + noise
return added_noise
def construct_field(self):
self.kdtree_array = []
for i in range (self.my_map_height):
for j in range (self.my_map_width):
x, y = self.my_map.cell_position(i,j)
value = self.my_map.get_cell(x, y)
if( value == 1.0 ):
self.kdtree_array.append([x, y])
self.kdtree = KDTree (self.kdtree_array)
self.update_field()
def update_field(self):
for i in range (self.my_map_height):
for j in range (self.my_map_width):
coordinate = self.my_map.cell_position(i,j)
dist, idx = self.kdtree.query([[coordinate[0],coordinate[1]]], k=1)
new_value = self.calculate (dist)
coordinate = self.my_map.cell_position(i,j)
#if( self.my_map.get_cell(coordinate[0], coordinate[1]) == 0.0):
self.my_map.set_cell(coordinate[0], coordinate[1], new_value)
def calculate (self, distance):
power = (-1.0 * ((distance*distance)/(2.0*self.laser_sigma_hit*self.laser_sigma_hit)))
value = math.pow( math.e, power)
return value
# Open file to store logs
if LOG_DATA:
f = open(log_path, "w")
# Init ros node
rospy.init_node("uwb_particle_filter")
rospy.loginfo("Initializing Particle Filter Node...")
rospy.loginfo("Retrieving map of environment...")
# Retrieve occupancy grid
occupancy_grid_msg = rospy.wait_for_message('/map', OccupancyGrid)
# Create a map object
occupancy_grid = Map(occupancy_grid_msg)
rospy.loginfo("Successfully loaded map!")
# NUM UWB tags
num_tags = 3
# List of all anchor names that will be placed in the environment
anchor_names = [
"9205", "C518", "9AAB", "D81B", "998D", "D499", "9BAC", "1C31", "91BA"
]
rospy.loginfo(
"Waiting for initial scan from lidar to get sensor resolution...")
# Get the number of lidar scans and range
initial_scan = rospy.wait_for_message('/scan_filtered', LaserScan)
