ftp.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2014,2016 Vladimir Ermakov.
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 <chrono>
18 #include <cerrno>
19 #include <condition_variable>
20 #include <mavros/mavros_plugin.h>
21 
22 #include <std_srvs/Empty.h>
23 #include <mavros_msgs/FileEntry.h>
24 #include <mavros_msgs/FileList.h>
25 #include <mavros_msgs/FileOpen.h>
26 #include <mavros_msgs/FileClose.h>
27 #include <mavros_msgs/FileRead.h>
28 #include <mavros_msgs/FileWrite.h>
29 #include <mavros_msgs/FileRemove.h>
30 #include <mavros_msgs/FileMakeDir.h>
31 #include <mavros_msgs/FileRemoveDir.h>
32 #include <mavros_msgs/FileTruncate.h>
33 #include <mavros_msgs/FileRename.h>
34 #include <mavros_msgs/FileChecksum.h>
35 
36 // enable debugging messages
37 //#define FTP_LL_DEBUG
38 
39 namespace mavros {
40 namespace std_plugins {
41 using utils::enum_value;
42 
48 class FTPRequest : public mavlink::common::msg::FILE_TRANSFER_PROTOCOL {
49 public:
52  struct PayloadHeader {
53  uint16_t seqNumber;
54  uint8_t session;
55  uint8_t opcode;
56  uint8_t size;
57  uint8_t req_opcode;
58  uint8_t padding[2];
59  uint32_t offset;
60  uint8_t data[];
61  };
62 
64  enum Opcode : uint8_t {
81 
82  kRspAck = 128,
84  };
85 
87  enum ErrorCode : uint8_t {
98  };
99 
100  static const char DIRENT_FILE = 'F';
101  static const char DIRENT_DIR = 'D';
102  static const char DIRENT_SKIP = 'S';
104  static const uint8_t DATA_MAXSZ = 251 - sizeof(PayloadHeader);
105 
106  uint8_t *raw_payload() {
107  return payload.data();
108  }
109 
110  inline PayloadHeader *header() {
111  return reinterpret_cast<PayloadHeader *>(payload.data());
112  }
113 
114  uint8_t *data() {
115  return header()->data;
116  }
117 
118  char *data_c() {
119  return reinterpret_cast<char *>(header()->data);
120  }
121 
122  uint32_t *data_u32() {
123  return reinterpret_cast<uint32_t *>(header()->data);
124  }
125 
133  void set_data_string(std::string &s)
134  {
135  size_t sz = (s.size() < DATA_MAXSZ - 1) ? s.size() : DATA_MAXSZ - 1;
136 
137  memcpy(data_c(), s.c_str(), sz);
138  data_c()[sz] = '\0';
139  header()->size = sz;
140  }
141 
143  return target_system;
144  }
145 
149  bool decode_valid(UAS *uas)
150  {
151 #ifdef FTP_LL_DEBUG
152  auto hdr = header();
153  ROS_DEBUG_NAMED("ftp", "FTP:rm: SEQ(%u) SESS(%u) OPCODE(%u) RQOP(%u) SZ(%u) OFF(%u)",
154  hdr->seqNumber, hdr->session, hdr->opcode, hdr->req_opcode, hdr->size, hdr->offset);
155 #endif
156 
157  return UAS_FCU(uas)->get_system_id() == target_system;
158  }
159 
163  void send(UAS *uas, uint16_t seqNumber)
164  {
165  target_network = 0;
166  target_system = uas->get_tgt_system();
167  target_component = uas->get_tgt_component();
168 
169  auto hdr = header();
170  hdr->seqNumber = seqNumber;
171  hdr->req_opcode = kCmdNone;
172 
173 #ifdef FTP_LL_DEBUG
174  ROS_DEBUG_NAMED("ftp", "FTP:sm: SEQ(%u) SESS(%u) OPCODE(%u) SZ(%u) OFF(%u)",
175  hdr->seqNumber, hdr->session, hdr->opcode, hdr->size, hdr->offset);
176 #endif
177 
178  UAS_FCU(uas)->send_message_ignore_drop(*this);
179  }
180 
182  mavlink::common::msg::FILE_TRANSFER_PROTOCOL{}
183  { }
184 
185  explicit FTPRequest(Opcode op, uint8_t session = 0) :
186  mavlink::common::msg::FILE_TRANSFER_PROTOCOL{}
187  {
188  header()->session = session;
189  header()->opcode = op;
190  }
191 };
192 
193 
198 public:
199  FTPPlugin() : PluginBase(),
200  ftp_nh("~ftp"),
201  op_state(OP::IDLE),
202  last_send_seqnr(0),
203  active_session(0),
204  is_error(false),
205  r_errno(0),
206  list_offset(0),
207  read_offset(0),
208  write_offset(0),
209  open_size(0),
210  read_size(0),
211  read_buffer {},
212  checksum_crc32(0)
213  { }
214 
215  void initialize(UAS &uas_) override
216  {
217  PluginBase::initialize(uas_);
218 
219  // since C++ generator do not produce field length defs make check explicit.
220  FTPRequest r;
221  ROS_ASSERT(r.payload.size() - sizeof(FTPRequest::PayloadHeader) == r.DATA_MAXSZ);
222 
223  list_srv = ftp_nh.advertiseService("list", &FTPPlugin::list_cb, this);
224  open_srv = ftp_nh.advertiseService("open", &FTPPlugin::open_cb, this);
225  close_srv = ftp_nh.advertiseService("close", &FTPPlugin::close_cb, this);
226  read_srv = ftp_nh.advertiseService("read", &FTPPlugin::read_cb, this);
227  write_srv = ftp_nh.advertiseService("write", &FTPPlugin::write_cb, this);
228  mkdir_srv = ftp_nh.advertiseService("mkdir", &FTPPlugin::mkdir_cb, this);
229  rmdir_srv = ftp_nh.advertiseService("rmdir", &FTPPlugin::rmdir_cb, this);
230  remove_srv = ftp_nh.advertiseService("remove", &FTPPlugin::remove_cb, this);
231  truncate_srv = ftp_nh.advertiseService("truncate", &FTPPlugin::truncate_cb, this);
232  reset_srv = ftp_nh.advertiseService("reset", &FTPPlugin::reset_cb, this);
233  rename_srv = ftp_nh.advertiseService("rename", &FTPPlugin::rename_cb, this);
234  checksum_srv = ftp_nh.advertiseService("checksum", &FTPPlugin::checksum_cb, this);
235  }
236 
238  {
239  return {
241  };
242  }
243 
244 private:
258 
260  typedef std::vector<uint8_t> V_FileData;
261 
262  enum class OP {
263  IDLE,
264  ACK,
265  LIST,
266  OPEN,
267  READ,
268  WRITE,
269  CHECKSUM
270  };
271 
273  uint16_t last_send_seqnr;
274  uint32_t active_session;
275 
277  std::condition_variable cond;
278  bool is_error;
279  int r_errno;
280 
281  // FTP:List
282  uint32_t list_offset;
283  std::string list_path;
284  std::vector<mavros_msgs::FileEntry> list_entries;
285 
286  // FTP:Open / FTP:Close
287  std::string open_path;
288  size_t open_size;
289  std::map<std::string, uint32_t> session_file_map;
290 
291  // FTP:Read
292  size_t read_size;
293  uint32_t read_offset;
294  V_FileData read_buffer;
295 
296  // FTP:Write
297  uint32_t write_offset;
298  V_FileData write_buffer;
299  V_FileData::iterator write_it;
300 
301  // FTP:CalcCRC32
302  uint32_t checksum_crc32;
303 
304  // Timeouts,
305  // computed as x4 time that needed for transmission of
306  // one message at 57600 baud rate
307  static constexpr int LIST_TIMEOUT_MS = 5000;
308  static constexpr int OPEN_TIMEOUT_MS = 200;
309  static constexpr int CHUNK_TIMEOUT_MS = 200;
310 
312  static constexpr size_t MAX_RESERVE_DIFF = 0x10000;
313 
317 
318  /* -*- message handler -*- */
319 
321  void handle_file_transfer_protocol(const mavlink::mavlink_message_t *msg, FTPRequest &req)
322  {
323  if (!req.decode_valid(m_uas)) {
324  ROS_DEBUG_NAMED("ftp", "FTP: Wrong System Id, MY %u, TGT %u",
325  UAS_FCU(m_uas)->get_system_id(), req.get_target_system_id());
326  return;
327  }
328 
329  const uint16_t incoming_seqnr = req.header()->seqNumber;
330  const uint16_t expected_seqnr = last_send_seqnr + 1;
331  if (incoming_seqnr != expected_seqnr) {
332  ROS_WARN_NAMED("ftp", "FTP: Lost sync! seqnr: %u != %u",
333  incoming_seqnr, expected_seqnr);
334  go_idle(true, EILSEQ);
335  return;
336  }
337 
338  last_send_seqnr = incoming_seqnr;
339 
340  // logic from QGCUASFileManager.cc
341  if (req.header()->opcode == FTPRequest::kRspAck)
342  handle_req_ack(req);
343  else if (req.header()->opcode == FTPRequest::kRspNak)
344  handle_req_nack(req);
345  else {
346  ROS_ERROR_NAMED("ftp", "FTP: Unknown request response: %u", req.header()->opcode);
347  go_idle(true, EBADRQC);
348  }
349  }
350 
352  {
353  switch (op_state) {
354  case OP::IDLE: send_reset(); break;
355  case OP::ACK: go_idle(false); break;
356  case OP::LIST: handle_ack_list(req); break;
357  case OP::OPEN: handle_ack_open(req); break;
358  case OP::READ: handle_ack_read(req); break;
359  case OP::WRITE: handle_ack_write(req); break;
360  case OP::CHECKSUM: handle_ack_checksum(req); break;
361  default:
362  ROS_ERROR_NAMED("ftp", "FTP: wrong op_state");
363  go_idle(true, EBADRQC);
364  }
365  }
366 
368  {
369  auto hdr = req.header();
370  auto error_code = static_cast<FTPRequest::ErrorCode>(req.data()[0]);
371  auto prev_op = op_state;
372 
373  ROS_ASSERT(hdr->size == 1 || (error_code == FTPRequest::kErrFailErrno && hdr->size == 2));
374 
375  op_state = OP::IDLE;
376  if (error_code == FTPRequest::kErrFailErrno)
377  r_errno = req.data()[1];
378  // translate other protocol errors to errno
379  else if (error_code == FTPRequest::kErrFail)
380  r_errno = EFAULT;
381  else if (error_code == FTPRequest::kErrInvalidDataSize)
382  r_errno = EMSGSIZE;
383  else if (error_code == FTPRequest::kErrInvalidSession)
384  r_errno = EBADFD;
385  else if (error_code == FTPRequest::kErrNoSessionsAvailable)
386  r_errno = EMFILE;
387  else if (error_code == FTPRequest::kErrUnknownCommand)
388  r_errno = ENOSYS;
389 
390  if (prev_op == OP::LIST && error_code == FTPRequest::kErrEOF) {
391  /* dir list done */
392  list_directory_end();
393  return;
394  }
395  else if (prev_op == OP::READ && error_code == FTPRequest::kErrEOF) {
396  /* read done */
397  read_file_end();
398  return;
399  }
400 
401  ROS_ERROR_NAMED("ftp", "FTP: NAK: %u Opcode: %u State: %u Errno: %d (%s)",
402  error_code, hdr->req_opcode, enum_value(prev_op), r_errno, strerror(r_errno));
403  go_idle(true);
404  }
405 
407  {
408  auto hdr = req.header();
409 
410  ROS_DEBUG_NAMED("ftp", "FTP:m: ACK List SZ(%u) OFF(%u)", hdr->size, hdr->offset);
411  if (hdr->offset != list_offset) {
412  ROS_ERROR_NAMED("ftp", "FTP: Wrong list offset, req %u, ret %u",
413  list_offset, hdr->offset);
414  go_idle(true, EBADE);
415  return;
416  }
417 
418  uint8_t off = 0;
419  uint32_t n_list_entries = 0;
420 
421  while (off < hdr->size) {
422  const char *ptr = req.data_c() + off;
423  const size_t bytes_left = hdr->size - off;
424 
425  size_t slen = strnlen(ptr, bytes_left);
426  if ((ptr[0] == FTPRequest::DIRENT_SKIP && slen > 1) ||
427  (ptr[0] != FTPRequest::DIRENT_SKIP && slen < 2)) {
428  ROS_ERROR_NAMED("ftp", "FTP: Incorrect list entry: %s", ptr);
429  go_idle(true, ERANGE);
430  return;
431  }
432  else if (slen == bytes_left) {
433  ROS_ERROR_NAMED("ftp", "FTP: Missing NULL termination in list entry");
434  go_idle(true, EOVERFLOW);
435  return;
436  }
437 
438  if (ptr[0] == FTPRequest::DIRENT_FILE ||
439  ptr[0] == FTPRequest::DIRENT_DIR) {
440  add_dirent(ptr, slen);
441  }
442  else if (ptr[0] == FTPRequest::DIRENT_SKIP) {
443  // do nothing
444  }
445  else {
446  ROS_WARN_NAMED("ftp", "FTP: Unknown list entry: %s", ptr);
447  }
448 
449  off += slen + 1;
450  n_list_entries++;
451  }
452 
453  if (hdr->size == 0) {
454  // dir empty, we are done
455  list_directory_end();
456  }
457  else {
458  ROS_ASSERT_MSG(n_list_entries > 0, "FTP:List don't parse entries");
459  // Possibly more to come, try get more
460  list_offset += n_list_entries;
461  send_list_command();
462  }
463  }
464 
466  {
467  auto hdr = req.header();
468 
469  ROS_DEBUG_NAMED("ftp", "FTP:m: ACK Open OPCODE(%u)", hdr->req_opcode);
470  ROS_ASSERT(hdr->size == sizeof(uint32_t));
471  open_size = *req.data_u32();
472 
473  ROS_INFO_NAMED("ftp", "FTP:Open %s: success, session %u, size %zu",
474  open_path.c_str(), hdr->session, open_size);
475  session_file_map.insert(std::make_pair(open_path, hdr->session));
476  go_idle(false);
477  }
478 
480  {
481  auto hdr = req.header();
482 
483  ROS_DEBUG_NAMED("ftp", "FTP:m: ACK Read SZ(%u)", hdr->size);
484  if (hdr->session != active_session) {
485  ROS_ERROR_NAMED("ftp", "FTP:Read unexpected session");
486  go_idle(true, EBADSLT);
487  return;
488  }
489 
490  if (hdr->offset != read_offset) {
491  ROS_ERROR_NAMED("ftp", "FTP:Read different offset");
492  go_idle(true, EBADE);
493  return;
494  }
495 
496  // kCmdReadFile return cunks of DATA_MAXSZ or smaller (last chunk)
497  // We requested specific amount of data, that can be smaller,
498  // but not larger.
499  const size_t bytes_left = read_size - read_buffer.size();
500  const size_t bytes_to_copy = std::min<size_t>(bytes_left, hdr->size);
501 
502  read_buffer.insert(read_buffer.end(), req.data(), req.data() + bytes_to_copy);
503 
504  if (bytes_to_copy == FTPRequest::DATA_MAXSZ) {
505  // Possibly more data
506  read_offset += bytes_to_copy;
507  send_read_command();
508  }
509  else
510  read_file_end();
511  }
512 
514  {
515  auto hdr = req.header();
516 
517  ROS_DEBUG_NAMED("ftp", "FTP:m: ACK Write SZ(%u)", hdr->size);
518  if (hdr->session != active_session) {
519  ROS_ERROR_NAMED("ftp", "FTP:Write unexpected session");
520  go_idle(true, EBADSLT);
521  return;
522  }
523 
524  if (hdr->offset != write_offset) {
525  ROS_ERROR_NAMED("ftp", "FTP:Write different offset");
526  go_idle(true, EBADE);
527  return;
528  }
529 
530  ROS_ASSERT(hdr->size == sizeof(uint32_t));
531  const size_t bytes_written = *req.data_u32();
532 
533  // check that reported size not out of range
534  const size_t bytes_left_before_advance = std::distance(write_it, write_buffer.end());
535  ROS_ASSERT_MSG(bytes_written <= bytes_left_before_advance, "Bad write size");
536  ROS_ASSERT(bytes_written != 0);
537 
538  // move iterator to written size
539  std::advance(write_it, bytes_written);
540 
541  const size_t bytes_to_copy = write_bytes_to_copy();
542  if (bytes_to_copy > 0) {
543  // More data to write
544  write_offset += bytes_written;
545  send_write_command(bytes_to_copy);
546  }
547  else
548  write_file_end();
549  }
550 
552  {
553  auto hdr = req.header();
554 
555  ROS_DEBUG_NAMED("ftp", "FTP:m: ACK CalcFileCRC32 OPCODE(%u)", hdr->req_opcode);
556  ROS_ASSERT(hdr->size == sizeof(uint32_t));
557  checksum_crc32 = *req.data_u32();
558 
559  ROS_DEBUG_NAMED("ftp", "FTP:Checksum: success, crc32: 0x%08x", checksum_crc32);
560  go_idle(false);
561  }
562 
563  /* -*- send helpers -*- */
564 
571  void go_idle(bool is_error_, int r_errno_ = 0)
572  {
573  op_state = OP::IDLE;
574  is_error = is_error_;
575  if (is_error && r_errno_ != 0) r_errno = r_errno_;
576  else if (!is_error) r_errno = 0;
577  cond.notify_all();
578  }
579 
580  void send_reset()
581  {
582  ROS_DEBUG_NAMED("ftp", "FTP:m: kCmdResetSessions");
583  if (!session_file_map.empty()) {
584  ROS_WARN_NAMED("ftp", "FTP: Reset closes %zu sessons",
585  session_file_map.size());
586  session_file_map.clear();
587  }
588 
589  op_state = OP::ACK;
591  req.send(m_uas, last_send_seqnr);
592  }
593 
595  inline void send_any_path_command(FTPRequest::Opcode op, const std::string &debug_msg, std::string &path, uint32_t offset)
596  {
597  ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: " << debug_msg << path << " off: " << offset);
598  FTPRequest req(op);
599  req.header()->offset = offset;
600  req.set_data_string(path);
601  req.send(m_uas, last_send_seqnr);
602  }
603 
605  send_any_path_command(FTPRequest::kCmdListDirectory, "kCmdListDirectory: ", list_path, list_offset);
606  }
607 
609  send_any_path_command(FTPRequest::kCmdOpenFileRO, "kCmdOpenFileRO: ", open_path, 0);
610  }
611 
613  send_any_path_command(FTPRequest::kCmdOpenFileWO, "kCmdOpenFileWO: ", open_path, 0);
614  }
615 
617  send_any_path_command(FTPRequest::kCmdCreateFile, "kCmdCreateFile: ", open_path, 0);
618  }
619 
621  {
622  ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: kCmdTerminateSession: " << session);
624  req.header()->offset = 0;
625  req.header()->size = 0;
626  req.send(m_uas, last_send_seqnr);
627  }
628 
630  {
631  // read operation always try read DATA_MAXSZ block (hdr->size ignored)
632  ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: kCmdReadFile: " << active_session << " off: " << read_offset);
633  FTPRequest req(FTPRequest::kCmdReadFile, active_session);
634  req.header()->offset = read_offset;
635  req.header()->size = 0 /* FTPRequest::DATA_MAXSZ */;
636  req.send(m_uas, last_send_seqnr);
637  }
638 
639  void send_write_command(const size_t bytes_to_copy)
640  {
641  // write chunk from write_buffer [write_it..bytes_to_copy]
642  ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: kCmdWriteFile: " << active_session << " off: " << write_offset << " sz: " << bytes_to_copy);
643  FTPRequest req(FTPRequest::kCmdWriteFile, active_session);
644  req.header()->offset = write_offset;
645  req.header()->size = bytes_to_copy;
646  std::copy(write_it, write_it + bytes_to_copy, req.data());
647  req.send(m_uas, last_send_seqnr);
648  }
649 
650  void send_remove_command(std::string &path) {
651  send_any_path_command(FTPRequest::kCmdRemoveFile, "kCmdRemoveFile: ", path, 0);
652  }
653 
654  bool send_rename_command(std::string &old_path, std::string &new_path)
655  {
656  std::ostringstream os;
657  os << old_path;
658  os << '\0';
659  os << new_path;
660 
661  std::string paths = os.str();
662  if (paths.size() >= FTPRequest::DATA_MAXSZ) {
663  ROS_ERROR_NAMED("ftp", "FTP: rename file paths is too long: %zu", paths.size());
664  r_errno = ENAMETOOLONG;
665  return false;
666  }
667 
668  send_any_path_command(FTPRequest::kCmdRename, "kCmdRename: ", paths, 0);
669  return true;
670  }
671 
672  void send_truncate_command(std::string &path, size_t length) {
673  send_any_path_command(FTPRequest::kCmdTruncateFile, "kCmdTruncateFile: ", path, length);
674  }
675 
676  void send_create_dir_command(std::string &path) {
677  send_any_path_command(FTPRequest::kCmdCreateDirectory, "kCmdCreateDirectory: ", path, 0);
678  }
679 
680  void send_remove_dir_command(std::string &path) {
681  send_any_path_command(FTPRequest::kCmdRemoveDirectory, "kCmdRemoveDirectory: ", path, 0);
682  }
683 
684  void send_calc_file_crc32_command(std::string &path) {
685  send_any_path_command(FTPRequest::kCmdCalcFileCRC32, "kCmdCalcFileCRC32: ", path, 0);
686  }
687 
688  /* -*- helpers -*- */
689 
690  void add_dirent(const char *ptr, size_t slen)
691  {
692  mavros_msgs::FileEntry ent;
693  ent.size = 0;
694 
695  if (ptr[0] == FTPRequest::DIRENT_DIR) {
696  ent.name.assign(ptr + 1, slen - 1);
697  ent.type = mavros_msgs::FileEntry::TYPE_DIRECTORY;
698 
699  ROS_DEBUG_STREAM_NAMED("ftp", "FTP:List Dir: " << ent.name);
700  }
701  else {
702  // ptr[0] == FTPRequest::DIRENT_FILE
703  std::string name_size(ptr + 1, slen - 1);
704 
705  auto sep_it = std::find(name_size.begin(), name_size.end(), '\t');
706  ent.name.assign(name_size.begin(), sep_it);
707  ent.type = mavros_msgs::FileEntry::TYPE_FILE;
708 
709  if (sep_it != name_size.end()) {
710  name_size.erase(name_size.begin(), sep_it + 1);
711  if (name_size.size() != 0)
712  ent.size = std::stoi(name_size);
713  }
714 
715  ROS_DEBUG_STREAM_NAMED("ftp", "FTP:List File: " << ent.name << " SZ: " << ent.size);
716  }
717 
718  list_entries.push_back(ent);
719  }
720 
722  ROS_DEBUG_NAMED("ftp", "FTP:List done");
723  go_idle(false);
724  }
725 
726  void list_directory(std::string &path)
727  {
728  list_offset = 0;
729  list_path = path;
730  list_entries.clear();
731  op_state = OP::LIST;
732 
733  send_list_command();
734  }
735 
736  bool open_file(std::string &path, int mode)
737  {
738  open_path = path;
739  open_size = 0;
740  op_state = OP::OPEN;
741 
742  if (mode == mavros_msgs::FileOpenRequest::MODE_READ)
743  send_open_ro_command();
744  else if (mode == mavros_msgs::FileOpenRequest::MODE_WRITE)
745  send_open_wo_command();
746  else if (mode == mavros_msgs::FileOpenRequest::MODE_CREATE)
747  send_create_command();
748  else {
749  ROS_ERROR_NAMED("ftp", "FTP: Unsupported open mode: %d", mode);
750  op_state = OP::IDLE;
751  r_errno = EINVAL;
752  return false;
753  }
754 
755  return true;
756  }
757 
758  bool close_file(std::string &path)
759  {
760  auto it = session_file_map.find(path);
761  if (it == session_file_map.end()) {
762  ROS_ERROR_NAMED("ftp", "FTP:Close %s: not opened", path.c_str());
763  r_errno = EBADF;
764  return false;
765  }
766 
767  op_state = OP::ACK;
768  send_terminate_command(it->second);
769  session_file_map.erase(it);
770  return true;
771  }
772 
773  void read_file_end() {
774  ROS_DEBUG_NAMED("ftp", "FTP:Read done");
775  go_idle(false);
776  }
777 
778  bool read_file(std::string &path, size_t off, size_t len)
779  {
780  auto it = session_file_map.find(path);
781  if (it == session_file_map.end()) {
782  ROS_ERROR_NAMED("ftp", "FTP:Read %s: not opened", path.c_str());
783  r_errno = EBADF;
784  return false;
785  }
786 
787  op_state = OP::READ;
788  active_session = it->second;
789  read_size = len;
790  read_offset = off;
791  read_buffer.clear();
792  if (read_buffer.capacity() < len ||
793  read_buffer.capacity() > len + MAX_RESERVE_DIFF) {
794  // reserve memory
795  read_buffer.reserve(len);
796  }
797 
798  send_read_command();
799  return true;
800  }
801 
802  void write_file_end() {
803  ROS_DEBUG_NAMED("ftp", "FTP:Write done");
804  go_idle(false);
805  }
806 
807  bool write_file(std::string &path, size_t off, V_FileData &data)
808  {
809  auto it = session_file_map.find(path);
810  if (it == session_file_map.end()) {
811  ROS_ERROR_NAMED("ftp", "FTP:Write %s: not opened", path.c_str());
812  r_errno = EBADF;
813  return false;
814  }
815 
816  op_state = OP::WRITE;
817  active_session = it->second;
818  write_offset = off;
819  write_buffer = std::move(data);
820  write_it = write_buffer.begin();
821 
822  send_write_command(write_bytes_to_copy());
823  return true;
824  }
825 
826  void remove_file(std::string &path) {
827  op_state = OP::ACK;
828  send_remove_command(path);
829  }
830 
831  bool rename_(std::string &old_path, std::string &new_path) {
832  op_state = OP::ACK;
833  return send_rename_command(old_path, new_path);
834  }
835 
836  void truncate_file(std::string &path, size_t length) {
837  op_state = OP::ACK;
838  send_truncate_command(path, length);
839  }
840 
841  void create_directory(std::string &path) {
842  op_state = OP::ACK;
843  send_create_dir_command(path);
844  }
845 
846  void remove_directory(std::string &path) {
847  op_state = OP::ACK;
848  send_remove_dir_command(path);
849  }
850 
851  void checksum_crc32_file(std::string &path) {
852  op_state = OP::CHECKSUM;
853  checksum_crc32 = 0;
854  send_calc_file_crc32_command(path);
855  }
856 
857  static constexpr int compute_rw_timeout(size_t len) {
858  return CHUNK_TIMEOUT_MS * (len / FTPRequest::DATA_MAXSZ + 1);
859  }
860 
862  return std::min<size_t>(std::distance(write_it, write_buffer.end()),
864  }
865 
866  bool wait_completion(const int msecs)
867  {
868  std::unique_lock<std::mutex> lock(cond_mutex);
869 
870  bool is_timedout = cond.wait_for(lock, std::chrono::milliseconds(msecs))
871  == std::cv_status::timeout;
872 
873  if (is_timedout) {
874  // If timeout occurs don't forget to reset state
875  op_state = OP::IDLE;
876  r_errno = ETIMEDOUT;
877  return false;
878  }
879  else
880  // if go_idle() occurs before timeout
881  return !is_error;
882  }
883 
884  /* -*- service callbacks -*- */
885 
889 #define SERVICE_IDLE_CHECK() \
890  if (op_state != OP::IDLE) { \
891  ROS_ERROR_NAMED("ftp", "FTP: Busy"); \
892  return false; \
893  }
894 
895  bool list_cb(mavros_msgs::FileList::Request &req,
896  mavros_msgs::FileList::Response &res)
897  {
899 
900  list_directory(req.dir_path);
901  res.success = wait_completion(LIST_TIMEOUT_MS);
902  res.r_errno = r_errno;
903  if (res.success) {
904  res.list = std::move(list_entries);
905  list_entries.clear(); // not shure that it's needed
906  }
907 
908  return true;
909  }
910 
911  bool open_cb(mavros_msgs::FileOpen::Request &req,
912  mavros_msgs::FileOpen::Response &res)
913  {
915 
916  // only one session per file
917  auto it = session_file_map.find(req.file_path);
918  if (it != session_file_map.end()) {
919  ROS_ERROR_NAMED("ftp", "FTP: File %s: already opened",
920  req.file_path.c_str());
921  return false;
922  }
923 
924  res.success = open_file(req.file_path, req.mode);
925  if (res.success) {
926  res.success = wait_completion(OPEN_TIMEOUT_MS);
927  res.size = open_size;
928  }
929  res.r_errno = r_errno;
930 
931  return true;
932  }
933 
934  bool close_cb(mavros_msgs::FileClose::Request &req,
935  mavros_msgs::FileClose::Response &res)
936  {
938 
939  res.success = close_file(req.file_path);
940  if (res.success) {
941  res.success = wait_completion(OPEN_TIMEOUT_MS);
942  }
943  res.r_errno = r_errno;
944 
945  return true;
946  }
947 
948  bool read_cb(mavros_msgs::FileRead::Request &req,
949  mavros_msgs::FileRead::Response &res)
950  {
952 
953  res.success = read_file(req.file_path, req.offset, req.size);
954  if (res.success)
955  res.success = wait_completion(compute_rw_timeout(req.size));
956  if (res.success) {
957  res.data = std::move(read_buffer);
958  read_buffer.clear(); // same as for list_entries
959  }
960  res.r_errno = r_errno;
961 
962  return true;
963  }
964 
965  bool write_cb(mavros_msgs::FileWrite::Request &req,
966  mavros_msgs::FileWrite::Response &res)
967  {
969 
970  const size_t data_size = req.data.size();
971  res.success = write_file(req.file_path, req.offset, req.data);
972  if (res.success) {
973  res.success = wait_completion(compute_rw_timeout(data_size));
974  }
975  write_buffer.clear();
976  res.r_errno = r_errno;
977 
978  return true;
979  }
980 
981  bool remove_cb(mavros_msgs::FileRemove::Request &req,
982  mavros_msgs::FileRemove::Response &res)
983  {
985 
986  remove_file(req.file_path);
987  res.success = wait_completion(OPEN_TIMEOUT_MS);
988  res.r_errno = r_errno;
989 
990  return true;
991  }
992 
993  bool rename_cb(mavros_msgs::FileRename::Request &req,
994  mavros_msgs::FileRename::Response &res)
995  {
997 
998  res.success = rename_(req.old_path, req.new_path);
999  if (res.success) {
1000  res.success = wait_completion(OPEN_TIMEOUT_MS);
1001  }
1002  res.r_errno = r_errno;
1003 
1004  return true;
1005  }
1006 
1007 
1008  bool truncate_cb(mavros_msgs::FileTruncate::Request &req,
1009  mavros_msgs::FileTruncate::Response &res)
1010  {
1012 
1013  // Note: emulated truncate() can take a while
1014  truncate_file(req.file_path, req.length);
1015  res.success = wait_completion(LIST_TIMEOUT_MS * 5);
1016  res.r_errno = r_errno;
1017 
1018  return true;
1019  }
1020 
1021  bool mkdir_cb(mavros_msgs::FileMakeDir::Request &req,
1022  mavros_msgs::FileMakeDir::Response &res)
1023  {
1025 
1026  create_directory(req.dir_path);
1027  res.success = wait_completion(OPEN_TIMEOUT_MS);
1028  res.r_errno = r_errno;
1029 
1030  return true;
1031  }
1032 
1033  bool rmdir_cb(mavros_msgs::FileRemoveDir::Request &req,
1034  mavros_msgs::FileRemoveDir::Response &res)
1035  {
1037 
1038  remove_directory(req.dir_path);
1039  res.success = wait_completion(OPEN_TIMEOUT_MS);
1040  res.r_errno = r_errno;
1041 
1042  return true;
1043  }
1044 
1045  bool checksum_cb(mavros_msgs::FileChecksum::Request &req,
1046  mavros_msgs::FileChecksum::Response &res)
1047  {
1049 
1050  checksum_crc32_file(req.file_path);
1051  res.success = wait_completion(LIST_TIMEOUT_MS);
1052  res.crc32 = checksum_crc32;
1053  res.r_errno = r_errno;
1054 
1055  return true;
1056  }
1057 
1058 #undef SERVICE_IDLE_CHECK
1059 
1064  bool reset_cb(std_srvs::Empty::Request &req,
1065  std_srvs::Empty::Response &res)
1066  {
1067  send_reset();
1068  return true;
1069  }
1070 };
1071 } // namespace std_plugins
1072 } // namespace mavros
1073 
void handle_file_transfer_protocol(const mavlink::mavlink_message_t *msg, FTPRequest &req)
handler for mavlink::common::msg::FILE_TRANSFER_PROTOCOL
Definition: ftp.cpp:321
bool truncate_cb(mavros_msgs::FileTruncate::Request &req, mavros_msgs::FileTruncate::Response &res)
Definition: ftp.cpp:1008
msg
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
int r_errno
store errno from server
Definition: ftp.cpp:279
ros::ServiceServer truncate_srv
Definition: ftp.cpp:255
void send_remove_command(std::string &path)
Definition: ftp.cpp:650
Opcode
Command opcodes.
Definition: ftp.cpp:64
void initialize(UAS &uas_) override
Plugin initializer.
Definition: ftp.cpp:215
bool rename_cb(mavros_msgs::FileRename::Request &req, mavros_msgs::FileRename::Response &res)
Definition: ftp.cpp:993
ros::ServiceServer checksum_srv
Definition: ftp.cpp:257
#define ROS_INFO_NAMED(name,...)
uint8_t data[]
command data, varies by Opcode
Definition: ftp.cpp:60
void remove_directory(std::string &path)
Definition: ftp.cpp:846
#define ROS_DEBUG_STREAM_NAMED(name, args)
void send_write_command(const size_t bytes_to_copy)
Definition: ftp.cpp:639
bool is_error
error signaling flag (timeout/proto error)
Definition: ftp.cpp:278
void send(UAS *uas, uint16_t seqNumber)
Encode and send message.
Definition: ftp.cpp:163
void truncate_file(std::string &path, size_t length)
Definition: ftp.cpp:836
#define ROS_WARN_NAMED(name,...)
void handle_ack_checksum(FTPRequest &req)
Definition: ftp.cpp:551
uint16_t seqNumber
sequence number for message
Definition: ftp.cpp:53
bool write_file(std::string &path, size_t off, V_FileData &data)
Definition: ftp.cpp:807
uint8_t req_opcode
Request opcode returned in kRspAck, kRspNak message.
Definition: ftp.cpp:57
MAVROS Plugin base.
Creates file at <path> for writing, returns <session>
Definition: ftp.cpp:71
void send_remove_dir_command(std::string &path)
Definition: ftp.cpp:680
uint16_t last_send_seqnr
seqNumber for send.
Definition: ftp.cpp:273
void checksum_crc32_file(std::string &path)
Definition: ftp.cpp:851
Offset past end of file for List and Read commands.
Definition: ftp.cpp:94
bool remove_cb(mavros_msgs::FileRemove::Request &req, mavros_msgs::FileRemove::Response &res)
Definition: ftp.cpp:981
void handle_req_ack(FTPRequest &req)
Definition: ftp.cpp:351
#define SERVICE_IDLE_CHECK()
Definition: ftp.cpp:889
uint32_t offset
Offsets for List and Read commands.
Definition: ftp.cpp:59
bool read_file(std::string &path, size_t off, size_t len)
Definition: ftp.cpp:778
bool close_file(std::string &path)
Definition: ftp.cpp:758
void handle_ack_read(FTPRequest &req)
Definition: ftp.cpp:479
static const char DIRENT_DIR
Definition: ftp.cpp:101
ignored, always acked
Definition: ftp.cpp:65
ros::ServiceServer rename_srv
Definition: ftp.cpp:254
FTP Request message abstraction class.
Definition: ftp.cpp:48
Terminates all open Read sessions.
Definition: ftp.cpp:67
bool read_cb(mavros_msgs::FileRead::Request &req, mavros_msgs::FileRead::Response &res)
Definition: ftp.cpp:948
ssize_t len
Opens file at <path> for writing, returns <session>
Definition: ftp.cpp:76
FTPRequest(Opcode op, uint8_t session=0)
Definition: ftp.cpp:185
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
Definition: ftp.cpp:237
ros::ServiceServer mkdir_srv
Definition: ftp.cpp:251
V_FileData::iterator write_it
Definition: ftp.cpp:299
std::condition_variable cond
wait condvar
Definition: ftp.cpp:277
ros::ServiceServer remove_srv
Definition: ftp.cpp:253
void set_data_string(std::string &s)
Copy string to payload.
Definition: ftp.cpp:133
#define ROS_DEBUG_NAMED(name,...)
bool rename_(std::string &old_path, std::string &new_path)
Definition: ftp.cpp:831
Opens file at <path> for reading, returns <session>
Definition: ftp.cpp:69
uint8_t get_tgt_system()
Return communication target system.
Definition: mavros_uas.h:159
bool open_file(std::string &path, int mode)
Definition: ftp.cpp:736
Truncate file at <path> to <offset> length.
Definition: ftp.cpp:77
bool write_cb(mavros_msgs::FileWrite::Request &req, mavros_msgs::FileWrite::Response &res)
Definition: ftp.cpp:965
UAS for plugins.
Definition: mavros_uas.h:67
bool close_cb(mavros_msgs::FileClose::Request &req, mavros_msgs::FileClose::Response &res)
Definition: ftp.cpp:934
Burst download session file.
Definition: ftp.cpp:80
#define ROS_ASSERT_MSG(cond,...)
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Definition: mavros_uas.h:42
void send_any_path_command(FTPRequest::Opcode op, const std::string &debug_msg, std::string &path, uint32_t offset)
Send any command with string payload (usually file/dir path)
Definition: ftp.cpp:595
bool wait_completion(const int msecs)
Definition: ftp.cpp:866
uint8_t get_system_id()
uint32_t active_session
session id of current operation
Definition: ftp.cpp:274
Reads <size> bytes from <offset> in <session>
Definition: ftp.cpp:70
Calculate CRC32 for file at <path>
Definition: ftp.cpp:79
Rename <path1> to <path2>
Definition: ftp.cpp:78
bool open_cb(mavros_msgs::FileOpen::Request &req, mavros_msgs::FileOpen::Response &res)
Definition: ftp.cpp:911
ros::ServiceServer open_srv
Definition: ftp.cpp:247
ros::ServiceServer rmdir_srv
Definition: ftp.cpp:252
ErrorCode
Error codes returned in Nak response.
Definition: ftp.cpp:87
void send_create_dir_command(std::string &path)
Definition: ftp.cpp:676
bool reset_cb(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Reset communication on both sides.
Definition: ftp.cpp:1064
This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure oursel...
Definition: ftp.cpp:52
ros::NodeHandle ftp_nh
Definition: ftp.cpp:245
ros::ServiceServer write_srv
Definition: ftp.cpp:250
void handle_req_nack(FTPRequest &req)
Definition: ftp.cpp:367
void handle_ack_write(FTPRequest &req)
Definition: ftp.cpp:513
Writes <size> bytes to <offset> in <session>
Definition: ftp.cpp:72
Command failed, errno sent back in PayloadHeader.data[1].
Definition: ftp.cpp:90
ros::ServiceServer read_srv
Definition: ftp.cpp:249
uint8_t padding[2]
32 bit aligment padding
Definition: ftp.cpp:58
List files in <path> from <offset>
Definition: ftp.cpp:68
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
ros::ServiceServer list_srv
Definition: ftp.cpp:246
static constexpr int compute_rw_timeout(size_t len)
Definition: ftp.cpp:857
uint8_t session
Session id for read and write commands.
Definition: ftp.cpp:54
bool rmdir_cb(mavros_msgs::FileRemoveDir::Request &req, mavros_msgs::FileRemoveDir::Response &res)
Definition: ftp.cpp:1033
static const char DIRENT_SKIP
Definition: ftp.cpp:102
bool send_rename_command(std::string &old_path, std::string &new_path)
Definition: ftp.cpp:654
Session is not currently open.
Definition: ftp.cpp:92
void go_idle(bool is_error_, int r_errno_=0)
Go to IDLE mode.
Definition: ftp.cpp:571
void create_directory(std::string &path)
Definition: ftp.cpp:841
#define ROS_ERROR_NAMED(name,...)
PayloadHeader.size is invalid.
Definition: ftp.cpp:91
std::vector< uint8_t > V_FileData
This type used in servicies to store &#39;data&#39; fileds.
Definition: ftp.cpp:260
void send_terminate_command(uint32_t session)
Definition: ftp.cpp:620
Removes Directory at <path>, must be empty.
Definition: ftp.cpp:75
void add_dirent(const char *ptr, size_t slen)
Definition: ftp.cpp:690
static const char DIRENT_FILE
Definition: ftp.cpp:100
ros::ServiceServer reset_srv
Definition: ftp.cpp:256
#define ROS_ASSERT(cond)
std::vector< mavros_msgs::FileEntry > list_entries
Definition: ftp.cpp:284
Terminates open Read session.
Definition: ftp.cpp:66
bool list_cb(mavros_msgs::FileList::Request &req, mavros_msgs::FileList::Response &res)
Definition: ftp.cpp:895
void send_calc_file_crc32_command(std::string &path)
Definition: ftp.cpp:684
bool mkdir_cb(mavros_msgs::FileMakeDir::Request &req, mavros_msgs::FileMakeDir::Response &res)
Definition: ftp.cpp:1021
static const uint8_t DATA_MAXSZ
payload.size() - header bytes
Definition: ftp.cpp:104
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void remove_file(std::string &path)
Definition: ftp.cpp:826
bool decode_valid(UAS *uas)
Decode and check target system.
Definition: ftp.cpp:149
PayloadHeader * header()
Definition: ftp.cpp:110
uint8_t get_tgt_component()
Return communication target component.
Definition: mavros_uas.h:166
void handle_ack_open(FTPRequest &req)
Definition: ftp.cpp:465
void list_directory(std::string &path)
Definition: ftp.cpp:726
def write_file(filename, text)
std::map< std::string, uint32_t > session_file_map
Definition: ftp.cpp:289
void handle_ack_list(FTPRequest &req)
Definition: ftp.cpp:406
ros::ServiceServer close_srv
Definition: ftp.cpp:248
void send_truncate_command(std::string &path, size_t length)
Definition: ftp.cpp:672
constexpr std::underlying_type< _T >::type enum_value(_T e)
Definition: utils.h:55
std::recursive_mutex mutex
bool checksum_cb(mavros_msgs::FileChecksum::Request &req, mavros_msgs::FileChecksum::Response &res)
Definition: ftp.cpp:1045


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26