diff --git a/cmake/SickLMS5xxConfig.cmake.in b/cmake/SickLMS5xxConfig.cmake.in index 2893013..a70ef51 100644 --- a/cmake/SickLMS5xxConfig.cmake.in +++ b/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 $) diff --git a/include/sick-lms5xx/parsing.hpp b/include/sick-lms5xx/parsing.hpp index ea8f1c0..fad9d10 100644 --- a/include/sick-lms5xx/parsing.hpp +++ b/include/sick-lms5xx/parsing.hpp @@ -191,7 +191,8 @@ enum SOPASCommand { RUN, LMDSCANDATA, LMCSTOPMEAS, - LMCSTARTMEAS + LMCSTARTMEAS, + GETSCANCONFIG }; /** diff --git a/include/sick-lms5xx/sopas.hpp b/include/sick-lms5xx/sopas.hpp index f900f89..5c07530 100644 --- a/include/sick-lms5xx/sopas.hpp +++ b/include/sick-lms5xx/sopas.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -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; /** @@ -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 ¶ms) override; diff --git a/python/sick.py b/python/sick.py index 051f8c6..3d254f2 100644 --- a/python/sick.py +++ b/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" diff --git a/src/parsing.cpp b/src/parsing.cpp index 7f14088..0e74490 100644 --- a/src/parsing.cpp +++ b/src/parsing.cpp @@ -1,5 +1,6 @@ #include #include +#include using namespace std; diff --git a/src/sopas.cpp b/src/sopas.cpp index 84b3dea..0cf18bc 100644 --- a/src/sopas.cpp +++ b/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 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 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::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 buffer;