From ca21d541dc64ff2b2c2be25e5a3541279b9f1c09 Mon Sep 17 00:00:00 2001 From: DudekW Date: Thu, 26 Jan 2017 14:52:17 +0100 Subject: [PATCH 1/5] add synchronization feature --- CMakeLists.txt | 4 +- include/LMS1xx/LMS1xx.h | 9 ++ include/LMS1xx/lms_structs.h | 79 ++++++++++++++++ launch/LMS1xx.launch | 19 +++- src/LMS1xx.cpp | 168 ++++++++++++++++++++++++++++++++++ src/LMS1xx_NTP_diagnostic.cpp | 60 ++++++++++++ src/LMS1xx_node.cpp | 60 ++++++------ 7 files changed, 367 insertions(+), 32 deletions(-) create mode 100644 src/LMS1xx_NTP_diagnostic.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c8dedc7..ce6d146 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,8 +15,10 @@ include_directories(include ${catkin_INCLUDE_DIRS}) add_executable(LMS1xx_node src/LMS1xx_node.cpp) target_link_libraries(LMS1xx_node LMS1xx ${catkin_LIBRARIES}) +add_executable(LMS1xx_NTP_diagnostic src/LMS1xx_NTP_diagnostic.cpp) +target_link_libraries(LMS1xx_NTP_diagnostic LMS1xx ${catkin_LIBRARIES}) -install(TARGETS LMS1xx LMS1xx_node +install(TARGETS LMS1xx LMS1xx_node LMS1xx_NTP_diagnostic ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/include/LMS1xx/LMS1xx.h b/include/LMS1xx/LMS1xx.h index d29ed78..5cbbbef 100644 --- a/include/LMS1xx/LMS1xx.h +++ b/include/LMS1xx/LMS1xx.h @@ -61,6 +61,15 @@ class LMS1xx */ void connect(std::string host, int port = 2111); + /*! + * @brief get max time offset and delay. + */ + NTPstatus getNTPstatus() const; + + /*! + * @brief set NTP settings. + */ + void setNTPsettings(const NTPcfg &cfg); /*! * @brief Disconnect from LMS1xx device. */ diff --git a/include/LMS1xx/lms_structs.h b/include/LMS1xx/lms_structs.h index 7c32367..4b33b1e 100644 --- a/include/LMS1xx/lms_structs.h +++ b/include/LMS1xx/lms_structs.h @@ -59,6 +59,67 @@ struct scanCfg int stopAngle; }; +/*! +* @class NTProleCfg +* @brief Structure containing NTP role configuration. +* +* @author Wojciech Dudek +*/ + +struct NTPcfg +{ + /*! + * @brief NTP role. + * None: 0 + * Client: 1 + * Server: 2 + */ + int NTProle; + /*! + * @brief Time synchronization interface. + * Ethernet: 0 + * CAN: 1 + */ + int timeSyncIfc; + /*! + * @brief Time server IP address. + * [0].[1].[2].[3] + */ + int serverIP[4]; + /*! + * @brief Time zone. + * Set values in number of hours relative to GMT, hex specially coded + * [GMT + …] -12d … +12d (00h … 18h) + */ + int timeZone; + /*! + * @brief Update time. + * Set values in seconds + */ + int updateTime; +}; + +/*! +* @class NTProleCfg +* @brief Structure containing NTP status. +* +* @author Wojciech Dudek +*/ + +struct NTPstatus +{ + /*! + * @brief Read maximum offset time. + * [Seconds as float according to IEEE754] + */ + float maxOffsetNTP; + /*! + * @brief Delay time. + * [Seconds as float according to IEEE754] + */ + float timeDelay; +}; + /*! * @class scanDataCfg * @brief Structure containing scan data configuration. @@ -201,6 +262,24 @@ struct scanData * */ uint16_t rssi2[1082]; + + /*! + * @brief Time stamp of the msg - seconds + * + */ + time_t msg_sec; + + /*! + * @brief Time stamp of the msg - micro seconds + * + */ + uint32_t msg_usec; + + /*! + * @brief Time since start up - micro seconds + * + */ + uint32_t msg_startup_usec; }; #endif // LMS1XX_LMS_STRUCTS_H_ diff --git a/launch/LMS1xx.launch b/launch/LMS1xx.launch index 105075a..3846de8 100644 --- a/launch/LMS1xx.launch +++ b/launch/LMS1xx.launch @@ -1,6 +1,19 @@ - - - + + + + + + + + + + + + + + + + diff --git a/src/LMS1xx.cpp b/src/LMS1xx.cpp index f61d45d..5bd14d3 100644 --- a/src/LMS1xx.cpp +++ b/src/LMS1xx.cpp @@ -36,8 +36,11 @@ #include "LMS1xx/LMS1xx.h" #include "console_bridge/console.h" +#include + LMS1xx::LMS1xx() : connected_(false) { + //console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_DEBUG); } LMS1xx::~LMS1xx() @@ -69,6 +72,78 @@ void LMS1xx::connect(std::string host, int port) } } +NTPstatus LMS1xx::getNTPstatus() const +{ + NTPstatus ntpStatus; + char buf[100]; + uint32_t bytes; + + // get max offset + sprintf(buf, "%c%s%c", 0x02, "sRN TSCTCmaxoffset", 0x03); + write(socket_fd_, buf, strlen(buf)); + int len = read(socket_fd_, buf, 100); + if (buf[0] != 0x02) + logWarn("invalid packet recieved"); + buf[len-1] = 0; + logDebug("RX: %s", buf); + sscanf((buf + 20), "%X", &bytes); + ntpStatus.maxOffsetNTP = *((float*)&bytes); + + // get dealy time + sprintf(buf, "%c%s%c", 0x02, "sRN TSCTCdelay", 0x03); + write(socket_fd_, buf, strlen(buf)); + len = read(socket_fd_, buf, 100); + if (buf[0] != 0x02) + logWarn("invalid packet recieved"); + buf[len-1] = 0; + logDebug("RX: %s", buf); + sscanf((buf + 16), "%X", &bytes); + + ntpStatus.timeDelay = *((float*)&bytes); + + return ntpStatus; +} + +void LMS1xx::setNTPsettings(const NTPcfg &cfg) +{ + char buf[100]; + // send NTP role + sprintf(buf, "%c%s %d%c", 0x02, + "sWN TSCRole", cfg.NTProle, 0x03); + logDebug("TX: %s", buf); + write(socket_fd_, buf, strlen(buf)); + int len = read(socket_fd_, buf, 100); + buf[len - 1] = 0; + // send time synchronization interface + sprintf(buf, "%c%s %d%c", 0x02, + "sWN TSCTCInterface", cfg.timeSyncIfc, 0x03); + logDebug("TX: %s", buf); + write(socket_fd_, buf, strlen(buf)); + len = read(socket_fd_, buf, 100); + buf[len - 1] = 0; + // send time server IP address c0 a8 0 1 + sprintf(buf, "%c%s %X %X %X %X%c", 0x02, + "sWN TSCTCSrvAddr", cfg.serverIP[0], cfg.serverIP[1], cfg.serverIP[2], cfg.serverIP[3], 0x03); + logDebug("TX: %s", buf); + write(socket_fd_, buf, strlen(buf)); + len = read(socket_fd_, buf, 100); + buf[len - 1] = 0; + // send time zone + sprintf(buf, "%c%s %X%c", 0x02, + "sWN TSCTCtimezone",cfg.timeZone+12, 0x03); + logDebug("TX: %s", buf); + write(socket_fd_, buf, strlen(buf)); + len = read(socket_fd_, buf, 100); + buf[len - 1] = 0; + // send update time + sprintf(buf, "%c%s +%d%c", 0x02, + "sWN TSCTCupdatetime", cfg.updateTime, 0x03); + logDebug("TX: %s", buf); + write(socket_fd_, buf, strlen(buf)); + len = read(socket_fd_, buf, 100); + buf[len - 1] = 0; +} + void LMS1xx::disconnect() { if (connected_) @@ -293,6 +368,11 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) tok = strtok(NULL, " "); //ScanCounter tok = strtok(NULL, " "); //PowerUpDuration tok = strtok(NULL, " "); //TransmissionDuration + uint32_t PowerUpDuration; + sscanf(tok, "%X", &PowerUpDuration); + logDebug("PowerUpDuration : %d\n", PowerUpDuration); + data->msg_startup_usec = PowerUpDuration; + tok = strtok(NULL, " "); //InputStatus tok = strtok(NULL, " "); //OutputStatus tok = strtok(NULL, " "); //ReservedByteA @@ -465,6 +545,94 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) } } } + + tok = strtok(NULL, " "); //PositionDataEnable + int PositionDataEnable; + sscanf(tok, "%d", &PositionDataEnable); + logDebug("NumberChannels8Bit : %d\n", PositionDataEnable); + if(PositionDataEnable == 1){ + tok = strtok(NULL, " "); //X position + tok = strtok(NULL, " "); //Y position + tok = strtok(NULL, " "); //Z position + tok = strtok(NULL, " "); //X rotation + tok = strtok(NULL, " "); //Y rotation + tok = strtok(NULL, " "); //Z rotation + tok = strtok(NULL, " "); //rotations type + tok = strtok(NULL, " "); //Transmits the name of device + } + + tok = strtok(NULL, " "); //NameEnable + int NameEnable; + sscanf(tok, "%d", &NameEnable); + logDebug("NameEnable : %d\n", NameEnable); + if(NameEnable == 1){ + tok = strtok(NULL, " "); //Length of name + tok = strtok(NULL, " "); //Device name in characters + } + + tok = strtok(NULL, " "); //CommentEnable + int CommentEnable; + sscanf(tok, "%d", &CommentEnable); + logDebug("CommentEnable : %d\n", CommentEnable); + if(CommentEnable == 1){ + tok = strtok(NULL, " "); //Length of comment + tok = strtok(NULL, " "); //Transmits a comment in characters + } + + tok = strtok(NULL, " "); //TimeEnable + int TimeEnable; + sscanf(tok, "%d", &TimeEnable); + logDebug("TimeEnable : %d\n", TimeEnable); + if(TimeEnable == 1){ + struct tm msg_time = {0}; + //Year + uint16_t year; + tok = strtok(NULL, " "); + sscanf(tok, "%hX", &year); + msg_time.tm_year = (int) year - 1900; + logDebug("NTP year: %hX\n", year); + logDebug("NTP year: %d\n", msg_time.tm_year); + // Month + uint8_t int_8_buf; + tok = strtok(NULL, " "); + sscanf(tok, "%hhX", &int_8_buf); + msg_time.tm_mon = (int) int_8_buf - 1; + logDebug("NTP month: %hhX\n", int_8_buf); + + // Day + tok = strtok(NULL, " "); + sscanf(tok, "%hhX", &int_8_buf); + msg_time.tm_mday = (int) int_8_buf; + logDebug("NTP day: %hhX\n", int_8_buf); + + // Hour + tok = strtok(NULL, " "); + sscanf(tok, "%hhX", &int_8_buf); + msg_time.tm_hour = (int) int_8_buf; + logDebug("NTP hour: %hhX\n", int_8_buf); + + // Minute + tok = strtok(NULL, " "); + sscanf(tok, "%hhX", &int_8_buf); + msg_time.tm_min = (int) int_8_buf; + logDebug("NTP minute: %hhX\n", int_8_buf); + + // Second + tok = strtok(NULL, " "); + sscanf(tok, "%hhX", &int_8_buf); + msg_time.tm_sec = (int) int_8_buf; + data->msg_sec = mktime(&msg_time); + logDebug("NTP sec: %lld\n", (long long) data->msg_sec); + + // Microsecond + uint32_t microsecond; + tok = strtok(NULL, " "); + sscanf(tok, "%X", µsecond); + data->msg_usec = microsecond ; + logDebug("NTP usec: %X\n", microsecond); + } + + } void LMS1xx::saveConfig() diff --git a/src/LMS1xx_NTP_diagnostic.cpp b/src/LMS1xx_NTP_diagnostic.cpp new file mode 100644 index 0000000..a55e4d8 --- /dev/null +++ b/src/LMS1xx_NTP_diagnostic.cpp @@ -0,0 +1,60 @@ +/* + * LMS1xx_NTP_diagnostic.cpp + * + * Created on: 24-01-2017 + * Author: Wojciech Dudek + *************************************************************************** + * This library is free software; you can redistribute it and/or * + * modify it under the terms of the GNU Lesser General Public * + * License as published by the Free Software Foundation; either * + * version 2.1 of the License, or (at your option) any later version. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * + * Lesser General Public License for more details. * + * * + * You should have received a copy of the GNU Lesser General Public * + * License along with this library; if not, write to the Free Software * + * Foundation, Inc., 59 Temple Place, * + * Suite 330, Boston, MA 02111-1307 USA * + * * + ***************************************************************************/ + +#include +#include "console_bridge/console.h" +#include + +int main(int argc, char** argv){ + + LMS1xx laser; + NTPstatus ntp_status; + scanCfg cfg; + scanOutputRange outputRange; + std::string host = "192.168.0.20"; + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_INFO); + + if (argc == 2){ + host = argv[1]; + }else{ + logWarn("\nUsage: LMS1xx_NTP_diagnostic \nUsing default: %s",host.c_str()); + } + + laser.connect(host); + while (!laser.isConnected()){ + logWarn("Unable to connect, retrying."); + sleep(1); + continue; + } + // get NTP stats + ntp_status = laser.getNTPstatus(); + // get scan params + cfg = laser.getScanCfg(); + outputRange = laser.getScanOutputRange(); + logInform("Laser configuration: scaningFrequency %d, angleResolution %d, startAngle %d, stopAngle %d", + cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle, cfg.stopAngle); + logInform("Laser output range: angleResolution %d, startAngle %d, stopAngle %d", + outputRange.angleResolution, outputRange.startAngle, outputRange.stopAngle); + logInform("Max NTP offset: %f", ntp_status.maxOffsetNTP); + logInform("NTP time delay: %f", ntp_status.timeDelay); +} \ No newline at end of file diff --git a/src/LMS1xx_node.cpp b/src/LMS1xx_node.cpp index b382043..e0082b0 100644 --- a/src/LMS1xx_node.cpp +++ b/src/LMS1xx_node.cpp @@ -26,7 +26,6 @@ #include #include "ros/ros.h" #include "sensor_msgs/LaserScan.h" - #define DEG2RAD M_PI/180.0 int main(int argc, char **argv) @@ -34,6 +33,7 @@ int main(int argc, char **argv) // laser data LMS1xx laser; scanCfg cfg; + NTPcfg cfg_ntp; scanOutputRange outputRange; scanDataCfg dataCfg; sensor_msgs::LaserScan scan_msg; @@ -42,6 +42,7 @@ int main(int argc, char **argv) std::string host; std::string frame_id; + ros::init(argc, argv, "lms1xx"); ros::NodeHandle nh; ros::NodeHandle n("~"); @@ -78,7 +79,7 @@ int main(int argc, char **argv) ROS_DEBUG("Laser configuration: scaningFrequency %d, angleResolution %d, startAngle %d, stopAngle %d", cfg.scaningFrequency, cfg.angleResolution, cfg.startAngle, cfg.stopAngle); - ROS_DEBUG("Laser output range:angleResolution %d, startAngle %d, stopAngle %d", + ROS_DEBUG("Laser output range: angleResolution %d, startAngle %d, stopAngle %d", outputRange.angleResolution, outputRange.startAngle, outputRange.stopAngle); scan_msg.header.frame_id = frame_id; @@ -116,18 +117,39 @@ int main(int argc, char **argv) dataCfg.position = false; dataCfg.deviceName = false; dataCfg.outputInterval = 1; + dataCfg.timestamp = true; ROS_DEBUG("Setting scan data configuration."); laser.setScanDataCfg(dataCfg); + // check if useNTP flag is set true + bool useNTP_flag; + n.param("useNTP", useNTP_flag, false); + if (useNTP_flag){ + ROS_DEBUG("Setting NTP configuration."); + // get NTP params from ROS + std::string IPstring; + n.param("NTProle", cfg_ntp.NTProle, 1); + n.param("timeSyncIfc", cfg_ntp.timeSyncIfc, 0); + n.param("serverIP", IPstring, "192.168.0.1"); + char * dup = strdup(IPstring.c_str()); + char * token = strtok(dup, "."); + for(int i = 0; i < 4; i++){ + cfg_ntp.serverIP[i] = std::atoi( token ); + token = strtok(NULL, "."); + } + free(dup); + n.param("timeZone", cfg_ntp.timeZone , 1); + n.param("updateTime", cfg_ntp.updateTime, 10); + ROS_DEBUG("Setting NTP configuration."); + laser.setNTPsettings(cfg_ntp); + } ROS_DEBUG("Starting measurements."); laser.startMeas(); ROS_DEBUG("Waiting for ready status."); ros::Time ready_status_timeout = ros::Time::now() + ros::Duration(5); - //while(1) - //{ status_t stat = laser.queryStatus(); ros::Duration(1.0).sleep(); if (stat != ready_for_measurement) @@ -137,25 +159,6 @@ int main(int argc, char **argv) ros::Duration(1).sleep(); continue; } - /*if (stat == ready_for_measurement) - { - ROS_DEBUG("Ready status achieved."); - break; - } - - if (ros::Time::now() > ready_status_timeout) - { - ROS_WARN("Timed out waiting for ready status. Trying again."); - laser.disconnect(); - continue; - } - - if (!ros::ok()) - { - laser.disconnect(); - return 1; - } - }*/ ROS_DEBUG("Starting device."); laser.startDevice(); // Log out to properly re-enable system after config @@ -169,7 +172,6 @@ int main(int argc, char **argv) scan_msg.header.stamp = start; ++scan_msg.header.seq; - scanData data; ROS_DEBUG("Reading scan data."); if (laser.getScanData(&data)) @@ -183,7 +185,12 @@ int main(int argc, char **argv) { scan_msg.intensities[i] = data.rssi1[i]; } - + // check if useNTP flag is set true + if (useNTP_flag){ + ROS_DEBUG("Publishing NTP time stamp."); + scan_msg.header.stamp.sec = data.msg_sec; + scan_msg.header.stamp.nsec = data.msg_usec*1000; + } ROS_DEBUG("Publishing scan data."); scan_pub.publish(scan_msg); } @@ -192,14 +199,11 @@ int main(int argc, char **argv) ROS_ERROR("Laser timed out on delivering scan, attempting to reinitialize."); break; } - ros::spinOnce(); } - laser.scanContinous(0); laser.stopMeas(); laser.disconnect(); } - return 0; } From a99477456783f2d68d1a843f03aa9d2ded2f916e Mon Sep 17 00:00:00 2001 From: DudekW Date: Mon, 29 May 2017 17:13:15 +0200 Subject: [PATCH 2/5] follow catkin naming convention --- CMakeLists.txt | 9 ++++++++- include/{LMS1xx => lms1xx}/LMS1xx.h | 4 ++-- include/{LMS1xx => lms1xx}/lms_buffer.h | 0 include/{LMS1xx => lms1xx}/lms_structs.h | 0 src/LMS1xx.cpp | 2 +- src/LMS1xx_NTP_diagnostic.cpp | 2 +- src/LMS1xx_node.cpp | 2 +- 7 files changed, 13 insertions(+), 6 deletions(-) rename include/{LMS1xx => lms1xx}/LMS1xx.h (98%) rename include/{LMS1xx => lms1xx}/lms_buffer.h (100%) rename include/{LMS1xx => lms1xx}/lms_structs.h (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index ce6d146..37e5dea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,10 @@ target_link_libraries(LMS1xx ${console_bridge_LIBRARIES}) # Regular catkin package follows. find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs) -catkin_package(CATKIN_DEPENDS roscpp) +catkin_package( +INCLUDE_DIRS + include +CATKIN_DEPENDS roscpp) include_directories(include ${catkin_INCLUDE_DIRS}) add_executable(LMS1xx_node src/LMS1xx_node.cpp) @@ -23,6 +26,10 @@ install(TARGETS LMS1xx LMS1xx_node LMS1xx_NTP_diagnostic LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +install(DIRECTORY include/lms1xx/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE) install(DIRECTORY meshes launch urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/include/LMS1xx/LMS1xx.h b/include/lms1xx/LMS1xx.h similarity index 98% rename from include/LMS1xx/LMS1xx.h rename to include/lms1xx/LMS1xx.h index 5cbbbef..267bb88 100644 --- a/include/LMS1xx/LMS1xx.h +++ b/include/lms1xx/LMS1xx.h @@ -24,8 +24,8 @@ #ifndef LMS1XX_H_ #define LMS1XX_H_ -#include -#include +#include +#include #include #include diff --git a/include/LMS1xx/lms_buffer.h b/include/lms1xx/lms_buffer.h similarity index 100% rename from include/LMS1xx/lms_buffer.h rename to include/lms1xx/lms_buffer.h diff --git a/include/LMS1xx/lms_structs.h b/include/lms1xx/lms_structs.h similarity index 100% rename from include/LMS1xx/lms_structs.h rename to include/lms1xx/lms_structs.h diff --git a/src/LMS1xx.cpp b/src/LMS1xx.cpp index 5bd14d3..d273148 100644 --- a/src/LMS1xx.cpp +++ b/src/LMS1xx.cpp @@ -33,7 +33,7 @@ #include #include -#include "LMS1xx/LMS1xx.h" +#include "lms1xx/LMS1xx.h" #include "console_bridge/console.h" #include diff --git a/src/LMS1xx_NTP_diagnostic.cpp b/src/LMS1xx_NTP_diagnostic.cpp index a55e4d8..a8d2f34 100644 --- a/src/LMS1xx_NTP_diagnostic.cpp +++ b/src/LMS1xx_NTP_diagnostic.cpp @@ -21,7 +21,7 @@ * * ***************************************************************************/ -#include +#include #include "console_bridge/console.h" #include diff --git a/src/LMS1xx_node.cpp b/src/LMS1xx_node.cpp index e0082b0..663b7c5 100644 --- a/src/LMS1xx_node.cpp +++ b/src/LMS1xx_node.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include "ros/ros.h" #include "sensor_msgs/LaserScan.h" #define DEG2RAD M_PI/180.0 From 35f12e7f326164b81ac8610bb7aaea05874e6bcc Mon Sep 17 00:00:00 2001 From: DudekW Date: Mon, 29 May 2017 17:51:22 +0200 Subject: [PATCH 3/5] add library to catkin_pkg config --- CMakeLists.txt | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 37e5dea..230d6bd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,24 +4,26 @@ project(lms1xx) # Build ROS-independent library. find_package(console_bridge REQUIRED) include_directories(include ${console_bridge_INCLUDE_DIRS}) -add_library(LMS1xx src/LMS1xx.cpp) -target_link_libraries(LMS1xx ${console_bridge_LIBRARIES}) +add_library(lms1xx src/LMS1xx.cpp) +target_link_libraries(lms1xx ${console_bridge_LIBRARIES}) # Regular catkin package follows. find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs) catkin_package( INCLUDE_DIRS include +LIBRARIES + lms1xx CATKIN_DEPENDS roscpp) include_directories(include ${catkin_INCLUDE_DIRS}) add_executable(LMS1xx_node src/LMS1xx_node.cpp) -target_link_libraries(LMS1xx_node LMS1xx ${catkin_LIBRARIES}) +target_link_libraries(LMS1xx_node lms1xx ${catkin_LIBRARIES}) add_executable(LMS1xx_NTP_diagnostic src/LMS1xx_NTP_diagnostic.cpp) -target_link_libraries(LMS1xx_NTP_diagnostic LMS1xx ${catkin_LIBRARIES}) +target_link_libraries(LMS1xx_NTP_diagnostic lms1xx ${catkin_LIBRARIES}) -install(TARGETS LMS1xx LMS1xx_node LMS1xx_NTP_diagnostic +install(TARGETS lms1xx LMS1xx_node LMS1xx_NTP_diagnostic ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} From 60422b553598542bf6329b681db699b81828f68d Mon Sep 17 00:00:00 2001 From: Wojciech Date: Mon, 26 Jun 2017 21:20:12 +0200 Subject: [PATCH 4/5] fix CMakeLists instructions order --- CMakeLists.txt | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 230d6bd..51eda66 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,11 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(lms1xx) -# Build ROS-independent library. + find_package(console_bridge REQUIRED) -include_directories(include ${console_bridge_INCLUDE_DIRS}) -add_library(lms1xx src/LMS1xx.cpp) -target_link_libraries(lms1xx ${console_bridge_LIBRARIES}) # Regular catkin package follows. find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs) @@ -22,6 +19,10 @@ target_link_libraries(LMS1xx_node lms1xx ${catkin_LIBRARIES}) add_executable(LMS1xx_NTP_diagnostic src/LMS1xx_NTP_diagnostic.cpp) target_link_libraries(LMS1xx_NTP_diagnostic lms1xx ${catkin_LIBRARIES}) +# Build ROS-independent library. +include_directories(include ${console_bridge_INCLUDE_DIRS}) +add_library(lms1xx SHARED src/LMS1xx.cpp) +target_link_libraries(lms1xx ${console_bridge_LIBRARIES}) install(TARGETS lms1xx LMS1xx_node LMS1xx_NTP_diagnostic ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} From 80ee76568a16beff33db18a59446637ada232f0b Mon Sep 17 00:00:00 2001 From: DudekW Date: Tue, 18 Jul 2017 19:22:50 +0200 Subject: [PATCH 5/5] use thread-safe strtok_r --- include/lms1xx/LMS1xx.h | 4 +- src/LMS1xx.cpp | 124 ++++++++++++++++++++-------------------- 2 files changed, 66 insertions(+), 62 deletions(-) diff --git a/include/lms1xx/LMS1xx.h b/include/lms1xx/LMS1xx.h index 267bb88..da2ffae 100644 --- a/include/lms1xx/LMS1xx.h +++ b/include/lms1xx/LMS1xx.h @@ -176,11 +176,13 @@ class LMS1xx * @brief Receive single scan message. * @param data pointer to scanData buffer structure. */ - static void parseScanData(char* buf, scanData* data); + void parseScanData(char* buf, scanData* data); +private: bool connected_; LMSBuffer buffer_; int socket_fd_; + char* strtok_saveptr; }; #endif /* LMS1XX_H_ */ diff --git a/src/LMS1xx.cpp b/src/LMS1xx.cpp index d273148..d555283 100644 --- a/src/LMS1xx.cpp +++ b/src/LMS1xx.cpp @@ -339,7 +339,7 @@ bool LMS1xx::getScanData(scanData* scan_data) // Will return pointer if a complete message exists in the buffer, // otherwise will return null. char* buffer_data = buffer_.getNextBuffer(); - + if (buffer_data) { parseScanData(buffer_data, scan_data); @@ -358,39 +358,40 @@ bool LMS1xx::getScanData(scanData* scan_data) void LMS1xx::parseScanData(char* buffer, scanData* data) { - char* tok = strtok(buffer, " "); //Type of command - tok = strtok(NULL, " "); //Command - tok = strtok(NULL, " "); //VersionNumber - tok = strtok(NULL, " "); //DeviceNumber - tok = strtok(NULL, " "); //Serial number - tok = strtok(NULL, " "); //DeviceStatus - tok = strtok(NULL, " "); //MessageCounter - tok = strtok(NULL, " "); //ScanCounter - tok = strtok(NULL, " "); //PowerUpDuration - tok = strtok(NULL, " "); //TransmissionDuration + + char* tok = strtok_r(buffer, " ",&strtok_saveptr); //Type of command + tok = strtok_r(NULL, " ",&strtok_saveptr); //Command + tok = strtok_r(NULL, " ",&strtok_saveptr); //VersionNumber + tok = strtok_r(NULL, " ",&strtok_saveptr); //DeviceNumber + tok = strtok_r(NULL, " ",&strtok_saveptr); //Serial number + tok = strtok_r(NULL, " ",&strtok_saveptr); //DeviceStatus + tok = strtok_r(NULL, " ",&strtok_saveptr); //MessageCounter + tok = strtok_r(NULL, " ",&strtok_saveptr); //ScanCounter + tok = strtok_r(NULL, " ",&strtok_saveptr); //PowerUpDuration + tok = strtok_r(NULL, " ",&strtok_saveptr); //TransmissionDuration uint32_t PowerUpDuration; sscanf(tok, "%X", &PowerUpDuration); logDebug("PowerUpDuration : %d\n", PowerUpDuration); data->msg_startup_usec = PowerUpDuration; - tok = strtok(NULL, " "); //InputStatus - tok = strtok(NULL, " "); //OutputStatus - tok = strtok(NULL, " "); //ReservedByteA - tok = strtok(NULL, " "); //ScanningFrequency - tok = strtok(NULL, " "); //MeasurementFrequency - tok = strtok(NULL, " "); - tok = strtok(NULL, " "); - tok = strtok(NULL, " "); - tok = strtok(NULL, " "); //NumberEncoders + tok = strtok_r(NULL, " ",&strtok_saveptr); //InputStatus + tok = strtok_r(NULL, " ",&strtok_saveptr); //OutputStatus + tok = strtok_r(NULL, " ",&strtok_saveptr); //ReservedByteA + tok = strtok_r(NULL, " ",&strtok_saveptr); //ScanningFrequency + tok = strtok_r(NULL, " ",&strtok_saveptr); //MeasurementFrequency + tok = strtok_r(NULL, " ",&strtok_saveptr); + tok = strtok_r(NULL, " ",&strtok_saveptr); + tok = strtok_r(NULL, " ",&strtok_saveptr); + tok = strtok_r(NULL, " ",&strtok_saveptr); //NumberEncoders int NumberEncoders; sscanf(tok, "%d", &NumberEncoders); for (int i = 0; i < NumberEncoders; i++) { - tok = strtok(NULL, " "); //EncoderPosition - tok = strtok(NULL, " "); //EncoderSpeed + tok = strtok_r(NULL, " ",&strtok_saveptr); //EncoderPosition + tok = strtok_r(NULL, " ",&strtok_saveptr); //EncoderSpeed } - tok = strtok(NULL, " "); //NumberChannels16Bit + tok = strtok_r(NULL, " ",&strtok_saveptr); //NumberChannels16Bit int NumberChannels16Bit; sscanf(tok, "%d", &NumberChannels16Bit); logDebug("NumberChannels16Bit : %d", NumberChannels16Bit); @@ -399,8 +400,9 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) { int type = -1; // 0 DIST1 1 DIST2 2 RSSI1 3 RSSI2 char content[6]; - tok = strtok(NULL, " "); //MeasuredDataContent + tok = strtok_r(NULL, " ",&strtok_saveptr); //MeasuredDataContent sscanf(tok, "%s", content); + if (!strcmp(content, "DIST1")) { type = 0; @@ -417,11 +419,11 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) { type = 3; } - tok = strtok(NULL, " "); //ScalingFactor - tok = strtok(NULL, " "); //ScalingOffset - tok = strtok(NULL, " "); //Starting angle - tok = strtok(NULL, " "); //Angular step width - tok = strtok(NULL, " "); //NumberData + tok = strtok_r(NULL, " ",&strtok_saveptr); //ScalingFactor + tok = strtok_r(NULL, " ",&strtok_saveptr); //ScalingOffset + tok = strtok_r(NULL, " ",&strtok_saveptr); //Starting angle + tok = strtok_r(NULL, " ",&strtok_saveptr); //Angular step width + tok = strtok_r(NULL, " ",&strtok_saveptr); //NumberData int NumberData; sscanf(tok, "%X", &NumberData); logDebug("NumberData : %d", NumberData); @@ -446,7 +448,7 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) for (int i = 0; i < NumberData; i++) { int dat; - tok = strtok(NULL, " "); //data + tok = strtok_r(NULL, " ",&strtok_saveptr); //data sscanf(tok, "%X", &dat); if (type == 0) @@ -469,7 +471,7 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) } } - tok = strtok(NULL, " "); //NumberChannels8Bit + tok = strtok_r(NULL, " ",&strtok_saveptr); //NumberChannels8Bit int NumberChannels8Bit; sscanf(tok, "%d", &NumberChannels8Bit); logDebug("NumberChannels8Bit : %d\n", NumberChannels8Bit); @@ -478,7 +480,7 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) { int type = -1; char content[6]; - tok = strtok(NULL, " "); //MeasuredDataContent + tok = strtok_r(NULL, " ",&strtok_saveptr); //MeasuredDataContent sscanf(tok, "%s", content); if (!strcmp(content, "DIST1")) { @@ -496,11 +498,11 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) { type = 3; } - tok = strtok(NULL, " "); //ScalingFactor - tok = strtok(NULL, " "); //ScalingOffset - tok = strtok(NULL, " "); //Starting angle - tok = strtok(NULL, " "); //Angular step width - tok = strtok(NULL, " "); //NumberData + tok = strtok_r(NULL, " ",&strtok_saveptr); //ScalingFactor + tok = strtok_r(NULL, " ",&strtok_saveptr); //ScalingOffset + tok = strtok_r(NULL, " ",&strtok_saveptr); //Starting angle + tok = strtok_r(NULL, " ",&strtok_saveptr); //Angular step width + tok = strtok_r(NULL, " ",&strtok_saveptr); //NumberData int NumberData; sscanf(tok, "%X", &NumberData); logDebug("NumberData : %d\n", NumberData); @@ -524,7 +526,7 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) for (int i = 0; i < NumberData; i++) { int dat; - tok = strtok(NULL, " "); //data + tok = strtok_r(NULL, " ",&strtok_saveptr); //data sscanf(tok, "%X", &dat); if (type == 0) @@ -546,40 +548,40 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) } } - tok = strtok(NULL, " "); //PositionDataEnable + tok = strtok_r(NULL, " ",&strtok_saveptr); //PositionDataEnable int PositionDataEnable; sscanf(tok, "%d", &PositionDataEnable); logDebug("NumberChannels8Bit : %d\n", PositionDataEnable); if(PositionDataEnable == 1){ - tok = strtok(NULL, " "); //X position - tok = strtok(NULL, " "); //Y position - tok = strtok(NULL, " "); //Z position - tok = strtok(NULL, " "); //X rotation - tok = strtok(NULL, " "); //Y rotation - tok = strtok(NULL, " "); //Z rotation - tok = strtok(NULL, " "); //rotations type - tok = strtok(NULL, " "); //Transmits the name of device + tok = strtok_r(NULL, " ",&strtok_saveptr); //X position + tok = strtok_r(NULL, " ",&strtok_saveptr); //Y position + tok = strtok_r(NULL, " ",&strtok_saveptr); //Z position + tok = strtok_r(NULL, " ",&strtok_saveptr); //X rotation + tok = strtok_r(NULL, " ",&strtok_saveptr); //Y rotation + tok = strtok_r(NULL, " ",&strtok_saveptr); //Z rotation + tok = strtok_r(NULL, " ",&strtok_saveptr); //rotations type + tok = strtok_r(NULL, " ",&strtok_saveptr); //Transmits the name of device } - tok = strtok(NULL, " "); //NameEnable + tok = strtok_r(NULL, " ",&strtok_saveptr); //NameEnable int NameEnable; sscanf(tok, "%d", &NameEnable); logDebug("NameEnable : %d\n", NameEnable); if(NameEnable == 1){ - tok = strtok(NULL, " "); //Length of name - tok = strtok(NULL, " "); //Device name in characters + tok = strtok_r(NULL, " ",&strtok_saveptr); //Length of name + tok = strtok_r(NULL, " ",&strtok_saveptr); //Device name in characters } - tok = strtok(NULL, " "); //CommentEnable + tok = strtok_r(NULL, " ",&strtok_saveptr); //CommentEnable int CommentEnable; sscanf(tok, "%d", &CommentEnable); logDebug("CommentEnable : %d\n", CommentEnable); if(CommentEnable == 1){ - tok = strtok(NULL, " "); //Length of comment - tok = strtok(NULL, " "); //Transmits a comment in characters + tok = strtok_r(NULL, " ",&strtok_saveptr); //Length of comment + tok = strtok_r(NULL, " ",&strtok_saveptr); //Transmits a comment in characters } - tok = strtok(NULL, " "); //TimeEnable + tok = strtok_r(NULL, " ",&strtok_saveptr); //TimeEnable int TimeEnable; sscanf(tok, "%d", &TimeEnable); logDebug("TimeEnable : %d\n", TimeEnable); @@ -587,38 +589,38 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) struct tm msg_time = {0}; //Year uint16_t year; - tok = strtok(NULL, " "); + tok = strtok_r(NULL, " ",&strtok_saveptr); sscanf(tok, "%hX", &year); msg_time.tm_year = (int) year - 1900; logDebug("NTP year: %hX\n", year); logDebug("NTP year: %d\n", msg_time.tm_year); // Month uint8_t int_8_buf; - tok = strtok(NULL, " "); + tok = strtok_r(NULL, " ",&strtok_saveptr); sscanf(tok, "%hhX", &int_8_buf); msg_time.tm_mon = (int) int_8_buf - 1; logDebug("NTP month: %hhX\n", int_8_buf); // Day - tok = strtok(NULL, " "); + tok = strtok_r(NULL, " ",&strtok_saveptr); sscanf(tok, "%hhX", &int_8_buf); msg_time.tm_mday = (int) int_8_buf; logDebug("NTP day: %hhX\n", int_8_buf); // Hour - tok = strtok(NULL, " "); + tok = strtok_r(NULL, " ",&strtok_saveptr); sscanf(tok, "%hhX", &int_8_buf); msg_time.tm_hour = (int) int_8_buf; logDebug("NTP hour: %hhX\n", int_8_buf); // Minute - tok = strtok(NULL, " "); + tok = strtok_r(NULL, " ",&strtok_saveptr); sscanf(tok, "%hhX", &int_8_buf); msg_time.tm_min = (int) int_8_buf; logDebug("NTP minute: %hhX\n", int_8_buf); // Second - tok = strtok(NULL, " "); + tok = strtok_r(NULL, " ",&strtok_saveptr); sscanf(tok, "%hhX", &int_8_buf); msg_time.tm_sec = (int) int_8_buf; data->msg_sec = mktime(&msg_time); @@ -626,7 +628,7 @@ void LMS1xx::parseScanData(char* buffer, scanData* data) // Microsecond uint32_t microsecond; - tok = strtok(NULL, " "); + tok = strtok_r(NULL, " ",&strtok_saveptr); sscanf(tok, "%X", µsecond); data->msg_usec = microsecond ; logDebug("NTP usec: %X\n", microsecond);