Go to the documentation of this file.00001
00006
00007
00008
00009
00010
00011
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
00071 mavlink_sub = mavlink_nh.subscribe("from", 10, mavlink_sub_cb,
00072 ros::TransportHints()
00073 .unreliable().maxDatagramSize(1024)
00074 .reliable());
00075
00076
00077 updater.setHardwareID(gcs_url);
00078 updater.add(gcs_link_diag);
00079
00080
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