gcs_bridge.cpp
Go to the documentation of this file.
00001 
00006 /*
00007  * Copyright 2014 Vladimir Ermakov.
00008  *
00009  * This file is part of the mavros package and subject to the license terms
00010  * in the top-level LICENSE file of the mavros repository.
00011  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00012  */
00013 
00014 #include <ros/ros.h>
00015 #include <ros/console.h>
00016 
00017 #include <mavros/utils.h>
00018 #include <mavros/mavlink_diag.h>
00019 #include <mavconn/interface.h>
00020 
00021 using namespace mavros;
00022 using namespace mavconn;
00023 
00024 ros::Publisher mavlink_pub;
00025 ros::Subscriber mavlink_sub;
00026 MAVConnInterface::Ptr gcs_link;
00027 
00028 
00029 void mavlink_pub_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
00030         auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();
00031 
00032         rmsg->header.stamp = ros::Time::now();
00033         mavros_msgs::mavlink::convert(*mmsg, *rmsg);
00034         mavlink_pub.publish(rmsg);
00035 };
00036 
00037 void mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg) {
00038         mavlink_message_t mmsg;
00039 
00040         if (mavros_msgs::mavlink::convert(*rmsg, mmsg))
00041                 gcs_link->send_message(&mmsg, rmsg->sysid, rmsg->compid);
00042         else
00043                 ROS_ERROR("Packet drop: illegal payload64 size");
00044 };
00045 
00046 int main(int argc, char *argv[])
00047 {
00048         ros::init(argc, argv, "gcs_bridge");
00049         ros::NodeHandle priv_nh("~");
00050         ros::NodeHandle mavlink_nh("mavlink");
00051         diagnostic_updater::Updater updater;
00052         mavros::MavlinkDiag gcs_link_diag("GCS bridge");
00053 
00054         std::string gcs_url;
00055         priv_nh.param<std::string>("gcs_url", gcs_url, "udp://@");
00056 
00057         try {
00058                 gcs_link = MAVConnInterface::open_url(gcs_url);
00059                 gcs_link_diag.set_mavconn(gcs_link);
00060                 gcs_link_diag.set_connection_status(true);
00061         }
00062         catch (mavconn::DeviceError &ex) {
00063                 ROS_FATAL("GCS: %s", ex.what());
00064                 return 1;
00065         }
00066 
00067         mavlink_pub = mavlink_nh.advertise<mavros_msgs::Mavlink>("to", 10);
00068         gcs_link->message_received.connect(mavlink_pub_cb);
00069 
00070         // prefer UDPROS, but allow TCPROS too
00071         mavlink_sub = mavlink_nh.subscribe("from", 10, mavlink_sub_cb,
00072                 ros::TransportHints()
00073                         .unreliable().maxDatagramSize(1024)
00074                         .reliable());
00075 
00076         // setup updater
00077         updater.setHardwareID(gcs_url);
00078         updater.add(gcs_link_diag);
00079 
00080         // updater spinner
00081         auto diag_timer = priv_nh.createTimer(ros::Duration(0.5),
00082                         [&](const ros::TimerEvent &evt) {
00083                                 updater.update();
00084                         });
00085         diag_timer.start();
00086 
00087         ros::spin();
00088         return 0;
00089 }
00090 


mavros
Author(s): Vladimir Ermakov
autogenerated on Thu Feb 9 2017 04:00:17