diff --git a/include/sick-lms5xx/parsing.hpp b/include/sick-lms5xx/parsing.hpp index 75cb5fb..62ff7d4 100644 --- a/include/sick-lms5xx/parsing.hpp +++ b/include/sick-lms5xx/parsing.hpp @@ -231,7 +231,40 @@ bool status_ok(const std::string &cmd_name, int status_code); bool validate_response(const char *data, size_t len); /** - * @brief Parse status from ascii SOPAS response + * @brief Obtain the raw token response from bytes + * + * @param data Bytes from the scanner + * @param len Number of bytes from the scanner + * @param buf Output TokenBuffer to easily access response tokens + * + * @return Error if the response tokens signified an error, ok() if buf is + * valid + */ +SickErr get_response_ascii(const char *data, size_t len, TokenBuffer &buf); + +/** + * @brief Parse the return for most methods, which consists of a command + * followed by a status code + * + * @param data Bytes from the scanner + * @param len Number of bytes from the scanner + * + * @return Status code signifying an error or success + */ +SickErr parse_generic_return(const char *data, size_t len); + +/** + * @brief Parse the generic error code which are emitted when the scanner has a problem unrelated to the exact method. + * + * @param data Bytes from the scanner + * @param len Number of bytes from the scanner + * + * @return Status code signifying an error or success + */ +SickErr parse_generic_error(const char *data, size_t len); + +/** + * @brief Parse status from ascii SOPAS response consisting only of answer method and status code * * @param data Data from scanner * @param len Length of \p data diff --git a/include/sick-lms5xx/sopas.hpp b/include/sick-lms5xx/sopas.hpp index d239afd..2debf81 100644 --- a/include/sick-lms5xx/sopas.hpp +++ b/include/sick-lms5xx/sopas.hpp @@ -12,6 +12,7 @@ namespace sick { using ScanCallback = std::function; ///< Callback type for complete scans +constexpr size_t CMD_BUFLEN = 4096; /** * @brief Class implementing SOPAS protocol abstractions on sockets. @@ -195,8 +196,8 @@ class SOPASProtocolASCII : public SOPASProtocol { int make_command_msg(char *data_out, SOPASCommand cmd, Args... args) { #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wformat-security" - int bytes_written = - std::sprintf(data_out, command_masks_[cmd].c_str(), args...); + int bytes_written = std::snprintf(data_out, CMD_BUFLEN, + command_masks_[cmd].c_str(), args...); #pragma GCC diagnostic pop if (bytes_written < 0) { throw std::runtime_error("sprintf fail"); @@ -215,7 +216,7 @@ class SOPASProtocolASCII : public SOPASProtocol { */ template SickErr send_command(SOPASCommand cmd, Args... args) { - std::array buffer; + std::array buffer; int bytes_written = make_command_msg(buffer.data(), cmd, args...); SickErr result = send_sopas_command_and_check_answer( diff --git a/include/sick-lms5xx/types.hpp b/include/sick-lms5xx/types.hpp index a00171e..111b48e 100644 --- a/include/sick-lms5xx/types.hpp +++ b/include/sick-lms5xx/types.hpp @@ -110,6 +110,7 @@ class SickErr { SickErr(int error_num) : code_(error_num), is_posix_err_(true) {} SickErr(sick_err_t err) : code_(static_cast(err)), is_posix_err_(false) {} + SickErr() : SickErr(static_cast(sick_err_t::Ok)) {} bool ok() const { return is_posix_err_ ? code_ == 0 : code_ == static_cast(sick_err_t::Ok); diff --git a/src/parsing.cpp b/src/parsing.cpp index 7f14088..cdf010c 100644 --- a/src/parsing.cpp +++ b/src/parsing.cpp @@ -228,7 +228,8 @@ bool ScanBatcher::parse_scan_telegram(const std::vector &buffer, if (channels_16bit.size() < 1) { return false; /* throw std::runtime_error(__func__ + */ - /* ": parse_scan_telegram() got no 16bit channels"); */ + /* ": parse_scan_telegram() got no 16bit + * channels"); */ } else { const Channel &range_cn = channels_16bit.front(); if (range_cn.description.find("DIST") == std::string::npos) { @@ -327,33 +328,55 @@ bool validate_response(const char *data, size_t len) { return n_stx == 1 && n_etx == 1; } +SickErr parse_generic_error(const char *data, size_t len) { + // generic errors + static const char pattern[]{"\x02sFA %2X\x03"}; + unsigned int status = 0; + int scanf_result = sscanf(data, pattern, &status); + if (scanf_result != 1) { + return sick_err_t::CustomError; + } + return static_cast(status); +} + +SickErr parse_generic_return(const char *data, size_t len) { + TokenBuffer buf(data, len); + std::string method(buf.next()); + std::string cmd_name(buf.next()); + if (buf.has_next()) { + int status_code = atoi(buf.next()); + if (status_ok(cmd_name, status_code)) { + return sick_err_t::Ok; + } else { + return sick_err_t::CustomErrorCommandFailure; + } + } else { + return sick_err_t::Ok; + } +} + SickErr status_from_bytes_ascii(const char *data, size_t len) { if (!validate_response(data, len)) { return sick_err_t::CustomErrorInvalidDatagram; } const std::string answer_method = method(data, len); if (answer_method == "sFA") { - // generic errors - static const char pattern[]{"\x02sFA %2X\x03"}; - unsigned int status = 0; - int scanf_result = sscanf(data, pattern, &status); - if (scanf_result != 1) { - return sick_err_t::CustomError; - } - return static_cast(status); + return parse_generic_error(data, len); } else { - TokenBuffer buf(data, len); - std::string method(buf.next()); - std::string cmd_name(buf.next()); - if (buf.has_next()) { - int status_code = atoi(buf.next()); - if (status_ok(cmd_name, status_code)) { - return sick_err_t::Ok; - } else { - return sick_err_t::CustomErrorCommandFailure; - } - } else - return sick_err_t::Ok; + return parse_generic_return(data, len); + } +} + +SickErr get_response_ascii(const char *data, size_t len, TokenBuffer &buf) { + if (!validate_response(data, len)) { + return sick_err_t::CustomErrorInvalidDatagram; + } + const std::string answer_method = method(data, len); + if (answer_method == "sFA") { + return parse_generic_error(data, len); + } else { + buf = TokenBuffer(data, len); + return SickErr(); } } diff --git a/src/sopas.cpp b/src/sopas.cpp index e987c24..c4ff8e3 100644 --- a/src/sopas.cpp +++ b/src/sopas.cpp @@ -58,7 +58,7 @@ SOPASProtocol::SOPASProtocol(const std::string &sensor_ip, const uint32_t port, SickErr SOPASProtocol::start_scan() { poller_ = std::thread([&] { - std::vector buffer(2 * 4096); + std::vector buffer(2 * CMD_BUFLEN); while (!stop_.load()) { int read_bytes = uninterrupted_recv(sock_fd_, buffer.data(), buffer.size()); @@ -105,32 +105,43 @@ int send_sopas_command(int sock_fd, const char *data, size_t len) { return uninterrupted_send(sock_fd, data, len); } -SickErr send_sopas_command_and_check_answer(int sock_fd, const char *data, - size_t len) { - int send_result = send_sopas_command(sock_fd, data, len); - if (send_result < 0) { +static SickErr socket2err(const int sock_result) { + if (sock_result < 0) { return SickErr(errno); - } else if (send_result == 0) { + } else if (sock_result == 0) { return sick_err_t::CustomErrorConnectionClosed; + } else { + return SickErr(); + } +} + +SickErr send_sopas_command_and_check_answer(int sock_fd, const char *data, + size_t len) { + const SickErr send_result = + socket2err(send_sopas_command(sock_fd, data, len)); + if (!send_result.ok()) { + return send_result; } - std::array recvbuf; + std::array recvbuf; // fill with 0s so we have a null-terminated string recvbuf.fill(0x00); - int recv_result = receive_sopas_reply(sock_fd, recvbuf.data(), 4096); - if (recv_result < 0) { - return SickErr(errno); - } else if (recv_result == 0) { - return sick_err_t::CustomErrorConnectionClosed; + + const int sock_recv_result = + receive_sopas_reply(sock_fd, recvbuf.data(), CMD_BUFLEN); + const SickErr recv_result = socket2err(sock_recv_result); + if (!recv_result.ok()) { + return recv_result; } - return status_from_bytes_ascii(recvbuf.data(), recv_result); + return status_from_bytes_ascii(recvbuf.data(), sock_recv_result); } SickErr SOPASProtocolASCII::set_access_mode(const uint8_t mode, const uint32_t pw_hash) { - std::array buffer; + std::array buffer; // authorized client mode with pw hash from telegram listing - int bytes_written = std::sprintf( - buffer.data(), command_masks_[SETACCESSMODE].c_str(), mode, pw_hash); + const int bytes_written = + std::snprintf(buffer.data(), CMD_BUFLEN, + command_masks_[SETACCESSMODE].c_str(), mode, pw_hash); if (bytes_written < 0) { return SickErr(errno); } @@ -203,14 +214,14 @@ SickErr SOPASProtocolASCII::run() { void SOPASProtocolASCII::stop(bool stop_laser) { SOPASProtocol::stop(); - std::array buffer; + std::array buffer; int len = make_command_msg(buffer.data(), LMDSCANDATA, 0); int bytes_sent = send_sopas_command(sock_fd_, buffer.data(), len); if (bytes_sent < 0) { throw std::runtime_error("Failed to send."); } while (true) { - int bytes_received = receive_sopas_reply(sock_fd_, &buffer[0], 4096); + int bytes_received = receive_sopas_reply(sock_fd_, &buffer[0], CMD_BUFLEN); std::string answer(&buffer[0], bytes_received); if (answer.find("LMDscandata") != std::string::npos) { SickErr status = status_from_bytes_ascii(buffer.data(), bytes_received);