def on_transfer(self, req):
old_path = [s for s in req.old_path.split("/") if s]
new_path = [s for s in req.new_path.split("/") if s]
success = False
if req.mode == req.MODE_OBJECT:
moved_obj = MapManager.remove_object(old_path)
if moved_obj is not None:
success = MapManager.add_object(new_path, moved_obj)
else:
success = False
elif req.mode == req.MODE_CONTAINER:
rospy.logerr("container transfer not implemented")
elif req.mode == req.MODE_CONTAINER_OBJECTS:
old_ct = MapManager.get_container(old_path)
new_ct = MapManager.get_container(new_path)
if old_ct and new_ct:
new_ct.Elements += old_ct.Elements
old_ct.Elements = []
success = True
rospy.loginfo(
"TRANSFER (from={} to={}): {}".format(req.old_path, req.new_path, success)
)
return static_map.srv.MapTransferResponse(success)
def on_get_waypoint(self, req):
pos = Position2D(None, validate=False)
pos.X = req.waypoint.pose.x
pos.Y = req.waypoint.pose.y
pos.A = req.waypoint.pose.theta
pos.HasAngle = req.waypoint.has_angle
w = MapManager.get_waypoint(req.waypoint.name, pos)
success = False
if w:
success = True
rospy.loginfo(
"GET Waypoint (name='{}' x={} y={} a={}): returning {}".format(
req.waypoint.name,
req.waypoint.pose.x,
req.waypoint.pose.y,
req.waypoint.pose.theta,
"None"
if success is False
else "name='{}' x={} y={} a={}".format(
w.Name, w.Position.X, w.Position.Y, w.Position.A
),
)
)
return static_map.srv.MapGetWaypointResponse(
success, self._create_waypoint_msg(w)
)
def on_set(self, req):
rospy.loginfo("SET:" + str(req.path))
path = [s for s in req.path.split("/") if s]
obj = self._object_from_msg(req.object)
res_obj = None
success = False
if req.mode == req.MODE_ADD:
success = MapManager.add_object(path, obj)
elif req.mode == req.MODE_REMOVE:
res_obj = MapManager.remove_object(path)
if success or res_obj:
MapManager.Dirty = True
return static_map.srv.MapSetObjectResponse(
success, self._create_object_msg(res_obj)
)
def __init__(self, robot_radius, robot_fov_radius, static_map_topic,
merged_occ_grid_topic, push_poses_topic, goal_topic,
simulated_pose_topic, simulated_fov_pointcloud_topic,
local_goal_pose_topic):
self.MOVE_COST = 1.0
self.PUSH_COST = 1.0
self.ONE_PUSH_DISTANCE = 0.05
self.POSITION_TOLERANCE = 0.01 # [m]
self.ANGLE_TOLERANCE = 0.02 # [rad]
self.map_manager = MapManager(robot_radius, robot_fov_radius,
static_map_topic, merged_occ_grid_topic,
push_poses_topic, simulated_pose_topic,
simulated_fov_pointcloud_topic)
self.global_planner = GlobalPlanner()
# Parameters
self.current_robot_pose = None
self.is_ready = False
self.blocked_obstacles = set()
self.debug_rate = rospy.Rate(4.0)
self.goal_topic = goal_topic
self.simulated_pose_topic = simulated_pose_topic
self.local_goal_pose_topic = local_goal_pose_topic
rospy.Subscriber(self.goal_topic, PoseStamped,
self._global_goal_pose_callback)
rospy.Subscriber(self.simulated_pose_topic, PoseStamped,
self._simulated_pose_callback)
rospy.wait_for_service(self.local_goal_pose_topic)
self.local_goal_pose_client = rospy.ServiceProxy(
self.local_goal_pose_topic, LocalGoalPose)
self.local_goal_pose_pub = rospy.Publisher(self.local_goal_pose_topic,
PoseStamped,
queue_size=1)
def on_get_container(self, req):
# Fetch it from the map
ct = MapManager.get_container([s for s in req.path.split("/") if s])
if ct is None:
return static_map.srv.MapGetContainerResponse(False, None)
# Construct the message reply
msg = static_map.srv.MapGetContainerResponse()
msg.success = True
msg.container = self._create_container_msg(ct, req.include_subcontainers)
rospy.loginfo(
"GET Container (path={}): {} object(s) returned.".format(
req.path, len(msg.container.objects)
)
)
return msg
def on_get_context(self, req):
terrain, robot_shape = MapManager.get_context()
msg = static_map.srv.MapGetContextResponse()
msg.success = True
msg.terrain_shape = self._create_object_msg(terrain)
msg.terrain_layers = []
# Robot shape
msg.robot_shape = static_map.msg.MapObject()
msg.robot_shape.shape_type = msg.robot_shape.SHAPE_RECT
msg.robot_shape.width = robot_shape.Width
msg.robot_shape.height = robot_shape.Height
for l in terrain.Layers:
msg_layer = static_map.msg.MapLayer()
msg_layer.name = l.Name
msg_layer.walls = [self._create_object_msg(w) for w in l.Walls]
msg.terrain_layers.append(msg_layer)
return msg
class NavManager:
def __init__(self, robot_radius, robot_fov_radius, static_map_topic,
merged_occ_grid_topic, push_poses_topic, goal_topic,
simulated_pose_topic, simulated_fov_pointcloud_topic,
local_goal_pose_topic):
self.MOVE_COST = 1.0
self.PUSH_COST = 1.0
self.ONE_PUSH_DISTANCE = 0.05
self.POSITION_TOLERANCE = 0.01 # [m]
self.ANGLE_TOLERANCE = 0.02 # [rad]
self.map_manager = MapManager(robot_radius, robot_fov_radius,
static_map_topic, merged_occ_grid_topic,
push_poses_topic, simulated_pose_topic,
simulated_fov_pointcloud_topic)
self.global_planner = GlobalPlanner()
# Parameters
self.current_robot_pose = None
self.is_ready = False
self.blocked_obstacles = set()
self.debug_rate = rospy.Rate(4.0)
self.goal_topic = goal_topic
self.simulated_pose_topic = simulated_pose_topic
self.local_goal_pose_topic = local_goal_pose_topic
rospy.Subscriber(self.goal_topic, PoseStamped,
self._global_goal_pose_callback)
rospy.Subscriber(self.simulated_pose_topic, PoseStamped,
self._simulated_pose_callback)
rospy.wait_for_service(self.local_goal_pose_topic)
self.local_goal_pose_client = rospy.ServiceProxy(
self.local_goal_pose_topic, LocalGoalPose)
self.local_goal_pose_pub = rospy.Publisher(self.local_goal_pose_topic,
PoseStamped,
queue_size=1)
def _get_safe_swept_area(self, obstacle, translation_vector,
occupancy_grid):
manipulation_area_map_points = obstacle.get_manipulation_area_map_points(
translation_vector)
# If any manipulation area map point is occupied by an obstacle, then
# return None because the manipulation area is not a safe swept
# area. Otherwise, return the set of all safe swept map points.
for point in manipulation_area_map_points:
if point not in obstacle.discretized_polygon and occupancy_grid[
point[0]][point[1]] == Utils.ROS_COST_LETHAL:
return None
return manipulation_area_map_points
def _apply_translation_to_pose(self, pose, translation_vector):
new_pose = copy.deepcopy(pose)
new_pose.pose.position.x = new_pose.pose.position.x + translation_vector[
0]
new_pose.pose.position.y = new_pose.pose.position.y + translation_vector[
1]
return new_pose
def _get_cost_at_index(self, sorted_dict, index):
try:
return sorted_dict.peekitem(index)[1]
except IndexError:
# If cost not found at index in dict, it means
return float('inf')
def _get_obstacle_at_index(self, sorted_dict, index):
try:
return sorted_dict.peekitem(index)[0]
except IndexError as e:
raise e
def _is_same_pose(self, pose_a, pose_b):
pose_a_yaw = Utils.yaw_from_geom_quat(pose_a.pose.orientation)
pose_b_yaw = Utils.yaw_from_geom_quat(pose_b.pose.orientation)
if (np.isclose(pose_a_yaw, pose_b_yaw, atol=self.ANGLE_TOLERANCE)
and np.isclose(pose_a.pose.position.x,
pose_b.pose.position.x,
atol=self.POSITION_TOLERANCE)
and np.isclose(pose_a.pose.position.y,
pose_b.pose.position.y,
atol=self.POSITION_TOLERANCE)):
return True
# Else
return False
def robot_goto(self, pose, is_manipulation):
pose.header.stamp = rospy.Time.now()
request = LocalGoalPoseRequest()
request.local_pose_goal = pose
request.is_manipulation = is_manipulation
result = self.local_goal_pose_client(request)
return result.real_pose
def _simulated_pose_callback(self, pose):
self.current_robot_pose = pose
if not self.is_ready:
self.is_ready = True
def _global_goal_pose_callback(self, goal_pose):
self.make_and_execute_plan_caller(goal_pose)
def manually_set_goal_robot_pose_from_map_coordinates_yaw(
self, coordX, coordY, yaw):
goal_pose = Utils.ros_pose_from_map_coord_yaw(
coordX, coordY, yaw,
self.map_manager.multilayered_map.info.resolution, 1,
self.map_manager.multilayered_map.frame_id)
self.make_and_execute_plan_caller(goal_pose)
def make_and_execute_plan_caller(self, r_goal):
Utils.publish_goal_pose_for_display(r_goal)
success = self.make_and_execute_plan(self.current_robot_pose, r_goal,
self.blocked_obstacles)
if success:
rospy.loginfo("Goal successfully reached.")
else:
rospy.logerr(
"Could not reach goal even after considering all available options."
)
def make_and_execute_plan(self, r_init, r_goal, blocked_obstacles):
r_cur = r_init
is_manip_success = True
blocked_obstacles = blocked_obstacles
eu_cost_l, min_cost_l = SortedDict(), SortedDict()
map_cur = self.map_manager.get_init_map()
p_opt = [
Plan([
Path.from_path(
self.global_planner.make_plan(map_cur, r_cur, r_goal),
False, self.MOVE_COST)
])
]
Utils.publish_plan(p_opt[0])
pass
while not self._is_same_pose(r_cur, r_goal):
map_cur = self.map_manager.get_map_copy()
if map_cur.has_free_space_been_created():
min_cost_l.clear()
obstacles = map_cur.obstacles
is_push_pose_valid = True
is_obstacle_same = True
if p_opt[0].obstacle is not None:
is_obstacle_same = p_opt[0].obstacle.compare(
obstacles[p_opt[0].obstacle.obstacle_id])
if not is_obstacle_same:
p_opt[0].obstacle = obstacles[
p_opt[0].obstacle.obstacle_id]
p_opt[0].safe_swept_area = self._get_safe_swept_area(
p_opt[0].obstacle, p_opt[0].translation_vector,
map_cur.merged_occ_grid)
if p_opt[0].next_step_component == Plan.C1 and not p_opt[
0].obstacle.is_obstacle_push_pose(
p_opt[0].push_pose):
is_push_pose_valid = False
is_plan_not_colliding = not p_opt[0].is_plan_colliding(map_cur)
is_plan_valid = is_plan_not_colliding and is_push_pose_valid and is_manip_success
if not is_plan_valid:
p_opt = [
Plan([
Path.from_path(
self.global_planner.make_plan(
map_cur, r_cur, r_goal), False, self.MOVE_COST)
])
]
Utils.publish_plan(p_opt[0])
self.make_plan(r_cur, r_goal, map_cur, obstacles,
blocked_obstacles, p_opt, eu_cost_l, min_cost_l)
Utils.debug_erase_partial_paths()
Utils.publish_plan(p_opt[0])
pass
# Stop condition if no plan is found
if p_opt[0].cost >= float("inf"):
return False
is_manip_success = True
r_next = p_opt[0].get_next_step()
c_next = p_opt[0].next_step_component
is_manipulation = c_next == Plan.C2
r_real = self.robot_goto(r_next, is_manipulation)
if is_manipulation and not self._is_same_pose(r_real, r_next):
is_manip_success = False
blocked_obstacles.add(p_opt[0].obstacle.obstacle_id)
r_cur = r_real
self.debug_rate.sleep()
# Endwhile
return True
def make_plan(self, r_cur, r_goal, map_cur, obstacles, blocked_obstacles,
p_opt, eu_cost_l, min_cost_l):
for obstacle in obstacles.values():
c3_est = float("inf")
for push_pose in obstacle.push_poses:
c3_est = min(
c3_est,
Utils.euclidean_distance_ros_poses(push_pose, r_goal))
eu_cost_l[obstacle] = c3_est
index_EL, index_ML = 0, 0
evaluated_obstacles = set()
while (min(self._get_cost_at_index(min_cost_l, index_ML),
self._get_cost_at_index(eu_cost_l, index_EL)) <
p_opt[0].cost):
if self._get_cost_at_index(min_cost_l,
index_ML) < self._get_cost_at_index(
eu_cost_l, index_EL):
evaluated_obstacle = self._get_obstacle_at_index(
min_cost_l, index_ML)
if evaluated_obstacle not in evaluated_obstacles:
p = self.plan_for_obstacle(evaluated_obstacle, p_opt,
map_cur, r_cur, r_goal,
blocked_obstacles)
if p is not None:
min_cost_l[evaluated_obstacle] = p.min_cost
else:
min_cost_l[evaluated_obstacle] = float("inf")
evaluated_obstacles.add(evaluated_obstacle)
index_ML = index_ML + 1
else:
evaluated_obstacle = self._get_obstacle_at_index(
eu_cost_l, index_EL)
if evaluated_obstacle not in evaluated_obstacles:
try:
# If the min_cost_L doesn't contain the obstacle, do except
min_cost_l.index(evaluated_obstacle)
except ValueError:
p = self.plan_for_obstacle(evaluated_obstacle, p_opt,
map_cur, r_cur, r_goal,
blocked_obstacles)
if p is not None:
min_cost_l[evaluated_obstacle] = p.min_cost
else:
min_cost_l[evaluated_obstacle] = float("inf")
evaluated_obstacles.add(evaluated_obstacle)
index_EL = index_EL + 1
def plan_for_obstacle(self, obstacle, p_opt, map_cur, r_cur, r_goal,
blocked_obstacles):
if obstacle.obstacle_id in blocked_obstacles:
return None
paths = SortedDict()
for push_pose in obstacle.push_poses:
push_pose_yaw = Utils.yaw_from_geom_quat(
push_pose.pose.orientation)
push_pose_unit_direction_vector = (math.cos(push_pose_yaw),
math.sin(push_pose_yaw))
c1 = Path.from_path(
self.global_planner.make_plan(map_cur, r_cur, push_pose, True),
False, self.MOVE_COST)
Utils.debug_publish_path(c1, "/simulated/debug_c1")
# If c1 does not exist
if not c1.path.poses:
continue
seq = 1
translation_vector = (push_pose_unit_direction_vector[0] *
self.ONE_PUSH_DISTANCE * float(seq),
push_pose_unit_direction_vector[1] *
self.ONE_PUSH_DISTANCE * float(seq))
safe_swept_area = self._get_safe_swept_area(
obstacle, translation_vector, map_cur.merged_occ_grid)
obstacle_sim_pose = self._apply_translation_to_pose(
push_pose, translation_vector)
c3_est = Path.from_poses(obstacle_sim_pose, r_goal, False,
self.MOVE_COST,
push_pose_unit_direction_vector,
self.ONE_PUSH_DISTANCE, map_cur.frame_id)
Utils.debug_publish_path(c3_est, "/simulated/debug_c3")
# TODO Make PUSH_COST an attribute of the object that is initialized to 1.0 until object is identified.
c_est = c1.cost + c3_est.cost + self.PUSH_COST * Utils.euclidean_distance(
translation_vector, (0.0, 0.0))
while c_est <= p_opt[0].cost and safe_swept_area is not None:
c2 = Path.from_poses(push_pose, obstacle_sim_pose, True,
self.PUSH_COST,
push_pose_unit_direction_vector,
self.ONE_PUSH_DISTANCE, map_cur.frame_id)
Utils.debug_publish_path(c2, "/simulated/debug_c2")
map_mod = copy.deepcopy(map_cur)
map_mod.manually_move_obstacle(obstacle.obstacle_id,
translation_vector)
c3 = Path.from_path(
self.global_planner.make_plan(map_mod, obstacle_sim_pose,
r_goal), False,
self.MOVE_COST)
Utils.debug_publish_path(c3, "/simulated/debug_c3")
# If c3 exists
if c3.path.poses:
p = Plan([c1, c2, c3], obstacle, translation_vector,
safe_swept_area, push_pose)
paths[p] = p.cost
if p.cost < p_opt[0].cost:
p_opt[0] = p
seq = seq + 1
translation_vector = (push_pose_unit_direction_vector[0] *
self.ONE_PUSH_DISTANCE * float(seq),
push_pose_unit_direction_vector[1] *
self.ONE_PUSH_DISTANCE * float(seq))
safe_swept_area = self._get_safe_swept_area(
obstacle, translation_vector, map_cur.merged_occ_grid)
obstacle_sim_pose = self._apply_translation_to_pose(
push_pose, translation_vector)
c3_est = Path.from_poses(obstacle_sim_pose, r_goal, False,
self.MOVE_COST,
push_pose_unit_direction_vector,
self.ONE_PUSH_DISTANCE,
map_cur.frame_id)
Utils.debug_publish_path(c3_est, "/simulated/debug_c3")
c_est = c1.cost + c3_est.cost + self.PUSH_COST * Utils.euclidean_distance(
translation_vector, (0.0, 0.0))
# Return path with lowest cost : the first in the ordered dictionnary
# If there is none, return None
try:
return paths.peekitem(0)[0]
except IndexError:
return None
def main():
WIDTH_SCREEN, HEIGHT_SCREEN = 800, 800
pygame.init()
screen = pygame.display.set_mode((WIDTH_SCREEN, HEIGHT_SCREEN))
pygame.display.set_caption("A* Pathfinding")
pygame.font.init()
screen.fill((150, 150, 150))
renderer = Renderer(WIDTH_SCREEN, HEIGHT_SCREEN)
# Initial call
data = MapManager.load_map_from_file(1)
# Main Loop
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
return
elif event.type == pygame.MOUSEBUTTONUP:
pos = pygame.mouse.get_pos()
if event.button == 1:
data = MapManager.write_map_with_mouse_click(
data, pos, constants.STATUS_BLOCK)
elif event.button == 3:
data = MapManager.write_map_with_mouse_click(
data, pos, constants.STATUS_DEFAULT)
steps = Finder.find_path_with_astar(data)
data = MapManager.draw_path_with_steps(data, steps)
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_KP1:
data = MapManager.load_map_from_file(1)
elif event.key == pygame.K_KP2:
data = MapManager.load_map_from_file(2)
elif event.key == pygame.K_KP3:
data = MapManager.load_map_from_file(3)
elif event.key == pygame.K_KP4:
data = MapManager.load_map_from_file(4)
elif event.key == pygame.K_KP5:
data = MapManager.load_map_from_file(5)
elif event.key == pygame.K_KP6:
data = MapManager.load_map_from_file(6)
elif event.key == pygame.K_KP7:
data = MapManager.load_map_from_file(7)
elif event.key == pygame.K_KP8:
data = MapManager.load_map_from_file(8)
elif event.key == pygame.K_KP9:
data = MapManager.load_map_from_file(9)
elif event.key == pygame.K_KP0:
data = MapManager.load_map_from_file(0)
elif event.key == pygame.K_f:
steps = Finder.find_path_with_astar(data)
data = MapManager.draw_path_with_steps(data, steps)
renderer.render(screen, data)
pygame.display.flip()
SCREEN_WIDTH = 80
SCREEN_HEIGHT = 60
tcod.console_init_root(SCREEN_WIDTH, SCREEN_HEIGHT, 'TESRL', False)
game = GAME
ui = UI(GAME, 0, 50, 80, 10)
ui.message_log = MESSAGE_LOG
MESSAGE_LOG.add_message('Welcome to tesRL!')
# level_map = ASSETS.get_map('main', 'test', 'test')
level_map = MapGen.create_random_map(50, 80, sorted(ASSETS.assets.get('rooms').keys()))
player = ASSETS.instantiate('creatures', 'base_player')
game.player = MapManager.place_object_randomly(level_map, player)
game.current_map = level_map
for _ in range(5):
MapManager.place_creature(level_map, 'draugr', *MapManager.find_random_free_space(game.current_map))
def handle_keys():
key = tcod.console_wait_for_keypress(True)
key = key.vk if key.vk is not tcod.KEY_CHAR else chr(key.c)
player = game.player
GAME.last_player_action = PlayerAction.OTHER_ACTION
if key == tcod.KEY_ESCAPE:
pass
if key in game.controls.actions['up']:
if GAME.game_state == GameState.PLAYING:
import numpy as np import cv2 from matplotlib import pyplot as plt import math # initialize the map, its size, its zoom rate WINDOW_NAME = "Map" MAP_HEIGHT = 640 ZOOM = 18 #starting_coords = (43.770572, -79.507184) # stong pond #starting_coords = (43.770972, -79.507276) # stong pond zoom 20 starting_coords = (43.723522, -79.801030) # loafers lake, brampton # save the map manager = MapManager(MAP_HEIGHT, ZOOM, starting_coords[0], starting_coords[1]) distance = manager.linear_meters_in_map print manager.linear_meters_in_map # read the map and cread a binary map areamap = read_map() mapfile = "map_stong.png" mapfile = "map_zoom20.png" mapfile = "square.png" mapfile = "Ellipes.png" mapfile = "map_loafers_2.jpg" bitmap = areamap.convert_to_binary(mapfile) # Cellural Decompostion