Add plugin for reporting terrain height estimate from the FCU

mavros_extras/CMakeLists.txt

@@ -88,6 +88,7 @@ add_library(mavros_extras
   src/plugins/play_tune.cpp
   src/plugins/px4flow.cpp
   src/plugins/rangefinder.cpp
+  src/plugins/terrain.cpp
   src/plugins/trajectory.cpp
   src/plugins/tunnel.cpp
   src/plugins/vibration.cpp

mavros_extras/mavros_plugins.xml

@@ -63,6 +63,9 @@
   <class name="rangefinder" type="mavros::extra_plugins::RangefinderPlugin" base_class_type="mavros::plugin::PluginBase">
     <description>Publish RANGEFINDER message data from FCU sensors in companion computer.</description>
   </class>
+  <class name="terrain" type="mavros::extra_plugins::TerrainPlugin" base_class_type="mavros::plugin::PluginBase">
+    <description>Publish TERRAIN_REPORT message data from FCU to companion computer.</description>
+  </class>
   <class name="trajectory" type="mavros::extra_plugins::TrajectoryPlugin" base_class_type="mavros::plugin::PluginBase">
     <description>Receive planned path from the FCU and send back corrected path (collision free, smoothed) to the FCU.</description>
   </class>

mavros_extras/src/plugins/terrain.cpp

@@ -0,0 +1,74 @@
+/**
+ * @brief Terrain plugin
+ * @file terrain.cpp
+ * @author Matt Anderson <anderson_rayner@hotmail.com>
+ *
+ * @addtogroup plugin
+ * @{
+ */
+/*
+ * Copyright 2021 Ardupilot.
+ *
+ * This file is part of the mavros package and subject to the license terms
+ * in the top-level LICENSE file of the mavros repository.
+ * https://github.com/mavlink/mavros/tree/master/LICENSE.md
+ */
+
+#include <mavros/mavros_plugin.h>
+#include <mavros_msgs/Terrain.h>
+
+namespace mavros {
+namespace extra_plugins {
+/**
+ * @brief Terrain height plugin.
+ *
+ * This plugin allows publishing of terrain height estimate from FCU to ROS.
+ *
+ */
+class TerrainPlugin : public plugin::PluginBase {
+public:
+	TerrainPlugin() : PluginBase(),
+		terrain_nh("~terrain")
+	{ }
+
+	void initialize(UAS &uas_) override
+	{
+		PluginBase::initialize(uas_);
+
+		terrain_pub = terrain_nh.advertise<mavros_msgs::Terrain>("in", 10);
+	}
+
+	Subscriptions get_subscriptions() override
+	{
+		return {
+			make_handler(&TerrainPlugin::handle_terrain)
+		};
+	}
+
+private:
+	ros::NodeHandle terrain_nh;
+
+	ros::Publisher terrain_pub;
+
+	void handle_terrain(const mavlink::mavlink_message_t *msg, mavlink::common::msg::TERRAIN_REPORT &terrain) {
+		auto terrain_msg = boost::make_shared<mavros_msgs::Terrain>();
+
+		terrain_msg->header.stamp = ros::Time::now();
+		terrain_msg->header.frame_id = "terrain";
+
+		terrain_msg->latitude = (double) terrain.lat / 1e7;
+		terrain_msg->longitude = (double) terrain.lon / 1e7;
+		terrain_msg->spacing = terrain.spacing;
+		terrain_msg->terrain_height = terrain.terrain_height;
+		terrain_msg->current_height = terrain.current_height;
+		terrain_msg->pending = terrain.pending;
+		terrain_msg->loaded = terrain.loaded;
+
+		terrain_pub.publish(terrain_msg);
+	}
+};
+}	// namespace extra_plugins
+}	// namespace mavros
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::TerrainPlugin, mavros::plugin::PluginBase)

mavros_msgs/CMakeLists.txt

@@ -59,6 +59,7 @@ add_message_files(
   RTKBaseline.msg
   State.msg
   StatusText.msg
+  Terrain.msg
   Thrust.msg
   TimesyncStatus.msg
   Trajectory.msg

mavros_msgs/msg/Terrain.msg

@@ -0,0 +1,12 @@
+# Message for TERRAIN_REPORT
+# https://mavlink.io/en/messages/common.html#TERRAIN_REPORT
+
+std_msgs/Header header
+
+float64 latitude
+float64 longitude
+uint16 spacing
+float32 terrain_height # in meters, terrain height
+float32 current_height # in meters, vehicle height above terrain
+uint16 pending
+uint16 loaded