From 959a35b3bdbb89e5210ac914d5d47d191afa9649 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Fri, 2 Aug 2019 13:10:19 +0200
Subject: [PATCH] add safe_landing_planner to CI

---
 .ci/Jenkinsfile-SITL_tests                    |   7 ++
 Makefile                                      |   1 +
 .../mavros/missions/MC_safe_landing.plan      | 107 ++++++++++++++++++
 test/mavros_posix_test_safe_landing.test      |  51 +++++++++
 test/rostest_avoidance_run.sh                 |   2 +-
 5 files changed, 167 insertions(+), 1 deletion(-)
 create mode 100644 integrationtests/python_src/px4_it/mavros/missions/MC_safe_landing.plan
 create mode 100644 test/mavros_posix_test_safe_landing.test

diff --git a/.ci/Jenkinsfile-SITL_tests b/.ci/Jenkinsfile-SITL_tests
index e8e79a82cc..8922bd1bb3 100644
--- a/.ci/Jenkinsfile-SITL_tests
+++ b/.ci/Jenkinsfile-SITL_tests
@@ -93,6 +93,13 @@ pipeline {
               vehicle: "iris_obs_avoid",
               run_script: "rostest_avoidance_run.sh"
             ],
+            [
+              name: "MC_safe_landing",
+              test: "mavros_posix_test_safe_landing.test",
+              mission: "MC_safe_landing",
+              vehicle: "iris_obs_avoid",
+              run_script: "rostest_avoidance_run.sh"
+            ],
 
           ]
 
diff --git a/Makefile b/Makefile
index a708a1a977..c72b8f817f 100644
--- a/Makefile
+++ b/Makefile
@@ -380,6 +380,7 @@ tests_offboard: rostest
 
 tests_avoidance: rostest
 	@"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_avoidance.test
+	@"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_safe_landing.test
 
 python_coverage:
 	@mkdir -p "$(SRC_DIR)"/build/python_coverage
diff --git a/integrationtests/python_src/px4_it/mavros/missions/MC_safe_landing.plan b/integrationtests/python_src/px4_it/mavros/missions/MC_safe_landing.plan
new file mode 100644
index 0000000000..f2593118ba
--- /dev/null
+++ b/integrationtests/python_src/px4_it/mavros/missions/MC_safe_landing.plan
@@ -0,0 +1,107 @@
+{
+    "fileType": "Plan",
+    "geoFence": {
+        "circles": [
+        ],
+        "polygons": [
+        ],
+        "version": 2
+    },
+    "groundStation": "QGroundControl",
+    "mission": {
+        "cruiseSpeed": 15,
+        "firmwareType": 12,
+        "hoverSpeed": 5,
+        "items": [
+            {
+                "AMSLAltAboveTerrain": null,
+                "Altitude": 18,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 22,
+                "doJumpId": 1,
+                "frame": 3,
+                "params": [
+                    15,
+                    0,
+                    0,
+                    null,
+                    47.3977394,
+                    8.5455942,
+                    18
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": null,
+                "Altitude": 18,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 2,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.3977394,
+                    8.5456982,
+                    18
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": null,
+                "Altitude": 18,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 3,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.3977432,
+                    8.545805,
+                    18
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": null,
+                "Altitude": 0,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 21,
+                "doJumpId": 4,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.3977432,
+                    8.545804,
+                    0
+                ],
+                "type": "SimpleItem"
+            }
+        ],
+        "plannedHomePosition": [
+            47.3977419,
+            8.5455941,
+            489
+        ],
+        "vehicleType": 2,
+        "version": 2
+    },
+    "rallyPoints": {
+        "points": [
+        ],
+        "version": 2
+    },
+    "version": 1
+}
diff --git a/test/mavros_posix_test_safe_landing.test b/test/mavros_posix_test_safe_landing.test
new file mode 100644
index 0000000000..4f764813b8
--- /dev/null
+++ b/test/mavros_posix_test_safe_landing.test
@@ -0,0 +1,51 @@
+<?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="MC_safe_landing"/>
+    <arg name="sdf" default="$(find avoidance)/sim/models/iris_downwards_camera/iris_downwards_camera.sdf"/>
+    <arg name="vehicle" default="iris_obs_avoid"/>
+    <arg name="world" default="$(find avoidance)/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 mavros)/launch/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_depth_camera"
+          args="0 0 0 1.57 3.14 0 fcu 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/depth/points"/>
+    <node name="safe_landing_planner_node" pkg="safe_landing_planner" type="safe_landing_planner_node" output="screen">
+    <param name="pointcloud_topics" value="$(arg pointcloud_topics)" />
+  </node>
+
+  <node name="waypoint_generator_node" pkg="safe_landing_planner" type="waypoint_generator_node" output="screen" >
+  </node>
+
+  <node name="dynparam_slpn" pkg="dynamic_reconfigure" type="dynparam" args="load safe_landing_planner_node $(find safe_landing_planner)/cfg/slpn.yaml" />
+  <node name="dynparam_wpgn" pkg="dynamic_reconfigure" type="dynparam" args="load waypoint_generator_node $(find safe_landing_planner)/cfg/wpgn.yaml" />
+    <!-- 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
index 9ab1ae29fe..2abcb62fad 100755
--- a/test/rostest_avoidance_run.sh
+++ b/test/rostest_avoidance_run.sh
@@ -9,7 +9,7 @@ cd ${PX4_SRC_DIR}/catkin_ws/
 git clone -b 0.3.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
+catkin build local_planner safe_landing_planner --cmake-args -DCMAKE_BUILD_TYPE=Release
 
 source ${PX4_SRC_DIR}/catkin_ws/devel/setup.bash
 source /usr/share/gazebo/setup.sh