From 09b795161e78e4843ed12d971640848a08ca0366 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Fri, 15 Mar 2019 05:37:23 +0100
Subject: [PATCH] Obstacle Avoidance testing in CI (#10780)

---
 .ci/Jenkinsfile-SITL_tests                    | 10 ++-
 Makefile                                      |  6 +-
 .../init.d-posix/1015_iris_obs_avoid          | 13 +++
 .../px4_it/mavros/missions/avoidance.plan     | 88 +++++++++++++++++++
 test/mavros_posix_test_avoidance.test         | 52 +++++++++++
 test/rostest_avoidance_run.sh                 | 20 +++++
 6 files changed, 187 insertions(+), 2 deletions(-)
 create mode 100644 ROMFS/px4fmu_common/init.d-posix/1015_iris_obs_avoid
 create mode 100644 integrationtests/python_src/px4_it/mavros/missions/avoidance.plan
 create mode 100644 test/mavros_posix_test_avoidance.test
 create mode 100755 test/rostest_avoidance_run.sh

diff --git a/.ci/Jenkinsfile-SITL_tests b/.ci/Jenkinsfile-SITL_tests
index c20e9d8d78..5170ad9378 100644
--- a/.ci/Jenkinsfile-SITL_tests
+++ b/.ci/Jenkinsfile-SITL_tests
@@ -79,6 +79,13 @@ pipeline {
               mission: "VTOL_mission_1",
               vehicle: "tiltrotor"
             ],
+            [
+              name: "MC_avoidance",
+              test: "mavros_posix_test_avoidance.test",
+              mission: "avoidance",
+              vehicle: "iris_obs_avoid",
+              run_script: "rostest_avoidance_run.sh"
+            ],
 
           ]
 
