debug_value.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/DebugValue.h>
20 
21 namespace mavros {
22 namespace extra_plugins {
25 public:
27  debug_nh("~debug_value")
28  { }
29 
30  void initialize(UAS &uas_) override
31  {
33 
34  // subscribers
36 
37  // publishers
38  debug_pub = debug_nh.advertise<mavros_msgs::DebugValue>("debug", 10);
39  debug_vector_pub = debug_nh.advertise<mavros_msgs::DebugValue>("debug_vector", 10);
40  named_value_float_pub = debug_nh.advertise<mavros_msgs::DebugValue>("named_value_float", 10);
41  named_value_int_pub = debug_nh.advertise<mavros_msgs::DebugValue>("named_value_int", 10);
42  }
43 
45  return {
50  };
51  }
52 
53 private:
55 
57 
62 
63  /* -*- helpers -*- */
64 
70  void debug_logger(const std::string &type, const mavros_msgs::DebugValue &dv)
71  {
72  using DV = mavros_msgs::DebugValue;
73 
74  std::string name = (dv.name == "") ? "UNK" : dv.name;
75 
76  std::ostringstream ss;
77  if (dv.type == DV::TYPE_NAMED_VALUE_INT) {
78  ss << dv.value_int;
79  }
80  else if (dv.type == DV::TYPE_DEBUG_VECT) {
81  ss << "[";
82  bool is_first = true;
83  for (auto v : dv.data) {
84  if (!is_first) {
85  ss << ", ";
86  }
87 
88  ss << v;
89  is_first = false;
90  }
91 
92  ss << "]";
93  }
94  else {
95  ss << dv.value_float;
96  }
97 
98 
99  ROS_DEBUG_STREAM_NAMED("debug_value", type << "\t"
100  << dv.header.stamp << "\t"
101  << name << "\t["
102  << dv.index << "]\tvalue:"
103  << ss.str());
104  }
105 
106  /* -*- message handlers -*- */
107 
114  void handle_debug(const mavlink::mavlink_message_t *msg, mavlink::common::msg::DEBUG &debug)
115  {
116  // [[[cog:
117  // p = "dv_msg"
118  // val = "debug"
119  //
120  // def common_filler(type_, time_f, index, name):
121  // if isinstance(index, str):
122  // index = val + "." + index
123  //
124  // _args = globals()
125  // _args.update(locals())
126  //
127  // cog.outl("""auto {p} = boost::make_shared<mavros_msgs::DebugValue>();""".format(**_args))
128  // cog.outl("""{p}->header.stamp = m_uas->synchronise_stamp({val}.{time_f});""".format(**_args))
129  // cog.outl("""{p}->type = mavros_msgs::DebugValue::{type_};""".format(**_args))
130  // cog.outl("""{p}->index = {index};""".format(**_args))
131  // if name:
132  // cog.outl("""{p}->name = mavlink::to_string({val}.{name});""".format(**_args))
133  //
134  // common_filler("TYPE_DEBUG", "time_boot_ms", "ind", None)
135  // cog.outl("""{p}->value_float = {val}.value;""".format(**locals()))
136  // ]]]
137  auto dv_msg = boost::make_shared<mavros_msgs::DebugValue>();
138  dv_msg->header.stamp = m_uas->synchronise_stamp(debug.time_boot_ms);
139  dv_msg->type = mavros_msgs::DebugValue::TYPE_DEBUG;
140  dv_msg->index = debug.ind;
141  dv_msg->value_float = debug.value;
142  // [[[end]]] (checksum: 82e058cb699846b34885ce3defc6ac58)
143 
144  debug_logger(debug.get_name(), *dv_msg);
145  debug_pub.publish(dv_msg);
146  }
147 
154  void handle_debug_vector(const mavlink::mavlink_message_t *msg, mavlink::common::msg::DEBUG_VECT &debug)
155  {
156  // [[[cog:
157  // common_filler("TYPE_DEBUG_VECT", "time_usec", -1, "name")
158  //
159  // fields = "xyz"
160  // pd = p + "->data"
161  // cog.outl("""{pd}.resize({l});""".format(l=len(fields), **locals()))
162  // for i, f in enumerate(fields):
163  // cog.outl("""{pd}[{i}] = {val}.{f};""".format(**locals()))
164  // ]]]
165  auto dv_msg = boost::make_shared<mavros_msgs::DebugValue>();
166  dv_msg->header.stamp = m_uas->synchronise_stamp(debug.time_usec);
167  dv_msg->type = mavros_msgs::DebugValue::TYPE_DEBUG_VECT;
168  dv_msg->index = -1;
169  dv_msg->name = mavlink::to_string(debug.name);
170  dv_msg->data.resize(3);
171  dv_msg->data[0] = debug.x;
172  dv_msg->data[1] = debug.y;
173  dv_msg->data[2] = debug.z;
174  // [[[end]]] (checksum: 966bca4d95f06349cf065f3887c69471)
175 
176  debug_logger(debug.get_name(), *dv_msg);
177  debug_vector_pub.publish(dv_msg);
178  }
179 
190  void handle_named_value_float(const mavlink::mavlink_message_t *msg, mavlink::common::msg::NAMED_VALUE_FLOAT &value)
191  {
192  // [[[cog:
193  // val="value"
194  // common_filler("TYPE_NAMED_VALUE_FLOAT", "time_boot_ms", -1, "name")
195  // cog.outl("""{p}->value_float = {val}.value;""".format(**locals()))
196  // ]]]
197  auto dv_msg = boost::make_shared<mavros_msgs::DebugValue>();
198  dv_msg->header.stamp = m_uas->synchronise_stamp(value.time_boot_ms);
199  dv_msg->type = mavros_msgs::DebugValue::TYPE_NAMED_VALUE_FLOAT;
200  dv_msg->index = -1;
201  dv_msg->name = mavlink::to_string(value.name);
202  dv_msg->value_float = value.value;
203  // [[[end]]] (checksum: 8d70c469d67ab346574c99d20b91a501)
204 
205  debug_logger(value.get_name(), *dv_msg);
206  named_value_float_pub.publish(dv_msg);
207  }
208 
215  void handle_named_value_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::NAMED_VALUE_INT &value)
216  {
217  // [[[cog:
218  // common_filler("TYPE_NAMED_VALUE_INT", "time_boot_ms", -1, "name")
219  // cog.outl("""{p}->value_int = {val}.value;""".format(**locals()))
220  // ]]]
221  auto dv_msg = boost::make_shared<mavros_msgs::DebugValue>();
222  dv_msg->header.stamp = m_uas->synchronise_stamp(value.time_boot_ms);
223  dv_msg->type = mavros_msgs::DebugValue::TYPE_NAMED_VALUE_INT;
224  dv_msg->index = -1;
225  dv_msg->name = mavlink::to_string(value.name);
226  dv_msg->value_int = value.value;
227  // [[[end]]] (checksum: b7ce125baba6e8918f6b866a9d5aa77c)
228 
229  debug_logger(value.get_name(), *dv_msg);
230  named_value_int_pub.publish(dv_msg);
231  }
232 
233  /* -*- callbacks -*- */
234 
240  {
241  switch (req->type) {
242  case mavros_msgs::DebugValue::TYPE_DEBUG: {
243  mavlink::common::msg::DEBUG debug {};
244 
245  debug.time_boot_ms = req->header.stamp.toNSec() / 1000000;
246  debug.ind = req->index;
247  debug.value = req->value_float;
248 
249  UAS_FCU(m_uas)->send_message_ignore_drop(debug);
250  break;
251  }
252  case mavros_msgs::DebugValue::TYPE_DEBUG_VECT: {
253  mavlink::common::msg::DEBUG_VECT debug {};
254 
255  debug.time_usec = req->header.stamp.toNSec() / 1000;
256  mavlink::set_string(debug.name, req->name);
257  // [[[cog:
258  // for i, f in enumerate("xyz"):
259  // cog.outl("debug.{f} = req->data[{i}];".format(**locals()))
260  // ]]]
261  debug.x = req->data[0];
262  debug.y = req->data[1];
263  debug.z = req->data[2];
264  // [[[end]]] (checksum: f4918ce98ca3183f93f6aff20d4ab7ec)
265 
266  UAS_FCU(m_uas)->send_message_ignore_drop(debug);
267  break;
268  }
269  //case mavros_msgs::DebugValue::TYPE_DEBUG_ARRAY: {
270  // return;
271  //}
272  case mavros_msgs::DebugValue::TYPE_NAMED_VALUE_FLOAT: {
273  mavlink::common::msg::NAMED_VALUE_FLOAT value {};
274 
275  value.time_boot_ms = req->header.stamp.toNSec() / 1000000;
276  mavlink::set_string(value.name, req->name);
277  value.value = req->value_float;
278 
279  UAS_FCU(m_uas)->send_message_ignore_drop(value);
280  break;
281  }
282  case mavros_msgs::DebugValue::TYPE_NAMED_VALUE_INT: {
283  mavlink::common::msg::NAMED_VALUE_INT value {};
284 
285  value.time_boot_ms = req->header.stamp.toNSec() / 1000000;
286  mavlink::set_string(value.name, req->name);
287  value.value = req->value_int;
288 
289  UAS_FCU(m_uas)->send_message_ignore_drop(value);
290  break;
291  }
292  default:
293  ROS_ERROR_NAMED("debug", "Wrong debug type (%d). Droping!...", req->type);
294  return;
295  }
296  }
297 };
298 } // namespace extra_plugins
299 } // namespace mavros
300 
std::shared_ptr< MAVConnInterface const > ConstPtr
Subscriptions get_subscriptions() override
Definition: debug_value.cpp:44
#define ROS_DEBUG_STREAM_NAMED(name, args)
void publish(const boost::shared_ptr< M > &message) const
void debug_cb(const mavros_msgs::DebugValue::ConstPtr &req)
Debug callbacks.
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))
void debug_logger(const std::string &type, const mavros_msgs::DebugValue &dv)
Helper function to log debug messages.
Definition: debug_value.cpp:70
std::atomic< uint8_t > type
#define UAS_FCU(uasobjptr)
ros::Time synchronise_stamp(uint32_t time_boot_ms)
Plugin for Debug msgs from MAVLink API.
Definition: debug_value.cpp:24
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void handle_named_value_float(const mavlink::mavlink_message_t *msg, mavlink::common::msg::NAMED_VALUE_FLOAT &value)
Handle NAMED_VALUE_FLOAT message. Message specification: https://mavlink.io/en/messages/common.html#NAMED_VALUE_FLOAT.
void initialize(UAS &uas_) override
Definition: debug_value.cpp:30
void handle_debug_vector(const mavlink::mavlink_message_t *msg, mavlink::common::msg::DEBUG_VECT &debug)
Handle DEBUG_VECT message. Message specification: https://mavlink.io/en/messages/common.html#DEBUG_VECT.
std::vector< HandlerInfo > Subscriptions
#define ROS_ERROR_NAMED(name,...)
void initialize(UAS &uas_) override
void handle_debug(const mavlink::mavlink_message_t *msg, mavlink::common::msg::DEBUG &debug)
Handle DEBUG message. Message specification: https://mavlink.io/en/messages/common.html#DEBUG.
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void handle_named_value_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::NAMED_VALUE_INT &value)
Handle NAMED_VALUE_INT message. Message specification: https://mavlink.io/en/messages/common.html#NAMED_VALUE_INT.


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