Beispiel #1
0
    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
Beispiel #2
0
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