From b52f0040e46b5625a8e7f3443a1a5405fb21ea35 Mon Sep 17 00:00:00 2001 From: rostest Date: Mon, 16 Dec 2019 17:13:42 +0100 Subject: [PATCH] Release 3.0.0 sick_lidar_localization phase III: ROS services SIM Konfiguration Release 3.0.0 sick_lidar_localization phase III: ROS services SIM Konfiguration --- CMakeLists.txt | 27 +- README.md | 158 +++++++- .../cola_configuration.h | 141 +++++++ .../sick_lidar_localization/cola_services.h | 310 +++++++++++++++ .../sick_lidar_localization/driver_monitor.h | 9 + .../random_generator.h | 34 +- .../testcase_generator.h | 13 + launch/sim_loc_driver.launch | 16 +- src/cola_configuration.cpp | 194 +++++++++ src/cola_service_node.cpp | 97 +++++ src/cola_services.cpp | 375 ++++++++++++++++++ src/cola_transmitter.cpp | 1 + src/driver.cpp | 4 +- src/driver_monitor.cpp | 61 ++- src/random_generator.cpp | 34 +- src/testcase_generator.cpp | 57 ++- srv/SickLocIsSystemReadySrv.srv | 20 + srv/SickLocRequestResultDataSrv.srv | 20 + srv/SickLocSetPoseSrv.srv | 30 ++ srv/SickLocSetResultEndiannessSrv.srv | 24 ++ srv/SickLocSetResultModeSrv.srv | 24 ++ srv/SickLocSetResultPortSrv.srv | 24 ++ srv/SickLocSetResultPoseEnabledSrv.srv | 24 ++ srv/SickLocSetResultPoseIntervalSrv.srv | 24 ++ srv/SickLocStartLocalizingSrv.srv | 20 + srv/SickLocStateSrv.srv | 21 + srv/SickLocStopAndSaveSrv.srv | 20 + srv/SickLocStopSrv.srv | 20 + test/scripts/clion.bash | 2 +- test/scripts/run.bash | 2 +- test/scripts/run_cola_examples.bash | 68 ++++ test/scripts/run_demo_simu.bash | 2 +- test/scripts/run_error_simu.bash | 4 +- test/scripts/run_simu.bash | 4 +- test/scripts/send_cola_examples.bash | 200 ++++++++++ test/src/test_server_thread.cpp | 67 +++- yaml/sim_loc_driver.yaml | 2 +- 37 files changed, 2084 insertions(+), 69 deletions(-) create mode 100644 include/sick_lidar_localization/cola_configuration.h create mode 100644 include/sick_lidar_localization/cola_services.h create mode 100644 src/cola_configuration.cpp create mode 100644 src/cola_service_node.cpp create mode 100644 src/cola_services.cpp create mode 100644 srv/SickLocIsSystemReadySrv.srv create mode 100644 srv/SickLocRequestResultDataSrv.srv create mode 100644 srv/SickLocSetPoseSrv.srv create mode 100644 srv/SickLocSetResultEndiannessSrv.srv create mode 100644 srv/SickLocSetResultModeSrv.srv create mode 100644 srv/SickLocSetResultPortSrv.srv create mode 100644 srv/SickLocSetResultPoseEnabledSrv.srv create mode 100644 srv/SickLocSetResultPoseIntervalSrv.srv create mode 100644 srv/SickLocStartLocalizingSrv.srv create mode 100644 srv/SickLocStateSrv.srv create mode 100644 srv/SickLocStopAndSaveSrv.srv create mode 100644 srv/SickLocStopSrv.srv create mode 100755 test/scripts/run_cola_examples.bash create mode 100755 test/scripts/send_cola_examples.bash diff --git a/CMakeLists.txt b/CMakeLists.txt index 74442a6..5d20edd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,6 +74,18 @@ add_service_files( FILES SickLocColaTelegramSrv.srv SickLocRequestTimestampSrv.srv + SickLocSetResultModeSrv.srv + SickLocSetResultPoseIntervalSrv.srv + SickLocStopAndSaveSrv.srv + SickLocIsSystemReadySrv.srv + SickLocSetPoseSrv.srv + SickLocSetResultPortSrv.srv + SickLocStartLocalizingSrv.srv + SickLocStopSrv.srv + SickLocRequestResultDataSrv.srv + SickLocSetResultEndiannessSrv.srv + SickLocSetResultPoseEnabledSrv.srv + SickLocStateSrv.srv SickLocTimeSyncSrv.srv ) @@ -145,8 +157,10 @@ include_directories( ## Declare a C++ library add_library(sick_localization_lib src/client_socket.cpp + src/cola_configuration.cpp src/cola_converter.cpp src/cola_parser.cpp + src/cola_services.cpp src/cola_transmitter.cpp src/driver_check_thread.cpp src/driver_monitor.cpp @@ -175,6 +189,7 @@ add_executable(sim_loc_driver src/driver.cpp) add_executable(sim_loc_driver_check src/driver_check.cpp) add_executable(sim_loc_time_sync src/time_sync.cpp) add_executable(pointcloud_converter src/pointcloud_converter.cpp) +add_executable(cola_service_node src/cola_service_node.cpp) ## Executables for test purposes add_executable(sim_loc_test_server test/src/test_server.cpp test/src/test_server_thread.cpp) @@ -218,6 +233,11 @@ add_dependencies(pointcloud_converter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) +add_dependencies(cola_service_node + sick_localization_lib + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) add_dependencies(unittest_sim_loc_parser sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} @@ -259,6 +279,11 @@ target_link_libraries(pointcloud_converter ${Boost_LIBRARIES} ${catkin_LIBRARIES} ) +target_link_libraries(cola_service_node + sick_localization_lib + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ) target_link_libraries(unittest_sim_loc_parser sick_localization_lib ${Boost_LIBRARIES} @@ -285,7 +310,7 @@ target_link_libraries(verify_sim_loc_driver # ) ## Mark executables and/or libraries for installation -install(TARGETS sick_localization_lib sim_loc_driver sim_loc_driver_check sim_loc_test_server sim_loc_time_sync pointcloud_converter unittest_sim_loc_parser verify_sim_loc_driver +install(TARGETS sick_localization_lib sim_loc_driver sim_loc_driver_check sim_loc_test_server sim_loc_time_sync pointcloud_converter cola_service_node unittest_sim_loc_parser verify_sim_loc_driver ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/README.md b/README.md index 6da0d27..589278d 100644 --- a/README.md +++ b/README.md @@ -33,12 +33,12 @@ https://supportportal.sick.com/Product_notes/lidar-loc-operation-instruction/. ```console cd ~/catkin_ws source ./devel/setup.bash -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=192.168.0.1 +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=192.168.0.1 ``` Note: The IP address of the SICK localization controller is 192.168.0.1 by default. Depending on your network configuration, -different IP addresses can be configured by commandline argument `localization_controller_ip_adress:=` or -by changing the value `localization_controller_default_ip_adress: ""` in the drivers configuration file +different IP addresses can be configured by commandline argument `localization_controller_ip_address:=` or +by changing the value `localization_controller_default_ip_address: ""` in the drivers configuration file [yaml/sim_loc_driver.yaml](yaml/sim_loc_driver.yaml). The sick_lidar_localization driver connects to the localization controller, receives result port telegrams and publishes @@ -216,7 +216,7 @@ returns the system timestamp from ticks using the software pll running in the dr [srv/SickLocTimeSyncSrv.srv](srv/SickLocTimeSyncSrv.srv). Example: ``` -> rosservice call SickLocTimeSync "{timestamp_lidar_ms: 123456}" +> rosservice call SickLocTimeSync "{timestamp_lidar_ms: 123t456}" vehicle_time_valid: True vehicle_time_sec: 1573119167 vehicle_time_nsec: 380565047 @@ -237,6 +237,128 @@ controller to 10 seconds or higher”). Therefore, the initial phase after start commands with 10 seconds delay by default). During the initial phase, the time synchronization service is not available and the vehicle system time is not valid. +## SIM configuration + +The localization controller can be configured by Cola telegrams. See manual Telegram-Listing-v1.1.0.241R.pdf for a list of +all settings. The following Cola telegrams are supported by the ros driver: + +Request | Response | Parameter | Description +--- | --- | --- | --- +**States Telegrams**||| +"sMN IsSystemReady" | "sAN IsSystemReady \" | 0:true, 1:false | Check if the system is ready +"sRN LocState" | "sRA LocState \" | 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING | Read localization state +"sMN LocStartLocalizing" | "sAN LocStartLocalizing \" | 0:failed, 1:success | Start localization +"sMN LocStop" | "sAN LocStop \" | 0:failed, 1:success | Stop localization or demo mapping +"sMN LocStopAndSave" | "sAN LocStopAndSave \" | 0:failed, 1:success | Stop localization, save settings +**Result Output Configuration Telegrams**||| +"sMN LocSetResultPort \" | "sAN LocSetResultPort \" | 0:failed, 1:success, \: uint16 (default: 2201) | Set TCP-port for result output +"sMN LocSetResultMode \" | "sAN LocSetResultMode \" | 0:failed, 1:success, \: uint8 (0:stream, 1: poll, default: stream) | Set mode of result output (stream or: poll) +"sMN LocSetResultPoseEnabled \" | "sAN LocSetResultPoseEnabled \" | 0:failed, 1:success, \: uint8 (0: disabled, 1: enabled, default: enabled) | Disable/enable result output +"sMN LocSetResultEndianness \" | "sAN LocSetResultEndianness \" | 0:failed, 1:success, \: uint8 (0: big endian, 1: little endian, default: big endian) | Set endianness of result output +"sMN LocSetResultPoseInterval \" | "sAN LocSetResultPoseInterval \" | 0:failed, 1:success, \: uint8 (0-255, interval in number of scans, 1: result with each processed scan, default: 1) | Set interval of result output +"sMN LocRequestResultData" | "sAN LocRequestResultData \" | 0:failed, 1:success | If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. +**SetPose Telegrams**||| +"sMN LocSetPose \ \ \ \" | "sAN LocSetPose \" | 0:failed, 1:success, \: int32 (x coordinate in mm), \: int32 (y coordinate in mm), \: int32 (yaw angle in millidegree, -180000 to +180000), \: uint16 (translation uncertainty in mm) | Initialize vehicle pose +**Result Output Settings** (queries, ros service SickLocColaTelegram only)||| +"sRN LocResultState" | "sRA LocResultState \" | 0:disabled, 1:enabled, MSB: error flag | Read state of the result output +"sRN LocResultPort" | "sRA LocResultPort \" | tcp port | Read TCP port used for result output +"sRN LocResultMode" | "sRA LocResultMode \" | 0:stream, 1: poll | Read result mode +"sRN LocResultEndianness" | "sRA LocResultEndianness \" | 0: big endian, 1: little endian | Read endianness of result output +**Timestamp Telegrams**||| +"sMN LocRequestTimestamp" | "sAN LocRequestTimestamp \" | 4 byte timestamp (ticks in milliseconds) | Query timestamp, see "Time synchronization" + +Note: Other commands like "sMN mSCreboot" (reboot controller) can be send to the localization controller using ros service +"SickLocColaTelegram", too. But only the subset of Cola telegrams listed above are officially supported under ROS. +The following examples show how to call Cola telegrams supported and tested by ros service "SickLocColaTelegram": + +``` +# States Telegrams +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN IsSystemReady', wait_response_timeout: 1}" # expected answer: "sAN IsSystemReady 1" # 0:true, 1:false # Check if the system is ready +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sRN LocState', wait_response_timeout: 1}" # expected answer: "sRA LocState 2" # 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING # Read localization state +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocStartLocalizing', wait_response_timeout: 1}" # expected answer: "sAN LocStartLocalizing 1" # 0:failed, 1:success # Start localization +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocStop', wait_response_timeout: 1}" # expected answer: "sAN LocStop 1" # 0:failed, 1:success # Stop localization or demo mapping +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocStopAndSave', wait_response_timeout: 1}" # expected answer: "sAN LocStopAndSave 1" # 0:failed, 1:success # Stop localization, save settings +# Result Output Configuration Telegrams +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocSetResultPort 2201', wait_response_timeout: 1}" # expected answer: "sAN LocSetResultPort 1" # 0:failed, 1:success, : uint16 (default: 2201) # Set TCP-port for result output +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocSetResultMode 0', wait_response_timeout: 1}" # expected answer: "sAN LocSetResultMode 1" # 0:failed, 1:success, : uint8 (0:stream, 1: poll, default: stream) # Set mode of result output (stream or: poll) +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocSetResultPoseEnabled 1', wait_response_timeout: 1}" # expected answer: "sAN LocSetResultPoseEnabled 1" # 0:failed, 1:success, : uint8 (0: disabled, 1: enabled, default: enabled) # Disable/enable result output +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocSetResultEndianness 0', wait_response_timeout: 1}" # expected answer: "sAN LocSetResultEndianness 1" # 0:failed, 1:success, : uint8 (0: big endian, 1: little endian, default: big endian) # Set endianness of result output +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocSetResultPoseInterval 1', wait_response_timeout: 1}" # expected answer: "sAN LocSetResultPoseInterval 1" # 0:failed, 1:success, : uint8 (0-255, interval in number of scans, 1: result with each processed scan, default: 1) # Set interval of result output +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocRequestResultData', wait_response_timeout: 1}" # expected answer: "sAN LocRequestResultData 1" # 0:failed, 1:success # If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. +# SetPose Telegrams +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocSetPose +10300 -5200 +30000 +1000', wait_response_timeout: 1}" # expected answer: "sAN LocSetPose 1" # 0:failed, 1:success, : int32 (x coordinate in mm), : int32 (y coordinate in mm), : int32 (yaw angle in millidegree, -180000 to +180000), : uint16 (translation uncertainty in mm) # Initialize vehicle pose +# Timestamp Telegrams +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocRequestTimestamp', wait_response_timeout: 1}" # expected answer: "sAN LocRequestTimestamp " # 4 byte timestamp (ticks in milliseconds) # Query timestamp, see "Time synchronization" +# Result Output Settings (queries, ros service SickLocColaTelegram only) +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" # expected answer: "sRA LocResultState 1" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sRN LocResultPort', wait_response_timeout: 1}" # expected answer: "sRA LocResultPort 2201" # tcp port # Read TCP port used for result output +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" # expected answer: "sRA LocResultMode 0" # 0:stream, 1: poll # Read result mode +rosservice call SickLocColaTelegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" # expected answer: "sRA LocResultEndianness 0" # 0: big endian, 1: little endian # Read endianness of result output +``` + +See file [test/scripts/send_cola_examples.bash](test/scripts/send_cola_examples.bash) for further examples. + +The operations listed above are implemented by specialized ros services defined in folder srv. These services create and +transmit the corresponding cola telegram to the controller and converts the parameter. + +Request | ros service | Interface definition | Description +--- | --- | --- | --- +**States Telegrams**||| +IsSystemReady | SickLocIsSystemReady | [srv/SickLocIsSystemReadySrv.srv](srv/SickLocIsSystemReadySrv.srv) | Check if the system is ready +LocState | SickLocState | [srv/SickLocStateSrv.srv](srv/SickLocStateSrv.srv) | Read localization state +LocStartLocalizing | SickLocStartLocalizing | [srv/SickLocStartLocalizingSrv.srv](srv/SickLocStartLocalizingSrv.srv) | Start localization +LocStop | SickLocStop | [srv/SickLocStopSrv.srv](srv/SickLocStopSrv.srv) | Stop localization or demo mapping +LocStopAndSave | SickLocStopAndSave | [srv/SickLocStopAndSaveSrv.srv](srv/SickLocStopAndSaveSrv.srv) | Stop localization, save settings +**Result Output Configuration Telegrams**||| +LocSetResultPort | SickLocSetResultPort | [srv/SickLocSetResultPortSrv.srv](srv/SickLocSetResultPortSrv.srv) | Set TCP-port for result output (default: 2201) +LocSetResultMode | SickLocSetResultMode | [srv/SickLocSetResultModeSrv.srv](srv/SickLocSetResultModeSrv.srv) | Set mode of result output (default: stream) +LocSetResultPoseEnabled | SickLocSetResultPoseEnabled | [srv/SickLocSetResultPoseEnabledSrv.srv](srv/SickLocSetResultPoseEnabledSrv.srv) | Disable/enable result output +LocSetResultEndianness | SickLocSetResultEndianness | [srv/SickLocSetResultEndiannessSrv.srv](srv/SickLocSetResultEndiannessSrv.srv) | Set endianness of result output +LocSetResultPoseInterval | SickLocSetResultPoseInterval | [srv/SickLocSetResultPoseIntervalSrv.srv](srv/SickLocSetResultPoseIntervalSrv.srv) | Set interval of result output +LocRequestResultData | SickLocRequestResultData | [srv/SickLocRequestResultDataSrv.srv](srv/SickLocRequestResultDataSrv.srv) | If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. +**SetPose Telegrams**||| +LocSetPose | SickLocSetPose | [srv/SickLocSetPoseSrv.srv](srv/SickLocSetPoseSrv.srv) | Initialize vehicle pose +**Timestamp Telegrams**||| +LocRequestTimestamp | SickLocRequestTimestamp | [srv/SickLocRequestTimestampSrv.srv](srv/SickLocRequestTimestampSrv.srv) | Query timestamp, see "Time synchronization" + +The following examples show how to call these services: + +``` +# ROS services for States Telegrams +rosservice call SickLocIsSystemReady "{}" # expected answer: "success: True" # Check if the system is ready +rosservice call SickLocState "{}" # expected answer: "state: 2, success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING +rosservice call SickLocStop "{}" # expected answer: "success: True" # Stop localization or demo mapping +rosservice call SickLocStopAndSave "{}" # expected answer: "success: True" # Stop localization, save settings +rosservice call SickLocStartLocalizing "{}" # expected answer: "success: True" # Start localization +# ROS services for Result Output Configuration Telegrams +rosservice call SickLocSetResultPort "{port: 2201}" # expected answer: "success: True" # Set TCP-port for result output +rosservice call SickLocSetResultMode "{mode: 0}" # expected answer: "success: True" # Set mode of result output (stream or: poll) +rosservice call SickLocSetResultPoseEnabled "{enabled: 1}" # expected answer: "success: True" # Disable/enable result output +rosservice call SickLocSetResultEndianness "{endianness: 0}" # expected answer: "success: True" # Set endianness of result output +rosservice call SickLocSetResultPoseInterval "{interval: 1}" # expected answer: "success: True" # Set interval of result output +rosservice call SickLocRequestResultData "{}" # expected answer: "success: True" # If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. +# ROS services for SetPose Telegrams +rosservice call SickLocSetPose "{posex: 10300, posey: -5200, yaw: 30000, uncertainty: 1000}" # expected answer: "success: True" # Initialize vehicle pose +# ROS services for Timestamp Telegrams +rosservice call SickLocRequestTimestamp "{}" # expected reponse: "timestamp_lidar_ms: , mean_time_vehicle_ms: , delta_time_ms: , ..." # Query timestamp, see "Time synchronization" +``` + +An initial result output configuration can be set by launch file [launch/sim_loc_driver.launch](launch/sim_loc_driver.launch), too: + +``` + + + + + + + + +``` + +If configured, these parameters are set initially at driver start using the corresponding ros services. +By default, the result output configuration is not set in the launch file and the SIM configuration applies. + ## Diagnostics The sick_lidar_localization driver publishes diagnostic messages on ros topic "/sick_lidar_localization/driver/diagnostic", @@ -280,7 +402,7 @@ The sick_lidar_localization driver is configured by file [yaml/sim_loc_driver.ya Parametername | Defaultvalue | Description --- | --- | --- -localization_controller_default_ip_adress | "192.168.0.1" | Default IP adress "192.168.0.1" of the localization controller (if not otherwise set by parameter "localization_controller_ip_adress") +localization_controller_default_ip_address | "192.168.0.1" | Default IP adress "192.168.0.1" of the localization controller (if not otherwise set by parameter "localization_controller_ip_address") result_telegrams_tcp_port | 2201 | TCP port number of the localization controller sending localization results cola_telegrams_tcp_port | 2111 | For requests and to transmit settings to the localization controller: IP port number 2111 and 2112 to send telegrams and to request data, SOPAS CoLa-A or CoLa-B protocols cola_binary | 0 | 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) @@ -299,8 +421,8 @@ software_pll_fifo_length | 7 | Length of software pll fifo, default: 7 time_sync_rate | 0.1 | Frequency to request timestamps from localization controller using ros service "SickLocRequestTimestamp" and to update software pll, default: 0.1 Note: The IP address of the SICK localization controller (192.168.0.1 by default) can be set by commandline argument -`localization_controller_ip_adress:=` when starting the driver with -`roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=`. +`localization_controller_ip_address:=` when starting the driver with +`roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=`. ## Testing @@ -312,7 +434,7 @@ cd ~/catkin_ws source ./devel/setup.bash rostopic echo "/sick_lidar_localization/driver/diagnostic" & rostopic echo "/sick_lidar_localization/driver/result_telegrams" & -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=192.168.0.1 +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=192.168.0.1 ``` Warnings and error messages will be printed in case of failures like unreachable controller, connection losts or @@ -349,7 +471,7 @@ cd ~/catkin_ws source ./devel/setup.bash roslaunch sick_lidar_localization sim_loc_test_server.launch & # start test server to generate result port telegrams sleep 3 # make sure ros core and sim_loc_test_server are up and running -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=127.0.0.1 +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 ``` The sick_lidar_localization ros driver will connect to the local test server, receive random based result port telegrams @@ -393,6 +515,22 @@ MessageCheckThread: check messages thread summary: 599 messages checked, 0 failu VerifierThread: verification thread summary: 585 messages checked, 0 failures. ``` +Use `run_cola_examples.bash` in folder src/sick_lidar_localization/test/scripts to run an automated offline test +of ros services for SIM configuration: + +```console +cd ~/catkin_ws/src/sick_lidar_localization/test/scripts +./run_cola_examples.bash +``` + +Examples of ros services for SIM configuration will be called and example cola telegrams will be tested offline. +In case of a successful test, the following summary will be displayed (example output): + +``` +MessageCheckThread: check messages thread summary: 1081 messages checked, 0 failures. +Services and cola telegram verification summary: 86 testcases, 0 failures. +``` + ## Error simulation and error handling The sick_lidar_localization ros driver monitors the telegram messages. In case of errors (network errors like unreachable @@ -462,7 +600,7 @@ source ./devel/setup.bash roslaunch sick_lidar_localization sim_loc_test_server.launch demo_circles:=true & sleep 3 # make sure ros core and sim_loc_test_server are up and running # Run ros driver, connect to localization controller, receive, convert and publish report telegrams -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=127.0.0.1 & +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 & # Visualize PointCloud2 and TF messages by rviz: rosrun tf static_transform_publisher 0 0 0 0 0 0 map pointcloud_sick_lidar_localization 10 & rosrun rviz rviz & diff --git a/include/sick_lidar_localization/cola_configuration.h b/include/sick_lidar_localization/cola_configuration.h new file mode 100644 index 0000000..35cebfc --- /dev/null +++ b/include/sick_lidar_localization/cola_configuration.h @@ -0,0 +1,141 @@ +/* + * @brief cola_configuration sets the initial SIM result output configuration + * using ros cola services. + * Configures the following result output settings from launch file: + * LocSetResultPort, LocSetResultMode, LocSetResultPoseEnabled, LocSetResultEndianness, LocSetResultPoseInterval, + * LocRequestResultData + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SIM_LOC_COLA_CONFIGURATION_H_INCLUDED +#define __SIM_LOC_COLA_CONFIGURATION_H_INCLUDED + +#include "sick_lidar_localization/cola_services.h" + +namespace sick_lidar_localization +{ + /*! + * Class sick_lidar_localization::ColaConfiguration sets the initial SIM result output configuration + * using ros cola services. + * Configures the following result output settings from launch file: + * LocSetResultPort, LocSetResultMode, LocSetResultPoseEnabled, LocSetResultEndianness, LocSetResultPoseInterval, + * LocRequestResultData + */ + class ColaConfiguration + { + public: + + /*! + * Constructor + */ + ColaConfiguration(ros::NodeHandle* nh = 0); + + /*! + * Destructor + */ + virtual ~ColaConfiguration(); + + /*! + * Starts transmitting the initial result output configuration to the localization controller. + * Configures the following result output settings from launch file: + * LocSetResultPort, LocSetResultMode, LocSetResultPoseEnabled, LocSetResultEndianness, LocSetResultPoseInterval, + * LocRequestResultData + * @return true on success, false in case of errors. + */ + virtual bool start(void); + + /*! + * Stops transmitting the initial result output configuration to the localization controller. + */ + virtual void stop(void); + + protected: + + /*! + * Thread callback, transmits the initial SIM result output configuration using ros cola services. + * Configures the following result output settings from launch file: + * LocSetResultPort, LocSetResultMode, LocSetResultPoseEnabled, LocSetResultEndianness, LocSetResultPoseInterval, + * LocRequestResultData + */ + virtual void runConfigurationThreadCb(void); + + /*! + * Calls a ros service until successfully executed or configuration thread cancelled. + * @param[in/out] service_telegram service request and response + * @param[in] service_client ros service client + * @param[in] retry_delay delay before next retry in case of errors + * @return true on success or false on failure + */ + template bool callService(T & service_telegram, ros::ServiceClient & service_client, double retry_delay) + { + service_telegram.response.success = false; + while(ros::ok() && m_nh && m_configuration_thread_running && (!service_client.call(service_telegram) || !service_telegram.response.success)) + { + ROS_WARN_STREAM("## ERROR ColaConfiguration: service call(" << sick_lidar_localization::Utils::flattenToString(service_telegram.request) << ") failed, response: " << sick_lidar_localization::Utils::flattenToString(service_telegram.response) << ", retrying"); + ros::Duration(retry_delay).sleep(); + } + ROS_INFO_STREAM("ColaConfiguration " << typeid(T).name() << " " << (service_telegram.response.success ? "successfull" : "failed")); + return service_telegram.response.success; + } + + /* + * member variables + */ + + ros::NodeHandle* m_nh; ///< ros node handle + boost::thread* m_configuration_thread; ///< thread to transmit configuration + bool m_configuration_thread_running; ///< true: m_configuration_thread is running, otherwise false + + }; // class ColaServices + +} // namespace sick_lidar_localization +#endif // __SIM_LOC_COLA_CONFIGURATION_H_INCLUDED diff --git a/include/sick_lidar_localization/cola_services.h b/include/sick_lidar_localization/cola_services.h new file mode 100644 index 0000000..7dc63b3 --- /dev/null +++ b/include/sick_lidar_localization/cola_services.h @@ -0,0 +1,310 @@ +/* + * @brief cola_services implements the following ros services for sick_lidar_localization: + * + * ROS service name | Definition file | Cola request telegram | Description + * "SickLocIsSystemReady" | srv/SickLocIsSystemReadySrv.srv | "sMN IsSystemReady" | Check if the system is ready + * "SickLocState" | srv/SickLocStateSrv.srv | "sRN LocState" | Read localization state + * "SickLocStartLocalizing" | srv/SickLocStartLocalizingSrv.srv | "sMN LocStartLocalizing" | Start localization + * "SickLocStop" | srv/SickLocStopSrv.srv | "sMN LocStop" | Stop localization or demo mapping + * "SickLocStopAndSave" | srv/SickLocStopAndSaveSrv.srv | "sMN LocStopAndSave" | Stop localization, save settings + * "SickLocSetResultPort" | srv/SickLocSetResultPortSrv.srv | "sMN LocSetResultPort " | Set TCP-port for result output (default: 2201) + * "SickLocSetResultMode" | srv/SickLocSetResultModeSrv.srv | "sMN LocSetResultMode " | Set mode of result output (default: stream) + * "SickLocSetResultPoseEnabled" | srv/SickLocSetResultPoseEnabledSrv.srv | "sMN LocSetResultPoseEnabled " | Disable/enable result output + * "SickLocSetResultEndianness" | srv/SickLocSetResultEndiannessSrv.srv | "sMN LocSetResultEndianness " | Set endianness of result output + * "SickLocSetResultPoseInterval" | srv/SickLocSetResultPoseIntervalSrv.srv | "sMN LocSetResultPoseInterval " | Set interval of result output + * "SickLocRequestResultData" | srv/SickLocRequestResultDataSrv.srv | "sMN LocRequestResultData" | If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. + * "SickLocSetPose" | srv/SickLocSetPoseSrv.srv | "sMN LocSetPose " | Initialize vehicle pose + * + * See Telegram-Listing-v1.1.0.241R.pdf for further details about Cola telegrams. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SIM_LOC_COLA_SERVICES_H_INCLUDED +#define __SIM_LOC_COLA_SERVICES_H_INCLUDED + +#include "sick_lidar_localization/cola_parser.h" +#include "sick_lidar_localization/SickLocColaTelegramSrv.h" +#include "sick_lidar_localization/SickLocIsSystemReadySrv.h" +#include "sick_lidar_localization/SickLocStateSrv.h" +#include "sick_lidar_localization/SickLocStartLocalizingSrv.h" +#include "sick_lidar_localization/SickLocStopSrv.h" +#include "sick_lidar_localization/SickLocStopAndSaveSrv.h" +#include "sick_lidar_localization/SickLocSetResultPortSrv.h" +#include "sick_lidar_localization/SickLocSetResultModeSrv.h" +#include "sick_lidar_localization/SickLocSetResultPoseEnabledSrv.h" +#include "sick_lidar_localization/SickLocSetResultEndiannessSrv.h" +#include "sick_lidar_localization/SickLocSetResultPoseIntervalSrv.h" +#include "sick_lidar_localization/SickLocRequestResultDataSrv.h" +#include "sick_lidar_localization/SickLocSetPoseSrv.h" +#include "sick_lidar_localization/utils.h" + +namespace sick_lidar_localization +{ + /*! + * Class sick_lidar_localization::ColaServices implements the following ros services using + * cola telegrams: + * + * ROS service name | Definition file | Cola request telegram | Description + * "SickLocIsSystemReady" | srv/SickLocIsSystemReadySrv.srv | "sMN IsSystemReady" | Check if the system is ready + * "SickLocState" | srv/SickLocStateSrv.srv | "sRN LocState" | Read localization state + * "SickLocStartLocalizing" | srv/SickLocStartLocalizingSrv.srv | "sMN LocStartLocalizing" | Start localization + * "SickLocStop" | srv/SickLocStopSrv.srv | "sMN LocStop" | Stop localization or demo mapping + * "SickLocStopAndSave" | srv/SickLocStopAndSaveSrv.srv | "sMN LocStopAndSave" | Stop localization, save settings + * "SickLocSetResultPort" | srv/SickLocSetResultPortSrv.srv | "sMN LocSetResultPort " | Set TCP-port for result output (default: 2201) + * "SickLocSetResultMode" | srv/SickLocSetResultModeSrv.srv | "sMN LocSetResultMode " | Set mode of result output (default: stream) + * "SickLocSetResultPoseEnabled" | srv/SickLocSetResultPoseEnabledSrv.srv | "sMN LocSetResultPoseEnabled " | Disable/enable result output + * "SickLocSetResultEndianness" | srv/SickLocSetResultEndiannessSrv.srv | "sMN LocSetResultEndianness " | Set endianness of result output + * "SickLocSetResultPoseInterval" | srv/SickLocSetResultPoseIntervalSrv.srv | "sMN LocSetResultPoseInterval " | Set interval of result output + * "SickLocRequestResultData" | srv/SickLocRequestResultDataSrv.srv | "sMN LocRequestResultData" | If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. + * "SickLocSetPose" | srv/SickLocSetPoseSrv.srv | "sMN LocSetPose " | Initialize vehicle pose + * + * See Telegram-Listing-v1.1.0.241R.pdf for further details about Cola telegrams. + */ + class ColaServices + { + public: + + /*! + * Constructor + */ + ColaServices(ros::NodeHandle* nh = 0); + + /*! + * Destructor + */ + virtual ~ColaServices(); + + /*! + * Callback for service messages (SickLocIsSystemReadySrv, Check if the system is ready). + * Sends a cola telegram "sMN IsSystemReady" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbIsSystemReady(sick_lidar_localization::SickLocIsSystemReadySrv::Request & service_request, sick_lidar_localization::SickLocIsSystemReadySrv::Response & service_response); + + /*! + * Callback for service messages (SickLocStateSrv, Read localization state). + * Sends a cola telegram "sRN LocState" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbLocState(sick_lidar_localization::SickLocStateSrv::Request & service_request, sick_lidar_localization::SickLocStateSrv::Response & service_response); + + /*! + * Callback for service messages (SickLocStartLocalizingSrv, Stop localization, save settings). + * Sends a cola telegram "Stop localization, save settings" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbLocStartLocalizing(sick_lidar_localization::SickLocStartLocalizingSrv::Request & service_request, sick_lidar_localization::SickLocStartLocalizingSrv::Response & service_response); + + /*! + * Callback for service messages (SickLocStopSrv, Stop localization, save settings). + * Sends a cola telegram "Stop localization, save settings" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbLocStop(sick_lidar_localization::SickLocStopSrv::Request & service_request, sick_lidar_localization::SickLocStopSrv::Response & service_response); + + /*! + * Callback for service messages (SickLocStopAndSaveSrv, Stop localization, save settings). + * Sends a cola telegram "sMN LocStopAndSave" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbLocStopAndSave(sick_lidar_localization::SickLocStopAndSaveSrv::Request & service_request, sick_lidar_localization::SickLocStopAndSaveSrv::Response & service_response); + + /*! + * Callback for service messages (SickLocSetResultPortSrv, Set TCP-port for result output, default: 2201). + * Sends a cola telegram "Set mode of result output (default: stream" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbLocSetResultPort(sick_lidar_localization::SickLocSetResultPortSrv::Request & service_request, sick_lidar_localization::SickLocSetResultPortSrv::Response & service_response); + + /*! + * Callback for service messages (SickLocSetResultModeSrv, Set mode of result output, default: stream). + * Sends a cola telegram "sMN LocSetResultMode " and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbLocSetResultMode(sick_lidar_localization::SickLocSetResultModeSrv::Request & service_request, sick_lidar_localization::SickLocSetResultModeSrv::Response & service_response); + + /*! + * Callback for service messages (SickLocSetResultPoseEnabledSrv, Disable/enable result output). + * Sends a cola telegram "sMN LocSetResultPoseEnabled " and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbLocSetResultPoseEnabled(sick_lidar_localization::SickLocSetResultPoseEnabledSrv::Request & service_request, sick_lidar_localization::SickLocSetResultPoseEnabledSrv::Response & service_response); + + /*! + * Callback for service messages (SickLocSetResultEndiannessSrv, Set endianness of result output). + * Sends a cola telegram "sMN LocSetResultEndianness " and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbLocSetResultEndianness(sick_lidar_localization::SickLocSetResultEndiannessSrv::Request & service_request, sick_lidar_localization::SickLocSetResultEndiannessSrv::Response & service_response); + + /*! + * Callback for service messages (SickLocSetResultPoseIntervalSrv, Set interval of result output). + * Sends a cola telegram "sMN LocSetResultPoseInterval " and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbLocSetResultPoseInterval(sick_lidar_localization::SickLocSetResultPoseIntervalSrv::Request & service_request, sick_lidar_localization::SickLocSetResultPoseIntervalSrv::Response & service_response); + + /*! + * Callback for service messages (SickLocRequestResultDataSrv, If in poll mode, trigger sending the localization result of the next processed scan via TCP interface). + * Sends a cola telegram "sMN LocRequestResultData" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbLocRequestResultData(sick_lidar_localization::SickLocRequestResultDataSrv::Request & service_request, sick_lidar_localization::SickLocRequestResultDataSrv::Response & service_response); + + /*! + * Callback for service messages (SickLocSetPoseSrv, Initialize vehicle pose). + * Sends a cola telegram "sMN LocSetPose " and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ + virtual bool serviceCbLocSetPose(sick_lidar_localization::SickLocSetPoseSrv::Request & service_request, sick_lidar_localization::SickLocSetPoseSrv::Response & service_response); + + protected: + + /*! + * Sends a cola telegram using ros service "SickLocColaTelegram", receives and returns the response from localization controller + * @param[in] cola_ascii_request request (Cola-ASCII, f.e. "sMN IsSystemReady") + * @return response from localization controller + */ + virtual sick_lidar_localization::SickLocColaTelegramMsg sendColaTelegram(const std::string & cola_ascii_request); + + /*! + * Converts and returns the parameter of a cola ascii telegram into a numeric value. + * @param[in] cola_arg parameter of a cola ascii telegram + * @param[in] base numeric base (10 for decimal values or 16 for hex strings) + * @param[in] default_value default value returned in case of parse errors + * @return parameter converted to integer value + */ + virtual int32_t convertColaArg(const std::string & cola_arg, int base = 10, int32_t default_value = 0); + + /*! + * Converts and returns the parameter of a cola ascii response into a boolean value. + * @param[in] cola_response_arg parameter of a cola ascii response + * @param[in] default_value default value returned in case of parse errors + * @return parameter converted to boolean value + */ + virtual bool convertColaResponseBool(const std::string & cola_response_arg, bool default_value); + + /*! + * Sends a cola telegram using ros service "SickLocColaTelegram", receives the response from localization controller + * and sets service_response.success to the parameter value of the recived response. Specialization of function + * sendColaTelegram() for Cola-ASCII telegrams returning a single success value. + * @param[in] cola_command_type request type (Cola-ASCII, f.e. "sMN") + * @param[in] cola_command_name request name (Cola-ASCII, f.e. "IsSystemReady") + * @param[in] cola_command_args request arguments (Cola-ASCII, f.e. "") + * @param[out] service_response service_response.success set to response value from localization controller + * @return true on success, false in case of errors. + */ + template bool serviceCbWithSuccessResponse(const std::string & cola_command_type, const std::string & cola_command_name, const std::string & cola_command_args, ResponseType &service_response) + { + service_response.success = false; + std::stringstream cola_ascii; + cola_ascii << cola_command_type << " " << cola_command_name; + if(cola_command_args != "") + cola_ascii << " " << cola_command_args; + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii.str()); + if(cola_response.command_name == cola_command_name && cola_response.parameter.size() == 1) + { + service_response.success = convertColaResponseBool(cola_response.parameter[0], false); + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(\"" << cola_ascii.str() << "\") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; + } + + /* + * member variables + */ + + double m_cola_response_timeout; ///< Timeout in seconds for cola responses from localization controller, default: 1 + std::vector m_service_server; ///< list of ros service provider for services listed above + ros::ServiceClient m_service_client; ///< client to call ros service "SickLocColaTelegram" to send cola telegrams and receive cola responses from localization controller + + }; // class ColaServices + +} // namespace sick_lidar_localization +#endif // __SIM_LOC_COLA_SERVICES_H_INCLUDED diff --git a/include/sick_lidar_localization/driver_monitor.h b/include/sick_lidar_localization/driver_monitor.h index 0a21fa0..3f19352 100644 --- a/include/sick_lidar_localization/driver_monitor.h +++ b/include/sick_lidar_localization/driver_monitor.h @@ -134,6 +134,14 @@ namespace sick_lidar_localization * (if started by a cola service request "SickLocColaTelegram") */ void stopColaTransmitter(void); + + /*! + * Returns true, if result telegrams have been received within configured timeout "monitoring_message_timeout". + * If no result telegrams have been received within the timeout, the localization status is queried by ros service + * "SickLocState". If localization is not activated (LocState != 2), this function returns true (no error). + * Otherwise, result telegrams are missing and false is returned (error). + */ + bool resultTelegramsReceiveStatusIsOk(void); /*! * Thread callback, implements the monitoring and restarts a DriverThread in case of tcp-errors. @@ -155,6 +163,7 @@ namespace sick_lidar_localization sick_lidar_localization::SetGet m_driver_message_recv_timestamp; ///< time of the last received driver message (threadsafe) double m_monitoring_rate; ///< frequency to monitor driver messages, default: once per second double m_receive_telegrams_timeout; ///< timeout for driver messages, shutdown tcp-sockets and reconnect after message timeout, default: 1 second + double m_cola_response_timeout; ///< timeout in seconds for cola responses from localization controller, default: 1 sick_lidar_localization::ColaTransmitter* m_cola_transmitter; ///< transmitter for cola commands (send requests, receive responses) boost::mutex m_service_cb_mutex; ///< mutex to protect serviceCbColaTelegram (one service request at a time) diff --git a/include/sick_lidar_localization/random_generator.h b/include/sick_lidar_localization/random_generator.h index 2b13da4..f7801c2 100644 --- a/include/sick_lidar_localization/random_generator.h +++ b/include/sick_lidar_localization/random_generator.h @@ -67,7 +67,7 @@ namespace sick_lidar_localization public: /*! - * Constructor + * UniformRandomInteger constructor * @param[in] lower_bound min. value of random distribution, random numbers will be generated within the range lower_bound up to upper_bound,(lower and upper bound included) * @param[in] upper_bound max. value of random distribution, random numbers will be generated within the range lower_bound up to upper_bound (lower and upper bound included) */ @@ -85,7 +85,7 @@ namespace sick_lidar_localization * @return binary random data of length data_size */ std::vector generate(int data_size); - + protected: /* @@ -98,5 +98,35 @@ namespace sick_lidar_localization }; // class UniformIntegerRandom + /*! + * class UniformRandomAsciiString generates uniform distributed ascii strings. + */ + class UniformRandomAsciiString + { + public: + + /*! + * UniformRandomAsciiString constructor + */ + UniformRandomAsciiString(); + + /*! + * Creates and returns a random ascii string + * @param[in] length length of string + * @return random ascii string + */ + std::string generate(int length); + + + protected: + + /* + * member data + */ + UniformRandomInteger m_random_generator; ///< random number generator + static const std::string s_ascii_chars; ///< list of ascii chars: " !\"#$%&'()*+,-./0123456789:;=?@ ABCDEFGHIJKLMNOPQRSTUVWXYZ[\\]^_abcdefghijklmnopqrstuvwxyz{|}~" + + }; // class UniformRandomAsciiString + } // namespace sick_lidar_localization #endif // __SIM_LOC_RANDOM_H_INCLUDED diff --git a/include/sick_lidar_localization/testcase_generator.h b/include/sick_lidar_localization/testcase_generator.h index f09a033..f8cc4f9 100644 --- a/include/sick_lidar_localization/testcase_generator.h +++ b/include/sick_lidar_localization/testcase_generator.h @@ -110,6 +110,18 @@ namespace sick_lidar_localization * @return result pose interval */ static uint32_t ResultPoseInterval(void){ return s_u32ResultPoseInterval; } + + /*! + * Returns true, if localization is active (default), otherwise false (localization deactivated) + * @return result telegrams are activated (true) or deactivated + */ + static bool LocalizationEnabled(void); + + /*! + * Returns true, if result telegrams are activated (i.e. localization on and result telegrams active), otherwise false (result telegrams deactivated) + * @return result telegrams are activated (true, default) or deactivated + */ + static bool ResultTelegramsEnabled(void); protected: @@ -146,6 +158,7 @@ namespace sick_lidar_localization static uint32_t createTimestampTicksMilliSec(void); static uint32_t s_u32ResultPoseInterval; ///< result pose interval, i.e. the interval in number of scans (default: 1, i.e. result telegram with each processed scan) + static std::map s_controller_settings; ///< test server settings, set by sMN or sRN requests }; // class TestcaseGenerator diff --git a/launch/sim_loc_driver.launch b/launch/sim_loc_driver.launch index 570a9f0..903f24e 100644 --- a/launch/sim_loc_driver.launch +++ b/launch/sim_loc_driver.launch @@ -2,10 +2,10 @@ - + - + @@ -18,6 +18,18 @@ + + + + + + + + + + + + diff --git a/src/cola_configuration.cpp b/src/cola_configuration.cpp new file mode 100644 index 0000000..20fa59a --- /dev/null +++ b/src/cola_configuration.cpp @@ -0,0 +1,194 @@ +/* + * @brief cola_configuration sets the initial SIM result output configuration + * using ros cola services. + * Configures the following result output settings from launch file: + * LocSetResultPort, LocSetResultMode, LocSetResultPoseEnabled, LocSetResultEndianness, LocSetResultPoseInterval, + * LocRequestResultData + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include + +#include "sick_lidar_localization/cola_configuration.h" + +/*! + * Constructor + */ +sick_lidar_localization::ColaConfiguration::ColaConfiguration(ros::NodeHandle* nh) +: m_nh(nh), m_configuration_thread_running(false), m_configuration_thread(0) +{ +} + +/*! + * Destructor + */ +sick_lidar_localization::ColaConfiguration::~ColaConfiguration() +{ + stop(); +} + +/*! + * Starts transmitting the initial result output configuration to the localization controller. + * Configures the following result output settings from launch file: + * LocSetResultPort, LocSetResultMode, LocSetResultPoseEnabled, LocSetResultEndianness, LocSetResultPoseInterval, + * LocRequestResultData + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaConfiguration::start(void) +{ + m_configuration_thread_running = true; + m_configuration_thread = new boost::thread(&sick_lidar_localization::ColaConfiguration::runConfigurationThreadCb, this); + return true; +} + +/*! + * Stops transmitting the initial result output configuration to the localization controller. + */ +void sick_lidar_localization::ColaConfiguration::stop(void) +{ + m_configuration_thread_running = false; + if(m_configuration_thread) + { + m_configuration_thread->join(); + delete(m_configuration_thread); + m_configuration_thread = 0; + } +} + + + +/*! + * Thread callback, transmits the initial SIM result output configuration using ros cola services. + * Configures the following result output settings from launch file: + * LocSetResultPort, LocSetResultMode, LocSetResultPoseEnabled, LocSetResultEndianness, LocSetResultPoseInterval, + * LocRequestResultData + */ +void sick_lidar_localization::ColaConfiguration::runConfigurationThreadCb(void) +{ + // Get settings from launch file + std::map sim_configuration_map = { + {"SickLocSetResultPort", -1}, // "LocSetResultPort" value="2201": Set TCP-port for result output (default: 2201) + {"SickLocSetResultMode", -1}, // "LocSetResultMode" value="0": Set mode of result output (0=stream or 1=poll, default:0) + {"SickLocSetResultPoseEnabled", -1}, // "LocSetResultPoseEnabled" value="1": Disable (0) or enable (1) result output (default: 1, enabled) + {"SickLocSetResultEndianness", -1}, // "LocSetResultEndianness" value="0": Set endianness of result output (0 = big endian, 1 = little endian, default: 0) + {"SickLocSetResultPoseInterval", -1}, // "LocSetResultPoseInterval" value="1": Set interval of result output (0-255, interval in number of scans, 1: result with each processed scan, default: 1) + {"SickLocRequestResultData", -1} // "LocRequestResultData" value="0": If in poll mode, trigger sending the localization result of the next processed scan via TCP interface (default: 0) + }; + double retry_delay = 1.0; + ros::param::param("/sick_lidar_localization/driver/tcp_connection_retry_delay", retry_delay, retry_delay); + ROS_INFO_STREAM("ColaConfiguration: configuration thread started"); + if(ros::ok() && m_nh && m_configuration_thread_running) + { + for(std::map::iterator iter_config = sim_configuration_map.begin(); iter_config != sim_configuration_map.end(); iter_config++) + { + ros::param::param(std::string("/cola_service_node/") + iter_config->first, iter_config->second, iter_config->second); + if(iter_config->second >= 0) + ROS_INFO_STREAM("ColaConfiguration: \"" << iter_config->first << "\": " << iter_config->second); + } + } + + // Transmit SickLocSetResultPort setting to localization + if(sim_configuration_map["SickLocSetResultPort"] >= 0 && ros::ok() && m_nh && m_configuration_thread_running) + { + ros::ServiceClient service_client = m_nh->serviceClient("SickLocSetResultPort"); + sick_lidar_localization::SickLocSetResultPortSrv service_telegram; + service_telegram.request.port = sim_configuration_map["SickLocSetResultPort"]; + callService(service_telegram, service_client, retry_delay); + } + + // Transmit SickLocSetResultMode setting to localization + if(sim_configuration_map["SickLocSetResultMode"] >= 0 && ros::ok() && m_nh && m_configuration_thread_running) + { + ros::ServiceClient service_client = m_nh->serviceClient("SickLocSetResultMode"); + sick_lidar_localization::SickLocSetResultModeSrv service_telegram; + service_telegram.request.mode = sim_configuration_map["SickLocSetResultMode"]; + callService(service_telegram, service_client, retry_delay); + } + + // Transmit SickLocSetResultPoseEnabled setting to localization + if(sim_configuration_map["SickLocSetResultPoseEnabled"] >= 0 && ros::ok() && m_nh && m_configuration_thread_running) + { + ros::ServiceClient service_client = m_nh->serviceClient("SickLocSetResultPoseEnabled"); + sick_lidar_localization::SickLocSetResultPoseEnabledSrv service_telegram; + service_telegram.request.enabled = sim_configuration_map["SickLocSetResultPoseEnabled"]; + callService(service_telegram, service_client, retry_delay); + } + + // Transmit SickLocSetResultEndianness setting to localization + if(sim_configuration_map["SickLocSetResultEndianness"] >= 0 && ros::ok() && m_nh && m_configuration_thread_running) + { + ros::ServiceClient service_client = m_nh->serviceClient("SickLocSetResultEndianness"); + sick_lidar_localization::SickLocSetResultEndiannessSrv service_telegram; + service_telegram.request.endianness = sim_configuration_map["SickLocSetResultEndianness"]; + callService(service_telegram, service_client, retry_delay); + } + + // Transmit SickLocSetResultEndianness setting to localization + if(sim_configuration_map["SickLocSetResultPoseInterval"] >= 0 && ros::ok() && m_nh && m_configuration_thread_running) + { + ros::ServiceClient service_client = m_nh->serviceClient("SickLocSetResultPoseInterval"); + sick_lidar_localization::SickLocSetResultPoseIntervalSrv service_telegram; + service_telegram.request.interval = sim_configuration_map["SickLocSetResultPoseInterval"]; + callService(service_telegram, service_client, retry_delay); + } + + // Transmit SickLocSetResultEndianness setting to localization + if(sim_configuration_map["SickLocRequestResultData"] > 0 && ros::ok() && m_nh && m_configuration_thread_running) + { + ros::ServiceClient service_client = m_nh->serviceClient("SickLocRequestResultData"); + sick_lidar_localization::SickLocRequestResultDataSrv service_telegram; + callService(service_telegram, service_client, retry_delay); + } + + m_configuration_thread_running = false; + ROS_INFO_STREAM("ColaConfiguration: configuration thread finished"); +} diff --git a/src/cola_service_node.cpp b/src/cola_service_node.cpp new file mode 100644 index 0000000..4c598ce --- /dev/null +++ b/src/cola_service_node.cpp @@ -0,0 +1,97 @@ +/* + * @brief cola_service_node advertises and runs the following ros services for sick_lidar_localization: + * + * ROS service name | Definition file | Cola request telegram | Description + * "SickLocIsSystemReady" | srv/SickLocIsSystemReadySrv.srv | "sMN IsSystemReady" | Check if the system is ready + * "SickLocState" | srv/SickLocStateSrv.srv | "sRN LocState" | Read localization state + * "SickLocStartLocalizing" | srv/SickLocStartLocalizingSrv.srv | "sMN LocStartLocalizing" | Start localization + * "SickLocStop" | srv/SickLocStopSrv.srv | "sMN LocStop" | Stop localization or demo mapping + * "SickLocStopAndSave" | srv/SickLocStopAndSaveSrv.srv | "sMN LocStopAndSave" | Stop localization, save settings + * "SickLocSetResultPort" | srv/SickLocSetResultPortSrv.srv | "sMN LocSetResultPort " | Set TCP-port for result output (default: 2201) + * "SickLocSetResultMode" | srv/SickLocSetResultModeSrv.srv | "sMN LocSetResultMode " | Set mode of result output (default: stream) + * "SickLocSetResultPoseEnabled" | srv/SickLocSetResultPoseEnabledSrv.srv | "sMN LocSetResultPoseEnabled " | Disable/enable result output + * "SickLocSetResultEndianness" | srv/SickLocSetResultEndiannessSrv.srv | "sMN LocSetResultEndianness " | Set endianness of result output + * "SickLocSetResultPoseInterval" | srv/SickLocSetResultPoseIntervalSrv.srv | "sMN LocSetResultPoseInterval " | Set interval of result output + * "SickLocRequestResultData" | srv/SickLocRequestResultDataSrv.srv | "sMN LocRequestResultData" | If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. + * "SickLocSetPose" | srv/SickLocSetPoseSrv.srv | "sMN LocSetPose " | Initialize vehicle pose + * + * See Telegram-Listing-v1.1.0.241R.pdf for further details about Cola telegrams. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include + +#include "sick_lidar_localization/cola_configuration.h" +#include "sick_lidar_localization/cola_services.h" + +int main(int argc, char** argv) +{ + // Ros configuration and initialization + ros::init(argc, argv, "cola_service_node"); + ros::NodeHandle nh; + ROS_INFO_STREAM("cola_service_node started."); + + // Initialize cola services + sick_lidar_localization::ColaServices cola_services(&nh); + + // Initial configuration from launch file + sick_lidar_localization::ColaConfiguration cola_configuration(&nh); + cola_configuration.start(); + + // Run ros event loop + ros::spin(); + + // Cleanup and exit + std::cout << "cola_service_node finished." << std::endl; + ROS_INFO_STREAM("cola_service_node finished."); + return EXIT_SUCCESS; +} diff --git a/src/cola_services.cpp b/src/cola_services.cpp new file mode 100644 index 0000000..7f8c5a0 --- /dev/null +++ b/src/cola_services.cpp @@ -0,0 +1,375 @@ +/* + * @brief cola_services implements the following ros services for sick_lidar_localization: + * + * ROS service name | Definition file | Cola request telegram | Description + * "SickLocIsSystemReady" | srv/SickLocIsSystemReadySrv.srv | "sMN IsSystemReady" | Check if the system is ready + * "SickLocState" | srv/SickLocStateSrv.srv | "sRN LocState" | Read localization state + * "SickLocStartLocalizing" | srv/SickLocStartLocalizingSrv.srv | "sMN LocStartLocalizing" | Start localization + * "SickLocStop" | srv/SickLocStopSrv.srv | "sMN LocStop" | Stop localization or demo mapping + * "SickLocStopAndSave" | srv/SickLocStopAndSaveSrv.srv | "sMN LocStopAndSave" | Stop localization, save settings + * "SickLocSetResultPort" | srv/SickLocSetResultPortSrv.srv | "sMN LocSetResultPort " | Set TCP-port for result output (default: 2201) + * "SickLocSetResultMode" | srv/SickLocSetResultModeSrv.srv | "sMN LocSetResultMode " | Set mode of result output (default: stream) + * "SickLocSetResultPoseEnabled" | srv/SickLocSetResultPoseEnabledSrv.srv | "sMN LocSetResultPoseEnabled " | Disable/enable result output + * "SickLocSetResultEndianness" | srv/SickLocSetResultEndiannessSrv.srv | "sMN LocSetResultEndianness " | Set endianness of result output + * "SickLocSetResultPoseInterval" | srv/SickLocSetResultPoseIntervalSrv.srv | "sMN LocSetResultPoseInterval " | Set interval of result output + * "SickLocRequestResultData" | srv/SickLocRequestResultDataSrv.srv | "sMN LocRequestResultData" | If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. + * "SickLocSetPose" | srv/SickLocSetPoseSrv.srv | "sMN LocSetPose " | Initialize vehicle pose + * + * See Telegram-Listing-v1.1.0.241R.pdf for further details about Cola telegrams. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include + +#include "sick_lidar_localization/cola_services.h" + +/*! + * Constructor + */ +sick_lidar_localization::ColaServices::ColaServices(ros::NodeHandle *nh) : m_cola_response_timeout(1.0) +{ + if(nh) + { + ros::param::param("/sick_lidar_localization/time_sync/cola_response_timeout", m_cola_response_timeout, m_cola_response_timeout); + // Advertise ros services + m_service_server.push_back(nh->advertiseService("SickLocIsSystemReady", &sick_lidar_localization::ColaServices::serviceCbIsSystemReady, this)); + m_service_server.push_back(nh->advertiseService("SickLocState", &sick_lidar_localization::ColaServices::serviceCbLocState, this)); + m_service_server.push_back(nh->advertiseService("SickLocStartLocalizing", &sick_lidar_localization::ColaServices::serviceCbLocStartLocalizing, this)); + m_service_server.push_back(nh->advertiseService("SickLocStop", &sick_lidar_localization::ColaServices::serviceCbLocStop, this)); + m_service_server.push_back(nh->advertiseService("SickLocStopAndSave", &sick_lidar_localization::ColaServices::serviceCbLocStopAndSave, this)); + m_service_server.push_back(nh->advertiseService("SickLocSetResultPort", &sick_lidar_localization::ColaServices::serviceCbLocSetResultPort, this)); + m_service_server.push_back(nh->advertiseService("SickLocSetResultMode", &sick_lidar_localization::ColaServices::serviceCbLocSetResultMode, this)); + m_service_server.push_back(nh->advertiseService("SickLocSetResultPoseEnabled", &sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseEnabled, this)); + m_service_server.push_back(nh->advertiseService("SickLocSetResultEndianness", &sick_lidar_localization::ColaServices::serviceCbLocSetResultEndianness, this)); + m_service_server.push_back(nh->advertiseService("SickLocSetResultPoseInterval", &sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseInterval, this)); + m_service_server.push_back(nh->advertiseService("SickLocRequestResultData", &sick_lidar_localization::ColaServices::serviceCbLocRequestResultData, this)); + m_service_server.push_back(nh->advertiseService("SickLocSetPose", &sick_lidar_localization::ColaServices::serviceCbLocSetPose, this)); + // Clients for ros services "SickLocColaTelegram" + m_service_client = nh->serviceClient("SickLocColaTelegram"); + } +} + +/*! + * Destructor + */ +sick_lidar_localization::ColaServices::~ColaServices() +{ + +} + +/*! + * Sends a cola telegram using ros service "SickLocColaTelegram", receives and returns the response from localization controller + * @param[in] cola_ascii_request request (Cola-ASCII, f.e. "sMN IsSystemReady") + * @return response from localization controller + */ +sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::ColaServices::sendColaTelegram(const std::string & cola_ascii_request) +{ + sick_lidar_localization::SickLocColaTelegramSrv cola_telegram; + sick_lidar_localization::SickLocColaTelegramMsg cola_response; + cola_telegram.request.cola_ascii_request = cola_ascii_request; + cola_telegram.request.wait_response_timeout = m_cola_response_timeout; + try + { + // Send cola telegram using ros service "SickLocColaTelegram", receive response from localization server + if (!m_service_client.call(cola_telegram) || cola_telegram.response.cola_ascii_response.empty()) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(): calling ros service \"SickLocColaTelegram\" failed with request: " + << sick_lidar_localization::Utils::flattenToString(cola_telegram.request) << " response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram.response)); + return cola_response; + } + ROS_DEBUG_STREAM("ColaServices::sendColaTelegram(): request " << sick_lidar_localization::Utils::flattenToString(cola_telegram.request) + << " response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram.response) << " succesfull."); + // Decode and return response + return sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram.response.cola_ascii_response); + } + catch(const std::exception & exc) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << sick_lidar_localization::Utils::flattenToString(cola_telegram.request) + << "): cola response " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", exception " << exc.what()); + } + return cola_response; +} + +/*! + * Converts and returns the parameter of a cola ascii telegram into a numeric value. + * @param[in] cola_arg parameter of a cola ascii telegram + * @param[in] base numeric base (10 for decimal values or 16 for hex strings) + * @param[in] default_value default value returned in case of parse errors + * @return parameter converted to integer value + */ +int32_t sick_lidar_localization::ColaServices::convertColaArg(const std::string & cola_arg, int base, int32_t default_value) +{ + try + { + return std::stoi(cola_arg, 0, base); + } + catch(const std::exception & exc) + { + ROS_WARN_STREAM("## ERROR ColaServices::convertColaArg(" << cola_arg << ") failed, exception " << exc.what()); + } + return default_value; + +} + +/*! + * Converts and returns the parameter of a cola ascii response into a boolean value. + * @param[in] cola_response_arg parameter of a cola ascii response + * @param[in] default_value default value returned in case of parse errors + * @return parameter converted to boolean value + */ +bool sick_lidar_localization::ColaServices::convertColaResponseBool(const std::string & cola_response_arg, bool default_value) +{ + return ((convertColaArg(cola_response_arg, 10, (default_value ? 1 : 0)) > 0) ? true : false); +} + +/*! + * Callback for service messages (SickLocIsSystemReadySrv, Check if the system is ready). + * Sends a cola telegram "sMN IsSystemReady" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbIsSystemReady( + sick_lidar_localization::SickLocIsSystemReadySrv::Request &service_request, + sick_lidar_localization::SickLocIsSystemReadySrv::Response &service_response) +{ + return serviceCbWithSuccessResponse("sMN", "IsSystemReady", "", service_response); +} + +/*! + * Callback for service messages (SickLocStateSrv, Read localization state). + * Sends a cola telegram "sRN LocState" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbLocState( + sick_lidar_localization::SickLocStateSrv::Request &service_request, + sick_lidar_localization::SickLocStateSrv::Response &service_response) +{ + service_response.success = false; + std::string cola_ascii = "sRN LocState"; + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if(cola_response.command_name == "LocState" && cola_response.parameter.size() == 1) + { + service_response.state = convertColaArg(cola_response.parameter[0], 10, -1); + service_response.success = (service_response.state != -1); + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(\"" << cola_ascii << "\") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service messages (SickLocStartLocalizingSrv, Stop localization, save settings). + * Sends a cola telegram "Stop localization, save settings" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbLocStartLocalizing( + sick_lidar_localization::SickLocStartLocalizingSrv::Request &service_request, + sick_lidar_localization::SickLocStartLocalizingSrv::Response &service_response) +{ + return serviceCbWithSuccessResponse("sMN", "LocStartLocalizing", "", service_response); +} + +/*! + * Callback for service messages (SickLocStopSrv, Stop localization, save settings). + * Sends a cola telegram "Stop localization, save settings" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbLocStop( + sick_lidar_localization::SickLocStopSrv::Request &service_request, + sick_lidar_localization::SickLocStopSrv::Response &service_response) +{ + return serviceCbWithSuccessResponse("sMN", "LocStop", "", service_response); +} + +/*! + * Callback for service messages (SickLocStopAndSaveSrv, Stop localization, save settings). + * Sends a cola telegram "sMN LocStopAndSave" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbLocStopAndSave( + sick_lidar_localization::SickLocStopAndSaveSrv::Request &service_request, + sick_lidar_localization::SickLocStopAndSaveSrv::Response &service_response) +{ + return serviceCbWithSuccessResponse("sMN", "LocStopAndSave", "", service_response); +} + +/*! + * Callback for service messages (SickLocSetResultPortSrv, Set TCP-port for result output, default: 2201). + * Sends a cola telegram "Set mode of result output (default: stream" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetResultPort( + sick_lidar_localization::SickLocSetResultPortSrv::Request &service_request, + sick_lidar_localization::SickLocSetResultPortSrv::Response &service_response) +{ + // return serviceCbWithSuccessResponse("sMN", "LocSetResultPort", std::string("+") + std::to_string(service_request.port), service_response); + service_response.success = false; + std::string cola_ascii = "sMN LocSetResultPort +" + std::to_string(service_request.port); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if(cola_response.command_name == "LocSetResultPort" && cola_response.parameter.size() > 0) // parameter[0]: Set Result (1: success) + { + service_response.success = convertColaResponseBool(cola_response.parameter[0], false); + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(\"" << cola_ascii << "\") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; + +} + +/*! + * Callback for service messages (SickLocSetResultModeSrv, Set mode of result output, default: stream). + * Sends a cola telegram "sMN LocSetResultMode " and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetResultMode( + sick_lidar_localization::SickLocSetResultModeSrv::Request &service_request, + sick_lidar_localization::SickLocSetResultModeSrv::Response &service_response) +{ + return serviceCbWithSuccessResponse("sMN", "LocSetResultMode", std::to_string(service_request.mode), service_response); +} + +/*! + * Callback for service messages (SickLocSetResultPoseEnabledSrv, Disable/enable result output). + * Sends a cola telegram "sMN LocSetResultPoseEnabled " and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseEnabled( + sick_lidar_localization::SickLocSetResultPoseEnabledSrv::Request &service_request, + sick_lidar_localization::SickLocSetResultPoseEnabledSrv::Response &service_response) +{ + return serviceCbWithSuccessResponse("sMN", "LocSetResultPoseEnabled", std::to_string(service_request.enabled), service_response); +} + +/*! + * Callback for service messages (SickLocSetResultEndiannessSrv, Set endianness of result output). + * Sends a cola telegram "sMN LocSetResultEndianness " and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetResultEndianness( + sick_lidar_localization::SickLocSetResultEndiannessSrv::Request &service_request, + sick_lidar_localization::SickLocSetResultEndiannessSrv::Response &service_response) +{ + return serviceCbWithSuccessResponse("sMN", "LocSetResultEndianness", std::to_string(service_request.endianness), service_response); +} + +/*! + * Callback for service messages (SickLocSetResultPoseIntervalSrv, Set interval of result output). + * Sends a cola telegram "sMN LocSetResultPoseInterval " and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseInterval( + sick_lidar_localization::SickLocSetResultPoseIntervalSrv::Request &service_request, + sick_lidar_localization::SickLocSetResultPoseIntervalSrv::Response &service_response) +{ + return serviceCbWithSuccessResponse("sMN", "LocSetResultPoseInterval", std::to_string(service_request.interval), service_response); +} + +/*! + * Callback for service messages (SickLocRequestResultDataSrv, If in poll mode, trigger sending the localization result of the next processed scan via TCP interface). + * Sends a cola telegram "sMN LocRequestResultData" and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbLocRequestResultData( + sick_lidar_localization::SickLocRequestResultDataSrv::Request &service_request, + sick_lidar_localization::SickLocRequestResultDataSrv::Response &service_response) +{ + return serviceCbWithSuccessResponse("sMN", "LocRequestResultData", "", service_response); +} + +/*! + * Callback for service messages (SickLocSetPoseSrv, Initialize vehicle pose). + * Sends a cola telegram "sMN LocSetPose " and receives the response from localization controller + * using ros service "SickLocColaTelegramSrv". + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors. + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetPose( + sick_lidar_localization::SickLocSetPoseSrv::Request &service_request, + sick_lidar_localization::SickLocSetPoseSrv::Response &service_response) +{ + std::stringstream cola_args; + cola_args << std::showpos << service_request.posex << " " << service_request.posey << " " << service_request.yaw << " " << service_request.uncertainty; + return serviceCbWithSuccessResponse("sMN", "LocSetPose", cola_args.str(), service_response); +} diff --git a/src/cola_transmitter.cpp b/src/cola_transmitter.cpp index c6fb559..82a7344 100644 --- a/src/cola_transmitter.cpp +++ b/src/cola_transmitter.cpp @@ -173,6 +173,7 @@ bool sick_lidar_localization::ColaTransmitter::receive(std::vector & te bool sick_lidar_localization::ColaTransmitter::receive(boost::asio::ip::tcp::socket & socket, std::vector & telegram, double timeout, ros::Time & receive_timestamp) { telegram.clear(); + telegram.reserve(1024); try { std::vector binETX = sick_lidar_localization::ColaParser::binaryETX(); diff --git a/src/driver.cpp b/src/driver.cpp index a74ae45..7564381 100644 --- a/src/driver.cpp +++ b/src/driver.cpp @@ -79,8 +79,8 @@ int main(int argc, char** argv) // * Binary result port protocol TCP/IP int tcp_port_results = 2201; // Default: The localization controller uses IP port number 2201 to send localization results int tcp_port_cola = 2111; // For requests and to transmit settings to the localization controller: IP port number 2111 and 2112 to send telegrams and to request data, SOPAS CoLa-A or CoLa-B protocols - ros::param::param("/sim_loc_driver/localization_controller_ip_adress" , server_adress, server_adress); - ros::param::param("/sick_lidar_localization/driver/localization_controller_default_ip_adress", server_default_adress, server_default_adress); + ros::param::param("/sim_loc_driver/localization_controller_ip_address" , server_adress, server_adress); + ros::param::param("/sick_lidar_localization/driver/localization_controller_default_ip_address", server_default_adress, server_default_adress); ros::param::param("/sick_lidar_localization/driver/result_telegrams_tcp_port", tcp_port_results, tcp_port_results); ros::param::param("/sick_lidar_localization/driver/cola_telegrams_tcp_port", tcp_port_cola, tcp_port_cola); server_adress = (server_adress.empty()) ? server_default_adress : server_adress; diff --git a/src/driver_monitor.cpp b/src/driver_monitor.cpp index 40ee0e1..c179a1d 100644 --- a/src/driver_monitor.cpp +++ b/src/driver_monitor.cpp @@ -86,6 +86,7 @@ sick_lidar_localization::DriverMonitor::DriverMonitor(ros::NodeHandle * nh, cons m_cola_binary = (cola_binary_mode == 1) ? true : false; // 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) ros::param::param("/sick_lidar_localization/driver/monitoring_rate", m_monitoring_rate, m_monitoring_rate); // frequency to monitor driver messages, default: once per second ros::param::param("/sick_lidar_localization/driver/monitoring_message_timeout", m_receive_telegrams_timeout, m_receive_telegrams_timeout); // timeout for driver messages, shutdown tcp-sockets and reconnect after message timeout, default: 1 second + ros::param::param("/sick_lidar_localization/time_sync/cola_response_timeout", m_cola_response_timeout, m_cola_response_timeout); m_initialized = true; } } @@ -247,6 +248,64 @@ bool sick_lidar_localization::DriverMonitor::serviceCbColaTelegram(sick_lidar_lo return false; } +/*! + * Returns true, if result telegrams have been received within configured timeout "monitoring_message_timeout". + * If no result telegrams have been received within the timeout, the localization status is queried by ros service + * "SickLocState". If localization is not activated (LocState != 2), this function returns true (no error). + * Otherwise, result telegrams are missing and false is returned (error). + */ +bool sick_lidar_localization::DriverMonitor::resultTelegramsReceiveStatusIsOk(void) +{ + // Check timestamp of last result telegram + if((ros::Time::now() - m_driver_message_recv_timestamp.get()).toSec() <= m_receive_telegrams_timeout) + return true; // OK: result telegram received within timeout + + // Call "sRN LocState" and check state of localization (no result telegrams when localization deactivated) + sick_lidar_localization::SickLocColaTelegramSrv::Request cola_telegram_request; + sick_lidar_localization::SickLocColaTelegramSrv::Response cola_telegram_response; + cola_telegram_request.cola_ascii_request = "sRN LocState"; + cola_telegram_request.wait_response_timeout = m_cola_response_timeout; + if(!serviceCbColaTelegram(cola_telegram_request, cola_telegram_response)) + { + ROS_WARN_STREAM("## ERROR DriverMonitor: serviceCbColaTelegram failed, cola request: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_request) << ", cola response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_response)); + return false; // Error: localization controller not responding + } + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram_response.cola_ascii_response); + if(cola_response.command_name != "LocState" || cola_response.parameter.size() != 1) + { + ROS_WARN_STREAM("## ERROR DriverMonitor: unexpected cola response from localization controller, cola request: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_request) << ", cola response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_response)); + return false; // Error decoding response from localization + } + if(cola_response.parameter[0] != "2") + { + ROS_INFO_STREAM("DriverMonitor: localization deactivated, no result telegrams received (cola response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ")"); + return true; // OK: SickLocState != "2": localization deactivated, no result telegrams send or received + } + + // Call "sRN LocResultState" and check state of result output (result telegrams enabled or disabled) + cola_telegram_request.cola_ascii_request = "sRN LocResultState"; + cola_telegram_request.wait_response_timeout = m_cola_response_timeout; + if(!serviceCbColaTelegram(cola_telegram_request, cola_telegram_response)) + { + ROS_WARN_STREAM("## ERROR DriverMonitor: serviceCbColaTelegram failed, cola request: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_request) << ", cola response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_response)); + return false; // Error: localization controller not responding + } + cola_response = sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram_response.cola_ascii_response); + if(cola_response.command_name != "LocResultState" || cola_response.parameter.size() != 1) + { + ROS_WARN_STREAM("## ERROR DriverMonitor: unexpected cola response from localization controller, cola request: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_request) << ", cola response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_response)); + return false; // Error decoding response from localization + } + if(cola_response.parameter[0] == "0") + { + ROS_INFO_STREAM("DriverMonitor: result telegrams deactivated, no result telegrams received (cola response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ")"); + return true; // OK: LocResultState == "0": Result telegrams deactivated + } + + // Timeout error: Localization and result telegrams activated, but no result telegrams received + ROS_WARN_STREAM("## ERROR DriverMonitor: Localization and result telegrams activated, timeout while waiting for result telegrams"); + return false; +} /*! * Thread callback, implements the monitoring and restarts a DriverThread in case of tcp-errors. @@ -280,7 +339,7 @@ void sick_lidar_localization::DriverMonitor::runMonitorThreadCb(void) && m_monitoring_thread_running && driver_thread->isRunning() && driver_thread->isConnected() - && (ros::Time::now() - m_driver_message_recv_timestamp.get()).toSec() <= m_receive_telegrams_timeout) + && resultTelegramsReceiveStatusIsOk()) { monitoring_delay.sleep(); } diff --git a/src/random_generator.cpp b/src/random_generator.cpp index ce18b9d..6bb9107 100644 --- a/src/random_generator.cpp +++ b/src/random_generator.cpp @@ -56,8 +56,11 @@ #include "sick_lidar_localization/random_generator.h" -/* - * Constructor +const std::string sick_lidar_localization::UniformRandomAsciiString::s_ascii_chars = " !\"#$%&'()*+,-./0123456789:;=?@ABCDEFGHIJKLMNOPQRSTUVWXYZ[\\]^_abcdefghijklmnopqrstuvwxyz{|}~"; ///< static list of ascii chars + + +/*! + * UniformRandomInteger constructor * @param[in] lower_bound min. value of random distribution, random numbers will be generated within the range lower_bound up to upper_bound * @param[in] upper_bound max. value of random distribution, random numbers will be generated within the range lower_bound up to upper_bound */ @@ -66,7 +69,7 @@ sick_lidar_localization::UniformRandomInteger::UniformRandomInteger(int lower_bo { } -/* +/*! * Returns a uniform distributed integer random number within the range lower_bound up to upper_bound */ int sick_lidar_localization::UniformRandomInteger::generate(void) @@ -74,10 +77,10 @@ int sick_lidar_localization::UniformRandomInteger::generate(void) return m_random_generator(); } -/* +/*! * Creates and returns uniform distributed binary random data of a given size * @param[in] data_size number of random bytes created, size of output data - * @return binary random data of size + * @return binary random data of length data_size */ std::vector sick_lidar_localization::UniformRandomInteger::generate(int data_size) { @@ -88,3 +91,24 @@ std::vector sick_lidar_localization::UniformRandomInteger::generate(int } return data; } + +/*! + * UniformRandomAsciiString constructor + */ +sick_lidar_localization::UniformRandomAsciiString::UniformRandomAsciiString(): m_random_generator(0, (int)(s_ascii_chars.length()) - 1) +{ +} + +/*! + * Creates and returns a random ascii string + * @param[in] length length of string + * @return random ascii string + */ +std::string sick_lidar_localization::UniformRandomAsciiString::generate(int length) +{ + std::string random_string; + random_string.reserve(length); + for(size_t n = 0; n < length; n++) + random_string.push_back(s_ascii_chars[m_random_generator.generate()]); + return random_string; +} \ No newline at end of file diff --git a/src/testcase_generator.cpp b/src/testcase_generator.cpp index 1dc42a6..0bb9f93 100644 --- a/src/testcase_generator.cpp +++ b/src/testcase_generator.cpp @@ -61,7 +61,42 @@ #include "sick_lidar_localization/utils.h" -uint32_t sick_lidar_localization::TestcaseGenerator::s_u32ResultPoseInterval = 1; ///< result pose interval, i.e. the interval in number of scans (default: 1, i.e. result telegram with each processed scan) +/*! + * result pose interval, i.e. the interval in number of scans (default: 1, i.e. result telegram with each processed scan) + */ +uint32_t sick_lidar_localization::TestcaseGenerator::s_u32ResultPoseInterval = 1; + +/*! + * test server settings, set by sMN or sRN requests + */ +std::map sick_lidar_localization::TestcaseGenerator::s_controller_settings = { + {"IsSystemReady", 1}, // 0:false, 1:true (default) + {"LocState", 2}, // controller state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + {"LocResultPort", 2201}, // tcp port for result telegrams (default: 2201) + {"LocResultMode", 0}, // 0:stream (default), 1:poll + {"LocResultState", 1}, // result output: 0: disabled, 1: enabled + {"LocResultEndianness", 0}, // 0: big endian (default), 1: little endian + {"LocMapState", 1}, // map state: 0:not active, 1:active + {"LocRequestResultData", 1} // in poll mode, trigger sending the localization result of the next processed scan via TCP interface. +}; + +/*! + * Returns true, if localization is active (default), otherwise false (localization deactivated) + * @return result telegrams are activated (true) or deactivated + */ +bool sick_lidar_localization::TestcaseGenerator::LocalizationEnabled(void) +{ + return s_controller_settings["LocState"] == 2; // localization on +} + +/*! + * Returns true, if result telegrams are activated (i.e. localization on and result telegrams active), otherwise false (result telegrams deactivated) + * @return result telegrams are activated (true, default) or deactivated + */ +bool sick_lidar_localization::TestcaseGenerator::ResultTelegramsEnabled(void) +{ + return LocalizationEnabled() && s_controller_settings["LocResultState"] > 0; // localization on and result telegrams activated, otherwise result telegrams deactivated +} /*! * Creates and returns a deterministic default testcase for result port telegrams (binary telegrams and SickLocResultPortTelegramMsg) @@ -222,22 +257,6 @@ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::Testcas return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {hexstr(ticks_ms)}); } - // static test server settings (sMN or sRN requests) - // tbd: export settings by function LocalizationEnabled(), ResultTelegramsEnabled(), ResultTelegramsBigEndian(), - // and use test server settings in test_server_thread to configure result telegrams: - // s_controller_settings["LocState"]==2 && s_controller_settings["LocResultState"]>0: localization on and result telegrams activated, otherwise result telegrams deactivated - // s_controller_settings["LocResultEndianness"]==0: big endian (default), s_controller_settings["LocResultEndianness"]>0: little endian - static std::map s_controller_settings = { - {"IsSystemReady", 1}, // 0:false, 1:true (default) - {"LocState", 2}, // controller state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING - {"LocResultPort", 2201}, // tcp port for result telegrams (default: 2201) - {"LocResultMode", 0}, // 0:stream (default), 1:poll - {"LocResultState", 1}, // result output: 0: disabled, 1: enabled - {"LocResultEndianness", 0}, // 0: big endian (default), 1: little endian - {"LocMapState", 1}, // map state: 0:not active, 1:active - {"LocRequestResultData", 1} // in poll mode, trigger sending the localization result of the next processed scan via TCP interface. - }; - // Set settings from Configuration Telegrams if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocStartLocalizing") { @@ -257,7 +276,7 @@ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::Testcas if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetResultPort" && cola_request.parameter.size() == 1) { s_controller_settings["LocResultPort"] = std::strtol(cola_request.parameter[0].c_str(), 0, 0); - return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)}); } if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetResultMode" && cola_request.parameter.size() == 1) { @@ -305,7 +324,7 @@ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::Testcas { if(cola_request.command_name == iter_settings->first) { - return sick_lidar_localization::ColaParser::createColaTelegram(response_type, cola_request.command_name, {decstr(iter_settings->second)}); + return sick_lidar_localization::ColaParser::createColaTelegram(response_type, cola_request.command_name, {hexstr(iter_settings->second)}); } } } diff --git a/srv/SickLocIsSystemReadySrv.srv b/srv/SickLocIsSystemReadySrv.srv new file mode 100644 index 0000000..58608fb --- /dev/null +++ b/srv/SickLocIsSystemReadySrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocIsSystemReady for sick localization. +# +# ROS service SickLocIsSystemReady check if the system is ready +# by sending cola command ("sMN IsSystemReady"). +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool success # true: system ready response received from localization controller, false: system not ready or service failed (timeout or error status from controller) + diff --git a/srv/SickLocRequestResultDataSrv.srv b/srv/SickLocRequestResultDataSrv.srv new file mode 100644 index 0000000..cb637df --- /dev/null +++ b/srv/SickLocRequestResultDataSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocRequestResultData for sick localization. +# +# If in poll mode, ROS service SickLocRequestResultData triggers sending the localization result of the next processed scan via TCP interface +# by sending cola command ("sMN LocRequestResultData"). +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) + diff --git a/srv/SickLocSetPoseSrv.srv b/srv/SickLocSetPoseSrv.srv new file mode 100644 index 0000000..89d0062 --- /dev/null +++ b/srv/SickLocSetPoseSrv.srv @@ -0,0 +1,30 @@ +# Definition of ROS service SickLocSetPose for sick localization. +# +# ROS service SickLocSetPose initializes the vehicle pose +# by sending cola command ("sMN LocSetPose ") +# with parameter +# : x coordinate in mm +# : y coordinate in mm +# : yaw angle in millidegree, -180000 to +180000 +# : translation uncertainty in mm +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +int32 posex # x coordinate in mm +int32 posey # y coordinate in mm +int32 yaw # yaw angle in millidegree, -180000 to +180000 +int32 uncertainty # translation uncertainty in mm + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) + diff --git a/srv/SickLocSetResultEndiannessSrv.srv b/srv/SickLocSetResultEndiannessSrv.srv new file mode 100644 index 0000000..2616eae --- /dev/null +++ b/srv/SickLocSetResultEndiannessSrv.srv @@ -0,0 +1,24 @@ +# Definition of ROS service SickLocSetResultEndianness for sick localization. +# +# ROS service SickLocSetResultEndianness sets the endianness of the result output +# by sending cola command ("sMN LocSetResultEndianness ") +# with parameter +# : 0: big endian, 1: little endian, default: big endian +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +int32 endianness # 0: big endian, 1: little endian, default: big endian + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) + diff --git a/srv/SickLocSetResultModeSrv.srv b/srv/SickLocSetResultModeSrv.srv new file mode 100644 index 0000000..643ee96 --- /dev/null +++ b/srv/SickLocSetResultModeSrv.srv @@ -0,0 +1,24 @@ +# Definition of ROS service SickLocSetResultMode for sick localization. +# +# ROS service SickLocSetResultMode sets the mode of the result output (stream or poll) +# by sending cola command ("sMN LocSetResultMode ") +# with parameter +# : 0:stream, 1: poll, default: stream +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +int32 mode # 0:stream, 1: poll, default: stream + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) + diff --git a/srv/SickLocSetResultPortSrv.srv b/srv/SickLocSetResultPortSrv.srv new file mode 100644 index 0000000..39df5fa --- /dev/null +++ b/srv/SickLocSetResultPortSrv.srv @@ -0,0 +1,24 @@ +# Definition of ROS service SickLocSetResultPort for sick localization. +# +# ROS service SickLocSetResultPort sets the TCP-port for result output telegrams +# by sending cola command ("sMN LocSetResultPort ") +# with parameter +# : TCP-port for result output telegrams (default: 2201) +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +int32 port # TCP-port for result output telegrams (default: 2201) + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) + diff --git a/srv/SickLocSetResultPoseEnabledSrv.srv b/srv/SickLocSetResultPoseEnabledSrv.srv new file mode 100644 index 0000000..c9367b9 --- /dev/null +++ b/srv/SickLocSetResultPoseEnabledSrv.srv @@ -0,0 +1,24 @@ +# Definition of ROS service SickLocSetResultPoseEnabled for sick localization. +# +# ROS service SickLocSetResultPoseEnabled enables or disables result output telegrams +# by sending cola command ("sMN LocSetResultPoseEnabled ") +# with parameter +# : 0: disabled, 1: enabled, default: enabled +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +int32 enabled # 0: result output disabled, result output 1: enabled, default: enabled + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) + diff --git a/srv/SickLocSetResultPoseIntervalSrv.srv b/srv/SickLocSetResultPoseIntervalSrv.srv new file mode 100644 index 0000000..823888f --- /dev/null +++ b/srv/SickLocSetResultPoseIntervalSrv.srv @@ -0,0 +1,24 @@ +# Definition of ROS service SickLocSetResultPoseInterval for sick localization. +# +# ROS service SickLocSetResultPoseInterval sets the interval of result output telegrams +# by sending cola command ("sMN LocSetResultPoseInterval ") +# with parameter +# : 0-255, interval in number of scans, 1: result with each processed scan, default: 1 +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +int32 interval # interval in number of scans, 0-255, 1 (default): result with each processed scan + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) + diff --git a/srv/SickLocStartLocalizingSrv.srv b/srv/SickLocStartLocalizingSrv.srv new file mode 100644 index 0000000..389e1b2 --- /dev/null +++ b/srv/SickLocStartLocalizingSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocStartLocalizing for sick localization. +# +# ROS service SickLocStartLocalizing starts localization +# by sending cola command ("sMN LocStartLocalizing"). +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller (localization started), false: service failed (timeout or error status from controller) + diff --git a/srv/SickLocStateSrv.srv b/srv/SickLocStateSrv.srv new file mode 100644 index 0000000..8f6bee5 --- /dev/null +++ b/srv/SickLocStateSrv.srv @@ -0,0 +1,21 @@ +# Definition of ROS service SickLocState for sick localization. +# +# ROS service SickLocState read localization state (0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING) +# by sending cola command ("sRN LocState"). +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +int32 state # localization state (0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING) +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) + diff --git a/srv/SickLocStopAndSaveSrv.srv b/srv/SickLocStopAndSaveSrv.srv new file mode 100644 index 0000000..f48a584 --- /dev/null +++ b/srv/SickLocStopAndSaveSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocStopAndSave for sick localization. +# +# ROS service SickLocStopAndSave stops localization and saves settings +# by sending cola command ("sMN LocStopAndSave"). +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) + diff --git a/srv/SickLocStopSrv.srv b/srv/SickLocStopSrv.srv new file mode 100644 index 0000000..16531c4 --- /dev/null +++ b/srv/SickLocStopSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocStop for sick localization. +# +# ROS service SickLocStopAndSave stops localization +# by sending cola command ("sMN LocStop"). +# +# See Telegram-Listing-v1.1.0.241R.pdf for further details about +# Cola telegrams and this command. + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) + diff --git a/test/scripts/clion.bash b/test/scripts/clion.bash index 3bcdfcc..0ff92f1 100755 --- a/test/scripts/clion.bash +++ b/test/scripts/clion.bash @@ -6,7 +6,7 @@ if [ -f ../../../../devel/setup.bash ] ; then source ../../../../devel/setup.b if [ -f ../../../../install/setup.bash ] ; then source ../../../../install/setup.bash ; fi # start edit resource-files -gedit ./run_simu.bash run_demo_simu.bash ./run_error_simu.bash ./run.bash & +gedit ./run_simu.bash ./run_cola_examples.bash ./send_cola_examples.bash run_demo_simu.bash ./run_error_simu.bash ./run.bash & # start clion echo -e "Starting clion...\nNote in case of clion/cmake errors:" diff --git a/test/scripts/run.bash b/test/scripts/run.bash index c584345..e237744 100755 --- a/test/scripts/run.bash +++ b/test/scripts/run.bash @@ -45,7 +45,7 @@ rostopic echo -p "/sick_lidar_localization/driver/diagnostic" | tee -a ~/.ros/lo # Run ros driver, connect to localization controller and receive, convert and publish telegrams # -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=192.168.0.1 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_driver.log & +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=192.168.0.1 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_driver.log & # # Run online test with localization controller and ros driver for some time, press 'q' to quit test diff --git a/test/scripts/run_cola_examples.bash b/test/scripts/run_cola_examples.bash new file mode 100755 index 0000000..cd1d121 --- /dev/null +++ b/test/scripts/run_cola_examples.bash @@ -0,0 +1,68 @@ +#!/bin/bash + +# +# Run cola examples (offline-test for cola telegrams and ros services provided by the sick_lidar_localization driver) +# against local test server simulating a localization controller +# + +# +# Environment +# + +pushd ../../../.. +printf "\033c" +source /opt/ros/melodic/setup.bash +source ./devel/setup.bash +# source ./install/setup.bash + +# +# Cleanup +# + +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +rm -rf ~/.ros/* +rosclean purge -y +if [ ! -d ~/.ros/log/cola_examples ] ; then mkdir -p ~/.ros/log/cola_examples ; fi + +# +# Run test server, simulate localization controller for offline tests +# + +roslaunch sick_lidar_localization sim_loc_test_server.launch 2>&1 >> ~/.ros/log/cola_examples/sim_loc_test_server.log & +sleep 3 # make sure ros core and sim_loc_test_server are up and running + +# +# Run ros driver, connect to localization controller and receive, convert and publish telegrams +# + +# roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 2>&1 | unbuffer -p tee -a ~/.ros/log/cola_examples/sim_loc_driver.log & +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 2>&1 >> ~/.ros/log/cola_examples/sim_loc_driver.log & +sleep 1 # make sure sim_loc_test_server and sim_loc_driver are up and running + +# +# Send cola examples using ros services provided by the sick_lidar_localization driver +# + +popd +./send_cola_examples.bash 2>&1 | unbuffer -p tee -a ~/.ros/log/cola_examples/send_cola_examples.log + +# +# Cleanup and exit +# + +./killall.bash +sleep 1 ; killall roslaunch ; sleep 1 +cat ~/.ros/log/cola_examples/send_cola_examples.log +# sleep 20 +mkdir -p ./log/cola_examples +cp -rf ~/.ros/log/cola_examples/*.log ./log/cola_examples +echo -e "\nsick_lidar_localization finished. Warnings and errors:" +grep "WARN" ./log/cola_examples/*.log +grep "ERR" ./log/cola_examples/*.log +echo -e "\nsim_loc_driver check summary:" +grep -i "check messages thread summary" ./log/cola_examples/*.log +grep -i "verification summary" ./log/cola_examples/*.log +echo -e "\nsick_lidar_localization summary:" +grep -R "summary" ./log/* + diff --git a/test/scripts/run_demo_simu.bash b/test/scripts/run_demo_simu.bash index 35f0664..21bb42d 100755 --- a/test/scripts/run_demo_simu.bash +++ b/test/scripts/run_demo_simu.bash @@ -36,7 +36,7 @@ sleep 3 # make sure ros core and sim_loc_test_server are up and running # Run ros driver, connect to localization controller and receive, convert and publish telegrams # -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=127.0.0.1 & +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 & # # Visualize PointCloud2 messages by rviz: diff --git a/test/scripts/run_error_simu.bash b/test/scripts/run_error_simu.bash index a02d533..bf3654b 100755 --- a/test/scripts/run_error_simu.bash +++ b/test/scripts/run_error_simu.bash @@ -43,7 +43,7 @@ if [ ! -d ~/.ros/log/error_simu ] ; then mkdir -p ~/.ros/log/error_simu ; fi # echo -e "sick_lidar_localization error simulation: starting sick_lidar_localization sim_loc_driver.launch ..." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=127.0.0.1 sim_loc_driver_check_cfg:=message_check_error_simu.yaml 2>&1 | unbuffer -p tee -a ~/.ros/log/error_simu/sim_loc_driver_error_simu.log & +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 sim_loc_driver_check_cfg:=message_check_error_simu.yaml 2>&1 | unbuffer -p tee -a ~/.ros/log/error_simu/sim_loc_driver_error_simu.log & echo -e "sick_lidar_localization error simulation: sick_lidar_localization sim_loc_driver.launch started." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log sleep 10 @@ -112,4 +112,6 @@ popd rm -rf ./log/error_simu mkdir ./log/error_simu cp -rf ~/.ros/log/error_simu/*.log ./log/error_simu +echo -e "\nsick_lidar_localization summary:" +grep -R "summary" ./log/* diff --git a/test/scripts/run_simu.bash b/test/scripts/run_simu.bash index d438832..a58651d 100755 --- a/test/scripts/run_simu.bash +++ b/test/scripts/run_simu.bash @@ -41,7 +41,7 @@ rostopic echo -p "/sick_lidar_localization/driver/diagnostic" | tee -a ~/.ros/lo # Run ros driver, connect to localization controller and receive, convert and publish telegrams # -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=127.0.0.1 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_driver.log & +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_driver.log & sleep 1 # make sure sim_loc_test_server and sim_loc_driver are up and running # @@ -91,4 +91,6 @@ echo -e "\nsim_loc_driver check and verification summary:" grep -i "unittest_sim_loc_parser finished" ./log/*.log grep -i "check messages thread summary" ./log/*.log grep -i "verification thread summary" ./log/*.log +echo -e "\nsick_lidar_localization summary:" +grep -R "summary" ./log/* diff --git a/test/scripts/send_cola_examples.bash b/test/scripts/send_cola_examples.bash new file mode 100755 index 0000000..2adc109 --- /dev/null +++ b/test/scripts/send_cola_examples.bash @@ -0,0 +1,200 @@ +#!/bin/bash + +# +# Run examples for Cola-ASCII telegrams and sick_lidar_localization services provided by sim_loc_driver +# +# Note: ROS sim_loc_driver and test server should be up and running, start by +# roslaunch sick_lidar_localization sim_loc_test_server.launch & +# roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=127.0.0.1 & +# + +# +# Function send_cola_telegram "" "" sends and echos a cola telegram using ros service SickLocColaTelegram +# Example: send_cola_telegram "{cola_ascii_request: 'sMN IsSystemReady', wait_response_timeout: 1}" "sAN IsSystemReady 1" +# +function send_cola_telegram() { + sleep 1 + echo -e "rosservice call SickLocColaTelegram \"$1\"" + echo -e "expected answer: \"$2\"" + answer=$(rosservice call SickLocColaTelegram "$1") + echo -e "$answer" + if [[ "$answer" == *"$2"* ]]; then + echo -e "OK: service responded with expected answer.\n" + else + testcase_error_counter=$((testcase_error_counter+1)) + echo -e "## ERROR: service did NOT respond with expected answer. Press any key to continue..." ; read -n1 -s key + fi + testcase_counter=$((testcase_counter+1)) +} + +# +# Function call_service "" "" "" just calls a ros service +# Example: call_service "SickLocIsSystemReady" "{}" "success: True" # Check if the system is ready +# +function call_service() { + sleep 1 + echo -e "rosservice call $1 \"$2\"" + answer=$(rosservice call $1 "$2") # call rosservice + answer=$(echo $answer|tr -d '\n') # remove line feeds + echo -e "expected answer: \"$3\"" + echo -e "received answer: \"$answer\"" + if [[ "$answer" == *"$3"* ]]; then + echo -e "OK: service responded with expected answer.\n" + else + testcase_error_counter=$((testcase_error_counter+1)) + echo -e "## ERROR: service did NOT respond with expected answer. Press any key to continue..." ; read -n1 -s key + fi + testcase_counter=$((testcase_counter+1)) +} + +# +# Environment +# + +pushd ../../../.. +source ./devel/setup.bash +testcase_counter=0 +testcase_error_counter=0 + +# +# Toggle localization on/off and result telegrams on/off +# + +for ((n=0;n<1;n++)) ; do + call_service SickLocIsSystemReady "{}" "success: True" # Check if the system is ready + call_service SickLocState "{}" "state: 2 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocStop "{}" "success: True" # Stop localization or demo mapping + call_service SickLocState "{}" "state: 1 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocState "{}" "state: 1 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocStartLocalizing "{}" "success: True" # Start localization + call_service SickLocState "{}" "state: 2 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocState "{}" "state: 2 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocSetResultPoseEnabled "{enabled: 0}" "success: True" # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 0" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + call_service SickLocState "{}" "state: 2 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocSetResultPoseEnabled "{enabled: 1}" "success: True" # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 1" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + call_service SickLocState "{}" "state: 2 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING +done + +# +# Configure SIM by both Cola telegrams and ros services +# + +for ((n=0;n<1;n++)) ; do + + # Cola-ASCII States Telegrams + send_cola_telegram "{cola_ascii_request: 'sMN IsSystemReady', wait_response_timeout: 1}" "sAN IsSystemReady 1" # 0:true, 1:false # Check if the system is ready + send_cola_telegram "{cola_ascii_request: 'sRN LocState', wait_response_timeout: 1}" "sRA LocState 2" # 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING # Read localization state + send_cola_telegram "{cola_ascii_request: 'sMN LocStop', wait_response_timeout: 1}" "sAN LocStop 1" # 0:failed, 1:success # Stop localization or demo mapping + send_cola_telegram "{cola_ascii_request: 'sMN LocStopAndSave', wait_response_timeout: 1}" "sAN LocStopAndSave 1" # 0:failed, 1:success # Stop localization, save settings + send_cola_telegram "{cola_ascii_request: 'sMN LocStartLocalizing', wait_response_timeout: 1}" "sAN LocStartLocalizing 1" # 0:failed, 1:success # Start localization + # Cola-ASCII Result Output Configuration Telegrams + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPort +2201', wait_response_timeout: 1}" "sAN LocSetResultPort 1 1" # 0:failed, 1:success, : uint16 (default: 2201) # Set TCP-port for result output + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultMode 0', wait_response_timeout: 1}" "sAN LocSetResultMode 1" # 0:failed, 1:success, : uint8 (0:stream, 1:poll, default: stream) # Set mode of result output (stream or poll) + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPoseEnabled 1', wait_response_timeout: 1}" "sAN LocSetResultPoseEnabled 1" # 0:failed, 1:success, : uint8 (0: disabled, 1: enabled, default: enabled) # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultEndianness 0', wait_response_timeout: 1}" "sAN LocSetResultEndianness 1" # 0:failed, 1:success, : uint8 (0: big endian, 1: little endian, default: big endian) # Set endianness of result output + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPoseInterval 1', wait_response_timeout: 1}" "sAN LocSetResultPoseInterval 1" # 0:failed, 1:success, : uint8 (0-255, interval in number of scans, 1: result with each processed scan, default: 1) # Set interval of result output + send_cola_telegram "{cola_ascii_request: 'sMN LocRequestResultData', wait_response_timeout: 1}" "sAN LocRequestResultData 1" # 0:failed, 1:success # If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. + # Cola-ASCII SetPose Telegrams + send_cola_telegram "{cola_ascii_request: 'sMN LocSetPose +10300 -5200 +30000 +1000', wait_response_timeout: 1}" "sAN LocSetPose 1" # 0:failed, 1:success, : int32 (x coordinate in mm), : int32 (y coordinate in mm), : int32 (yaw angle in millidegree, -180000 to +180000), : uint16 (translation uncertainty in mm) # Initialize vehicle pose + # Cola-ASCII Result Output Settings (queries, ros service SickLocColaTelegram only) + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 1" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultPort', wait_response_timeout: 1}" "sRA LocResultPort 899" # tcp port # Read TCP port used for result output (hex value) + send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 0" # 0:stream, 1: poll # Read result mode + send_cola_telegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" "sRA LocResultEndianness 0" # 0: big endian, 1: little endian # Read endianness of result output + # Cola-ASCII Timestamp Telegrams + send_cola_telegram "{cola_ascii_request: 'sMN LocRequestTimestamp', wait_response_timeout: 1}" "sAN LocRequestTimestamp" # "sAN LocRequestTimestamp " with 4 byte timestamp (ticks in milliseconds) # Query timestamp, see "Time synchronization" + + # Cola-ASCII Start/stop localization and query settings + send_cola_telegram "{cola_ascii_request: 'sMN LocStop', wait_response_timeout: 1}" "sAN LocStop 1" # 0:failed, 1:success # Stop localization or demo mapping + send_cola_telegram "{cola_ascii_request: 'sRN LocState', wait_response_timeout: 1}" "sRA LocState 1" # 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING # Read localization state + send_cola_telegram "{cola_ascii_request: 'sMN LocStartLocalizing', wait_response_timeout: 1}" "sAN LocStartLocalizing 1" # 0:failed, 1:success # Start localization + send_cola_telegram "{cola_ascii_request: 'sRN LocState', wait_response_timeout: 1}" "sRA LocState 2" # 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING # Read localization state + # Cola-ASCII Set tcp port for result telegram and query settings + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPort +2345', wait_response_timeout: 1}" "sAN LocSetResultPort 1 1" # 0:failed, 1:success, : uint16 (default: 2201) # Set TCP-port for result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultPort', wait_response_timeout: 1}" "sRA LocResultPort 929" # tcp port # Read TCP port used for result output (hex value) + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPort +2201', wait_response_timeout: 1}" "sAN LocSetResultPort 1 1" # 0:failed, 1:success, : uint16 (default: 2201) # Set TCP-port for result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultPort', wait_response_timeout: 1}" "sRA LocResultPort 899" # tcp port # Read TCP port used for result output (hex value) + # Cola-ASCII Set result mode and query settings + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultMode 1', wait_response_timeout: 1}" "sAN LocSetResultMode 1" # 0:failed, 1:success, : uint8 (0:stream, 1:poll, default: stream) # Set mode of result output (stream or poll) + send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 1" # 0:stream, 1: poll # Read result mode + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultMode 0', wait_response_timeout: 1}" "sAN LocSetResultMode 1" # 0:failed, 1:success, : uint8 (0:stream, 1:poll, default: stream) # Set mode of result output (stream or poll) + send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 0" # 0:stream, 1: poll # Read result mode + # Cola-ASCII Set result pose enabled/disabled and query settings + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPoseEnabled 0', wait_response_timeout: 1}" "sAN LocSetResultPoseEnabled 1" # 0:failed, 1:success, : uint8 (0: disabled, 1: enabled, default: enabled) # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 0" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPoseEnabled 1', wait_response_timeout: 1}" "sAN LocSetResultPoseEnabled 1" # 0:failed, 1:success, : uint8 (0: disabled, 1: enabled, default: enabled) # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 1" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + # Cola-ASCII Set result endianness and query settings + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultEndianness 1', wait_response_timeout: 1}" "sAN LocSetResultEndianness 1" # 0:failed, 1:success, : uint8 (0: big endian, 1: little endian, default: big endian) # Set endianness of result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" "sRA LocResultEndianness 1" # 0:big endian, 1:little endian # Read endianness of result output + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultEndianness 0', wait_response_timeout: 1}" "sAN LocSetResultEndianness 1" # 0:failed, 1:success, : uint8 (0: big endian, 1: little endian, default: big endian) # Set endianness of result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" "sRA LocResultEndianness 0" # 0:big endian, 1:little endian # Read endianness of result output + + # ROS services for States Telegrams + call_service SickLocIsSystemReady "{}" "success: True" # Check if the system is ready + call_service SickLocState "{}" "state: 2 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocStop "{}" "success: True" # Stop localization or demo mapping + call_service SickLocStopAndSave "{}" "success: True" # Stop localization, save settings + call_service SickLocStartLocalizing "{}" "success: True" # Start localization + # ROS services for Result Output Configuration Telegrams + call_service SickLocSetResultPort "{port: 2201}" "success: True" # Set TCP-port for result output + call_service SickLocSetResultMode "{mode: 0}" "success: True" # Set mode of result output (stream or poll) + call_service SickLocSetResultPoseEnabled "{enabled: 1}" "success: True" # Disable/enable result output + call_service SickLocSetResultEndianness "{endianness: 0}" "success: True" # Set endianness of result output + call_service SickLocSetResultPoseInterval "{interval: 1}" "success: True" # Set interval of result output + call_service SickLocRequestResultData "{}" "success: True" # If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. + # ROS services for SetPose Telegrams + call_service SickLocSetPose "{posex: 10300, posey: -5200, yaw: 30000, uncertainty: 1000}" "success: True" # Initialize vehicle pose + # ROS services for Timestamp Telegrams + call_service SickLocRequestTimestamp "{}" "timestamp_lidar_ms" # "timestamp_lidar_ms: mean_time_vehicle_ms: delta_time_ms: ..." # Query timestamp, see "Time synchronization" + + # Change settings with ROS services for States Telegrams + call_service SickLocIsSystemReady "{}" "success: True" # Check if the system is ready + call_service SickLocState "{}" "state: 2 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocStop "{}" "success: True" # Stop localization or demo mapping + call_service SickLocState "{}" "state: 1 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocStartLocalizing "{}" "success: True" # Start localization + call_service SickLocState "{}" "state: 2 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + # Change settings with ROS services for Result Output Configuration Telegrams + call_service SickLocSetResultPort "{port: 2345}" "success: True" # Set TCP-port for result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultPort', wait_response_timeout: 1}" "sRA LocResultPort 929" # tcp port # Read TCP port used for result output (hex value) + call_service SickLocSetResultPort "{port: 2201}" "success: True" # Set TCP-port for result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultPort', wait_response_timeout: 1}" "sRA LocResultPort 899" # tcp port # Read TCP port used for result output (hex value) + call_service SickLocSetResultMode "{mode: 1}" "success: True" # Set mode of result output (stream or poll) + send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 1" # 0:stream, 1:poll # Read result mode + call_service SickLocSetResultMode "{mode: 0}" "success: True" # Set mode of result output (stream or poll) + send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 0" # 0:stream, 1:poll # Read result mode + call_service SickLocSetResultPoseEnabled "{enabled: 0}" "success: True" # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 0" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + call_service SickLocSetResultPoseEnabled "{enabled: 1}" "success: True" # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 1" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + call_service SickLocSetResultEndianness "{endianness: 1}" "success: True" # Set endianness of result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" "sRA LocResultEndianness 1" # 0:big endian, 1:little endian # Read endianness of result output + call_service SickLocSetResultEndianness "{endianness: 0}" "success: True" # Set endianness of result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" "sRA LocResultEndianness 0" # 0:big endian, 1:little endian # Read endianness of result output + +done +echo -e "send_cola_examples.bash finished.\n" +echo -e "Services and cola telegram verification summary: $testcase_counter testcases, $testcase_error_counter failures.\n" + +# +# Debug stuff +# +# rossrv show SickLocColaTelegramSrv +# rosservice info SickLocColaTelegram +# rossrv show SickLocRequestTimestampSrv +# rosservice info SickLocRequestTimestamp +# for ((n=0;n<=10;n++)) ; do +# sleep 1 ; rosservice call SickLocColaTelegram "{cola_ascii_request: 'sRN LocMapState', wait_response_timeout: 1}" # expected reponse: cola_ascii_response="sRA LocMapState 1" +# sleep 1 ; rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocRequestTimestamp', wait_response_timeout: 1}" # expected reponse: cola_ascii_response="sAN LocRequestTimestamp " +# sleep 1 ; rosservice call SickLocRequestTimestamp "{}" # expected reponse: "timestamp_lidar_ms: , mean_time_vehicle_ms: , delta_time_ms: , ..." +# sleep 1 ; rosservice call SickLocTimeSync "{timestamp_lidar_ms: 123456}" # expected reponse: "vehicle_time_valid: True vehicle_time_sec: 1573119167 vehicle_time_nsec: 380565047" +# done +# sleep 1 ; +# if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi +# if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +# sleep 1 ; killall roslaunch ; sleep 20 +# ps -elf + diff --git a/test/src/test_server_thread.cpp b/test/src/test_server_thread.cpp index a22fd25..359818c 100644 --- a/test/src/test_server_thread.cpp +++ b/test/src/test_server_thread.cpp @@ -344,39 +344,43 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadResultCb(boost::a send_telegrams_delay.sleep(); boost::system::error_code error_code; if (m_error_simulation_flag.get() == DONT_SEND) // error simulation: testserver does not send any telegrams + { + ROS_DEBUG_STREAM("TestServerThread for cresult telegrams: error simulation, server not sending any telegrams"); continue; + } if (m_error_simulation_flag.get() == SEND_RANDOM_TCP) // error simulation: testserver sends invalid random tcp packets { std::vector random_data = random_generator.generate(random_length.generate()); // binary random data of random size boost::asio::write(*p_socket, boost::asio::buffer(random_data.data(), random_data.size()), boost::asio::transfer_exactly(random_data.size()), error_code); ROS_DEBUG_STREAM("TestServerThread for result telegrams: send random data " << sick_lidar_localization::Utils::toHexString(random_data)); + continue; } - else + // create testcase is a result port telegram with random based sythetical data + sick_lidar_localization::SickLocResultPortTestcaseMsg testcase = sick_lidar_localization::TestcaseGenerator::createRandomResultPortTestcase(); + if(m_demo_move_in_circles) // simulate a sensor moving in circles { - // create testcase is a result port telegram with random based sythetical data - sick_lidar_localization::SickLocResultPortTestcaseMsg testcase = sick_lidar_localization::TestcaseGenerator::createRandomResultPortTestcase(); - if(m_demo_move_in_circles) // simulate a sensor moving in circles - { - testcase = sick_lidar_localization::TestcaseGenerator::createResultPortCircles(2.0, circle_yaw); - circle_yaw = sick_lidar_localization::Utils::normalizeAngle(circle_yaw + 1.0 * M_PI / 180); - } - if (m_error_simulation_flag.get() == SEND_INVALID_TELEGRAMS) // error simulation: testserver sends invalid telegrams (invalid data, false checksums, etc.) + testcase = sick_lidar_localization::TestcaseGenerator::createResultPortCircles(2.0, circle_yaw); + circle_yaw = sick_lidar_localization::Utils::normalizeAngle(circle_yaw + 1.0 * M_PI / 180); + } + if (m_error_simulation_flag.get() == SEND_INVALID_TELEGRAMS) // error simulation: testserver sends invalid telegrams (invalid data, false checksums, etc.) + { + int number_random_bytes = ((random_integer.generate()) % (testcase.binary_data.size())); + for(int cnt_random_bytes = 0; cnt_random_bytes < number_random_bytes; cnt_random_bytes++) { - int number_random_bytes = ((random_integer.generate()) % (testcase.binary_data.size())); - for(int cnt_random_bytes = 0; cnt_random_bytes < number_random_bytes; cnt_random_bytes++) - { - int byte_cnt = ((random_integer.generate()) % (testcase.binary_data.size())); - testcase.binary_data[byte_cnt] = (uint8_t)(random_generator.generate() & 0xFF); - } - ROS_DEBUG_STREAM("TestServerThread for result telegrams: send random binary telegram " << sick_lidar_localization::Utils::toHexString(testcase.binary_data)); + int byte_cnt = ((random_integer.generate()) % (testcase.binary_data.size())); + testcase.binary_data[byte_cnt] = (uint8_t)(random_generator.generate() & 0xFF); } - // send binary result port telegram to tcp client + ROS_DEBUG_STREAM("TestServerThread for result telegrams: send random binary telegram " << sick_lidar_localization::Utils::toHexString(testcase.binary_data)); + } + // send binary result port telegram to tcp client (if localization is "on") + if(sick_lidar_localization::TestcaseGenerator::LocalizationEnabled() && sick_lidar_localization::TestcaseGenerator::ResultTelegramsEnabled()) + { size_t bytes_written = boost::asio::write(*p_socket, boost::asio::buffer(testcase.binary_data.data(), testcase.binary_data.size()), boost::asio::transfer_exactly(testcase.binary_data.size()), error_code); if (error_code || bytes_written != testcase.binary_data.size()) { std::stringstream error_info; error_info << "## ERROR TestServerThread for result telegrams: failed to send binary result port telegram, " << bytes_written << " of " << testcase.binary_data.size() << " bytes send, error code: " << error_code.message(); - if(m_error_simulation_flag.get() == NO_ERROR) + if (m_error_simulation_flag.get() == NO_ERROR) { ROS_WARN_STREAM(error_info.str() << ", close socket and leave worker thread for result telegrams"); break; @@ -406,6 +410,9 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadResultCb(boost::a void sick_lidar_localization::TestServerThread::runWorkerThreadColaCb(boost::asio::ip::tcp::socket* p_socket) { ROS_INFO_STREAM("TestServerThread: worker thread for command requests started"); + sick_lidar_localization::UniformRandomInteger random_generator(0,255); + sick_lidar_localization::UniformRandomInteger random_length(1, 128); + sick_lidar_localization::UniformRandomAsciiString random_ascii; while(ros::ok() && m_worker_thread_running && p_socket && p_socket->is_open()) { // Read command request from tcp client @@ -413,6 +420,19 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadColaCb(boost::asi ros::Time receive_timestamp; if(sick_lidar_localization::ColaTransmitter::receive(*p_socket, request.telegram_data, 1, receive_timestamp)) { + if (m_error_simulation_flag.get() == DONT_SEND) // error simulation: testserver does not send any telegrams + { + ROS_DEBUG_STREAM("TestServerThread for command requests: error simulation, server not sending any telegrams"); + continue; + } + if (m_error_simulation_flag.get() == SEND_RANDOM_TCP) // error simulation: testserver sends invalid random tcp packets + { + boost::system::error_code error_code; + std::vector random_data = random_generator.generate(random_length.generate()); // binary random data of random size + boost::asio::write(*p_socket, boost::asio::buffer(random_data.data(), random_data.size()), boost::asio::transfer_exactly(random_data.size()), error_code); + ROS_DEBUG_STREAM("TestServerThread for command requests: send random data " << sick_lidar_localization::Utils::toHexString(random_data)); + continue; + } // command requests received, generate and send a synthetical response bool cola_binary = sick_lidar_localization::ColaAsciiBinaryConverter::IsColaBinary(request.telegram_data); if(cola_binary) @@ -422,9 +442,14 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadColaCb(boost::asi sick_lidar_localization::SickLocColaTelegramMsg telegram_msg = sick_lidar_localization::ColaParser::decodeColaTelegram(ascii_telegram); // Generate a synthetical response depending on the request sick_lidar_localization::SickLocColaTelegramMsg telegram_answer = sick_lidar_localization::TestcaseGenerator::createColaResponse(telegram_msg); - - // tbd: handle DONT_SEND, SEND_RANDOM_TCP and SEND_INVALID_TELEGRAMS as done in runWorkerThreadResultCb - + if (m_error_simulation_flag.get() == SEND_INVALID_TELEGRAMS) // error simulation: testserver sends invalid telegrams (invalid data, false checksums, etc.) + { + telegram_answer.command_name = random_ascii.generate(random_length.generate()); // random ascii string + telegram_answer.parameter.clear(); + for(int n = random_length.generate(); n > 0; n--) + telegram_answer.parameter.push_back(random_ascii.generate(random_length.generate())); // random ascii string + ROS_DEBUG_STREAM("TestServerThread for result telegrams: send random cola response " << sick_lidar_localization::Utils::flattenToString(telegram_answer)); + } // Send command response to tcp client std::vector binary_response = sick_lidar_localization::ColaParser::encodeColaTelegram(telegram_answer); std::string ascii_response = sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(binary_response); diff --git a/yaml/sim_loc_driver.yaml b/yaml/sim_loc_driver.yaml index efe7e2d..3c47dec 100644 --- a/yaml/sim_loc_driver.yaml +++ b/yaml/sim_loc_driver.yaml @@ -3,7 +3,7 @@ sick_lidar_localization: # Driver configuration. See Operation-Instruction-v1.1.0.241R.pdf, page 51, "IP port number and protocol" for default tcp ports. driver: - localization_controller_default_ip_adress: "192.168.0.1" # Default IP adress "192.168.0.1" of the localization controller (if not otherwise set by parameter "localization_controller_ip_adress") + localization_controller_default_ip_address: "192.168.0.1" # Default IP address "192.168.0.1" of the localization controller (if not otherwise set by parameter "localization_controller_ip_address") result_telegrams_tcp_port: 2201 # TCP port number of the localization controller sending localization results. To transmit the localization results to the vehicle controller, the localization controller uses IP port number 2201 to send localization results in a single direction to the external vehicle controller. cola_telegrams_tcp_port: 2111 # For requests and to transmit settings to the localization controller: IP port number 2111 and 2112 to send telegrams and to request data, SOPAS CoLa-A or CoLa-B protocols cola_binary: 0 # 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!)