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 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()
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()
