Release 1.8.0 with support for field montoring TiM7xxS

Author: rostest

.gitignore

@@ -83,5 +83,6 @@ tools/sick_generic_device_finder/venv
 
 tools/sick_generic_device_finder/.idea
 
-
 doc/angle_compensation/ampl_phase_offset\.png
+
+test/emulator/scandata/20210126-tim781s-test-fieldsets.pcapng.json

CMakeLists.txt

@@ -5,6 +5,9 @@
 ####################################################
 cmake_minimum_required(VERSION 2.8.3)
 
+# build options: set OFF for relese version, ON for development and test
+option(ENABLE_EMULATOR "Build emulator for offline and unittests" OFF) # OFF (release) or ON (development)
+option(BUILD_DEBUG_TARGET "Build debug target" OFF) # OFF (release) or ON (development)
 
 # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
 # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wpedantic")
@@ -30,10 +33,12 @@ macro(use_cxx11)
     endif ()
 endmacro(use_cxx11)
 
-#set(CMAKE_BUILD_TYPE Debug) #uncomment to activate debug mode for lib_sick as well
-#set(CMAKE_ENABLE_EXPORTS 1) #uncomment to activate debug mode for lib_sick as well
-# https://cmake.org/cmake/help/v3.5/prop_tgt/ENABLE_EXPORTS.html
-# set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++03")
+if(BUILD_DEBUG_TARGET)
+        set(CMAKE_BUILD_TYPE Debug) #uncomment to activate debug mode for lib_sick as well
+        set(CMAKE_ENABLE_EXPORTS 1) #uncomment to activate debug mode for lib_sick as well
+        # https://cmake.org/cmake/help/v3.5/prop_tgt/ENABLE_EXPORTS.html
+        # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++03")
+endif(BUILD_DEBUG_TARGET)
 
 # Switch on, if you use c11-specific commands
 use_cxx11()
@@ -60,6 +65,9 @@ set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-format-overflow")
 
 find_package(Boost REQUIRED COMPONENTS system serialization)
 
