GCS Bridge Plugin

mavros/include/mavros/mavros_uas.h

@@ -40,6 +40,12 @@ namespace mavros {
  */
 #define UAS_FCU(uasobjptr)				\
 	((uasobjptr)->fcu_link)
+
+/**
+ * @brief helper accessor to GCS link interface
+ */
+#define UAS_GCS(uasobjptr)				\
+	((uasobjptr)->gcs_link)
 
 /**
  * @brief helper accessor to diagnostic updater
@@ -83,6 +89,11 @@ class UAS {
 	 * @brief MAVLink FCU device conection
 	 */
 	mavconn::MAVConnInterface::Ptr fcu_link;
+
+    /**
+	 * @brief MAVLink gcs device conection
+	 */
+	mavconn::MAVConnInterface::Ptr gcs_link;
 
 	/**
 	 * @brief Mavros diagnostic updater

mavros/src/lib/mavros.cpp

@@ -96,23 +96,24 @@ MavRos::MavRos() :
 		fcu_link->set_protocol_version(mavconn::Protocol::V10);
 	}
 
-	if (gcs_url != "") {
-		ROS_INFO_STREAM("GCS URL: " << gcs_url);
-		try {
-			gcs_link = MAVConnInterface::open_url(gcs_url, system_id, component_id);
-
-			gcs_link_diag.set_mavconn(gcs_link);
-			gcs_diag_updater.setHardwareID(gcs_url);
-			gcs_diag_updater.add(gcs_link_diag);
-		}
-		catch (mavconn::DeviceError &ex) {
-			ROS_FATAL("GCS: %s", ex.what());
-			ros::shutdown();
-			return;
-		}
-	}
-	else
-		ROS_INFO("GCS bridge disabled");
+    if (gcs_url != "") {
+        ROS_INFO_STREAM("GCS URL: " << gcs_url);
+        try {
+            gcs_link = MAVConnInterface::open_url(gcs_url, system_id, component_id);
+
+            gcs_link_diag.set_mavconn(gcs_link);
+            gcs_diag_updater.setHardwareID(gcs_url);
+            gcs_diag_updater.add(gcs_link_diag);
+            UAS_GCS(&mav_uas) = gcs_link;
+        }
+        catch (mavconn::DeviceError &ex) {
+            ROS_FATAL("GCS: %s", ex.what());
+            ros::shutdown();
+            return;
+        }
+    }
+    else
+        ROS_INFO("GCS bridge disabled");
 
 	// ROS mavlink bridge
 	mavlink_pub = mavlink_nh.advertise<mavros_msgs::Mavlink>("from", 100);

mavros_extras/CMakeLists.txt

@@ -80,6 +80,7 @@ add_library(mavros_extras
   src/plugins/vibration.cpp
   src/plugins/vision_pose_estimate.cpp
   src/plugins/vision_speed_estimate.cpp
+  src/plugins/gcs_bridge_plugin.cpp
 )
 target_link_libraries(mavros_extras
   ${mavros_LIBRARIES}

mavros_extras/mavros_plugins.xml

@@ -47,4 +47,7 @@
 	<class name="vision_speed_estimate" type="mavros::extra_plugins::VisionSpeedEstimatePlugin" base_class_type="mavros::plugin::PluginBase">
 		<description>Send vision speed estimate to FCU.</description>
 	</class>
+    <class name="gcs_bridge_plugin" type="mavros::extra_plugins::GCSBridgePlugin" base_class_type="mavros::plugin::PluginBase" output="screen">
+        <description> Establishes a connection between FCU, companion computer, and GCS. </description>
+    </class>
 </library>

mavros_extras/src/plugins/gcs_bridge_plugin.cpp

@@ -0,0 +1,76 @@
+/**
+ * @brief GCSBridge plugin
+ * @file gcs_bridge_plugin.cpp
+ * @author Amy Wagoner <arwagoner@gmail.com>
+ *
+ * @addtogroup plugin
+ * @{
+ */
+/*
+ * Copyright 2018 Amy Wagoner.
+ *
+ * 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>
+
+
+namespace mavros {
+namespace extra_plugins{
+/**
+ * @brief GCS Bridge plugin
+ *
+ *
+ */
+class GCSBridgePlugin : public plugin::PluginBase {
+public:
+    GCSBridgePlugin() : PluginBase(),
+    mavlink_nh("mavlink")
+	{ }
+
+    void initialize(UAS &uas_)
+    {
+        PluginBase::initialize(uas_);
+
+        mavlink_sub = mavlink_nh.subscribe("from", 10, &GCSBridgePlugin::mavlink_sub_cb,this);
+        mavlink_pub = mavlink_nh.advertise<mavros_msgs::Mavlink>("to", 10);
+    }
+
+    Subscriptions get_subscriptions()
+    {
+        return { /* Rx disabled */ };
+    }
+
+private:
+    ros::NodeHandle mavlink_nh;
+
+    ros::Subscriber mavlink_sub;
+    ros::Publisher mavlink_pub;
+
+    void mavlink_pub_cb(const mavlink::mavlink_message_t *mmsg, const mavconn::Framing framing)
+    {
+        auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();
+
+        rmsg->header.stamp = ros::Time::now();
+        mavros_msgs::mavlink::convert(*mmsg, *rmsg, mavros::utils::enum_value(framing));
+        mavlink_pub.publish(rmsg);
+    }
+
+    void mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg)
+    {
+        mavlink::mavlink_message_t mmsg;
+        if (mavros_msgs::mavlink::convert(*rmsg, mmsg))
+        {
+            UAS_GCS(m_uas)->send_message_ignore_drop(&mmsg);
+        }
+        else
+            ROS_ERROR("Packet drop: convert error.");
+    }
+};
+}	// namespace extra_plugins
+}	// namespace mavros
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::GCSBridgePlugin, mavros::plugin::PluginBase)