Tools: autotest: add tests for gripper mission items

Tools/autotest/arducopter.py

@@ -1774,6 +1774,28 @@ class AutoTestCopter(AutoTest):
         if ex is not None:
             raise ex
 
+    def test_gripper_mission(self):
+        self.context_push()
+        ex = None
+        try:
+            self.load_mission("copter-gripper-mission.txt")
+            self.mavproxy.send('mode loiter\n')
+            self.wait_ready_to_arm()
+            self.arm_vehicle()
+            self.mavproxy.send('mode auto\n')
+            self.wait_mode('AUTO')
+            self.set_rc(3, 1500)
+            self.mavproxy.expect("Gripper Grabbed")
+            self.mavproxy.expect("Gripper Released")
+        except Exception as e:
+            self.progress("Exception caught")
+            self.mavproxy.send('mode land\n')
+            ex = e
+        self.context_pop()
+        self.mav.motors_disarmed_wait()
+        if ex is not None:
+            raise ex
+
     def autotest(self):
         """Autotest ArduCopter in SITL."""
         self.check_test_syntax(test_file=os.path.realpath(__file__))
@@ -1988,6 +2010,9 @@ class AutoTestCopter(AutoTest):
             # Gripper test
             self.run_test("Test gripper", self.test_gripper)
 
+            self.run_test("Test gripper mission items",
+                          self.test_gripper_mission);
+
             '''vision position'''  # expects vehicle to be disarmed
             self.run_test("Fly Vision Position", self.fly_vision_position)
 

Tools/autotest/arduplane.py

@@ -663,6 +663,25 @@ class AutoTestPlane(AutoTest):
         if x is None:
             raise NotAchievedException("No CAMERA_FEEDBACK message received")
 
+    def test_gripper_mission(self):
+        self.context_push()
+        ex = None
+        try:
+            self.wp_load(os.path.join(testdir, "plane-gripper-mission.txt"))
+            self.mavproxy.send('switch 1\n')  # auto mode
+            self.wait_mode('AUTO')
+            self.wait_ready_to_arm()
+            self.arm_vehicle()
+            self.mavproxy.expect("Gripper Grabbed")
+            self.mavproxy.expect("Gripper Released")
+            self.mavproxy.expect("Auto disarmed")
+        except Exception as e:
+            self.progress("Exception caught")
+            ex = e
+        self.context_pop()
+        if ex is not None:
+            raise ex
+
     def autotest(self):
         """Autotest ArduPlane in SITL."""
         self.check_test_syntax(test_file=os.path.realpath(__file__))
@@ -679,6 +698,9 @@ class AutoTestPlane(AutoTest):
             self.set_rc(3, 1000)
             self.set_rc(8, 1800)
 
+            self.run_test("Test Gripper mission items",
+                          self.test_gripper_mission)
+
             self.set_parameter("RC12_OPTION", 9)
             self.reboot_sitl() # needed for RC12_OPTION to take effect
 
@@ -734,6 +756,9 @@ class AutoTestPlane(AutoTest):
                           lambda: self.fly_mission(
                               os.path.join(testdir, "ap1.txt")))
 
+            self.run_test("Test Gripper mission items",
+                          self.test_gripper_mission)
+
             self.run_test("Log download",
                           lambda: self.log_download(
                               self.buildlogs_path("ArduPlane-log.bin")))

Tools/autotest/ardusub.py

@@ -10,6 +10,7 @@ from pymavlink import mavutil
 from pysim import util
 
 from common import AutoTest
+from common import NotAchievedException
 
 # get location of scripts
 testdir = os.path.dirname(os.path.realpath(__file__))
@@ -130,6 +131,33 @@ class AutoTestSub(AutoTest):
 
         self.progress("Mission OK")
 
+    def test_gripper_mission(self):
+        self.context_push()
+        ex = None
+        try:
+            try:
+                self.get_parameter("GRIP_ENABLE", timeout=5)
+            except NotAchievedException as e:
+                self.progress("Skipping; Gripper not enabled in config?")
+                return
+
+            self.mavproxy.send('wp load %s\n' %
+                               os.path.join(testdir,
+                                            "sub-gripper-mission.txt"))
+            self.mavproxy.send('mode loiter\n')
+            self.wait_ready_to_arm()
+            self.arm_vehicle()
+            self.mavproxy.send('mode auto\n')
+            self.wait_mode('AUTO')
+            self.mavproxy.expect("Gripper Grabbed")
+            self.mavproxy.expect("Gripper Released")
+        except Exception as e:
+            self.progress("Exception caught")
+            ex = e
+        self.context_pop()
+        if ex is not None:
+            raise ex
+
     def autotest(self):
         """Autotest ArduSub in SITL."""
         self.check_test_syntax(test_file=os.path.realpath(__file__))
@@ -163,6 +191,9 @@ class AutoTestSub(AutoTest):
                           lambda: self.dive_mission(
                               os.path.join(testdir, "sub_mission.txt")))
 
+            self.run_test("Test gripper mission items",
+                          self.test_gripper_mission);
+
             self.run_test("Log download",
                           lambda: self.log_download(
                               self.buildlogs_path("ArduSub-log.bin")))

Tools/autotest/common.py

@@ -713,8 +713,8 @@ class AutoTest(ABC):
             self.progress("ACK received: %s" % str(m))
             if m.command == command:
                 if m.result != want_result:
-                    raise ValueError("Expected %s got %s" % (command,
-                                                             m.command))
+                    raise ValueError("Expected %s got %s" % (want_result,
+                                                             m.result))
                 break
 
     #################################################

Tools/autotest/copter-gripper-mission.txt

@@ -0,0 +1,6 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.080017	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	5.000000	1
+2	0	0	211	0.000000	1.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+3	0	0	211	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+4	0	3	21	0.000000	0.000000	0.000000	1.000000	0.000000	0.000000	100.000000	1

Tools/autotest/plane-gripper-mission.txt

@@ -0,0 +1,10 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.362045	149.165878	584.280029	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.362125	149.165039	10.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364845	149.163452	11.000000	1
+3	0	0	211	0.000000	1.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+4	0	0	211	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365002	149.165665	10.460000	1
+6	0	3	189	0.000000	0.000000	0.000000	0.000000	-35.364429	149.165558	9.120000	1
+7	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.362869	149.165161	5.000000	1
+8	0	3	21	0.000000	0.000000	0.000000	1.000000	0.000000	0.000000	0.000000	1

Tools/autotest/rover-gripper-mission.txt

@@ -0,0 +1,6 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	-0.110000	1
+1	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.362698	149.165146	0.000000	1
+2	0	0	211	0.000000	1.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.363304	149.164978	0.000000	1
+4	0	0	211	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1

Tools/autotest/sub-gripper-mission.txt

@@ -0,0 +1,8 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	33.810313	-118.393867	0	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	-5.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	33.810612	-118.393867	0.000000	1
+3	0	0	211	0.000000	1.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	33.810612	-118.393578	0.000000	1
+5	0	0	211	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	33.810612	-118.393588	0.000000	1