ftp.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright 2014 Vladimir Ermakov.
00011  *
00012  * This file is part of the mavros package and subject to the license terms
00013  * in the top-level LICENSE file of the mavros repository.
00014  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
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 // enable debugging messages
00038 //#define FTP_LL_DEBUG
00039 
00040 #ifdef __APPLE__
00041 #define EBADE 50   /* Invalid exchange */
00042 #define EBADFD 81  /* File descriptor in bad state */
00043 #define EBADRQC 54 /* Invalid request code */
00044 #define EBADSLT 55 /* Invalid slot */
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,      // target_network
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         // FTP:List
00287         uint32_t list_offset;
00288         std::string list_path;
00289         std::vector<mavros_msgs::FileEntry> list_entries;
00290 
00291         // FTP:Open / FTP:Close
00292         std::string open_path;
00293         size_t open_size;
00294         std::map<std::string, uint32_t> session_file_map;
00295 
00296         // FTP:Read
00297         size_t read_size;
00298         uint32_t read_offset;
00299         V_FileData read_buffer;
00300 
00301         // FTP:Write
00302         uint32_t write_offset;
00303         V_FileData write_buffer;
00304         V_FileData::iterator write_it;
00305 
00306         // FTP:CalcCRC32
00307         uint32_t checksum_crc32;
00308 
00309         // Timeouts,
00310         // computed as x4 time that needed for transmission of
00311         // one message at 57600 baud rate
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         /* -*- message handler -*- */
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                 // logic from QGCUASFileManager.cc
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                 // translate other protocol errors to errno
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                         /* dir list done */
00393                         list_directory_end();
00394                         return;
00395                 }
00396                 else if (prev_op == OP_READ && error_code == FTPRequest::kErrEOF) {
00397                         /* read done */
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                                 // do nothing
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                         // dir empty, we are done
00455                         list_directory_end();
00456                 }
00457                 else {
00458                         ROS_ASSERT_MSG(n_list_entries > 0, "FTP:List don't parse entries");
00459                         // Possibly more to come, try get more
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                 // kCmdReadFile return cunks of DATA_MAXSZ or smaller (last chunk)
00495                 // We requested specific amount of data, that can be smaller,
00496                 // but not larger.
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                         // Possibly more data
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                 // check that reported size not out of range
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                 // move iterator to written size
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                         // More data to write
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         /* -*- send helpers -*- */
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                 // read operation always try read DATA_MAXSZ block (hdr->size ignored)
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 /* FTPRequest::DATA_MAXSZ */;
00627                 req.send(uas, last_send_seqnr);
00628         }
00629 
00630         void send_write_command(const size_t bytes_to_copy) {
00631                 // write chunk from write_buffer [write_it..bytes_to_copy]
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         /* -*- helpers -*- */
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                         // ptr[0] == FTPRequest::DIRENT_FILE
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                         // reserve memory
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                         // If timeout occurs don't forget to reset state
00857                         op_state = OP_IDLE;
00858                         r_errno = ETIMEDOUT;
00859                         return false;
00860                 }
00861                 else
00862                         // if go_idle() occurs before timeout
00863                         return !is_error;
00864         }
00865 
00866         /* -*- service callbacks -*- */
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();   // not shure that it's needed
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                 // only one session per file
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();    // same as for list_entries
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                 // Note: emulated truncate() can take a while
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 };      // namespace mavplugin
01042 
01043 PLUGINLIB_EXPORT_CLASS(mavplugin::FTPPlugin, mavplugin::MavRosPlugin)


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19