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)