+if(ENABLE_EMULATOR)
+        find_package(jsoncpp REQUIRED) # install libjsoncpp by running "sudo apt-get install libjsoncpp-dev"
+endif(ENABLE_EMULATOR)
 
 find_package(catkin REQUIRED COMPONENTS
         roscpp
@@ -86,8 +94,9 @@ generate_dynamic_reconfigure_options(
         cfg/tf_dyn.cfg
 )
 
+# driver messages
 add_message_files(
-
+        DIRECTORY msg
         FILES
         SickImu.msg
         RadarObject.msg
@@ -98,8 +107,92 @@ add_message_files(
         RadarPreHeader.msg
         RadarScan.msg
         Encoder.msg
+        LFErecFieldMsg.msg
+        LFErecMsg.msg
+        LIDoutputstateMsg.msg
+)
+
+# driver services
+add_service_files(
+        DIRECTORY srv
+        FILES
+        ColaMsgSrv.srv
+        ECRChangeArrSrv.srv
+        LIDoutputstateSrv.srv
 )
 
+if(ENABLE_EMULATOR)
+    # emulator messages
+    add_message_files(
+        DIRECTORY test/emulator/msg
+        FILES
+        SickLocColaTelegramMsg.msg
+        SickLocDiagnosticMsg.msg
+        SickLocResultPortCrcMsg.msg
+        SickLocResultPortHeaderMsg.msg
+        SickLocResultPortPayloadMsg.msg
+        SickLocResultPortTelegramMsg.msg
+        SickLocResultPortTestcaseMsg.msg
+    )
+
+    # emulator services
+    add_service_files(
+        DIRECTORY test/emulator/srv
+        FILES
+        SickLocColaTelegramSrv.srv
+        SickLocRequestTimestampSrv.srv
+        SickLocSetResultModeSrv.srv
+        SickLocSetResultPoseIntervalSrv.srv
+        SickLocIsSystemReadySrv.srv
+        SickLocSetPoseSrv.srv
+        SickLocSetResultPortSrv.srv
+        SickLocStartLocalizingSrv.srv
+        SickLocStopSrv.srv
+        SickLocRequestResultDataSrv.srv
+        SickLocSetResultEndiannessSrv.srv
+        SickLocSetResultPoseEnabledSrv.srv
+        SickLocStateSrv.srv
+        SickLocTimeSyncSrv.srv
+        SickDevGetLidarConfigSrv.srv
+        SickDevGetLidarIdentSrv.srv
+        SickDevGetLidarStateSrv.srv
+        SickDevSetLidarConfigSrv.srv
+        SickGetSoftwareVersionSrv.srv
+        SickLocAutoStartActiveSrv.srv
+        SickLocAutoStartSavePoseIntervalSrv.srv
+        SickLocAutoStartSavePoseSrv.srv
+        SickLocForceUpdateSrv.srv
+        SickLocInitializePoseSrv.srv
+        SickLocInitialPoseSrv.srv
+        SickLocMapSrv.srv
+        SickLocMapStateSrv.srv
+        SickLocOdometryActiveSrv.srv
+        SickLocOdometryPortSrv.srv
+        SickLocOdometryRestrictYMotionSrv.srv
+        SickLocReflectorsForSupportActiveSrv.srv
+        SickLocResultEndiannessSrv.srv
+        SickLocResultModeSrv.srv
+        SickLocResultPortSrv.srv
+        SickLocResultPoseIntervalSrv.srv
+        SickLocResultStateSrv.srv
+        SickLocRingBufferRecordingActiveSrv.srv
+        SickLocSaveRingBufferRecordingSrv.srv
+        SickLocSetAutoStartActiveSrv.srv
+        SickLocSetAutoStartSavePoseIntervalSrv.srv
+        SickLocSetMapSrv.srv
+        SickLocSetOdometryActiveSrv.srv
+        SickLocSetOdometryPortSrv.srv
+        SickLocSetOdometryRestrictYMotionSrv.srv
+        SickLocSetReflectorsForSupportActiveSrv.srv
+        SickLocSetRingBufferRecordingActiveSrv.srv
+        SickLocStartDemoMappingSrv.srv
+        SickReportUserMessageSrv.srv
+        SickSavePermanentSrv.srv
+        SickDevSetIMUActiveSrv.srv
+        SickDevIMUActiveSrv.srv
+    ) 
+endif(ENABLE_EMULATOR)
+
 generate_messages(
         DEPENDENCIES
         std_msgs
@@ -139,6 +232,10 @@ add_library(sick_scan_lib
         include/sick_scan/softwarePLL.h
         driver/src/softwarePLL.cpp
         driver/src/helper/angle_compensator.cpp
+        driver/src/sick_generic_field_mon.cpp
+        driver/src/sick_scan_marker.cpp
+        driver/src/sick_scan_messages.cpp
+        driver/src/sick_scan_services.cpp
         )
 
 add_dependencies(sick_scan_lib ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
@@ -201,7 +298,6 @@ install(
         sick_scan_test
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
-
 install(FILES include/${PROJECT_NAME}/abstract_parser.h
         include/${PROJECT_NAME}/sick_scan_common.h
         DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
@@ -219,3 +315,51 @@ install(DIRECTORY meshes/
 
 install(DIRECTORY urdf/
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf)
+
+if(ENABLE_EMULATOR)
+    #
+    # emulator
+    #
+    # message("ENABLE_EMULATOR: build with emulator...")
+
+    add_executable(sick_scan_emulator
+        test/emulator/src/test_server.cpp
+        test/emulator/src/test_server_thread.cpp
+        test/emulator/src/client_socket.cpp
+        test/emulator/src/cola_converter.cpp
+        test/emulator/src/cola_encoder.cpp
+        test/emulator/src/cola_parser.cpp
+        test/emulator/src/cola_transmitter.cpp
+        test/emulator/src/crc/crc16ccitt_false.cpp
+        test/emulator/src/pcapng_json_parser.cpp
+        test/emulator/src/random_generator.cpp
+        test/emulator/src/result_port_parser.cpp
+        test/emulator/src/ros_wrapper.cpp
+        test/emulator/src/SoftwarePLL.cpp
+        test/emulator/src/testcase_generator.cpp
+        test/emulator/src/utils.cpp
+        )
+
+    target_link_libraries(sick_scan_emulator
+        ${catkin_LIBRARIES}
+        ${roslib_LIBRARIES}
+        jsoncpp_lib # ${jsoncpp_LIBRARIES}
+        sick_scan_lib)
+
+    target_compile_definitions(sick_scan_emulator PUBLIC __ROS_VERSION=1)
+
+    target_include_directories(sick_scan_emulator PUBLIC test/emulator/include)
+
+    install(TARGETS sick_scan_emulator
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+    install(DIRECTORY test/emulator/launch/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
+
+    install(DIRECTORY test/emulator/yaml/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/yaml)
+
+    install(DIRECTORY test/emulator/scandata/
+        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scandata)
+
+endif(ENABLE_EMULATOR)

README.md

@@ -45,9 +45,11 @@ ROS Device Driver for SICK lidar and radar sensors - supported scanner types:
 |                    |                                                                                                                                  | Scan-Rate: 15 Hz   |                 |
 | TiM571             | [1079742](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/tim5xx/tim571-2050101/p/p412444)                 | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| ✔ [stable]|
 |                    |                                                                                                                                  | Scan-Rate: 15 Hz   |                 |
-| TiM781             | [1096807](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim781-2174101/p/p594148)                 | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| ✔ [stable]|
+| TiM771S            | [1105052](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim771s-2174104/p/p660929)                  | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| ✔ [stable]|
 |                    |                                                                                                                                  | Scan-Rate: 15 Hz   |                 |
-| TiM781S            | [1096363](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim781s-2174104/p/p594149)                 | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| ✔ [stable]|
+| TiM781             | [1096807](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim781-2174101/p/p594148)                   | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| ✔ [stable]|
+|                    |                                                                                                                                  | Scan-Rate: 15 Hz   |                 |
+| TiM781S            | [1096363](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim781s-2174104/p/p594149)                  | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| ✔ [stable]|
 |                    |                                                                                                                                  | Scan-Rate: 15 Hz   |                 |
 | LMS511-10100 PRO   | [e.g. 1046135](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/lms5xx/c/g179651)     | 1 layer max. range: 80 m, ang. resol. 0.167 [deg]| ✔ [stable]|
 |                    |                                                                                                                                  | Scan-Rate: 100 Hz   |                 |
@@ -200,14 +202,18 @@ The use of the parameters can be looked up in the launch files. This is also rec
 - `frame_id`
   Frame id used for the published data
 
+
 ### Further useful parameters and features
+
 - `timelimit`
   Timelimit in [sec] for max. wait time of incoming sensor reply
 
 - `sw_pll_only_publish`
   If true, the internal Software PLL is fored to sync the scan generation time stamp to a system timestamp
 
-- Angle compensation: For highest angle accuracy the NAV-Lidar series supports an [angle compensation mechanism](./doc/angular_compensation.md).
+- Angle compensation: For highest angle accuracy the NAV-Lidar series supports an [angle compensation mechanism](./doc/angular_compensation.md).
+
+- The **TiM7xxS** family has [extended settings for field monitoring](./doc/tim7xxs_extensions.md).
 
 ## Sopas Mode
 This driver supports both COLA-B (binary) and COLA-A (ASCII) communication with the laser scanner. Binary mode is activated by default. Since this mode generates less network traffic.

cfg/SickScan.cfg

@@ -74,4 +74,5 @@ gen.add("scan_freq",	double_t, 0, "Scan frequency set to 0 to use scanner defaul
 gen.add("encoder_mode",	int_t, 0, "-1:No Encoder, 0:Off, 1:Single increment, 2:Direction Phase, 3:Direction Level",-1 ,-1,3)
 # gen.add("mean_filter", int_t,  0, "Number of averages for mean filter 0 means filter is disabled", 0, 0, 100)
 # gen.add("mirror_scan",bool_t, 0, "Scan direction's changed. E.g. for overhead mounting or NAV 310 ( in contrast to other sick scanners NAV 310 is clockwise rotating ).",False)
+gen.add("use_safty_fields",    bool_t,   0, "Whether or not to use safty fields. Only tim 7xx5 supported at the moment",                      True)
 exit(gen.generate(PACKAGE, "sick_scan", "SickScan"))

doc/tim7xxs_extensions.md

@@ -0,0 +1,136 @@
+# Extensions for TiM7xxS
+
+The TiM7xxS family has the following extended settings for field monitoring:
+
+## Field monitoring messages
+
+TiM7xxS scanner support field monitoring. Fields can be configured by Sopas ET. Once they are configured, sick_scan publishes ros messages containing the monitoring information from the lidar. 
+
+By default, field monitoring is enabled in the launch file [sick_tim_7xxS.launch](../launch/sick_tim_7xxS.launch) by following settings:
+```
+<param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
+<param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
+<param name="activate_lidinputstate" type="bool" value="True"/> <!-- activate field monitoring by lidinputstate messages -->
+```
+
+The driver queries the field configuration from the lidar and activates field monitoring by sending cola commands `"sEN LFErec 1"` and `"sEN LIDoutputstate 1"` at startup. Field monitoring is deactivated when driver exits. During runtime, it's possible to query, activate or deactivate monitoring using ros service ColaMsg with the following command (see section [Cola commands](#cola-commands)):
+```
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LFErec 1'}" # activate LFErec messages 
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LFErec 0'}" # deactivate LFErec messages 
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LFErec'}"   # query activation status of LFErec messages 
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDoutputstate 1'}" # activate LIDoutputstate messages 
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDoutputstate 0'}" # deactivate LIDoutputstate messages 
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LIDoutputstate'}"   # query activation status of LIDoutputstate messages 
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDinputstate 1'}"  # activate LIDinputstate messages 
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDinputstate 0'}"  # deactivate LIDinputstate messages 
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LIDinputstate'}"    # query activation status of LIDinputstate messages 
+```
+
+LFErec and LIDoutputstate messages are defined in [LFErecMsg.msg](../msg/LFErecMsg.msg) and [LFErecFieldMsg.msg](../msg/LFErecFieldMsg.msg) resp. [LIDoutputstateMsg.msg](../msg/LIDoutputstateMsg.msg) and published on topics `"/sick_tim_7xxS/lferec"` resp. `"/sick_tim_7xxS/lidoutputstate"`.
+To view the field monitoring messages, run
+```
+rostopic echo "/sick_tim_7xxS/lferec"
+rostopic echo "/sick_tim_7xxS/lidoutputstate"
+```
+or use rviz to visualize monitored fields and their status (see section [Visualization with rviz](#visualization-with-rviz))
+
+The most important values of the field monitoring messages are
+
+-  `field_index` (uint8) and `field_result_mrs` (uint8) for each field of a LFErec message with result status<br/><ul>
+   <li>0: invalid / incorrect,</li>
+   <li>1: free / clear, or</li>
+   <li>2: infringed.</li>
+   </ul>
+
+- `output_state` (uint8) for each LIDoutputstate message with status 0 (not active), 1 (active) or 2 (not used).
+
+Note: Field monitoring currently supports binary cola messages only, which is the default. If cola ascii is activated, please switch back to cola binary for field monitoring.
+
+## Visualization with rviz
+
+The point cloud, the monitored fields and their status can be visualized using rviz. Use the [rviz configuration file](../test/emulator/config/rviz_emulator_cfg.rviz) 
+and run
+```
+rosrun rviz rviz -d ./src/sick_scan/test/emulator/config/rviz_emulator_cfg.rviz
+```
+
+Otherwise you can just add visualizations of type `/cloud/PointCloud2` and `/sick_tim_7xxS/marker`:
+
+![tim7xxs_screenshot01.png](tim7xxs_screenshot01.png) 
+
+The following screenshot shows an example with 2 fields (the 3. field is not configured), the first field with status "Clear", the second with status "Infringed":
+
+![tim7xxs_screenshot02.png](tim7xxs_screenshot02.png)
+
+Note: Some combinations of rviz, OpenGL 3, VMware and graphic card drivers may cause visualization issues. In case of missing markers, try rviz with Open GL 2 using the command
+```
+rosrun rviz rviz -d ./src/sick_scan/test/emulator/config/rviz_emulator_cfg.rviz --opengl 210
+```
+
+## Cola commands
+
+Cola commands can be sent for diagnosis and development using the ros service ColaMsg. This service is implemented in sick_scan and started by 
+```
+<param name="start_services" type="bool" value="True"/>
+```
+in the launch file [sick_tim_7xxS.launch](../launch/sick_tim_7xxS.launch). The ros service sends the given cola command to the lidar and returns its response.
+
+Example for cola command `"sRN SCdevicestate"` and response `"sRA SCdevicestate \\x00"` with error status 0 (no error):
+```
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN SCdevicestate'}"
+response: "sRA SCdevicestate \\x00"
+```
+
+## Tools, emulation and unittests
+
+Package sick_scan implements some tools to support unittests, development and emulation of Tim781S devices:
+
+- sick_scan_emulator to emulate lidar devices and enable unittests (currently for Tim781S only)
+
+- pcap_json_converter to convert pcapng-files to json.
+
+### TiM781S emulation
+
+sick_scan_emulator implements a simple test server for cola commands. It rececives Cola-commands, returns Tim781S-like responses and sends Scandata from a json-file. Run
+```
+roslaunch sick_scan emulator.launch
+```
+to emulate a local Tim781S device. Then start and connect the sick_scan driver by
+```
+roslaunch sick_scan sick_tim_7xxS.launch hostname:=127.0.0.1
+```
+
+Note that sick_scan_emulator just implements a simple server for offline tests. It does not emulate a lidar device completely and should only be used for development and testing.
+
+Scandata messages are parsed from json-file(s). These json-files are configured in the launch file [emulator.launch](../test/emulator/launch/emulator.launch) and converted form wireshark-records (pcapng-files) using pcap_json_converter.py (see section Pcapng converter tool](#pcapng-converter-tool)).
+
+### Unittests
+
+Folder `test/emulator/scandata` contains scandata examples for unittests. To run an offline unittest for TiM781S enter the following commands:
+```
+cd test/scripts
+./run_simu_tim781s.bash
+```
+or start emulator, driver and rviz by running
+```
+source ./install/setup.bash
+# Start sick_scan emulator
+roslaunch sick_scan emulator.launch &
+sleep 1
+# Start rviz
+rosrun rviz rviz -d ./src/sick_scan/test/emulator/config/rviz_emulator_cfg.rviz --opengl 210 &
+sleep 1
+# Start sick_scan driver for TiM871S
+roslaunch sick_scan sick_tim_7xxS.launch hostname:=127.0.0.1
+```
+
+### Pcapng converter tool
+
+The pcapng converter tool [pcap_json_converter.py](../test/pcap_json_converter/pcap_json_converter.py) converts pcapng-files to json-files. Run the following steps to create a json-file with scandata for the emulator:
+
+1. Start wireshark and filter the tcp traffic on port 2112 with the filter expression `tcp and tcp.port==2112`.
+2. Start TiM781S and run the sick_scan driver.
+3. Capture the network traffic for some time.
+4. Stop capturing and save the network traffic in a pcapng-file.
+5. Convert the pcapng-file to json by `python pcap_json_converter.py --pcap_filename=<filepath>.pcapng`. Result is a jsonfile `<filepath>.pcapng.json`
+6. Set the resulting json-file in the emulator configuration [emulator.launch](../test/emulator/launch/emulator.launch) by `<arg name="scandatafiles" default="<filepath>.pcapng.json"/>`