Retrieve cmd response tokens

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

include/sick-lms5xx/sopas.hpp

@@ -12,6 +12,7 @@ namespace sick {
 
 using ScanCallback =
     std::function<void(const Scan &)>; ///< 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 <typename... Args>
   SickErr send_command(SOPASCommand cmd, Args... args) {
-    std::array<char, 4096> buffer;
+    std::array<char, CMD_BUFLEN> buffer;
     int bytes_written = make_command_msg(buffer.data(), cmd, args...);
 
     SickErr result = send_sopas_command_and_check_answer(

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<int>(err)), is_posix_err_(false) {}
+  SickErr() : SickErr(static_cast<int>(sick_err_t::Ok)) {}
   bool ok() const {
     return is_posix_err_ ? code_ == 0
                          : code_ == static_cast<int>(sick_err_t::Ok);

src/parsing.cpp

@@ -228,7 +228,8 @@ bool ScanBatcher::parse_scan_telegram(const std::vector<char> &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<sick_err_t>(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<sick_err_t>(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();
   }
 }
 

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<char> buffer(2 * 4096);
+    std::vector<char> 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<char, 4096> recvbuf;
+  std::array<char, CMD_BUFLEN> 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<char, 128> buffer;
+  std::array<char, CMD_BUFLEN> 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<char, 4096> buffer;
+  std::array<char, CMD_BUFLEN> 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);