Add better option handling to integration testing script.

integrationtests/run_tests.bash

@@ -8,13 +8,32 @@ set -e
 # TODO move to docker image
 pip install px4tools pymavlink -q
 
-# handle cleaning command
+# A POSIX variable
+OPTIND=1         # Reset in case getopts has been used previously in the shell.
+
+# Initialize our own variables:
 do_clean=true
-if [ "$1" = "-o" ]
-then
-	echo not cleaning
-	do_clean=false
-fi
+gui=false
+
+while getopts "h?og" opt; do
+    case "$opt" in
+    h|\?)
+		echo """
+		$0 [-h] [-o] [-g]
+		-h show help
+		-o don't clean before building (to save time)
+		-g run gazebo gui
+		"""
+        exit 0
+        ;;
+    o)  do_clean=false
+		echo "not cleaning"
+        ;;
+    g)  gui=true
+        ;;
+    esac
+done
+
 
 # determine the directory of the source given the directory of this script
 pushd `dirname $0` > /dev/null
@@ -89,8 +108,13 @@ echo -e "TEST_RESULT_TARGET_DIR\t: $TEST_RESULT_TARGET_DIR"
 # however, stop executing tests after the first failure
 set +e
 echo "=====> run tests"
-test $? -eq 0 && rostest px4 mavros_posix_tests_iris.launch
-test $? -eq 0 && rostest px4 mavros_posix_tests_standard_vtol.launch
+test $? -eq 0 && rostest px4 mavros_posix_tests_iris.launch gui:=$gui
+
+# commented out optical flow test for now since ci server has
+# an issue producing the simulated flow camera currently
+#test $? -eq 0 && rostest px4 mavros_posix_tests_iris_opt_flow.launch gui:=$gui
+
+test $? -eq 0 && rostest px4 mavros_posix_tests_standard_vtol.launch gui:=$gui
 TEST_RESULT=$?
 echo "<====="
 

launch/mavros_posix_tests_iris_opt_flow.launch

@@ -0,0 +1,23 @@
+<launch>
+    <!-- Posix SITL MAVROS integration tests -->
+
+    <arg name="ns" default="/"/>
+    <arg name="headless" default="true"/>
+    <arg name="gui" default="false"/>
+    <arg name="est" default="lpe"/>
+
+    <include file="$(find px4)/launch/mavros_posix_sitl.launch">
+        <arg name="ns" value="$(arg ns)"/>
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="vehicle" value="iris_opt_flow"/>
+        <arg name="est" value="$(arg est)"/>
+    </include>
+
+    <group ns="$(arg ns)">
+        <test test-name="mavros_flow_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="120.0" />
+        <test test-name="mavros_flow_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" />
+    </group>
+</launch>
+
+<!-- vim: set ft=xml et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->

src/modules/mavlink/mavlink_messages.cpp

@@ -3803,6 +3803,100 @@ class MavlinkStreamHighLatency : public MavlinkStream
 	}
 };
 
+
+class MavlinkStreamGroundTruth : public MavlinkStream
+{
+public:
+	const char *get_name() const
+	{
+		return MavlinkStreamGroundTruth::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "GROUND_TRUTH";
+	}
+
+	static uint16_t get_id_static()
+	{
+		return MAVLINK_MSG_ID_HIL_STATE_QUATERNION;
+	}
+
+	uint16_t get_id()
+	{
+		return get_id_static();
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkStreamGroundTruth(mavlink);
+	}
+
+	unsigned get_size()
+	{
+		return (_att_time > 0 || _gpos_time > 0) ? MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN +
+			MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+	}
+
+private:
+	MavlinkOrbSubscription *_att_sub;
+	MavlinkOrbSubscription *_gpos_sub;
+	uint64_t _att_time;
+	uint64_t _gpos_time;
+	struct vehicle_attitude_s _att;
+	struct vehicle_global_position_s _gpos;
+
+	/* do not allow top copying this class */
+	MavlinkStreamGroundTruth(MavlinkStreamGroundTruth &);
+	MavlinkStreamGroundTruth &operator = (const MavlinkStreamGroundTruth &);
+
+protected:
+	explicit MavlinkStreamGroundTruth(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_groundtruth))),
+		_gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position_groundtruth))),
+		_att_time(0),
+		_gpos_time(0),
+		_att(),
+		_gpos()
+	{}
+
+	void send(const hrt_abstime t)
+	{
+		bool att_updated = _att_sub->update(&_att_time, &_att);
+		bool gpos_updated = _gpos_sub->update(&_gpos_time, &_gpos);
+
+		if (att_updated || gpos_updated) {
+
+			mavlink_hil_state_quaternion_t msg = {};
+
+			if (att_updated) {
+				msg.attitude_quaternion[0] = _att.q[0];
+				msg.attitude_quaternion[1] = _att.q[1];
+				msg.attitude_quaternion[2] = _att.q[2];
+				msg.attitude_quaternion[3] = _att.q[3];
+				msg.rollspeed = _att.rollspeed;
+				msg.pitchspeed = _att.pitchspeed;
+				msg.yawspeed = _att.yawspeed;
+			}
+
+			if (gpos_updated) {
+				msg.lat = _gpos.lat;
+				msg.lon = _gpos.lon;
+				msg.alt = _gpos.alt;
+				msg.vx = _gpos.vel_n;
+				msg.vy = _gpos.vel_e;
+				msg.vz = _gpos.vel_d;
+				msg.ind_airspeed = 0;
+				msg.true_airspeed = 0;
+				msg.xacc = 0;
+				msg.yacc = 0;
+				msg.zacc = 0;
+			}
+			mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg);
+		}
+	}
+};
+
 const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static),
 	new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static),
@@ -3850,5 +3944,6 @@ const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static),
 	new StreamListItem(&MavlinkStreamMountOrientation::new_instance, &MavlinkStreamMountOrientation::get_name_static, &MavlinkStreamMountOrientation::get_id_static),
 	new StreamListItem(&MavlinkStreamHighLatency::new_instance, &MavlinkStreamHighLatency::get_name_static, &MavlinkStreamWind::get_id_static),
+	new StreamListItem(&MavlinkStreamGroundTruth::new_instance, &MavlinkStreamGroundTruth::get_name_static, &MavlinkStreamGroundTruth::get_id_static),
 	nullptr
 };