def main(route_json_path):
route = Route(route_json_path, dry_run=True, start_map=None)
keyboard_input = KeyboardInput()
# Loop through the positions in our list one at a time
while not route.is_complete():
next_pose = route.get_next_waypoint()
print(next_pose)
if keyboard_input.check_hit():
char = keyboard_input.get_char()
# Skip the waypoint if n is pressed (ord == 110)
if "n" == char:
print("Skipping waypoint: ", next_pose.name)
break
# If the space bar is pressed, just wait until
if " " == char:
print("Pausing")
pause(keyboard_input)
# Wait for a second to make sure the ARK has time to accept the goal
time.sleep(1.0)
def main(route_json_path, start_map, start_waypoint_index):
# Start autonomy
while not ARK.start_autonomy() and not rospy.is_shutdown():
rospy.loginfo("Failed to start autonomy. Retrying in 3 seconds...")
rospy.sleep(3)
rospy.loginfo("ARK autonomy started")
route = Route(route_json_path,
start_map=start_map,
start_waypoint_index=start_waypoint_index)
# Start a ROS node called "PositionCommander"
rospy.init_node("PositionCommander", anonymous=True)
# Get a handle to the topic the ARK uses to accept goal
pub = rospy.Publisher("/ark_bridge/send_goal", SendGoal, queue_size=1)
# Get a handle to the topic the ARK uses to accept goal
cancel_goal_publisher = rospy.Publisher("/ark_bridge/cancel_goal",
Empty,
queue_size=1)
# Get a handle to the topic the ARK uses to return its status
rospy.Subscriber("/ark_bridge/path_planner_status", GoalStatusArray,
path_planner_status_callback)
keyboard_input = KeyboardInput()
filter_adjuster = LaserFilterAdjuster()
# Loop through the positions in our list one at a time
while not route.is_complete() and not rospy.is_shutdown():
next_pose = route.get_next_waypoint()
filter_adjuster.set_radius(next_pose.get_upper_lidar_threshold())
rospy.loginfo("Moving to " + next_pose.name)
# Get a message for the ark and send it to the new goal topic
pub.publish(ARK.create_goal_message(next_pose))
# Wait for a second to make sure the ARK has time to accept the goal
rospy.sleep(1)
# Loop until the ARK says it is done running a goal
while (tracking_goal) and not rospy.is_shutdown():
if keyboard_input.check_hit():
char = keyboard_input.get_char()
# Skip the waypoint if n is pressed (ord == 110)
if "n" == char:
rospy.loginfo("Skipping waypoint: {}".format(
next_pose.name))
cancel_goal_publisher.publish(Empty())
# If the space bar is pressed, just wait until
elif " " == char:
rospy.loginfo("Pausing")
pause(keyboard_input)
# Set the threshold to use for the lidars using (0-9)
elif char.isdigit():
filter_adjuster.set_radius(int(char))
rospy.sleep(
1.0) # sleep to allow the point clouds to update
elif "w" == char:
filter_adjuster.increase_radius()
rospy.sleep(
1.0) # sleep to allow the point clouds to update
elif "s" == char:
filter_adjuster.decrease_radius()
rospy.sleep(
1.0) # sleep to allow the point clouds to update
rospy.sleep(1)
rospy.loginfo("Route completed")
# stop autonomy once the goal is reached
ARK.stop_autonomy()
rospy.loginfo("Autonomoy stopped")