00001
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include <chrono>
00028 #include <cerrno>
00029 #include <condition_variable>
00030 #include <mavros/mavros_plugin.h>
00031 #include <pluginlib/class_list_macros.h>
00032
00033 #include <std_srvs/Empty.h>
00034 #include <mavros/FileEntry.h>
00035 #include <mavros/FileList.h>
00036 #include <mavros/FileOpen.h>
00037 #include <mavros/FileClose.h>
00038 #include <mavros/FileRead.h>
00039 #include <mavros/FileWrite.h>
00040 #include <mavros/FileRemove.h>
00041 #include <mavros/FileMakeDir.h>
00042 #include <mavros/FileRemoveDir.h>
00043 #include <mavros/FileTruncate.h>
00044 #include <mavros/FileRename.h>
00045 #include <mavros/FileChecksum.h>
00046
00047
00048
00049
00050 namespace mavplugin {
00051
00057 class FTPRequest {
00058 public:
00061 struct PayloadHeader {
00062 uint16_t seqNumber;
00063 uint8_t session;
00064 uint8_t opcode;
00065 uint8_t size;
00066 uint8_t req_opcode;
00067 uint8_t padding[2];
00068 uint32_t offset;
00069 uint8_t data[];
00070 };
00071
00073 enum Opcode : uint8_t {
00074 kCmdNone,
00075 kCmdTerminateSession,
00076 kCmdResetSessions,
00077 kCmdListDirectory,
00078 kCmdOpenFileRO,
00079 kCmdReadFile,
00080 kCmdCreateFile,
00081 kCmdWriteFile,
00082 kCmdRemoveFile,
00083 kCmdCreateDirectory,
00084 kCmdRemoveDirectory,
00085 kCmdOpenFileWO,
00086 kCmdTruncateFile,
00087 kCmdRename,
00088 kCmdCalcFileCRC32,
00089
00090 kRspAck = 128,
00091 kRspNak
00092 };
00093
00095 enum ErrorCode : uint8_t {
00096 kErrNone,
00097 kErrFail,
00098 kErrFailErrno,
00099 kErrInvalidDataSize,
00100 kErrInvalidSession,
00101 kErrNoSessionsAvailable,
00102 kErrEOF,
00103 kErrUnknownCommand
00104 };
00105
00106 static const char DIRENT_FILE = 'F';
00107 static const char DIRENT_DIR = 'D';
00108 static const char DIRENT_SKIP = 'S';
00109 static const uint8_t DATA_MAXSZ = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
00110
00111 uint8_t *raw_payload() {
00112 return message.payload;
00113 }
00114
00115 inline PayloadHeader *header() {
00116 return reinterpret_cast<PayloadHeader *>(message.payload);
00117 }
00118
00119 uint8_t *data() {
00120 return header()->data;
00121 }
00122
00123 char *data_c() {
00124 return reinterpret_cast<char *>(header()->data);
00125 }
00126
00127 uint32_t *data_u32() {
00128 return reinterpret_cast<uint32_t *>(header()->data);
00129 }
00130
00138 void set_data_string(std::string &s) {
00139 size_t sz = (s.size() < DATA_MAXSZ - 1)? s.size() : DATA_MAXSZ - 1;
00140
00141 memcpy(data_c(), s.c_str(), sz);
00142 data_c()[sz] = '\0';
00143 header()->size = sz;
00144 }
00145
00146 uint8_t get_target_system_id() {
00147 return message.target_system;
00148 }
00149
00153 bool decode(UAS *uas, const mavlink_message_t *msg) {
00154 mavlink_msg_file_transfer_protocol_decode(msg, &message);
00155
00156 #ifdef FTP_LL_DEBUG
00157 auto hdr = header();
00158 ROS_DEBUG_NAMED("ftp", "FTP:rm: SEQ(%u) SESS(%u) OPCODE(%u) RQOP(%u) SZ(%u) OFF(%u)",
00159 hdr->seqNumber, hdr->session, hdr->opcode, hdr->req_opcode, hdr->size, hdr->offset);
00160 #endif
00161
00162 return UAS_FCU(uas)->get_system_id() == message.target_system;
00163 }
00164
00168 void send(UAS *uas, uint16_t seqNumber) {
00169 mavlink_message_t msg;
00170
00171 auto hdr = header();
00172 hdr->seqNumber = seqNumber;
00173 hdr->req_opcode = kCmdNone;
00174
00175 #ifdef FTP_LL_DEBUG
00176 ROS_DEBUG_NAMED("ftp", "FTP:sm: SEQ(%u) SESS(%u) OPCODE(%u) SZ(%u) OFF(%u)",
00177 hdr->seqNumber, hdr->session, hdr->opcode, hdr->size, hdr->offset);
00178 #endif
00179
00180 mavlink_msg_file_transfer_protocol_pack_chan(UAS_PACK_CHAN(uas), &msg,
00181 0,
00182 UAS_PACK_TGT(uas),
00183 raw_payload());
00184 UAS_FCU(uas)->send_message(&msg);
00185 }
00186
00187 FTPRequest() :
00188 message{}
00189 { }
00190
00191 explicit FTPRequest(Opcode op, uint8_t session = 0) :
00192 message{}
00193 {
00194 header()->session = session;
00195 header()->opcode = op;
00196 }
00197
00198 private:
00199 mavlink_file_transfer_protocol_t message;
00200 };
00201
00202
00206 class FTPPlugin : public MavRosPlugin {
00207 public:
00208 FTPPlugin() :
00209 uas(nullptr),
00210 op_state(OP_IDLE),
00211 last_send_seqnr(0),
00212 active_session(0),
00213 is_error(false),
00214 list_offset(0),
00215 read_offset(0),
00216 open_size(0),
00217 read_size(0),
00218 read_buffer{}
00219 { }
00220
00221 void initialize(UAS &uas_,
00222 ros::NodeHandle &nh,
00223 diagnostic_updater::Updater &diag_updater)
00224 {
00225 uas = &uas_;
00226
00227 ftp_nh = ros::NodeHandle(nh, "ftp");
00228
00229 list_srv = ftp_nh.advertiseService("list", &FTPPlugin::list_cb, this);
00230 open_srv = ftp_nh.advertiseService("open", &FTPPlugin::open_cb, this);
00231 close_srv = ftp_nh.advertiseService("close", &FTPPlugin::close_cb, this);
00232 read_srv = ftp_nh.advertiseService("read", &FTPPlugin::read_cb, this);
00233 write_srv = ftp_nh.advertiseService("write", &FTPPlugin::write_cb, this);
00234 mkdir_srv = ftp_nh.advertiseService("mkdir", &FTPPlugin::mkdir_cb, this);
00235 rmdir_srv = ftp_nh.advertiseService("rmdir", &FTPPlugin::rmdir_cb, this);
00236 remove_srv = ftp_nh.advertiseService("remove", &FTPPlugin::remove_cb, this);
00237 truncate_srv = ftp_nh.advertiseService("truncate", &FTPPlugin::truncate_cb, this);
00238 reset_srv = ftp_nh.advertiseService("reset", &FTPPlugin::reset_cb, this);
00239 rename_srv = ftp_nh.advertiseService("rename", &FTPPlugin::rename_cb, this);
00240 checksum_srv = ftp_nh.advertiseService("checksum", &FTPPlugin::checksum_cb, this);
00241 }
00242
00243 std::string const get_name() const {
00244 return "FTP";
00245 }
00246
00247 const message_map get_rx_handlers() {
00248 return {
00249 MESSAGE_HANDLER(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, &FTPPlugin::handle_file_transfer_protocol),
00250 };
00251 }
00252
00253 private:
00254 UAS *uas;
00255 ros::NodeHandle ftp_nh;
00256 ros::ServiceServer list_srv;
00257 ros::ServiceServer open_srv;
00258 ros::ServiceServer close_srv;
00259 ros::ServiceServer read_srv;
00260 ros::ServiceServer write_srv;
00261 ros::ServiceServer mkdir_srv;
00262 ros::ServiceServer rmdir_srv;
00263 ros::ServiceServer remove_srv;
00264 ros::ServiceServer rename_srv;
00265 ros::ServiceServer truncate_srv;
00266 ros::ServiceServer reset_srv;
00267 ros::ServiceServer checksum_srv;
00268
00270 typedef std::vector<uint8_t> V_FileData;
00271
00272 enum OpState {
00273 OP_IDLE,
00274 OP_ACK,
00275 OP_LIST,
00276 OP_OPEN,
00277 OP_READ,
00278 OP_WRITE,
00279 OP_CHECKSUM
00280 };
00281
00282 OpState op_state;
00283 uint16_t last_send_seqnr;
00284 uint32_t active_session;
00285
00286 std::mutex cond_mutex;
00287 std::condition_variable cond;
00288 bool is_error;
00289 int r_errno;
00290
00291
00292 uint32_t list_offset;
00293 std::string list_path;
00294 std::vector<mavros::FileEntry> list_entries;
00295
00296
00297 std::string open_path;
00298 size_t open_size;
00299 std::map<std::string, uint32_t> session_file_map;
00300
00301
00302 size_t read_size;
00303 uint32_t read_offset;
00304 V_FileData read_buffer;
00305
00306
00307 uint32_t write_offset;
00308 V_FileData write_buffer;
00309 V_FileData::iterator write_it;
00310
00311
00312 uint32_t checksum_crc32;
00313
00314
00315
00316
00317 static constexpr int LIST_TIMEOUT_MS = 5000;
00318 static constexpr int OPEN_TIMEOUT_MS = 200;
00319 static constexpr int CHUNK_TIMEOUT_MS = 200;
00320
00322 static constexpr size_t MAX_RESERVE_DIFF = 0x10000;
00323
00326
00327
00328
00329 void handle_file_transfer_protocol(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00330 FTPRequest req;
00331 if (!req.decode(uas, msg)) {
00332 ROS_DEBUG_NAMED("ftp", "FTP: Wrong System Id, MY %u, TGT %u",
00333 UAS_FCU(uas)->get_system_id(), req.get_target_system_id());
00334 return;
00335 }
00336
00337 const uint16_t incoming_seqnr = req.header()->seqNumber;
00338 const uint16_t expected_seqnr = last_send_seqnr + 1;
00339 if (incoming_seqnr != expected_seqnr) {
00340 ROS_WARN_NAMED("ftp", "FTP: Lost sync! seqnr: %u != %u",
00341 incoming_seqnr, expected_seqnr);
00342 go_idle(true, EILSEQ);
00343 return;
00344 }
00345
00346 last_send_seqnr = incoming_seqnr;
00347
00348
00349 if (req.header()->opcode == FTPRequest::kRspAck)
00350 handle_req_ack(req);
00351 else if (req.header()->opcode == FTPRequest::kRspNak)
00352 handle_req_nack(req);
00353 else {
00354 ROS_ERROR_NAMED("ftp", "FTP: Unknown request response: %u", req.header()->opcode);
00355 go_idle(true, EBADRQC);
00356 }
00357 }
00358
00359 void handle_req_ack(FTPRequest &req) {
00360 switch (op_state) {
00361 case OP_IDLE: send_reset(); break;
00362 case OP_ACK: go_idle(false); break;
00363 case OP_LIST: handle_ack_list(req); break;
00364 case OP_OPEN: handle_ack_open(req); break;
00365 case OP_READ: handle_ack_read(req); break;
00366 case OP_WRITE: handle_ack_write(req); break;
00367 case OP_CHECKSUM: handle_ack_checksum(req); break;
00368 default:
00369 ROS_ERROR_NAMED("ftp", "FTP: wrong op_state");
00370 go_idle(true, EBADRQC);
00371 }
00372 }
00373
00374 void handle_req_nack(FTPRequest &req) {
00375 auto hdr = req.header();
00376 auto error_code = static_cast<FTPRequest::ErrorCode>(req.data()[0]);
00377 OpState prev_op = op_state;
00378
00379 ROS_ASSERT(hdr->size == 1 || (error_code == FTPRequest::kErrFailErrno && hdr->size == 2));
00380
00381 op_state = OP_IDLE;
00382 if (error_code == FTPRequest::kErrFailErrno)
00383 r_errno = req.data()[1];
00384
00385 else if (error_code == FTPRequest::kErrFail)
00386 r_errno = EFAULT;
00387 else if (error_code == FTPRequest::kErrInvalidDataSize)
00388 r_errno = EMSGSIZE;
00389 else if (error_code == FTPRequest::kErrInvalidSession)
00390 r_errno = EBADFD;
00391 else if (error_code == FTPRequest::kErrNoSessionsAvailable)
00392 r_errno = EMFILE;
00393 else if (error_code == FTPRequest::kErrUnknownCommand)
00394 r_errno = ENOSYS;
00395
00396 if (prev_op == OP_LIST && error_code == FTPRequest::kErrEOF) {
00397
00398 list_directory_end();
00399 return;
00400 }
00401 else if (prev_op == OP_READ && error_code == FTPRequest::kErrEOF) {
00402
00403 read_file_end();
00404 return;
00405 }
00406
00407 ROS_ERROR_NAMED("ftp", "FTP: NAK: %u Opcode: %u State: %u Errno: %d (%s)",
00408 error_code, hdr->req_opcode, prev_op, r_errno, strerror(r_errno));
00409 go_idle(true);
00410 }
00411
00412 void handle_ack_list(FTPRequest &req) {
00413 auto hdr = req.header();
00414
00415 ROS_DEBUG_NAMED("ftp", "FTP:m: ACK List SZ(%u) OFF(%u)", hdr->size, hdr->offset);
00416 if (hdr->offset != list_offset) {
00417 ROS_ERROR_NAMED("ftp", "FTP: Wring list offset, req %u, ret %u",
00418 list_offset, hdr->offset);
00419 go_idle(true, EBADE);
00420 return;
00421 }
00422
00423 uint8_t off = 0;
00424 uint32_t n_list_entries = 0;
00425
00426 while (off < hdr->size) {
00427 const char *ptr = req.data_c() + off;
00428 const size_t bytes_left = hdr->size - off;
00429
00430 size_t slen = strnlen(ptr, bytes_left);
00431 if ((ptr[0] == FTPRequest::DIRENT_SKIP && slen > 1) ||
00432 (ptr[0] != FTPRequest::DIRENT_SKIP && slen < 2)) {
00433 ROS_ERROR_NAMED("ftp", "FTP: Incorrect list entry: %s", ptr);
00434 go_idle(true, ERANGE);
00435 return;
00436 }
00437 else if (slen == bytes_left) {
00438 ROS_ERROR_NAMED("ftp", "FTP: Missing NULL termination in list entry");
00439 go_idle(true, EOVERFLOW);
00440 return;
00441 }
00442
00443 if (ptr[0] == FTPRequest::DIRENT_FILE ||
00444 ptr[0] == FTPRequest::DIRENT_DIR) {
00445 add_dirent(ptr, slen);
00446 }
00447 else if (ptr[0] == FTPRequest::DIRENT_SKIP) {
00448
00449 }
00450 else {
00451 ROS_WARN_NAMED("ftp", "FTP: Unknown list entry: %s", ptr);
00452 }
00453
00454 off += slen + 1;
00455 n_list_entries++;
00456 }
00457
00458 if (hdr->size == 0) {
00459
00460 list_directory_end();
00461 }
00462 else {
00463 ROS_ASSERT_MSG(n_list_entries > 0, "FTP:List don't parse entries");
00464
00465 list_offset += n_list_entries;
00466 send_list_command();
00467 }
00468 }
00469
00470 void handle_ack_open(FTPRequest &req) {
00471 auto hdr = req.header();
00472
00473 ROS_DEBUG_NAMED("ftp", "FTP:m: ACK Open OPCODE(%u)", hdr->req_opcode);
00474 ROS_ASSERT(hdr->size == sizeof(uint32_t));
00475 open_size = *req.data_u32();
00476
00477 ROS_INFO_NAMED("ftp", "FTP:Open %s: success, session %u, size %zu",
00478 open_path.c_str(), hdr->session, open_size);
00479 session_file_map.insert(std::make_pair(open_path, hdr->session));
00480 go_idle(false);
00481 }
00482
00483 void handle_ack_read(FTPRequest &req) {
00484 auto hdr = req.header();
00485
00486 ROS_DEBUG_NAMED("ftp", "FTP:m: ACK Read SZ(%u)", hdr->size);
00487 if (hdr->session != active_session) {
00488 ROS_ERROR_NAMED("ftp", "FTP:Read unexpected session");
00489 go_idle(true, EBADSLT);
00490 return;
00491 }
00492
00493 if (hdr->offset != read_offset) {
00494 ROS_ERROR_NAMED("ftp", "FTP:Read different offset");
00495 go_idle(true, EBADE);
00496 return;
00497 }
00498
00499
00500
00501
00502 const size_t bytes_left = read_size - read_buffer.size();
00503 const size_t bytes_to_copy = std::min<size_t>(bytes_left, hdr->size);
00504
00505 read_buffer.insert(read_buffer.end(), req.data(), req.data() + bytes_to_copy);
00506
00507 if (bytes_to_copy == FTPRequest::DATA_MAXSZ) {
00508
00509 read_offset += bytes_to_copy;
00510 send_read_command();
00511 }
00512 else
00513 read_file_end();
00514 }
00515
00516 void handle_ack_write(FTPRequest &req) {
00517 auto hdr = req.header();
00518
00519 ROS_DEBUG_NAMED("ftp", "FTP:m: ACK Write SZ(%u)", hdr->size);
00520 if (hdr->session != active_session) {
00521 ROS_ERROR_NAMED("ftp", "FTP:Write unexpected session");
00522 go_idle(true, EBADSLT);
00523 return;
00524 }
00525
00526 if (hdr->offset != write_offset) {
00527 ROS_ERROR_NAMED("ftp", "FTP:Write different offset");
00528 go_idle(true, EBADE);
00529 return;
00530 }
00531
00532 ROS_ASSERT(hdr->size == sizeof(uint32_t));
00533 const size_t bytes_written = *req.data_u32();
00534
00535
00536 const size_t bytes_left_before_advance = std::distance(write_it, write_buffer.end());
00537 ROS_ASSERT_MSG(bytes_written <= bytes_left_before_advance, "Bad write size");
00538 ROS_ASSERT(bytes_written != 0);
00539
00540
00541 std::advance(write_it, bytes_written);
00542
00543 const size_t bytes_to_copy = write_bytes_to_copy();
00544 if (bytes_to_copy > 0) {
00545
00546 write_offset += bytes_written;
00547 send_write_command(bytes_to_copy);
00548 }
00549 else
00550 write_file_end();
00551 }
00552
00553 void handle_ack_checksum(FTPRequest &req) {
00554 auto hdr = req.header();
00555
00556 ROS_DEBUG_NAMED("ftp", "FTP:m: ACK CalcFileCRC32 OPCODE(%u)", hdr->req_opcode);
00557 ROS_ASSERT(hdr->size == sizeof(uint32_t));
00558 checksum_crc32 = *req.data_u32();
00559
00560 ROS_DEBUG_NAMED("ftp", "FTP:Checksum: success, crc32: 0x%08x", checksum_crc32);
00561 go_idle(false);
00562 }
00563
00564
00565
00572 void go_idle(bool is_error_, int r_errno_ = 0) {
00573 op_state = OP_IDLE;
00574 is_error = is_error_;
00575 if (is_error && r_errno_ != 0) r_errno = r_errno_;
00576 else if (!is_error) r_errno = 0;
00577 cond.notify_all();
00578 }
00579
00580 void send_reset() {
00581 ROS_DEBUG_NAMED("ftp", "FTP:m: kCmdResetSessions");
00582 if (session_file_map.size() > 0) {
00583 ROS_WARN_NAMED("ftp", "FTP: Reset closes %zu sessons",
00584 session_file_map.size());
00585 session_file_map.clear();
00586 }
00587
00588 op_state = OP_ACK;
00589 FTPRequest req(FTPRequest::kCmdResetSessions);
00590 req.send(uas, last_send_seqnr);
00591 }
00592
00594 inline void send_any_path_command(FTPRequest::Opcode op, const std::string debug_msg, std::string &path, uint32_t offset) {
00595 ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: " << debug_msg << path << " off: " << offset);
00596 FTPRequest req(op);
00597 req.header()->offset = offset;
00598 req.set_data_string(path);
00599 req.send(uas, last_send_seqnr);
00600 }
00601
00602 void send_list_command() {
00603 send_any_path_command(FTPRequest::kCmdListDirectory, "kCmdListDirectory: ", list_path, list_offset);
00604 }
00605
00606 void send_open_ro_command() {
00607 send_any_path_command(FTPRequest::kCmdOpenFileRO, "kCmdOpenFileRO: ", open_path, 0);
00608 }
00609
00610 void send_open_wo_command() {
00611 send_any_path_command(FTPRequest::kCmdOpenFileWO, "kCmdOpenFileWO: ", open_path, 0);
00612 }
00613
00614 void send_create_command() {
00615 send_any_path_command(FTPRequest::kCmdCreateFile, "kCmdCreateFile: ", open_path, 0);
00616 }
00617
00618 void send_terminate_command(uint32_t session) {
00619 ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: kCmdTerminateSession: " << session);
00620 FTPRequest req(FTPRequest::kCmdTerminateSession, session);
00621 req.header()->offset = 0;
00622 req.header()->size = 0;
00623 req.send(uas, last_send_seqnr);
00624 }
00625
00626 void send_read_command() {
00627
00628 ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: kCmdReadFile: " << active_session << " off: " << read_offset);
00629 FTPRequest req(FTPRequest::kCmdReadFile, active_session);
00630 req.header()->offset = read_offset;
00631 req.header()->size = 0 ;
00632 req.send(uas, last_send_seqnr);
00633 }
00634
00635 void send_write_command(const size_t bytes_to_copy) {
00636
00637 ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: kCmdWriteFile: " << active_session << " off: " << write_offset << " sz: " << bytes_to_copy);
00638 FTPRequest req(FTPRequest::kCmdWriteFile, active_session);
00639 req.header()->offset = write_offset;
00640 req.header()->size = bytes_to_copy;
00641 std::copy(write_it, write_it + bytes_to_copy, req.data());
00642 req.send(uas, last_send_seqnr);
00643 }
00644
00645 void send_remove_command(std::string &path) {
00646 send_any_path_command(FTPRequest::kCmdRemoveFile, "kCmdRemoveFile: ", path, 0);
00647 }
00648
00649 bool send_rename_command(std::string &old_path, std::string &new_path) {
00650 std::ostringstream os;
00651 os << old_path;
00652 os << '\0';
00653 os << new_path;
00654
00655 std::string paths = os.str();
00656 if (paths.size() >= FTPRequest::DATA_MAXSZ) {
00657 ROS_ERROR_NAMED("ftp", "FTP: rename file paths is too long: %zu", paths.size());
00658 r_errno = ENAMETOOLONG;
00659 return false;
00660 }
00661
00662 send_any_path_command(FTPRequest::kCmdRename, "kCmdRename: ", paths, 0);
00663 return true;
00664 }
00665
00666 void send_truncate_command(std::string &path, size_t length) {
00667 send_any_path_command(FTPRequest::kCmdTruncateFile, "kCmdTruncateFile: ", path, length);
00668 }
00669
00670 void send_create_dir_command(std::string &path) {
00671 send_any_path_command(FTPRequest::kCmdCreateDirectory, "kCmdCreateDirectory: ", path, 0);
00672 }
00673
00674 void send_remove_dir_command(std::string &path) {
00675 send_any_path_command(FTPRequest::kCmdRemoveDirectory, "kCmdRemoveDirectory: ", path, 0);
00676 }
00677
00678 void send_calc_file_crc32_command(std::string &path) {
00679 send_any_path_command(FTPRequest::kCmdCalcFileCRC32, "kCmdCalcFileCRC32: ", path, 0);
00680 }
00681
00682
00683
00684 void add_dirent(const char *ptr, size_t slen) {
00685 mavros::FileEntry ent;
00686 ent.size = 0;
00687
00688 if (ptr[0] == FTPRequest::DIRENT_DIR) {
00689 ent.name.assign(ptr + 1, slen - 1);
00690 ent.type = mavros::FileEntry::TYPE_DIRECTORY;
00691
00692 ROS_DEBUG_STREAM_NAMED("ftp", "FTP:List Dir: " << ent.name);
00693 }
00694 else {
00695
00696 std::string name_size(ptr + 1, slen - 1);
00697
00698 auto sep_it = std::find(name_size.begin(), name_size.end(), '\t');
00699 ent.name.assign(name_size.begin(), sep_it);
00700 ent.type = mavros::FileEntry::TYPE_FILE;
00701
00702 if (sep_it != name_size.end()) {
00703 name_size.erase(name_size.begin(), sep_it + 1);
00704 if (name_size.size() != 0)
00705 ent.size = std::stoi(name_size);
00706 }
00707
00708 ROS_DEBUG_STREAM_NAMED("ftp", "FTP:List File: " << ent.name << " SZ: " << ent.size);
00709 }
00710
00711 list_entries.push_back(ent);
00712 }
00713
00714 void list_directory_end() {
00715 ROS_DEBUG_NAMED("ftp", "FTP:List done");
00716 go_idle(false);
00717 }
00718
00719 void list_directory(std::string &path) {
00720 list_offset = 0;
00721 list_path = path;
00722 list_entries.clear();
00723 op_state = OP_LIST;
00724
00725 send_list_command();
00726 }
00727
00728 bool open_file(std::string &path, int mode) {
00729 open_path = path;
00730 open_size = 0;
00731 op_state = OP_OPEN;
00732
00733 if (mode == mavros::FileOpenRequest::MODE_READ)
00734 send_open_ro_command();
00735 else if (mode == mavros::FileOpenRequest::MODE_WRITE)
00736 send_open_wo_command();
00737 else if (mode == mavros::FileOpenRequest::MODE_CREATE)
00738 send_create_command();
00739 else {
00740 ROS_ERROR_NAMED("ftp", "FTP: Unsupported open mode: %d", mode);
00741 op_state = OP_IDLE;
00742 r_errno = EINVAL;
00743 return false;
00744 }
00745
00746 return true;
00747 }
00748
00749 bool close_file(std::string &path) {
00750 auto it = session_file_map.find(path);
00751 if (it == session_file_map.end()) {
00752 ROS_ERROR_NAMED("ftp", "FTP:Close %s: not opened", path.c_str());
00753 r_errno = EBADF;
00754 return false;
00755 }
00756
00757 op_state = OP_ACK;
00758 send_terminate_command(it->second);
00759 session_file_map.erase(it);
00760 return true;
00761 }
00762
00763 void read_file_end() {
00764 ROS_DEBUG_NAMED("ftp", "FTP:Read done");
00765 go_idle(false);
00766 }
00767
00768 bool read_file(std::string &path, size_t off, size_t len) {
00769 auto it = session_file_map.find(path);
00770 if (it == session_file_map.end()) {
00771 ROS_ERROR_NAMED("ftp", "FTP:Read %s: not opened", path.c_str());
00772 r_errno = EBADF;
00773 return false;
00774 }
00775
00776 op_state = OP_READ;
00777 active_session = it->second;
00778 read_size = len;
00779 read_offset = off;
00780 read_buffer.clear();
00781 if (read_buffer.capacity() < len ||
00782 read_buffer.capacity() > len + MAX_RESERVE_DIFF) {
00783
00784 read_buffer.reserve(len);
00785 }
00786
00787 send_read_command();
00788 return true;
00789 }
00790
00791 void write_file_end() {
00792 ROS_DEBUG_NAMED("ftp", "FTP:Write done");
00793 go_idle(false);
00794 }
00795
00796 bool write_file(std::string &path, size_t off, V_FileData &data) {
00797 auto it = session_file_map.find(path);
00798 if (it == session_file_map.end()) {
00799 ROS_ERROR_NAMED("ftp", "FTP:Write %s: not opened", path.c_str());
00800 r_errno = EBADF;
00801 return false;
00802 }
00803
00804 op_state = OP_WRITE;
00805 active_session = it->second;
00806 write_offset = off;
00807 write_buffer = std::move(data);
00808 write_it = write_buffer.begin();
00809
00810 send_write_command(write_bytes_to_copy());
00811 return true;
00812 }
00813
00814 void remove_file(std::string &path) {
00815 op_state = OP_ACK;
00816 send_remove_command(path);
00817 }
00818
00819 bool rename_(std::string &old_path, std::string &new_path) {
00820 op_state = OP_ACK;
00821 return send_rename_command(old_path, new_path);
00822 }
00823
00824 void truncate_file(std::string &path, size_t length) {
00825 op_state = OP_ACK;
00826 send_truncate_command(path, length);
00827 }
00828
00829 void create_directory(std::string &path) {
00830 op_state = OP_ACK;
00831 send_create_dir_command(path);
00832 }
00833
00834 void remove_directory(std::string &path) {
00835 op_state = OP_ACK;
00836 send_remove_dir_command(path);
00837 }
00838
00839 void checksum_crc32_file(std::string &path) {
00840 op_state = OP_CHECKSUM;
00841 checksum_crc32 = 0;
00842 send_calc_file_crc32_command(path);
00843 }
00844
00845 static constexpr int compute_rw_timeout(size_t len) {
00846 return CHUNK_TIMEOUT_MS * (len / FTPRequest::DATA_MAXSZ + 1);
00847 }
00848
00849 size_t write_bytes_to_copy() {
00850 return std::min<size_t>(std::distance(write_it, write_buffer.end()),
00851 FTPRequest::DATA_MAXSZ);
00852 }
00853
00854 bool wait_completion(const int msecs) {
00855 std::unique_lock<std::mutex> lock(cond_mutex);
00856
00857 bool is_timedout = cond.wait_for(lock, std::chrono::milliseconds(msecs))
00858 == std::cv_status::timeout;
00859
00860 if (is_timedout) {
00861
00862 op_state = OP_IDLE;
00863 r_errno = ETIMEDOUT;
00864 return false;
00865 }
00866 else
00867
00868 return !is_error;
00869 }
00870
00871
00872
00876 #define SERVICE_IDLE_CHECK() \
00877 if (op_state != OP_IDLE) { \
00878 ROS_ERROR_NAMED("ftp", "FTP: Busy"); \
00879 return false; \
00880 }
00881
00882 bool list_cb(mavros::FileList::Request &req,
00883 mavros::FileList::Response &res) {
00884 SERVICE_IDLE_CHECK();
00885
00886 list_directory(req.dir_path);
00887 res.success = wait_completion(LIST_TIMEOUT_MS);
00888 res.r_errno = r_errno;
00889 if (res.success) {
00890 res.list = std::move(list_entries);
00891 list_entries.clear();
00892 }
00893
00894 return true;
00895 }
00896
00897 bool open_cb(mavros::FileOpen::Request &req,
00898 mavros::FileOpen::Response &res) {
00899 SERVICE_IDLE_CHECK();
00900
00901
00902 auto it = session_file_map.find(req.file_path);
00903 if (it != session_file_map.end()) {
00904 ROS_ERROR_NAMED("ftp", "FTP: File %s: already opened",
00905 req.file_path.c_str());
00906 return false;
00907 }
00908
00909 res.success = open_file(req.file_path, req.mode);
00910 if (res.success) {
00911 res.success = wait_completion(OPEN_TIMEOUT_MS);
00912 res.size = open_size;
00913 }
00914 res.r_errno = r_errno;
00915
00916 return true;
00917 }
00918
00919 bool close_cb(mavros::FileClose::Request &req,
00920 mavros::FileClose::Response &res) {
00921 SERVICE_IDLE_CHECK();
00922
00923 res.success = close_file(req.file_path);
00924 if (res.success) {
00925 res.success = wait_completion(OPEN_TIMEOUT_MS);
00926 }
00927 res.r_errno = r_errno;
00928
00929 return true;
00930 }
00931
00932 bool read_cb(mavros::FileRead::Request &req,
00933 mavros::FileRead::Response &res) {
00934 SERVICE_IDLE_CHECK();
00935
00936 res.success = read_file(req.file_path, req.offset, req.size);
00937 if (res.success)
00938 res.success = wait_completion(compute_rw_timeout(req.size));
00939 if (res.success) {
00940 res.data = std::move(read_buffer);
00941 read_buffer.clear();
00942 }
00943 res.r_errno = r_errno;
00944
00945 return true;
00946 }
00947
00948 bool write_cb(mavros::FileWrite::Request &req,
00949 mavros::FileWrite::Response &res) {
00950 SERVICE_IDLE_CHECK();
00951
00952 const size_t data_size = req.data.size();
00953 res.success = write_file(req.file_path, req.offset, req.data);
00954 if (res.success) {
00955 res.success = wait_completion(compute_rw_timeout(data_size));
00956 }
00957 write_buffer.clear();
00958 res.r_errno = r_errno;
00959
00960 return true;
00961 }
00962
00963 bool remove_cb(mavros::FileRemove::Request &req,
00964 mavros::FileRemove::Response &res) {
00965 SERVICE_IDLE_CHECK();
00966
00967 remove_file(req.file_path);
00968 res.success = wait_completion(OPEN_TIMEOUT_MS);
00969 res.r_errno = r_errno;
00970
00971 return true;
00972 }
00973
00974 bool rename_cb(mavros::FileRename::Request &req,
00975 mavros::FileRename::Response &res) {
00976 SERVICE_IDLE_CHECK();
00977
00978 res.success = rename_(req.old_path, req.new_path);
00979 if (res.success) {
00980 res.success = wait_completion(OPEN_TIMEOUT_MS);
00981 }
00982 res.r_errno = r_errno;
00983
00984 return true;
00985 }
00986
00987
00988 bool truncate_cb(mavros::FileTruncate::Request &req,
00989 mavros::FileTruncate::Response &res) {
00990 SERVICE_IDLE_CHECK();
00991
00992
00993 truncate_file(req.file_path, req.length);
00994 res.success = wait_completion(LIST_TIMEOUT_MS * 5);
00995 res.r_errno = r_errno;
00996
00997 return true;
00998 }
00999
01000 bool mkdir_cb(mavros::FileMakeDir::Request &req,
01001 mavros::FileMakeDir::Response &res) {
01002 SERVICE_IDLE_CHECK();
01003
01004 create_directory(req.dir_path);
01005 res.success = wait_completion(OPEN_TIMEOUT_MS);
01006 res.r_errno = r_errno;
01007
01008 return true;
01009 }
01010
01011 bool rmdir_cb(mavros::FileRemoveDir::Request &req,
01012 mavros::FileRemoveDir::Response &res) {
01013 SERVICE_IDLE_CHECK();
01014
01015 remove_directory(req.dir_path);
01016 res.success = wait_completion(OPEN_TIMEOUT_MS);
01017 res.r_errno = r_errno;
01018
01019 return true;
01020 }
01021
01022 bool checksum_cb(mavros::FileChecksum::Request &req,
01023 mavros::FileChecksum::Response &res) {
01024 SERVICE_IDLE_CHECK();
01025
01026 checksum_crc32_file(req.file_path);
01027 res.success = wait_completion(LIST_TIMEOUT_MS);
01028 res.crc32 = checksum_crc32;
01029 res.r_errno = r_errno;
01030
01031 return true;
01032 }
01033
01034 #undef SERVICE_IDLE_CHECK
01035
01040 bool reset_cb(std_srvs::Empty::Request &req,
01041 std_srvs::Empty::Response &res) {
01042 send_reset();
01043 return true;
01044 }
01045 };
01046
01047 };
01048
01049 PLUGINLIB_EXPORT_CLASS(mavplugin::FTPPlugin, mavplugin::MavRosPlugin)
01050