adsb.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2017 Nuno Marques.
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 
17 #include <mavros/mavros_plugin.h>
18 
19 #include <mavros_msgs/ADSBVehicle.h>
20 
21 namespace mavros {
22 namespace extra_plugins {
23 using mavlink::common::ADSB_EMITTER_TYPE;
24 using mavlink::common::ADSB_ALTITUDE_TYPE;
25 
32 public:
34  adsb_nh("~adsb")
35  { }
36 
37  void initialize(UAS &uas_) override
38  {
40 
41  adsb_pub = adsb_nh.advertise<mavros_msgs::ADSBVehicle>("vehicle", 10);
42  adsb_sub = adsb_nh.subscribe("send", 10, &ADSBPlugin::adsb_cb, this);
43  }
44 
46  {
47  return {
49  };
50  }
51 
52 private:
54 
57 
58  void handle_adsb(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ADSB_VEHICLE &adsb)
59  {
60  auto adsb_msg = boost::make_shared<mavros_msgs::ADSBVehicle>();
61 
62  adsb_msg->header.stamp = ros::Time::now(); //TODO: request add time_boot_ms to msg definition
63  // [[[cog:
64  // def ent(ros, mav=None, scale=None, to_ros=None, to_mav=None):
65  // return (ros, mav or ros, scale, to_ros, to_mav)
66  //
67  // TR_TAB = (
68  // ent('ICAO_address'),
69  // ent('callsign', to_ros='mavlink::to_string', to_mav='mavlink::set_string_z({mm}.{mav}, {rmp}->{ros})'),
70  // ent('latitude', 'lat', '1e7'),
71  // ent('longitude', 'lon', '1e7'),
72  // ent('altitude', 'altitude', '1e3'),
73  // ent('altitude_type', ),
74  // ent('heading', scale='1e2'),
75  // ent('hor_velocity', scale='1e2'),
76  // ent('ver_velocity', scale='1e2'),
77  // ent('altitude_type'),
78  // ent('emitter_type'),
79  // ent('tslc', to_ros='ros::Duration', to_mav='{mm}.{mav} = {rmp}->{ros}.sec'),
80  // ent('flags'),
81  // ent('squawk'),
82  // )
83  //
84  // for ros, mav, scale, to_ros, _ in TR_TAB:
85  // if to_ros is None:
86  // scale_ex = '' if scale is None else ' / ' + scale
87  // cog.outl("""adsb_msg->{ros} = adsb.{mav}{scale_ex};""".format(**locals()))
88  // else:
89  // cog.outl("""adsb_msg->{ros} = {to_ros}(adsb.{mav});""".format(**locals()))
90  // ]]]
91  adsb_msg->ICAO_address = adsb.ICAO_address;
92  adsb_msg->callsign = mavlink::to_string(adsb.callsign);
93  adsb_msg->latitude = adsb.lat / 1e7;
94  adsb_msg->longitude = adsb.lon / 1e7;
95  adsb_msg->altitude = adsb.altitude / 1e3;
96  adsb_msg->altitude_type = adsb.altitude_type;
97  adsb_msg->heading = adsb.heading / 1e2;
98  adsb_msg->hor_velocity = adsb.hor_velocity / 1e2;
99  adsb_msg->ver_velocity = adsb.ver_velocity / 1e2;
100  adsb_msg->altitude_type = adsb.altitude_type;
101  adsb_msg->emitter_type = adsb.emitter_type;
102  adsb_msg->tslc = ros::Duration(adsb.tslc);
103  adsb_msg->flags = adsb.flags;
104  adsb_msg->squawk = adsb.squawk;
105  // [[[end]]] (checksum: b9c515e7a6fe688b91f4e72e655b9154)
106 
107  ROS_DEBUG_STREAM_NAMED("adsb", "ADSB: recv type: " << utils::to_string_enum<ADSB_ALTITUDE_TYPE>(adsb.altitude_type)
108  << " emitter: " << utils::to_string_enum<ADSB_EMITTER_TYPE>(adsb.emitter_type)
109  << " flags: 0x" << std::hex << adsb.flags);
110 
111  adsb_pub.publish(adsb_msg);
112  }
113 
115  {
116  mavlink::common::msg::ADSB_VEHICLE adsb{};
117 
118  // [[[cog:
119  // for ros, mav, scale, _, to_mav in TR_TAB:
120  // if to_mav is None:
121  // scale_ex = '' if scale is None else ' * ' + scale
122  // cog.outl("""adsb.{mav} = req->{ros}{scale_ex};""".format(**locals()))
123  // else:
124  // cog.outl(to_mav.format(mm='adsb', rmp='req', **locals()) + ';')
125  // ]]]
126  adsb.ICAO_address = req->ICAO_address;
127  mavlink::set_string_z(adsb.callsign, req->callsign);
128  adsb.lat = req->latitude * 1e7;
129  adsb.lon = req->longitude * 1e7;
130  adsb.altitude = req->altitude * 1e3;
131  adsb.altitude_type = req->altitude_type;
132  adsb.heading = req->heading * 1e2;
133  adsb.hor_velocity = req->hor_velocity * 1e2;
134  adsb.ver_velocity = req->ver_velocity * 1e2;
135  adsb.altitude_type = req->altitude_type;
136  adsb.emitter_type = req->emitter_type;
137  adsb.tslc = req->tslc.sec;
138  adsb.flags = req->flags;
139  adsb.squawk = req->squawk;
140  // [[[end]]] (checksum: 8583d9ea3a3eefae10ccd7037c06b46d)
141 
142  ROS_DEBUG_STREAM_NAMED("adsb", "ADSB: send type: " << utils::to_string_enum<ADSB_ALTITUDE_TYPE>(adsb.altitude_type)
143  << " emitter: " << utils::to_string_enum<ADSB_EMITTER_TYPE>(adsb.emitter_type)
144  << " flags: 0x" << std::hex << adsb.flags);
145 
146  UAS_FCU(m_uas)->send_message_ignore_drop(adsb);
147  }
148 };
149 } // namespace extra_plugins
150 } // namespace mavros
151 
std::shared_ptr< MAVConnInterface const > ConstPtr
#define ROS_DEBUG_STREAM_NAMED(name, args)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
ros::Subscriber adsb_sub
Definition: adsb.cpp:56
Subscriptions get_subscriptions() override
Definition: adsb.cpp:45
void adsb_cb(const mavros_msgs::ADSBVehicle::ConstPtr &req)
Definition: adsb.cpp:114
ADS-B Vehicle plugin.
Definition: adsb.cpp:31
#define UAS_FCU(uasobjptr)
void initialize(UAS &uas_) override
Definition: adsb.cpp:37
void handle_adsb(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ADSB_VEHICLE &adsb)
Definition: adsb.cpp:58
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::vector< HandlerInfo > Subscriptions
static Time now()
void initialize(UAS &uas_) override
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)


mavros_extras
Author(s): Vladimir Ermakov , Amilcar Lucas
autogenerated on Tue Jun 1 2021 02:36:36