gcs_bridge.cpp
Go to the documentation of this file.
1 
6 /*
7  * Copyright 2014,2016 Vladimir Ermakov.
8  *
9  * This file is part of the mavros package and subject to the license terms
10  * in the top-level LICENSE file of the mavros repository.
11  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
12  */
13 
14 #include <ros/ros.h>
15 #include <ros/console.h>
16 
17 #include <mavros/utils.h>
18 #include <mavros/mavlink_diag.h>
19 #include <mavconn/interface.h>
20 
21 using namespace mavros;
22 using namespace mavconn;
23 
27 
28 
29 void mavlink_pub_cb(const mavlink::mavlink_message_t *mmsg, const mavconn::Framing framing)
30 {
31  auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();
32 
33  rmsg->header.stamp = ros::Time::now();
35  mavlink_pub.publish(rmsg);
36 }
37 
39 {
40  mavlink::mavlink_message_t mmsg;
41 
42  if (mavros_msgs::mavlink::convert(*rmsg, mmsg))
43  gcs_link->send_message(&mmsg); // !!! queue exception -> fall of gcs_bridge. intentional.
44  else
45  ROS_ERROR("Packet drop: convert error.");
46 }
47 
48 int main(int argc, char *argv[])
49 {
50  ros::init(argc, argv, "gcs_bridge");
51  ros::NodeHandle priv_nh("~");
52  ros::NodeHandle mavlink_nh("mavlink");
54  mavros::MavlinkDiag gcs_link_diag("GCS bridge");
55 
56  std::string gcs_url;
57  priv_nh.param<std::string>("gcs_url", gcs_url, "udp://@");
58 
59  try {
61  gcs_link_diag.set_mavconn(gcs_link);
62  gcs_link_diag.set_connection_status(true);
63  }
64  catch (mavconn::DeviceError &ex) {
65  ROS_FATAL("GCS: %s", ex.what());
66  return 1;
67  }
68 
69  mavlink_pub = mavlink_nh.advertise<mavros_msgs::Mavlink>("to", 10);
70  gcs_link->message_received_cb = mavlink_pub_cb;
71 
72  // prefer UDPROS, but allow TCPROS too
73  mavlink_sub = mavlink_nh.subscribe("from", 10, mavlink_sub_cb,
75  .unreliable().maxDatagramSize(1024)
76  .reliable());
77 
78  // setup updater
79  updater.setHardwareID(gcs_url);
80  updater.add(gcs_link_diag);
81 
82  // updater spinner
83  auto diag_timer = priv_nh.createTimer(ros::Duration(0.5),
84  [&](const ros::TimerEvent &evt) {
85  updater.update();
86  });
87  diag_timer.start();
88 
89  ros::spin();
90  return 0;
91 }
std::shared_ptr< MAVConnInterface const > ConstPtr
#define ROS_FATAL(...)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void setHardwareID(const std::string &hwid)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void start()
ros::Subscriber mavlink_sub
Definition: gcs_bridge.cpp:25
void add(const std::string &name, TaskFunction f)
updater
void set_connection_status(bool connected)
Definition: mavlink_diag.h:34
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
void mavlink_pub_cb(const mavlink::mavlink_message_t *mmsg, const mavconn::Framing framing)
Definition: gcs_bridge.cpp:29
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
MAVConnInterface::Ptr gcs_link
Definition: gcs_bridge.cpp:26
static Ptr open_url(std::string url, uint8_t system_id=1, uint8_t component_id=MAV_COMP_ID_UDP_BRIDGE)
ros::Publisher mavlink_pub
Definition: gcs_bridge.cpp:24
static Time now()
int main(int argc, char *argv[])
Definition: gcs_bridge.cpp:48
#define ROS_ERROR(...)
void set_mavconn(const mavconn::MAVConnInterface::Ptr &link)
Definition: mavlink_diag.h:30
void mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg)
Definition: gcs_bridge.cpp:38
constexpr std::underlying_type< _T >::type enum_value(_T e)
Definition: utils.h:55
std::shared_ptr< MAVConnInterface > Ptr


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:16:10