log_transfer.cpp
Go to the documentation of this file.
1 #include <mavros/mavros_plugin.h>
2 #include <mavros_msgs/LogData.h>
3 #include <mavros_msgs/LogEntry.h>
4 #include <mavros_msgs/LogRequestData.h>
5 #include <mavros_msgs/LogRequestEnd.h>
6 #include <mavros_msgs/LogRequestList.h>
7 
8 namespace mavros {
9 namespace extra_plugins {
11 public:
13  nh("~log_transfer") {}
14 
15  void initialize(UAS& uas) override
16  {
18 
19  log_entry_pub = nh.advertise<mavros_msgs::LogEntry>("raw/log_entry", 1000);
20  log_data_pub = nh.advertise<mavros_msgs::LogData>("raw/log_data", 1000);
21 
22  log_request_list_srv = nh.advertiseService("raw/log_request_list",
24  log_request_data_srv = nh.advertiseService("raw/log_request_data",
26  log_request_end_srv = nh.advertiseService("raw/log_request_end",
28  }
29 
31  {
32  return {
35  };
36  }
37 
38 private:
42 
43  void handle_log_entry(const mavlink::mavlink_message_t*, mavlink::common::msg::LOG_ENTRY& le)
44  {
45  auto msg = boost::make_shared<mavros_msgs::LogEntry>();
46  msg->header.stamp = ros::Time::now();
47  msg->id = le.id;
48  msg->num_logs = le.num_logs;
49  msg->last_log_num = le.last_log_num;
50  msg->time_utc = ros::Time(le.time_utc);
51  msg->size = le.size;
52  log_entry_pub.publish(msg);
53  }
54 
55  void handle_log_data(const mavlink::mavlink_message_t*, mavlink::common::msg::LOG_DATA& ld)
56  {
57  auto msg = boost::make_shared<mavros_msgs::LogData>();
58  msg->header.stamp = ros::Time::now();
59  msg->id = ld.id;
60  msg->offset = ld.ofs;
61 
62  auto count = ld.count;
63  if (count > ld.data.max_size()) {
64  count = ld.data.max_size();
65  }
66  msg->data.insert(msg->data.cbegin(), ld.data.cbegin(), ld.data.cbegin() + count);
67  log_data_pub.publish(msg);
68  }
69 
70  bool log_request_list_cb(mavros_msgs::LogRequestList::Request &req,
71  mavros_msgs::LogRequestList::Response &res)
72  {
73  mavlink::common::msg::LOG_REQUEST_LIST msg = {};
74  m_uas->msg_set_target(msg);
75  msg.start = req.start;
76  msg.end = req.end;
77 
78  res.success = true;
79  try {
80  UAS_FCU(m_uas)->send_message(msg);
81  } catch (std::length_error&) {
82  res.success = false;
83  }
84  return true;
85  }
86 
87  bool log_request_data_cb(mavros_msgs::LogRequestData::Request &req,
88  mavros_msgs::LogRequestData::Response &res)
89  {
90  mavlink::common::msg::LOG_REQUEST_DATA msg = {};
91  m_uas->msg_set_target(msg);
92  msg.id = req.id;
93  msg.ofs = req.offset;
94  msg.count = req.count;
95 
96  res.success = true;
97  try {
98  UAS_FCU(m_uas)->send_message(msg);
99  } catch (std::length_error&) {
100  res.success = false;
101  }
102  return true;
103  }
104 
105  bool log_request_end_cb(mavros_msgs::LogRequestEnd::Request &,
106  mavros_msgs::LogRequestEnd::Response &res)
107  {
108  mavlink::common::msg::LOG_REQUEST_END msg = {};
109  m_uas->msg_set_target(msg);
110  res.success = true;
111  try {
112  UAS_FCU(m_uas)->send_message(msg);
113  } catch (std::length_error&) {
114  res.success = false;
115  }
116  return true;
117  }
118 };
119 } // namespace extra_plugins
120 } // namespace mavros
121 
msg
bool log_request_data_cb(mavros_msgs::LogRequestData::Request &req, mavros_msgs::LogRequestData::Response &res)
void handle_log_entry(const mavlink::mavlink_message_t *, mavlink::common::msg::LOG_ENTRY &le)
void publish(const boost::shared_ptr< M > &message) const
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
void msg_set_target(_T &msg)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool log_request_list_cb(mavros_msgs::LogRequestList::Request &req, mavros_msgs::LogRequestList::Response &res)
#define UAS_FCU(uasobjptr)
void handle_log_data(const mavlink::mavlink_message_t *, mavlink::common::msg::LOG_DATA &ld)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool log_request_end_cb(mavros_msgs::LogRequestEnd::Request &, mavros_msgs::LogRequestEnd::Response &res)
Subscriptions get_subscriptions() override
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