TERRAIN Protocol Integration

mavros/launch/apm.launch

@@ -10,6 +10,9 @@
 	<arg name="fcu_protocol" default="v2.0" />
 	<arg name="respawn_mavros" default="false" />
 	<arg name="namespace" default="mavros"/>
+	<arg name="terrain_data_path" default="" />
+	<arg name="terrain_auto_download" default="false" />
+
 	<include file="$(find-pkg-share mavros)/launch/node.launch">
 		<arg name="pluginlists_yaml" value="$(find-pkg-share mavros)/launch/apm_pluginlists.yaml" />
 		<arg name="config_yaml" value="$(find-pkg-share mavros)/launch/apm_config.yaml" />
@@ -21,4 +24,13 @@
 		<arg name="respawn_mavros" value="$(var respawn_mavros)" />
 		<arg name="namespace" value="$(var namespace)"/>
 	</include>
+
+	<node pkg="mavros_extras" exec="terrain_server_node" output="$(var log_output)">
+		<param name="terrain_data_path" value="$(var terrain_data_path)" />
+		<param name="auto_download" value="$(var terrain_auto_download)" />
+		<param name="download_host" value="terrain.ardupilot.org" />
+		<param name="srtm_source" value="SRTM3" />
+		<param name="send_rate_hz" value="5.0" />
+		<param name="max_cache_tiles" value="64" />
+	</node>
 </launch>

mavros/mavros_plugins.xml

@@ -140,4 +140,4 @@ Required by all plugins.</description>
 @plugin wind_estimation</description>
   </class>
 </library>
-<!-- [[[end]]] (sum: Nnd9SLlM2Y) -->
+<!-- [[[end]]] (sum: EQW037XnHj) -->

mavros_extras/CMakeLists.txt

@@ -18,6 +18,7 @@ set(CMAKE_C_EXTENSIONS ON)
 set(CMAKE_CXX_EXTENSIONS ON)
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 
 # find mavros dependencies
 find_package(rclcpp REQUIRED)
@@ -141,7 +142,7 @@ add_library(mavros_extras_plugins SHARED
   src/plugins/vision_pose_estimate.cpp
   src/plugins/vision_speed_estimate.cpp
   src/plugins/wheel_odometry.cpp
-  # [[[end]]] (sum: E3zv4+Yl1z)
+  # [[[end]]] (sum: WzAyzLqsp3)
 )
 target_link_libraries(mavros_extras_plugins PUBLIC
   ${geographic_msgs_TARGETS}
@@ -216,9 +217,19 @@ install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME}
 )
 
+# Python terrain server library + entry-point
+ament_python_install_package(${PROJECT_NAME}
+  SETUP_CFG setup.cfg
+)
+install(PROGRAMS
+  scripts/terrain_tile_server
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_cmake_gmock REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
 
   find_package(ament_lint_auto REQUIRED)
 
@@ -231,6 +242,8 @@ if(BUILD_TESTING)
   endif()
 
   ament_lint_auto_find_test_dependencies()
+
+  ament_add_pytest_test(test_srtm test/test_srtm.py)
 endif()
 
 #ament_export_dependencies(console_bridge)