def __init__(self):
self.map_s = MapStorage(debug=True)
# Create default marker for display of swarm / debug
point_marker = Marker()
# point_marker.type = Marker.ARROW
point_marker.type = Marker.SPHERE
point_marker.action = Marker.ADD
# point_marker.scale.x = 2.5
# point_marker.scale.y = 0.35
# point_marker.scale.z = 0.35
point_marker.scale.x = 0.2
point_marker.scale.y = 0.2
point_marker.scale.z = 0.2
point_marker.color.r = 1.0
point_marker.color.g = 0.0
point_marker.color.b = 0.0
point_marker.color.a = 1.0
self.mark_p = MarkerPlacer("rviz_map_test", "map", 1000, point_marker)
self.x = 0.0
self.y = 0.0
self.start_x = self.map_s.min_x_pos+0.05
self.start_y = self.map_s.min_y_pos+0.05
self.end_x = self.map_s.max_x_pos
self.end_y = self.map_s.max_y_pos
self.delta_dist = self.map_s.resolution
def dist(pos0, pos1):
dx = float(pos0[0]) - float(pos1[0])
dy = float(pos0[1]) - float(pos1[1])
return math.sqrt(dx**2.0 + dy**2.0)
if __name__ == "__main__":
rospy.init_node("astar_tester")
t = Timer("MapStorage")
map = MapStorage()
map.downsample_map_by_half()
map.downsample_map_by_half()
map.expand_map()
map.expand_map()
map.expand_map()
t.print_delta()
def_marker = Marker()
def_marker.type = Marker.SPHERE
def_marker.action = Marker.ADD
def_marker.scale.x = 0.10
def_marker.scale.y = 0.10
def_marker.scale.z = 0.10
def_marker.color.r = 0.0
def_marker.color.g = 1.0
def_marker.color.b = 0.0
def_marker.color.a = 1.0
markers = MarkerPlacer("astar_markers", "map", 1000, def_marker)
# Original map
# Heuristic: rank = dist(n, goal) + cost
# cost = current[1].cost + X
# Map: 2x down, 2x expand