def call_a_star():
    """call A* algorithm"""
    global path, closed_set, open_set, came_from, augmented_occ, optimized_path
    time_start = time.clock()
    path = a_star_search(grid, augmented_occ, start_point, goal_point, goal_distance, closed_set=closed_set,
                         open_set=open_set, came_from=came_from)
    time_elapsed = time.clock() - time_start
    if len(path) <= 0:
        sys.stderr.write("Warning: A* returns no result!\n")
    else:
        print 'A* finished in %ss, path length = %d' % (str(time_elapsed), len(path))
        time_start = time.clock()
        optimized_path = optimize_path(grid, augmented_occ, path)
        time_elapsed = time.clock() - time_start
        print 'Path optimization finished in %ss, optimized length = %d' % (str(time_elapsed), len(optimized_path))
        if len(optimized_path) > 0 and optimized_path[0][0] == -1:
            print 'Unknown area direction: %f degree' % math.degrees(optimized_path[0][1])
def start_navigator():
    global tf_listener, nav_pub, last_map_augmented_occ, last_path, last_goal, last_map, last_target_point, last_robot_point, is_watch_clockwise, last_path_fail, is_watch_mode, last_watch_angle, current_goal_angle
    rospy.init_node("map_navigator")
    tf_listener = tf.TransformListener()
    rospy.Service(SERVICE_START_NAVIGATION, StartNavigation, handle_start_navigation)
    rospy.Service(SERVICE_GENERATE_NAVIGATION_TARGET, GenerateNavigationTarget, handle_generate_nav_target)
    # noinspection PyTypeChecker
    rospy.Subscriber(map_topic, OccupancyGrid, map_received)
    nav_pub = rospy.Publisher(robot_control_topic, Twist, queue_size=1)
    rospy.loginfo("Map Navigator node started")
    rate = rospy.Rate(working_frequency)
    while not rospy.is_shutdown():
        if current_goal is not None and current_map is not None:
            with goal_lock:
                try:
                    t = tf_listener.getLatestCommonTime(map_frame, robot_frame)
                    (transform, quaternion) = tf_listener.lookupTransform(map_frame, robot_frame, t)
                except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
                    rospy.logerr("Failed to lookup transform: %s" % e)
                    move_robot(0, 0)
                    rate.sleep()
                    continue
                resolution = current_map.info.resolution
                robot_point = (int(math.floor(transform[0] / resolution)), int(math.floor(transform[1] / resolution)))
                if last_robot_point is not None and last_robot_point != robot_point:
                    print "Move from %s to %s" % (str(last_robot_point), str(robot_point))
                last_robot_point = robot_point
                if last_map != current_map:
                    print "Preprocessing map..."
                    last_map_augmented_occ = preprocess_map(current_map)
                if last_map != current_map or last_goal != current_goal:
                    print "Running a*..."
                    current_goal_cell = convert_way_point_to_map_cell(current_map, current_goal)
                    robot_point_cell = convert_point_to_map_cell(current_map, robot_point)
                    last_path = a_star_search(
                        current_map, last_map_augmented_occ, robot_point_cell, current_goal_cell, current_goal_distance
                    )
                    print "Returned path length: %d" % len(last_path)
                    if len(last_path) > 0:
                        print "Optimizing path..."
                        last_path = optimize_path(current_map, last_map_augmented_occ, last_path)
                        print "Optimized path length: %d" % len(last_path)
                    if len(last_path) == 0:
                        rospy.logwarn(
                            "Failed to find a path from (%d, %d) to (%d, %d)"
                            % (robot_point[0], robot_point[1], current_goal.x, current_goal.y)
                        )
                        last_target_point = None
                        last_path_fail = True
                    else:
                        # pop first target point (start point)
                        last_target_point = convert_map_cell_to_point(current_map, last_path.pop())
                        print "Start point %s" % str(last_target_point)
                        last_path_fail = False
                last_goal = current_goal
                last_map = current_map
                if last_path_fail:
                    move_robot(0, angle_watch_area)  # stay here and watch around
                    rate.sleep()
                    continue
                # keep while structure in case we want to change equality check to similarity check
                while robot_point == last_target_point:
                    if len(last_path) > 0:
                        # pop next target point
                        next_cell = last_path.pop()
                        if next_cell[0] == -1:
                            is_watch_mode = True
                            last_target_point = None
                            last_watch_angle = next_cell[1]
                            print "Unknown area ahead, wait and watch %f degree" % math.degrees(last_watch_angle)
                        else:
                            is_watch_mode = False
                            last_target_point = convert_map_cell_to_point(current_map, next_cell)
                            print "Next target point %s" % str(last_target_point)
                        print "Point remains: %d" % len(last_path)
                    else:
                        last_target_point = None
                        break
                robot_angle = euler_from_quaternion(quaternion)[2]
                if last_target_point is None:
                    if is_watch_mode:
                        delta_angle = get_angle_diff(robot_angle, last_watch_angle)
                        if delta_angle < -angle_watch_area / 2:
                            is_watch_clockwise = False
                        elif delta_angle > angle_watch_area / 2:
                            is_watch_clockwise = True
                        if is_watch_clockwise:
                            move_robot(0, -speed_angular_watch)
                        else:
                            move_robot(0, speed_angular_watch)
                    else:
                        if current_goal_angle is None and current_goal_distance > 0:
                            current_goal_angle = math.atan2(
                                current_goal.y - robot_point[1], current_goal.x - robot_point[0]
                            )
                        if current_goal_angle is None or adjust_angle(robot_angle, current_goal_angle):
                            if len(nav_target_list) > 0:
                                next_nav_target = nav_target_list.pop(0)
                                rospy.loginfo(
                                    "Navigation to (%d, %d) (angle=%f) has been finished!"
                                    % (current_goal.x, current_goal.y, current_goal_angle)
                                )
                                angle = next_nav_target.angle if not next_nav_target.ignore_angle else None
                                set_goal(next_nav_target.point, angle, next_nav_target.goal_distance)
                            else:
                                rospy.loginfo("Navigation of ALL targets has been finished!")
                                set_goal(None, None, None)
                                move_robot(0, 0)
                else:  # move to target point
                    dx = (last_target_point[0] + 0.5) * resolution - transform[0]
                    dy = (last_target_point[1] + 0.5) * resolution - transform[1]
                    dist = math.sqrt(dx * dx + dy * dy)
                    target_angle = math.atan2(dy, dx)
                    if adjust_angle(robot_angle, target_angle):
                        move_robot(speed_linear_base + math.log(1 + dist, 2) * speed_linear_log_haste, 0)
        rate.sleep()