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