def move(target_x, target_y, get_telemetry, navigate):
current_pos = get_telemetry(frame_id='aruco_map')
navigate(x=target_x,
y=target_y,
z=current_pos.z,
yaw=float('nan'),
speed=settings.SPEED,
frame_id='aruco_map')
shared.wait_till_arrive(get_telemetry, target_x, target_y, current_pos.z)
def task1(target_x, target_y):
# target_x = 3
# target_y = 3
current_pos = get_telemetry(frame_id='aruco_map')
navigate(x=target_x,
y=target_y,
z=current_pos.z,
speed=settings.SPEED,
frame_id='aruco_map')
shared.wait_till_arrive(get_telemetry, target_x, target_y, current_pos.z)
def take_off(get_telemetry, navigate):
# Taking off with height: ~1.5m
start_pos = get_telemetry()
print(start_pos)
print("Height: ~{:}m".format(settings.VIEW_HIGHT))
navigate(z=settings.VIEW_HIGHT,
speed=settings.SPEED,
frame_id='body',
auto_arm=True)
shared.wait_till_arrive(get_telemetry, 0, 0,
start_pos.z + settings.VIEW_HIGHT, 'map', 1)
# Safety check for the error of throttling speed
shared.safety_check(get_telemetry, arming)
target_x = 2
target_y = 1
print("Starting the task after:")
count_down(3)
print("Height: ~1.5m")
navigate(x=0, y=0, z=1.5, speed=settings.SPEED, frame_id='body', auto_arm=True)
#shared.wait_till_arrive(get_telemetry, 0, 0, 1.5, 'aruco_map',1)
count_down(3)
current_pos = get_telemetry("aruco_map")
navigate(x=current_pos.x, y=current_pos.y, z=current_pos.z, speed=settings.SPEED, frame_id='aruco_map')
shared.wait_till_arrive(get_telemetry, current_pos.x, current_pos.y, current_pos.z)
current_pos = get_telemetry("aruco_map")
print("Height: 1.5m move to ArUco (2,1)")
navigate(x=(current_pos.x)+1, y=(current_pos.y)+1, z=current_pos.z, speed=settings.SPEED, frame_id='aruco_map')
shared.wait_till_arrive(get_telemetry, current_pos.x+1, current_pos.y+1, current_pos.z)
#count_down(3)
target_x += 1
# TODO: Test navigate in not passing some parameters
#navigate(x=target_x, z=settings.VIEW_HIGHT, speed=settings.SPEED, frame_id='aruco_map')
# TODO: Test navigate with passing the paremeter as it is
#navigate(x=x, y=target_y, z=settings.VIEW_HIGHT, speed=settings.SPEED, frame_id='aruco_map')
# TODO: To find out how to know the position of the x and y in aruco markers from navigate not from stroing inside the program
print("Landing")
print("Height: ~2.5m")
navigate(x=0,
y=0,
z=settings.VIEW_HIGHT,
speed=settings.SPEED,
frame_id='body',
auto_arm=True)
#shared.wait_till_arrive(get_telemetry, 0, 0, settings.VIEW_HIGHT, 'map')
count_down(3)
print("Height: 2m move to ArUco (5,3)")
navigate(x=target_x,
y=target_y,
z=settings.VIEW_HIGHT,
speed=settings.SPEED,
frame_id='aruco_map')
shared.wait_till_arrive(get_telemetry, target_x, target_y, settings.VIEW_HIGHT)
print("Ready to be controlled from keyboard")
rospy.Subscriber("keyboard_value", String, key_callback)
print("Get ready in:")
count_down(5)
while (out_flag == 0):
navigate(x=target_x,
y=target_y,
z=settings.VIEW_HIGHT,
speed=settings.SPEED,
frame_id='aruco_map')
shared.wait_till_arrive(get_telemetry, target_x, target_y,
settings.VIEW_HIGHT)
print(target_x, target_y, out_flag)