@@ -111,6 +118,7 @@ def createTestNode(Map test_def) {
       cleanWs()
       docker.image("px4io/px4-dev-ros-kinetic:2019-03-08").inside('-e HOME=${WORKSPACE}') {
         stage(test_def.name) {
+          def run_script = test_def.get('run_script', 'rostest_px4_run.sh')
           def test_ok = true
           sh('export')
 
@@ -119,7 +127,7 @@ def createTestNode(Map test_def) {
 
           // run test
           try {
-            sh('px4-px4_sitl_default*/px4/test/rostest_px4_run.sh ' + test_def.test + ' mission:=' + test_def.mission + ' vehicle:=' + test_def.vehicle)
+            sh('px4-px4_sitl_default*/px4/test/' + run_script + ' ' + test_def.test + ' mission:=' + test_def.mission + ' vehicle:=' + test_def.vehicle)
 
           } catch (exc) {
             // save all test artifacts for debugging
diff --git a/Makefile b/Makefile
index b1eeb4fe12..a2357254c5 100644
--- a/Makefile
+++ b/Makefile
@@ -337,7 +337,7 @@ format:
 
 # Testing
 # --------------------------------------------------------------------
-.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard
+.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance
 .PHONY: rostest python_coverage test_mixer_multirotor
 
 test_mixer_multirotor:
@@ -374,6 +374,9 @@ tests_offboard: rostest
 	@"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_attctl.test
 	@"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test
 
+tests_avoidance:
+	@"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_avoidance.test
+
 python_coverage:
 	@mkdir -p "$(SRC_DIR)"/build/python_coverage
 	@cd "$(SRC_DIR)"/build/python_coverage && cmake "$(SRC_DIR)" $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=px4_sitl_default -DPYTHON_COVERAGE=ON
@@ -384,6 +387,7 @@ python_coverage:
 	@coverage combine `find . -name .coverage\*`
 	@coverage report -m
 
+
 # static analyzers (scan-build, clang-tidy, cppcheck)
 # --------------------------------------------------------------------
 .PHONY: scan-build px4_sitl_default-clang clang-tidy clang-tidy-fix clang-tidy-quiet
diff --git a/ROMFS/px4fmu_common/init.d-posix/1015_iris_obs_avoid b/ROMFS/px4fmu_common/init.d-posix/1015_iris_obs_avoid
new file mode 100644
index 0000000000..3a32eceb0d
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/1015_iris_obs_avoid
@@ -0,0 +1,13 @@
+#!/bin/sh
+#
+# @name 3DR Iris Quadrotor SITL (Obstacle Avoidance)
+#
+# @type Quadrotor Wide
+#
+
+sh /etc/init.d-posix/10016_iris
+
+if [ $AUTOCNF = yes ]
+then
+	param set COM_OBS_AVOID 1
+fi
diff --git a/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan b/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan
new file mode 100644
index 0000000000..88c9d0f4d5
--- /dev/null
+++ b/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan
@@ -0,0 +1,88 @@
+{
+    "fileType": "Plan",
+    "geoFence": {
+        "circles": [
+        ],
+        "polygons": [
+        ],
+        "version": 2
+    },
+    "groundStation": "QGroundControl",
+    "mission": {
+        "cruiseSpeed": 15,
+        "firmwareType": 12,
+        "hoverSpeed": 5,
+        "items": [
+            {
+                "AMSLAltAboveTerrain": null,
+                "Altitude": 4,
+                "AltitudeMode": 0,
+                "autoContinue": true,
+                "command": 22,
+                "doJumpId": 1,
+                "frame": 3,
+                "params": [
+                    15,
+                    0,
+                    0,
+                    null,
+                    47.3977432,
+                    8.5456085,
+                    4
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": null,
+                "Altitude": 4,
+                "AltitudeMode": 0,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 2,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.3977432,
+                    8.5458765,
+                    4
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": null,
+                "Altitude": -1,
+                "AltitudeMode": 0,
+                "autoContinue": true,
+                "command": 21,
+                "doJumpId": 3,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.3977432,
+                    8.5458812,
+                    -1
+                ],
+                "type": "SimpleItem"
+            }
+        ],
+        "plannedHomePosition": [
+            47.3977419,
+            8.5458854,
+            487.923
+        ],
+        "vehicleType": 2,
+        "version": 2
+    },
+    "rallyPoints": {
+        "points": [
+        ],
+        "version": 2
+    },
+    "version": 1
+}
diff --git a/test/mavros_posix_test_avoidance.test b/test/mavros_posix_test_avoidance.test
new file mode 100644
index 0000000000..2410cbf394
--- /dev/null
+++ b/test/mavros_posix_test_avoidance.test
@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- Posix SITL MAVROS integration tests -->
+    <!-- Test a mission -->
+    <arg name="est" default="ekf2"/>
+    <arg name="gui" default="false"/>
+    <arg name="interactive" default="false"/>
+    <arg name="mission" default="avoidance"/>
+    <arg name="sdf" default="$(find local_planner)/../sim/models/iris_depth_camera_3/iris_depth_camera.sdf"/>
+    <arg name="vehicle" default="iris_obs_avoid"/>
+    <arg name="world" default="$(find local_planner)/../sim/worlds/boxes1.world"/>
+    <!-- PX4 SITL and Gazebo -->
+    <include file="$(find px4)/launch/posix_sitl.launch">
+        <arg name="est" value="$(arg est)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="interactive" value="$(arg interactive)"/>
+        <arg name="respawn_gazebo" value="true"/>
+        <arg name="sdf" value="$(arg sdf)"/>
+        <arg name="vehicle" value="$(arg vehicle)"/>
+        <arg name="verbose" value="true"/>
+        <arg name="world" value="$(arg world)"/>
+    </include>
+    <!-- MAVROS -->
+    <include file="$(find mavros)/launch/node.launch">
+        <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml"/>
+        <arg name="config_yaml" value="$(find local_planner)/resource/px4_config.yaml"/>
+        <arg name="gcs_url" value=""/>
+        <arg name="fcu_url" value="udp://:14540@localhost:14557"/>
+        <arg name="tgt_system" value="1"/>
+        <arg name="tgt_component" value="1"/>
+        <arg name="respawn_mavros" value="true"/>
+    </include>
+    <!-- Transformation Publishers -->
+    <node pkg="tf" type="static_transform_publisher" name="tf_front_camera"
+          args="0 0 0 -1.57 0 -1.57 fcu front_camera_link 10"/>
+    <node pkg="tf" type="static_transform_publisher" name="tf_right_camera"
+          args="0 0 0 -3.14 0 -1.57 fcu right_camera_link 10"/>
+    <node pkg="tf" type="static_transform_publisher" name="tf_left_camera"
+          args="0 0 0 0 0 -1.57 fcu left_camera_link 10"/>
+    <!-- Suppress console outputs -->
+    <env name="ROSCONSOLE_CONFIG_FILE" value="$(find local_planner)/resource/custom_rosconsole.conf"/>
+    <!-- Launch Local Planner -->
+    <arg name="pointcloud_topics" default="[/camera_front/depth/points,/camera_left/depth/points,/camera_right/depth/points]"/>
+    <node name="local_planner_node" pkg="local_planner" type="local_planner_node" output="screen">
+        <param name="goal_x_param" value="17" />
+        <param name="goal_y_param" value="15"/>
+        <param name="goal_z_param" value="3" />
+        <rosparam param="pointcloud_topics" subst_value="True">$(arg pointcloud_topics)</rosparam>
+    </node>
+    <!-- ROStest -->
+    <test test-name="$(arg mission)" pkg="px4" type="mission_test.py" time-limit="300.0" args="$(arg mission).plan"/>
+</launch>
diff --git a/test/rostest_avoidance_run.sh b/test/rostest_avoidance_run.sh
new file mode 100755
index 0000000000..688b3ba2f1
--- /dev/null
+++ b/test/rostest_avoidance_run.sh
@@ -0,0 +1,20 @@
+#! /bin/bash
+
+DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
+PX4_SRC_DIR=${DIR}/..
+
+source /opt/ros/${ROS_DISTRO:-kinetic}/setup.bash
+mkdir -p ${PX4_SRC_DIR}/catkin_ws/src
+cd ${PX4_SRC_DIR}/catkin_ws/
+git clone -b '0.1.0' --single-branch --depth 1 https://github.com/PX4/avoidance.git src/avoidance
+
+catkin init
+catkin build local_planner --cmake-args -DCMAKE_BUILD_TYPE=Release
+
+source ${PX4_SRC_DIR}/catkin_ws/devel/setup.bash
+source /usr/share/gazebo/setup.sh
+
+export CATKIN_SETUP_UTIL_ARGS=--extend
+export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:${PX4_SRC_DIR}/catkin_ws/src/avoidance/sim/models
+
+source $DIR/rostest_px4_run.sh "$@"