simple_offboard: add terrain_frame_mode parameter, CopterExpress/clover_vm#14

`altitude` mode takes the current altitude from the estimator
`range` mode takes the current altitude from a simple range topic

clover/src/simple_offboard.cpp

@@ -30,6 +30,7 @@
 #include <geometry_msgs/QuaternionStamped.h>
 #include <sensor_msgs/NavSatFix.h>
 #include <sensor_msgs/BatteryState.h>
+#include <sensor_msgs/Range.h>
 #include <mavros_msgs/CommandBool.h>
 #include <mavros_msgs/SetMode.h>
 #include <mavros_msgs/PositionTarget.h>
@@ -86,6 +87,7 @@ float default_speed;
 bool auto_release;
 bool land_only_in_offboard, nav_from_sp, check_kill_switch;
 std::map<string, string> reference_frames;
+string terrain_frame_mode;
 
 // Publishers
 ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub, state_pub;
@@ -205,18 +207,27 @@ inline bool waitTransform(const string& target, const string& source,
 	return false;
 }
 
+void publishTerrain(const double distance, const ros::Time& stamp)
+{
+	if (!waitTransform(local_frame, body.child_frame_id, stamp, ros::Duration(0.1))) return;
+
+	auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, stamp);
+	t.child_frame_id = terrain.child_frame_id;
+	t.transform.translation.z -= distance;
+	static_transform_broadcaster->sendTransform(t);
+}
+
 void handleAltitude(const Altitude& alt)
 {
-	// publish terrain frame
 	if (!std::isfinite(alt.bottom_clearance)) return;
-	// terrain.header.stamp = alt.header.stamp;
+	publishTerrain(alt.bottom_clearance, alt.header.stamp);
+}
 
-	if (!waitTransform(local_frame, body.child_frame_id, alt.header.stamp, ros::Duration(0.1))) return;
-
-	auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, alt.header.stamp);
-	t.child_frame_id = terrain.child_frame_id;
-	t.transform.translation.z -= alt.bottom_clearance;
-	static_transform_broadcaster->sendTransform(t);
+void handleRange(const Range& range)
+{
+	if (!std::isfinite(range.range)) return;
+	// TODO: check it's facing down
+	publishTerrain(range.range, range.header.stamp);
 }
 
 #define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout))
@@ -1101,6 +1112,7 @@ int main(int argc, char **argv)
 	nh_priv.param("default_speed", default_speed, 0.5f);
 	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
 	nh_priv.param<string>("terrain_frame", terrain.child_frame_id, "terrain");
+	nh_priv.param<string>("terrain_frame_mode", terrain_frame_mode, "altitude");
 	nh_priv.getParam("reference_frames", reference_frames);
 
 	// Default reference frames
@@ -1139,7 +1151,15 @@ int main(int argc, char **argv)
 	ros::Subscriber altitude_sub;
 	if (!body.child_frame_id.empty() && !terrain.child_frame_id.empty()) {
 		terrain.header.frame_id = local_frame;
-		altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude);
+		if (terrain_frame_mode == "altitude") {
+			altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude);
+		} else if (terrain_frame_mode == "range") {
+			string range_topic = nh_priv.param("range_topic", string("rangefinder/range"));
+			altitude_sub = nh.subscribe(range_topic, 1, &handleRange);
+		} else {
+			ROS_FATAL("Unknown terrain_frame_mode: %s, valid values: altitude, range", terrain_frame_mode.c_str());
+			ros::shutdown();
+		}
 	}
 
 	// Setpoint publishers

clover/test/offboard.py

@@ -3,6 +3,7 @@ import pytest
 from pytest import approx
 import threading
 import mavros_msgs.msg
+from mavros_msgs.srv import SetMode
 from geometry_msgs.msg import PoseStamped
 from clover import srv
 from clover.msg import State
@@ -45,6 +46,7 @@ def test_offboard(node, tf_buffer):
     telem = get_telemetry()
     assert telem.connected == False
 
+    # mocked state publisher
     state_pub = rospy.Publisher('/mavros/state', mavros_msgs.msg.State, latch=True, queue_size=1)
     state_msg = mavros_msgs.msg.State(mode='OFFBOARD', armed=True)
 
@@ -59,6 +61,13 @@ def test_offboard(node, tf_buffer):
     threading.Thread(target=publish_state, daemon=True).start()
     rospy.sleep(0.5)
 
+    # set_mode service mock
+    def set_mode(req):
+        state_msg.mode = req.custom_mode # set mocked mode to requested
+        return True,
+
+    rospy.Service('/mavros/set_mode', SetMode, set_mode)
+
     telem = get_telemetry()
     assert telem.connected == False
 
@@ -157,7 +166,23 @@ def test_offboard(node, tf_buffer):
     assert state.z_frame_id == 'map'
     assert state.yaw_frame_id == 'test'
 
-    # auto_arm should invalidate the setpoint
+    # auto_arm should not invalidate the setpoint if not effective
+    res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 1
+    assert state.y == 2
+    assert state.z == 1
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'test'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+
+    # auto_arm should invalidate the setpoint if effective
+    state_msg.mode = 'STABILIZED' # pretend we are not in OFFBOARD mode
+    rospy.sleep(1)
     res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True)
     assert res.success == True
     state = get_state()
@@ -170,6 +195,8 @@ def test_offboard(node, tf_buffer):
     assert state.xy_frame_id == 'map'
     assert state.z_frame_id == 'map'
     assert state.yaw_frame_id == 'map'
+    state_msg.mode = 'OFFBOARD'
+    rospy.sleep(1)
 
     # set_attitude should invalidate the setpoint
     res = set_attitude()