From 26d65407fecdb8b4ee9d2aaac75ab08b6cf2bd78 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Mon, 19 Dec 2022 18:34:21 +0300
Subject: [PATCH] Tests for set_altitude

---
 clover/test/offboard.py   | 49 +++++++++++++++++++++++++++++++++++++++
 clover/test/offboard.test |  2 ++
 2 files changed, 51 insertions(+)

diff --git a/clover/test/offboard.py b/clover/test/offboard.py
index a4b9c3c5..d7b1bbc4 100755
--- a/clover/test/offboard.py
+++ b/clover/test/offboard.py
@@ -30,6 +30,8 @@ def get_navigate_target(tf_buffer):
 
 def test_offboard(node, tf_buffer):
     navigate = rospy.ServiceProxy('navigate', srv.Navigate)
+    set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
+    set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude)
     set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
     set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
     set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
@@ -184,6 +186,48 @@ def test_offboard(node, tf_buffer):
     assert state.z_frame_id == 'map'
     assert state.yaw_frame_id == 'map'
 
+    # test set_altitude
+    res = set_altitude(z=7, frame_id='test')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 7
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+    assert state.yaw_frame_id == 'map'
+
+    # test set_position
+    res = set_position(x=nan, y=nan, z=13, yaw=nan, yaw_rate=nan, frame_id='test2')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_POSITION
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 13
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test2'
+    assert state.yaw_frame_id == 'map'
+
+    # set_altitude should not change the mode
+    res = set_altitude(z=3, frame_id='test')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_POSITION
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 3
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+    assert state.yaw_frame_id == 'map'
+
     # test set_velocity
     res = set_velocity(vx=1, frame_id='body')
     state = get_state()
@@ -197,6 +241,11 @@ def test_offboard(node, tf_buffer):
     assert state.z_frame_id == 'map'
     assert state.yaw_frame_id == 'map'
 
+    # set_altitude should not work in velocity mode
+    res = set_altitude(z=3, frame_id='test')
+    assert res.success == False
+    assert res.message.startswith('Altitude cannot be set in')
+
     # test set_attitude
     res = set_attitude(roll=0.1, pitch=0.2, yaw=0.3, thrust=0.5)
     assert res.success == True
diff --git a/clover/test/offboard.test b/clover/test/offboard.test
index dcf13cc7..470fe4fe 100644
--- a/clover/test/offboard.test
+++ b/clover/test/offboard.test
@@ -3,6 +3,8 @@
 
     <node pkg="tf2_ros" type="static_transform_publisher" name="test_frame" args="10 20 30 0 0 0 map test"/>
 
+    <node pkg="tf2_ros" type="static_transform_publisher" name="test2_frame" args="100 200 300 0 0 0 map test2"/>
+
     <param name="test_module" value="$(find clover)/test/offboard.py"/>
     <test test-name="offboard_test" pkg="ros_pytest" type="ros_pytest_runner"/>
 </launch>