Add autotest scripts (#443)

clover/src/autotest/autotest_aruco.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+
+import rospy
+import math
+import signal
+import sys
+import dynamic_reconfigure.client
+from clover import srv
+from std_srvs.srv import Trigger
+from sensor_msgs.msg import Range
+from aruco_pose.msg import MarkerArray
+from util import handle_response
+
+rospy.init_node('autotest_aruco', disable_signals=True) # disable signals to allow interrupting with ctrl+c
+
+flow_client = dynamic_reconfigure.client.Client('optical_flow')
+get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
+land = handle_response(rospy.ServiceProxy('land', Trigger))
+
+def interrupt(sig, frame):
+    print('\nInterrupted, landing...')
+    land()
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, interrupt)
+
+def print_current_map_position():
+    telem = get_telemetry()
+    dist = rospy.wait_for_message('rangefinder/range', Range).range
+    print('Map position:\tx={:.1f}\ty={:.1f}\tz={:.1f}\tyaw={:.1f}\tdist={:.2f}'.format(telem.x, telem.y, telem.z, telem.yaw, dist))
+
+def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
+        frame_id='body', tolerance=0.2, auto_arm=False):
+
+    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
+        frame_id=frame_id, auto_arm=auto_arm)
+
+    if not res.success:
+        return res
+
+    while not rospy.is_shutdown():
+        telem = get_telemetry(frame_id='navigate_target')
+        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
+            return res
+        rospy.sleep(0.2)
+
+markers = rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=3)
+left = min(marker.pose.position.x for marker in markers.markers)
+bottom = min(marker.pose.position.y for marker in markers.markers)
+width = max(marker.pose.position.x for marker in markers.markers)
+height = max(marker.pose.position.y for marker in markers.markers)
+center_x = left + width / 2
+center_y = bottom + height / 2
+
+print('Map rect: %g %g - %g %g' % (left, bottom, width, height))
+
+input('Take off and hover 1 m [enter] ')
+navigate_wait(x=0, y=0, z=1, frame_id='body', auto_arm=True)
+print_current_map_position()
+
+input('Go to corner %g %g 1.5 speed 1 [enter] ' % (width, height))
+navigate_wait(x=width, y=height, z=1.5, speed=1, frame_id='aruco_map')
+print_current_map_position()
+
+input('Go to center %g %g 1.5 speed 5 [enter] ' % (center_x, center_y))
+navigate_wait(x=center_x, y=center_y, z=1.5, speed=5, frame_id='aruco_map')
+print_current_map_position()
+
+input('Disable optical flow and keep hovering [enter] ')
+flow_client.update_configuration({'enabled': False})
+rospy.sleep(5)
+
+input('Enable optical flow back [enter] ')
+flow_client.update_configuration({'enabled': True})
+
+input('Go to side 1 %g 2 heading top [enter] ' % (center_y))
+navigate_wait(x=1, y=center_y, z=2, yaw=1.57, frame_id='aruco_map')
+print_current_map_position()
+
+marker_id = markers.markers[0].id
+input('Go to marker %d z=1.5 [enter] ' % marker_id)
+navigate_wait(x=0, y=0, z=1.5, yaw=0, frame_id='aruco_%d' % marker_id)
+print_current_map_position()
+
+input('Perform landing [enter] ')
+land()

