AK-1282_sick getconfig libsick

cmake/SickLMS5xxConfig.cmake.in

@@ -12,7 +12,7 @@ else()
 endif()
 set(SickLMS5xx_LIBRARIES ${SickLMS5xx_LIBRARY})
 
-# Souce the target definitions if this package is used as a binary rather than built
+# Source the target definitions if this package is used as a binary rather than built
 # alongside the client project. CMAKE_CURRENT_LIST_DIR in that context will be the dir
 # where this package installs its cmake files (aka INSTALL_CONFIGDIR in main
 # CMakeLists.txt)
@@ -26,4 +26,4 @@ find_dependency(Eigen3 3.3.4 REQUIRED)
 find_dependency(PCL 1.11 REQUIRED COMPONENTS common io)
 # link the deps so that client projects get them also
 target_link_libraries(SickLMS5xx::SickLMS5xx INTERFACE Eigen3::Eigen pcl_common pcl_io)
-target_include_directories(SickLMS5xx::SickLMS5xx SYSTEM INTERFACE ${SickLMS5xx_INCLUDE_DIR})
+target_include_directories(SickLMS5xx::SickLMS5xx INTERFACE $<INSTALL_INTERFACE:include>)

include/sick-lms5xx/parsing.hpp

@@ -191,7 +191,8 @@ enum SOPASCommand {
   RUN,
   LMDSCANDATA,
   LMCSTOPMEAS,
-  LMCSTARTMEAS
+  LMCSTARTMEAS,
+  GETSCANCONFIG
 };
 
 /**

include/sick-lms5xx/sopas.hpp

@@ -5,6 +5,7 @@
 #include <memory>
 #include <sick-lms5xx/network.hpp>
 #include <sick-lms5xx/parsing.hpp>
+#include <string>
 #include <thread>
 #include <unistd.h>
 
@@ -27,9 +28,8 @@ class SOPASProtocol {
   std::thread poller_;     ///< scanner polling thread
   ScanBatcher batcher_;    ///< batcher for partial telegrams
 
-  int sock_fd_; ///< socket file descriptor
-
 public:
+  int sock_fd_; ///< socket file descriptor
   using SOPASProtocolPtr = std::shared_ptr<SOPASProtocol>;
 
   /**
@@ -71,6 +71,8 @@ class SOPASProtocol {
    */
   virtual SickErr configure_ntp_client(const std::string &ip) = 0;
 
+  virtual std::string send_raw_command(SOPASCommand cmd) = 0;
+
   /**
    * @brief Set new scan configuration
    *
@@ -166,9 +168,10 @@ class SOPASProtocolASCII : public SOPASProtocol {
       {RUN, "\x02sMN Run\x03"},
       {LMDSCANDATA, "\x02sEN LMDscandata %u\x03"},
       {LMCSTOPMEAS, "\x02sMN LMCstopmeas\x03"},
-      {LMCSTARTMEAS,
-       "\x02sMN LMCstartmeas\x03"}}; ///<    map from commands to format strings
-                                     ///<    to fill arguments into
+      {LMCSTARTMEAS, "\x02sMN LMCstartmeas\x03"},
+      {GETSCANCONFIG,
+       "\x02sRN LMPscancfg\x03"}}; ///<    map from commands to format strings
+                                   ///<    to fill arguments into
 
 public:
   SickErr set_access_mode(const uint8_t mode = 3,
@@ -216,6 +219,8 @@ class SOPASProtocolASCII : public SOPASProtocol {
     return result;
   }
 
+  std::string send_raw_command(SOPASCommand cmd) override;
+
   SickErr configure_ntp_client(const std::string &ip) override;
 
   SickErr set_scan_config(const lms5xx::LMSConfigParams &params) override;

python/sick.py

@@ -244,6 +244,9 @@ def main():
     storeparams_cmd = "\x02sMN mEEwriteall\x03"
     reply = send_command(storeparams_cmd)
 
+    get_params_cmd = "\x02sRN LMPscancfg\x03"
+    reply = send_command(get_params_cmd)
+
     run_cmd = "\x02sMN Run\x03"
     reply = send_command(run_cmd)
     # stop_cmd = "\x02sMN LMCstopmeas\x03"

src/parsing.cpp

@@ -1,5 +1,6 @@
 #include <iostream>
 #include <sick-lms5xx/parsing.hpp>
+#include <string>
 
 using namespace std;
 

src/sopas.cpp

@@ -125,6 +125,63 @@ SickErr send_sopas_command_and_check_answer(int sock_fd, const char *data,
   return status_from_bytes_ascii(recvbuf.data(), recv_result);
 }
 
+std::string SOPASProtocolASCII::send_raw_command(SOPASCommand cmd) {
+  std::cout << "buffer" << std::endl;
+  std::array<char, 4096> buffer_w;
+  std::cout << "buffer created" << std::endl;
+  std::string out_str = "\x02sRN LMPscancfg\x03";
+  int bytes_written = std::sprintf(buffer_w.data(), out_str.c_str(), 0);
+  // int bytes_written = make_command_msg(buffer_w.data(), cmd, 0);
+  std::cout << "bytes written " << std::endl;
+
+  // int send_result =
+  //     send_sopas_command(this->sock_fd_, buffer_w.data(), bytes_written);
+
+  int send_result;
+  while ((send_result =
+              send(this->sock_fd_, buffer_w.data(), bytes_written, 0)) == -1 &&
+         errno == EINTR) {
+    continue;
+  }
+  std::cout << "send result " << std::endl;
+  if (send_result < 0) {
+    return "SEND RESULT < 0";
+  } else if (send_result == 0) {
+    return "SEND RESULT = 0";
+  }
+
+  std::cout << "create rcv buffer " << std::endl;
+  int len = 4096;
+  std::array<char, 4096> recvbuf;
+  recvbuf.fill(0x00);
+
+  std::cout << "rcv result " << std::endl;
+
+  int recv_result;
+  while ((recv_result = recv(this->sock_fd_, recvbuf.data(), len, 0)) == -1 &&
+         errno == EINTR) {
+    continue;
+  }
+  if (recv_result < 0) {
+    return "RCV RESULT < 0";
+  } else if (recv_result == 0) {
+    return "RCV RESULT = 0";
+  }
+  auto data = recvbuf.data();
+  std::array<char, 4096>::value_type *ptr = data;
+  std::cout << "rcv result finished printing .... :  " << std::endl;
+
+  for (int i = 0; i < buffer_w.size(); ++i) {
+    std::cout << ptr[i];
+  }
+  if (!validate_response(data, len)) {
+    return "No valid response";
+  }
+  std::string answer_method = method(data, len);
+
+  return data;
+}
+
 SickErr SOPASProtocolASCII::set_access_mode(const uint8_t mode,
                                             const uint32_t pw_hash) {
   std::array<char, 128> buffer;