clover/src/autotest/autotest_flight.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+
+import rospy
+import math
+from math import nan
+import signal
+import sys
+from clover import srv
+from std_srvs.srv import Trigger
+from sensor_msgs.msg import Range
+from util import handle_response
+
+rospy.init_node('autotest_flight', disable_signals=True) # disable signals to allow interrupting with ctrl+c
+
+get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
+navigate_global = handle_response(rospy.ServiceProxy('navigate_global', srv.NavigateGlobal))
+set_position = handle_response(rospy.ServiceProxy('set_position', srv.SetPosition))
+set_velocity = handle_response(rospy.ServiceProxy('set_velocity', srv.SetVelocity))
+set_attitude = handle_response(rospy.ServiceProxy('set_attitude', srv.SetAttitude))
+set_rates = handle_response(rospy.ServiceProxy('set_rates', srv.SetRates))
+land = handle_response(rospy.ServiceProxy('land', Trigger))
+
+def interrupt(sig, frame):
+    print('\nInterrupted, landing...')
+    land()
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, interrupt)
+
+def navigate_wait(x=0, y=0, z=0, yaw=nan, yaw_rate=0, speed=0.5, \
+        frame_id='body', tolerance=0.2, auto_arm=False):
+
+    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
+        frame_id=frame_id, auto_arm=auto_arm)
+
+    if not res.success:
+        return res
+
+    while not rospy.is_shutdown():
+        telem = get_telemetry(frame_id='navigate_target')
+        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
+            return res
+        rospy.sleep(0.2)
+
+def print_distance():
+    dist = rospy.wait_for_message('rangefinder/range', Range).range
+    print('Distance: {:.2f}'.format(dist))
+
+input('Take off and hover 1 m [enter] ')
+navigate_wait(z=1, frame_id='body', auto_arm=True)
+print_distance()
+start = get_telemetry()
+
+input('Fly forward 2 m [enter] ')
+navigate_wait(x=2, frame_id='navigate_target')
+print_distance()
+
+input('Climb 0.5 m [enter] ')
+navigate_wait(z=0.5, frame_id='navigate_target')
+print_distance()
+
+input('Rotate left 90° [enter] ')
+navigate(yaw=math.pi / 2, frame_id='navigate_target')
+rospy.sleep(3)
+
+input('Use set_velocity to fly forward 2 m speed 1 [enter]')
+set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
+rospy.sleep(2)
+set_position(frame_id='body')
+
+input('Rotate right 90° [enter] ')
+navigate(yaw=-math.pi / 2, frame_id='navigate_target')
+rospy.sleep(3)
+
+input('Use set_attitude to fly backwards [enter]')
+set_attitude(pitch=-0.3, roll=0, yaw=0, thrust=0.5, frame_id='body')
+rospy.sleep(0.3)
+set_position(frame_id='body')
+
+input('Use set_attitude to fly right [enter]')
+set_attitude(pitch=0, roll=0.3, yaw=0, thrust=0.5, frame_id='body')
+rospy.sleep(0.5)
+set_position(frame_id='body')
+
+input('Use set_rates to fly right [enter]')
+set_rates(roll_rate=1.2, thrust=0.5)
+rospy.sleep(0.4)
+set_position(frame_id='body')
+
+input('Rotate 360° to the right using yaw_rate [enter]')
+set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1)
+rospy.sleep(2 * math.pi)
+set_position(frame_id='body')
+
+input('Return to start point [enter]')
+navigate_wait(x=start.x, y=start.y, z=start.z, yaw=start.yaw, speed=1, frame_id='map')
+
+input('Land [enter]')
+land()

clover/src/autotest/autotest_led.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+import rospy
+import functools
+from clover.srv import SetLEDEffect
+from led_msgs.srv import SetLEDs
+from led_msgs.msg import LEDStateArray, LEDState
+from util import handle_response
+
+rospy.init_node('autotest_led', disable_signals=True)
+
+set_leds = handle_response(rospy.ServiceProxy('led/set_leds', SetLEDs))
+set_effect = handle_response(rospy.ServiceProxy('led/set_effect', SetLEDEffect))
+
+led_count = len(rospy.wait_for_message('led/state', LEDStateArray, timeout=10).leds)
+print('LED count =', led_count)
+
+print('== Testing effects ==')
+
+input('Fill red [enter] ')
+set_effect(r=255, g=0, b=0)
+
+input('Fill green [enter] ')
+set_effect(r=0, g=100, b=0)
+
+input('Blink white [enter] ')
+set_effect(effect='blink', r=255, g=255, b=255)
+rospy.sleep(3)
+
+input('Blink fast violet [enter] ')
+set_effect(effect='blink_fast', r=220, g=20, b=250)
+rospy.sleep(3)
+
+input('Fade to blue [enter] ')
+set_effect(effect='fade', r=0, g=0, b=255)
+
+input('Wipe to yellow [enter] ')
+set_effect(effect='wipe', r=255, g=255, b=40)
+
+input('Flash red [enter] ')
+set_effect(effect='flash', r=255, g=0, b=0)
+rospy.sleep(1)
+
+input('Rainbow [enter] ')
+set_effect(effect='rainbow')
+rospy.sleep(4)
+
+input('Rainbow fill [enter] ')
+set_effect(effect='rainbow_fill')
+rospy.sleep(4)
+
+input('Turn off [enter] ')
+set_effect()
+
+print('== Testing low-level control ==')
+
+input('Fill orange [enter] ')
+set_leds(leds=[LEDState(index=i, r=245, g=155, b=0) for i in range(led_count)])
+
+input('Fill blue gradient [enter] ')
+set_leds(leds=[LEDState(index=i, r=0, g=20, b=int(255 * i / led_count)) for i in range(led_count)])
+
+input('Animate green dot [enter] ')
+set_effect()
+for i in range(led_count):
+    if i > 0:
+        set_leds(leds=[LEDState(index=i - 1, r=0, g=0, b=0)])
+    set_leds(leds=[LEDState(index=i, r=0, g=255, b=0)])
+    rospy.sleep(0.05)
+
+input('Turn off [enter] ')
+set_effect()

clover/src/autotest/util.py

@@ -0,0 +1,11 @@
+import functools
+
+# decorator to handle response and print error message
+def handle_response(fn):
+    @functools.wraps(fn)
+    def wrapper(*args, **kwargs):
+        res = fn(*args, **kwargs)
+        if not res.success:
+            print('\033[91mError:\033[0m {}'.format(res.message))
+        return res
+    return wrapper