From f8a028a1af3ea423341a430d679292559b41f8c6 Mon Sep 17 00:00:00 2001 From: Manuel Aiple Date: Fri, 5 May 2023 07:22:58 +0000 Subject: [PATCH] Release v2.9.0 - RMSxxxx support and NAV350 support --- .gitignore | 2 +- CHANGELOG.md | 19 +- CMakeLists.txt | 54 +- INSTALL-GENERIC.md | 6 +- INSTALL-ROS1.md | 28 +- INSTALL-ROS2.md | 52 +- README.md | 15 +- REQUIREMENTS.md | 5 + USAGE.md | 56 +- cfg/SickScan.cfg | 2 +- ...929-tokenized-msgpacks-multiscan-rviz.png} | Bin doc/addDevice.md | 4 +- doc/combi_recording.md | 2 +- .../tutorial_rms1_lms1_combination.md | 4 +- doc/driverComponentsDiagram1.txt | 4 +- doc/driverComponentsDiagram2.txt | 4 +- doc/messageSequenceDiagram2.txt | 4 +- doc/nav350.md | 84 ++ doc/nav350_ros1_screenshot2.jpg | Bin 0 -> 57947 bytes doc/radar.md | 13 +- doc/radar_datagram.md | 5 +- doc/sick_scan_segment_xd.md | 93 +- doc/sick_scan_segment_xd_01.png | Bin 0 -> 28071 bytes doc/sick_scan_segment_xd_02.png | Bin 0 -> 5812 bytes doc/sick_scansegment_xd_sopas_examples.md | 4 +- doc/software_overview.md | 14 +- driver/src/sick_generic_callback.cpp | 21 + driver/src/sick_generic_caller.cpp | 4 +- driver/src/sick_generic_field_mon.cpp | 4 + driver/src/sick_generic_laser.cpp | 47 +- driver/src/sick_generic_monitoring.cpp | 6 +- driver/src/sick_generic_parser.cpp | 104 +- driver/src/sick_generic_radar.cpp | 32 +- driver/src/sick_lmd_scandata_parser.cpp | 129 ++- driver/src/sick_nav_scandata_parser.cpp | 950 ++++++++++++++++++ driver/src/sick_scan_common.cpp | 712 ++++++++++--- driver/src/sick_scan_common_tcp.cpp | 2 +- driver/src/sick_scan_services.cpp | 193 ++-- driver/src/sick_scan_xd_api/api_impl.cpp | 186 ++++ driver/src/sick_scansegment_xd/config.cpp | 38 +- .../sick_scansegment_xd/msgpack_exporter.cpp | 2 +- .../sick_scansegment_xd/msgpack_parser.cpp | 26 +- .../ros_msgpack_publisher.cpp | 56 +- .../scansegment_threads.cpp | 30 +- driver/src/softwarePLL.cpp | 19 + examples/ros2_example_application/README.md | 2 +- include/sick_scan/sick_generic_callback.h | 10 +- include/sick_scan/sick_generic_parser.h | 24 +- include/sick_scan/sick_generic_radar.h | 4 +- include/sick_scan/sick_lmd_scandata_parser.h | 5 +- include/sick_scan/sick_nav_scandata.h | 233 +++++ include/sick_scan/sick_nav_scandata_parser.h | 119 +++ include/sick_scan/sick_ros_wrapper.h | 27 +- include/sick_scan/sick_scan_common.h | 66 +- include/sick_scan/sick_scan_services.h | 13 +- include/sick_scan/softwarePLL.h | 2 + include/sick_scan_xd_api/sick_scan_api.h | 94 +- include/sick_scansegment_xd/config.h | 16 +- .../ros_msgpack_publisher.h | 90 +- launch/demo_roscon.launch | 2 +- launch/laser_radar_combo.launch | 9 +- launch/sick_lms_5xx.launch | 2 +- launch/sick_mrs_1xxx.launch | 2 +- launch/sick_mrs_1xxx_cartographer.launch | 2 +- launch/sick_mrs_6xxx.launch | 2 +- ...egment_xd.launch => sick_multiscan.launch} | 18 +- ..._xd.launch.py => sick_multiscan.launch.py} | 4 +- ...ick_nav_3xx.launch => sick_nav_31x.launch} | 4 +- ..._1xxx.launch.py => sick_nav_31x.launch.py} | 2 +- launch/sick_nav_350.launch | 60 ++ ...s_3xx.launch.py => sick_nav_350.launch.py} | 3 +- launch/sick_nav_3xx_ascii.launch | 64 -- launch/sick_rms_1xxx.launch | 77 -- launch/sick_rms_1xxx_ascii.launch | 74 -- launch/sick_rms_2xxx.launch | 81 -- launch/sick_rms_2xxx.launch.py | 39 - launch/sick_rms_3xx_emul.launch | 69 -- ...ck_rms_3xx.launch => sick_rms_xxxx.launch} | 11 +- ..._3xx.launch.py => sick_rms_xxxx.launch.py} | 2 +- launch/test_002_combi_live.launch | 6 +- launch/test_002_combi_live_traffic.launch | 6 +- launch/test_006_radar_emul_vis.launch | 6 +- msg/NAVLandmarkData.msg | 6 + msg/NAVOdomVelocity.msg | 7 + msg/NAVPoseData.msg | 21 + msg/NAVReflectorData.msg | 30 + msg/RadarObject.msg | 4 +- package.xml | 2 +- python/api/sick_scan_api.py | 159 +++ .../src/include/sick_scan/NAVLandmarkData.h | 286 ++++++ .../src/include/sick_scan/NAVOdomVelocity.h | 238 +++++ roswrap/src/include/sick_scan/NAVPoseData.h | 360 +++++++ .../src/include/sick_scan/NAVReflectorData.h | 405 ++++++++ roswrap/src/rossimu/kinetic/src/rossimu.cpp | 7 +- ..._emu.rviz => rviz2_cfg_multiscan_emu.rviz} | 0 ....rviz => rviz2_cfg_multiscan_emu_360.rviz} | 6 +- ...=> rviz2_cfg_multiscan_emu_laserscan.rviz} | 2 +- ...rviz => rviz2_cfg_multiscan_emu_pcap.rviz} | 0 ....rviz => rviz2_cfg_multiscan_windows.rviz} | 0 ...z => rviz2_cfg_multiscan_windows_360.rviz} | 6 +- ...viz2_cfg_multiscan_windows_laserscan.rviz} | 0 .../config/rviz2_cfg_nav350_windows.rviz | 196 ++++ .../config/rviz2_cfg_rmsxxxx_windows.rviz | 223 ++++ .../config/rviz2_emulator_cfg_nav350.rviz | 222 ++++ ...0_emu.rviz => rviz_cfg_multiscan_emu.rviz} | 0 ...0.rviz => rviz_cfg_multiscan_emu_360.rviz} | 4 +- ...iz => rviz_cfg_multiscan_emu_360_2nd.rviz} | 0 ... => rviz_cfg_multiscan_emu_laserscan.rviz} | 20 +- ....rviz => rviz_emulator_api_multiscan.rviz} | 5 +- ...=> rviz_emulator_api_multiscan_polar.rviz} | 0 .../config/rviz_emulator_cfg_nav350.rviz | 109 +- .../config/rviz_emulator_cfg_rms2xxx.rviz | 6 +- .../config/rviz_emulator_cfg_ros2_rms.rviz | 4 +- .../rviz_emulator_cfg_ros2_rms2xxx.rviz | 37 +- test/emulator/launch/emulator_rms3xx.launch | 15 - ...18_rms_1xxx_ascii_rms2_objects.pcapng.json | 4 +- .../pcap_json_converter.cmd | 3 - .../multiscan_laserscan_msg_to_pointcloud.py | 117 +++ ...cap_player.py => multiscan_pcap_player.py} | 24 +- ...s100_receiver.py => multiscan_receiver.py} | 8 +- ...rver.py => multiscan_sopas_test_server.py} | 14 +- .../sick_scan_xd_api/sick_scan_xd_api_test.py | 41 +- test/python/sopas_json_test_server.py | 68 +- test/scripts/run_linux_ros1_api_test.bash | 133 ++- ...x_ros1_simu_add_transform_dynamically.bash | 34 +- ..._linux_ros1_simu_add_transform_static.bash | 39 +- test/scripts/run_linux_ros1_simu_mrs100.bash | 91 -- .../run_linux_ros1_simu_multiscan.bash | 89 ++ test/scripts/run_linux_ros1_simu_nav350.bash | 57 ++ test/scripts/run_linux_ros1_simu_rms1xxx.bash | 52 - .../run_linux_ros1_simu_rms1xxx_ascii.bash | 47 - .../run_linux_ros1_simu_rms2xxx_ascii.bash | 46 - test/scripts/run_linux_ros1_simu_rms3xx.bash | 52 - test/scripts/run_linux_ros1_simu_rmsxxxx.bash | 55 + ...ash => run_linux_ros2_simu_multiscan.bash} | 32 +- test/scripts/run_linux_ros2_simu_nav350.bash | 37 + test/scripts/run_linux_ros2_simu_rms1xxx.bash | 53 - test/scripts/run_linux_ros2_simu_rms2xxx.bash | 50 - test/scripts/run_linux_ros2_simu_rms3xx.bash | 53 - test/scripts/run_linux_ros2_simu_rmsxxxx.bash | 48 + test/scripts/run_linux_simu_mrs100.bash | 36 - test/scripts/run_linux_simu_multiscan.bash | 36 + ...r.cmd => run_win64_multiscan_emulator.cmd} | 8 +- ...n_win64_ros2_simu_mrs100_with_emulator.cmd | 31 - ...run_win64_ros2_simu_mrs100_with_player.cmd | 63 -- ...in64_ros2_simu_multiscan_with_emulator.cmd | 31 + ..._win64_ros2_simu_multiscan_with_player.cmd | 60 ++ test/scripts/run_win64_ros2_simu_nav350.cmd | 35 + test/scripts/run_win64_ros2_simu_rmsxxxx.cmd | 36 + test/scripts/run_win64_simu_mrs100.cmd | 71 -- test/scripts/run_win64_simu_multiscan.cmd | 63 ++ test/sick_scan_test_mrs_6xxx_all_echos.xml | 2 +- .../sick_scan_xd_api_test.cpp | 36 + .../sick_scan_xd_api_wrapper.c | 77 ++ ...00_recv.cpp => lidar3d_multiscan_recv.cpp} | 20 +- .../test_server/src/test_server_cola_msg.cpp | 2 +- 156 files changed, 6558 insertions(+), 1961 deletions(-) rename doc/{20210929-tokenized-msgpacks-mrs100-rviz.png => 20210929-tokenized-msgpacks-multiscan-rviz.png} (100%) create mode 100644 doc/nav350.md create mode 100644 doc/nav350_ros1_screenshot2.jpg create mode 100644 doc/sick_scan_segment_xd_01.png create mode 100644 doc/sick_scan_segment_xd_02.png create mode 100644 driver/src/sick_nav_scandata_parser.cpp create mode 100644 include/sick_scan/sick_nav_scandata.h create mode 100644 include/sick_scan/sick_nav_scandata_parser.h rename launch/{sick_scansegment_xd.launch => sick_multiscan.launch} (95%) rename launch/{sick_scansegment_xd.launch.py => sick_multiscan.launch.py} (92%) rename launch/{sick_nav_3xx.launch => sick_nav_31x.launch} (99%) rename launch/{sick_rms_1xxx.launch.py => sick_nav_31x.launch.py} (96%) create mode 100644 launch/sick_nav_350.launch rename launch/{sick_rms_3xx.launch.py => sick_nav_350.launch.py} (96%) delete mode 100644 launch/sick_nav_3xx_ascii.launch delete mode 100644 launch/sick_rms_1xxx.launch delete mode 100644 launch/sick_rms_1xxx_ascii.launch delete mode 100644 launch/sick_rms_2xxx.launch delete mode 100644 launch/sick_rms_2xxx.launch.py delete mode 100644 launch/sick_rms_3xx_emul.launch rename launch/{sick_rms_3xx.launch => sick_rms_xxxx.launch} (92%) rename launch/{sick_nav_3xx.launch.py => sick_rms_xxxx.launch.py} (96%) create mode 100644 msg/NAVLandmarkData.msg create mode 100644 msg/NAVOdomVelocity.msg create mode 100644 msg/NAVPoseData.msg create mode 100644 msg/NAVReflectorData.msg create mode 100644 roswrap/src/include/sick_scan/NAVLandmarkData.h create mode 100644 roswrap/src/include/sick_scan/NAVOdomVelocity.h create mode 100644 roswrap/src/include/sick_scan/NAVPoseData.h create mode 100644 roswrap/src/include/sick_scan/NAVReflectorData.h rename test/emulator/config/{rviz2_cfg_mrs100_emu.rviz => rviz2_cfg_multiscan_emu.rviz} (100%) rename test/emulator/config/{rviz2_cfg_mrs100_emu_360.rviz => rviz2_cfg_multiscan_emu_360.rviz} (97%) rename test/emulator/config/{rviz2_cfg_mrs100_emu_laserscan.rviz => rviz2_cfg_multiscan_emu_laserscan.rviz} (99%) rename test/emulator/config/{rviz2_cfg_mrs100_emu_pcap.rviz => rviz2_cfg_multiscan_emu_pcap.rviz} (100%) rename test/emulator/config/{rviz2_cfg_mrs100_windows.rviz => rviz2_cfg_multiscan_windows.rviz} (100%) rename test/emulator/config/{rviz2_cfg_mrs100_windows_360.rviz => rviz2_cfg_multiscan_windows_360.rviz} (97%) rename test/emulator/config/{rviz2_cfg_mrs100_windows_laserscan.rviz => rviz2_cfg_multiscan_windows_laserscan.rviz} (100%) create mode 100644 test/emulator/config/rviz2_cfg_nav350_windows.rviz create mode 100644 test/emulator/config/rviz2_cfg_rmsxxxx_windows.rviz create mode 100644 test/emulator/config/rviz2_emulator_cfg_nav350.rviz rename test/emulator/config/{rviz_cfg_mrs100_emu.rviz => rviz_cfg_multiscan_emu.rviz} (100%) rename test/emulator/config/{rviz_cfg_mrs100_emu_360.rviz => rviz_cfg_multiscan_emu_360.rviz} (98%) rename test/emulator/config/{rviz_cfg_mrs100_emu_360_2nd.rviz => rviz_cfg_multiscan_emu_360_2nd.rviz} (100%) rename test/emulator/config/{rviz_cfg_mrs100_emu_laserscan.rviz => rviz_cfg_multiscan_emu_laserscan.rviz} (97%) rename test/emulator/config/{rviz_emulator_api_mrs100.rviz => rviz_emulator_api_multiscan.rviz} (97%) rename test/emulator/config/{rviz_emulator_api_mrs100_polar.rviz => rviz_emulator_api_multiscan_polar.rviz} (100%) delete mode 100644 test/emulator/launch/emulator_rms3xx.launch create mode 100644 test/python/multiscan_laserscan_msg_to_pointcloud.py rename test/python/{mrs100_pcap_player.py => multiscan_pcap_player.py} (81%) rename test/python/{mrs100_receiver.py => multiscan_receiver.py} (97%) rename test/python/{mrs100_sopas_test_server.py => multiscan_sopas_test_server.py} (92%) delete mode 100644 test/scripts/run_linux_ros1_simu_mrs100.bash create mode 100644 test/scripts/run_linux_ros1_simu_multiscan.bash create mode 100644 test/scripts/run_linux_ros1_simu_nav350.bash delete mode 100644 test/scripts/run_linux_ros1_simu_rms1xxx.bash delete mode 100644 test/scripts/run_linux_ros1_simu_rms1xxx_ascii.bash delete mode 100644 test/scripts/run_linux_ros1_simu_rms2xxx_ascii.bash delete mode 100644 test/scripts/run_linux_ros1_simu_rms3xx.bash create mode 100644 test/scripts/run_linux_ros1_simu_rmsxxxx.bash rename test/scripts/{run_linux_ros2_simu_mrs100.bash => run_linux_ros2_simu_multiscan.bash} (75%) create mode 100644 test/scripts/run_linux_ros2_simu_nav350.bash delete mode 100644 test/scripts/run_linux_ros2_simu_rms1xxx.bash delete mode 100644 test/scripts/run_linux_ros2_simu_rms2xxx.bash delete mode 100644 test/scripts/run_linux_ros2_simu_rms3xx.bash create mode 100644 test/scripts/run_linux_ros2_simu_rmsxxxx.bash delete mode 100644 test/scripts/run_linux_simu_mrs100.bash create mode 100644 test/scripts/run_linux_simu_multiscan.bash rename test/scripts/{run_win64_mrs100_emulator.cmd => run_win64_multiscan_emulator.cmd} (84%) delete mode 100644 test/scripts/run_win64_ros2_simu_mrs100_with_emulator.cmd delete mode 100644 test/scripts/run_win64_ros2_simu_mrs100_with_player.cmd create mode 100644 test/scripts/run_win64_ros2_simu_multiscan_with_emulator.cmd create mode 100644 test/scripts/run_win64_ros2_simu_multiscan_with_player.cmd create mode 100644 test/scripts/run_win64_ros2_simu_nav350.cmd create mode 100644 test/scripts/run_win64_ros2_simu_rmsxxxx.cmd delete mode 100644 test/scripts/run_win64_simu_mrs100.cmd create mode 100644 test/scripts/run_win64_simu_multiscan.cmd rename test/src/sick_scansegment_xd/{lidar3d_mrs100_recv.cpp => lidar3d_multiscan_recv.cpp} (81%) diff --git a/.gitignore b/.gitignore index 594307c2..0c42f5b6 100644 --- a/.gitignore +++ b/.gitignore @@ -93,7 +93,6 @@ demo/scan.csv # Note: emulator testfiles (pcapng and json records) are ignored except a few very short test examples test/emulator/scandata/*.* -!test/emulator/scandata/20221018_rms_1xxx_ascii_rms2_objects.pcapng.json test/scripts/run_win64_ros1_simu_lms5xx.cmd test/scripts/make_win64_vs2022.cmd @@ -127,3 +126,4 @@ doc/sick_scan_api/20220905_Anmerkungen_Manuel_technical_pretest.pdf doc/sick_scan_api/2022-08-24.minutes.technische.details.generische.api.pdf tools/python_installer/python-3.9.13-amd64.exe +*.bak diff --git a/CHANGELOG.md b/CHANGELOG.md index 9cf494b0..37f74051 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,10 +4,19 @@ Possible types are **Added** for new features. **Changed** for changes to the ex features that will be removed in future versions **Removed** for deprecated features that were removed in this release. **Fixed** for all bug fixes. **Security** to prompt users to update in case of closed vulnerabilities. + ## Unreleased ## ## Released ## +### v2.9.0 - RMSxxxx support and NAV350 support + - **Added** RMSxxxx support, unification of RMS-1xxx and RMS-2xxx Note: RMSxxxx supports ASCII-communication mode only (Cola-A). + - **Update** #159 (nav310 angle setting compability), merge with NAV310 angle settings branch https://github.com/SICKAG/sick_scan_xd/tree/159-nav310-angle-setting-compability + - **Update** Documentation LD-LRS3600,LD-LRS3601,LD-LRS3611,LD-OEM1500,LD-OEM1501 support + - **Update** Removed obsolete RMS-3xx + - **Added** NAV350 support + - **Update** Merge lms511_field_mon (fix lms511 field parsing and wait api), default_echo_setting (activate last echo by default), monitoring_ros2_qos (ROS2 QoS configuration), scansegment_xd_support (update build instructions), rename-mrs100-multiscan, rename-fullframe-topic + ### v2.8.15 - Release Jan. 2023 - **Update** Win64 build instructions - **Update** API documentation, driver states diagrams and typos @@ -17,8 +26,8 @@ features that will be removed in future versions **Removed** for deprecated feat - **Fix** catkin_lint warnings - **Fix** ROS-2 Humble build -### v2.8.14 - Laserscan messages for Multiscan - - **Update** Laserscan messages for Multiscan lidar, #96 +### v2.8.14 - Laserscan messages for multiScan136 + - **Update** Laserscan messages for multiScan136 lidar, #96 ### v2.8.13 - Dynamical pointcloud transform and QoS configuration - **Update** Configuration of ROS quality of service by launchfile, #101 @@ -41,8 +50,8 @@ features that will be removed in future versions **Removed** for deprecated feat - **Update** Range filter settings, #98 and #108 - **Update** Preparation for RMS1xxx support (tutorial, preparation for RMS1/RMS2, not activated) -### v2.8.6 - Multiscan update - - **Update** Multiscan update for 16-bit RSSI and modified SOPAS startup sequence +### v2.8.6 - multiScan136 update + - **Update** multiScan136 update for 16-bit RSSI and modified SOPAS startup sequence ### v2.8.5 - LRS4000 update - **Update** LRS4000 extended configuration (glare detection sensitivity, echo-, mean-, median-filter) @@ -86,7 +95,7 @@ features that will be removed in future versions **Removed** for deprecated feat - **Added** Supported for LMDscandatascalefactor (LRS4xxx) ### v2.7.0 - - - **Added** V2.7.0: Support for Multiscan136 (sick_scansegment_xd) + - **Added** V2.7.0: Support for multiScan136 (sick_scansegment_xd) - **Fixed** Timestamp LaserScan-message corrected (identical timestamps in LaserScan- and PointCloud2-messages, both by Software-PLL) ### v2.6.8 - diff --git a/CMakeLists.txt b/CMakeLists.txt index ab46c58f..b55dcdcb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,35 +6,39 @@ cmake_minimum_required(VERSION 3.5) project(sick_scan) # Emulator build options +option(ENABLE_EMULATOR "Build emulator for offline and unittests (ON) or without emulator (OFF)" ON) if(CMAKE_ENABLE_EMULATOR EQUAL 1) - option(ENABLE_EMULATOR "Build emulator for offline and unittests (ON) or without emulator (OFF)" ON) # OFF (release) or ON (development) + set(ENABLE_EMULATOR ON CACHE BOOL "Build emulator for offline and unittests (ON) or without emulator (OFF)" FORCE) # OFF (release) or ON (development) message(STATUS "Option CMAKE_ENABLE_EMULATOR = ${CMAKE_ENABLE_EMULATOR}, building ${PROJECT_NAME} with emulator support, ENABLE_EMULATOR=${ENABLE_EMULATOR}") else() - option(ENABLE_EMULATOR "Build emulator for offline and unittests (ON) or without emulator (OFF)" OFF) # OFF (release) or ON (development) + set(ENABLE_EMULATOR OFF CACHE BOOL "Build emulator for offline and unittests (ON) or without emulator (OFF)" FORCE) # OFF (release) or ON (development) message(STATUS "Option CMAKE_ENABLE_EMULATOR = ${CMAKE_ENABLE_EMULATOR}, building ${PROJECT_NAME} without emulator support, ENABLE_EMULATOR=${ENABLE_EMULATOR}") endif() # LDMRS build options +option(BUILD_WITH_LDMRS_SUPPORT "Build ${PROJECT_NAME} with LDMRS support (ON, libsick_ldmrs from https://github.com/SICKAG/libsick_ldmrs required) or without LDMRS support (OFF)" OFF) if(WIN32) - option(BUILD_WITH_LDMRS_SUPPORT "Build ${PROJECT_NAME} with LDMRS support (ON, libsick_ldmrs from https://github.com/SICKAG/libsick_ldmrs required) or without LDMRS support (OFF)" OFF) + set(BUILD_WITH_LDMRS_SUPPORT OFF CACHE BOOL "Build ${PROJECT_NAME} with LDMRS support (ON, libsick_ldmrs from https://github.com/SICKAG/libsick_ldmrs required) or without LDMRS support (OFF)" FORCE) elseif(LDMRS EQUAL 0) - option(BUILD_WITH_LDMRS_SUPPORT "Build ${PROJECT_NAME} with LDMRS support (ON, libsick_ldmrs from https://github.com/SICKAG/libsick_ldmrs required) or without LDMRS support (OFF)" OFF) + set(BUILD_WITH_LDMRS_SUPPORT OFF CACHE BOOL "Build ${PROJECT_NAME} with LDMRS support (ON, libsick_ldmrs from https://github.com/SICKAG/libsick_ldmrs required) or without LDMRS support (OFF)" FORCE) else() - option(BUILD_WITH_LDMRS_SUPPORT "Build ${PROJECT_NAME} with LDMRS support (ON, libsick_ldmrs from https://github.com/SICKAG/libsick_ldmrs required) or without LDMRS support (OFF)" ON) + set(BUILD_WITH_LDMRS_SUPPORT ON CACHE BOOL "Build ${PROJECT_NAME} with LDMRS support (ON, libsick_ldmrs from https://github.com/SICKAG/libsick_ldmrs required) or without LDMRS support (OFF)" FORCE) endif() -# MRS100/SCANSEGMENT_XD/MULTISCAN136 build options +# SCANSEGMENT_XD/MULTISCAN136 build options +option(BUILD_WITH_SCANSEGMENT_XD_SUPPORT "Build ${PROJECT_NAME} with SCANSEGMENT_XD support (ON) or without SCANSEGMENT_XD support (OFF)" ON) if(SCANSEGMENT_XD EQUAL 0) - option(BUILD_WITH_SCANSEGMENT_XD_SUPPORT "Build ${PROJECT_NAME} with SCANSEGMENT_XD support (ON) or without SCANSEGMENT_XD support (OFF)" OFF) + set(BUILD_WITH_SCANSEGMENT_XD_SUPPORT OFF CACHE BOOL "Build ${PROJECT_NAME} with SCANSEGMENT_XD support (ON) or without SCANSEGMENT_XD support (OFF)" FORCE) else() - option(BUILD_WITH_SCANSEGMENT_XD_SUPPORT "Build ${PROJECT_NAME} with SCANSEGMENT_XD support (ON) or without SCANSEGMENT_XD support (OFF)" ON) + set(BUILD_WITH_SCANSEGMENT_XD_SUPPORT ON CACHE BOOL "Build ${PROJECT_NAME} with SCANSEGMENT_XD support (ON) or without SCANSEGMENT_XD support (OFF)" FORCE) endif() # Linker option visibilty +option(BUILD_LIB_HIDE_FUNCTIONS "Build ${PROJECT_NAME} library with function hiding except for SickScanApi-functions (ON) or without function hiding (OFF)" ON) if(VISIBILITY EQUAL 0) - option(BUILD_LIB_HIDE_FUNCTIONS "Build ${PROJECT_NAME} library with function hiding except for SickScanApi-functions (ON) or without function hiding (OFF)" ON) + set(BUILD_LIB_HIDE_FUNCTIONS ON CACHE BOOL "Build ${PROJECT_NAME} library with function hiding except for SickScanApi-functions (ON) or without function hiding (OFF)" FORCE) else() - option(BUILD_LIB_HIDE_FUNCTIONS "Build ${PROJECT_NAME} library with function hiding except for SickScanApi-functions (ON) or without function hiding (OFF)" OFF) + set(BUILD_LIB_HIDE_FUNCTIONS OFF CACHE BOOL "Build ${PROJECT_NAME} library with function hiding except for SickScanApi-functions (ON) or without function hiding (OFF)" FORCE) endif() # Debug or Release build options @@ -79,7 +83,7 @@ if(NOT WIN32) set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-format-overflow -fno-var-tracking-assignments") endif() -if($ENV{ROS_VERSION}) +if(NOT DEFINED ROS_VERSION AND DEFINED ENV{ROS_VERSION}) set(ROS_VERSION $ENV{ROS_VERSION}) endif() add_compile_options(-D__ROS_VERSION=${ROS_VERSION}) @@ -172,6 +176,10 @@ if(ROS_VERSION EQUAL 1) RadarPreHeaderStatusBlock.msg RadarScan.msg SickImu.msg + NAVLandmarkData.msg + NAVOdomVelocity.msg + NAVPoseData.msg + NAVReflectorData.msg ) add_message_files( DIRECTORY msg/ldmrs @@ -291,6 +299,10 @@ if(ROS_VERSION EQUAL 2) "msg/LFErecFieldMsg.msg" "msg/LFErecMsg.msg" "msg/LIDoutputstateMsg.msg" + "msg/NAVLandmarkData.msg" + "msg/NAVOdomVelocity.msg" + "msg/NAVPoseData.msg" + "msg/NAVReflectorData.msg" "msg/ros2/RadarObject.msg" "msg/RadarPreHeader.msg" "msg/RadarPreHeaderDeviceBlock.msg" @@ -331,8 +343,8 @@ else() message(STATUS "Building ${PROJECT_NAME} without ldmrs support") endif() -# Support for MRS100/SCANSEGMENT_XD/MULTISCAN136 -if(BUILD_WITH_SCANSEGMENT_XD_SUPPORT EQUAL ON) +# Support for SCANSEGMENT_XD/MULTISCAN136 +if(${BUILD_WITH_SCANSEGMENT_XD_SUPPORT} OR BUILD_WITH_SCANSEGMENT_XD_SUPPORT EQUAL ON) message(STATUS "Building ${PROJECT_NAME} with SCANSEGMENT_XD support") add_compile_options(-DSCANSEGMENT_XD_SUPPORT=1) # msgpack11 @@ -495,6 +507,7 @@ if(ROS_VERSION EQUAL 1) driver/src/${PROJECT_NAME}_services.cpp driver/src/${PROJECT_NAME}_xd_api/api_impl.cpp driver/src/${PROJECT_NAME}_xd_api/${PROJECT_NAME}_api_converter.cpp + driver/src/sick_nav_scandata_parser.cpp driver/src/softwarePLL.cpp driver/src/tcp/Mutex.cpp driver/src/tcp/SickThread.cpp @@ -580,6 +593,7 @@ else() # i.e. (ROS_VERSION EQUAL 0 OR ROS_VERSION EQUAL 2) driver/src/sick_generic_callback.cpp driver/src/sick_generic_field_mon.cpp driver/src/sick_generic_imu.cpp + driver/src/sick_nav_scandata_parser.cpp driver/src/sick_generic_laser.cpp driver/src/sick_generic_monitoring.cpp driver/src/sick_generic_parser.cpp @@ -675,7 +689,7 @@ if(ROS_VERSION EQUAL 2) if(WIN32 OR EXISTS "/opt/ros/eloquent" OR EXISTS "/opt/ros/foxy" OR EXISTS "/opt/ros/galactic") # rosidl_typesupport for ROS2 eloquent, foxy, galaxy rosidl_target_interfaces(${PROJECT_NAME}_lib ${PROJECT_NAME} "rosidl_typesupport_cpp") - rosidl_target_interfaces(${PROJECT_NAME}_shared_lib ${PROJECT_NAME} "rosidl_typesupport_cpp") + rosidl_target_interfaces(${PROJECT_NAME}_shared_lib ${PROJECT_NAME} "rosidl_typesupport_cpp") # rosidl_target_interfaces(sick_generic_caller ${PROJECT_NAME} "rosidl_typesupport_cpp") else() # rosidl_typesupport for ROS2 humble or later rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") @@ -704,10 +718,20 @@ endif() # install sick_scan_shared_lib incl. API headerfiles if(ROS_VERSION EQUAL 0) + include(GNUInstallDirs) set_target_properties(${PROJECT_NAME}_shared_lib PROPERTIES PUBLIC_HEADER "include/sick_scan_xd_api/sick_scan_api.h;python/api/sick_scan_api.py") install(TARGETS ${PROJECT_NAME}_shared_lib + EXPORT sick_scan_xd_Export LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/sick_scan_xd" + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + ) + + # Export sick_scan_shared_lib target to be used by other projects + install(EXPORT sick_scan_xd_Export + FILE sick_scan_xd-config.cmake + NAMESPACE sick_scan_xd:: + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/sick_scan_xd ) endif() diff --git a/INSTALL-GENERIC.md b/INSTALL-GENERIC.md index 0d90a8cc..642a0414 100644 --- a/INSTALL-GENERIC.md +++ b/INSTALL-GENERIC.md @@ -26,7 +26,7 @@ Run the following steps to build sick_scan_xd on Linux (no ROS required): popd ``` -4. Build msgpack11 library (required only once for Multiscan136/sick_scansegment_xd): +4. Build msgpack11 library (required only once for multiScan136): ``` mkdir -p ./build pushd ./build @@ -53,7 +53,7 @@ Note: libsick_ldmrs is only required to support LDMRS sensors. If you do not nee cmake -DROS_VERSION=0 -DLDMRS=0 -G "Unix Makefiles" ../sick_scan_xd ``` -Note: msgpack11 is only required to support Multiscan136/sick_scansegment_xd. If you do not need or want to support Multiscan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without Multiscan136/sick_scansegment_xd support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`: +Note: msgpack11 is only required to support multiScan136. If you do not need or want to support multiScan136, you can skip building msgpack. To build sick_generic_caller without multiScan136 support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`: ``` cmake -DROS_VERSION=0 -DSCANSEGMENT_XD=0 -G "Unix Makefiles" ../sick_scan_xd ``` @@ -120,7 +120,7 @@ After successful build, binary files `sick_generic_caller.exe` and `sick_scan_sh Note: LDMRS sensors are currently not supported on Windows. -Note: msgpack11 is only required to support Multiscan136/sick_scansegment_xd. If you do not need or want to support Multiscan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without Multiscan136/sick_scansegment_xd support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`: +Note: msgpack11 is only required to support multiScan136. If you do not need or want to support multiScan136, you can skip building msgpack. To build sick_generic_caller without multiScan136 support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`: ``` cmake -DROS_VERSION=0 -DSCANSEGMENT_XD=0 -G "%_cmake_string%" .. ``` diff --git a/INSTALL-ROS1.md b/INSTALL-ROS1.md index fcd96920..70f51b82 100644 --- a/INSTALL-ROS1.md +++ b/INSTALL-ROS1.md @@ -16,9 +16,9 @@ Run the following steps to build sick_scan_xd on Linux with ROS 1: git clone https://github.com/SICKAG/msgpack11.git git clone https://github.com/SICKAG/sick_scan_xd.git popd + rm -rf ./build ./build_isolated/ ./devel ./devel_isolated/ ./install ./install_isolated/ ./log/ # remove any files from a previous build ``` - -3. Build msgpack11 library (required only once for Multiscan136/sick_scansegment_xd): +3. Build msgpack11 library (required only once for multiScan136): ``` mkdir -p ./build/msgpack11 pushd ./build/msgpack11 @@ -42,17 +42,37 @@ Note: libsick_ldmrs is only required to support LDMRS sensors. If you do not nee catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -Wno-dev ``` -Note: msgpack11 is only required to support Multiscan136/sick_scansegment_xd. If you do not need or want to support Multiscan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without Multiscan136/sick_scansegment_xd support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`: +Note: msgpack11 is only required to support multiScan136. If you do not need or want to support multiScan136, you can skip building msgpack. To build sick_generic_caller without multiScan136 support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`: ``` catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DSCANSEGMENT_XD=0 -Wno-dev ``` -cmake flags can be combined. Use flags `-DLDMRS=0 -DSCANSEGMENT_XD=0` to build without LDMRS and scansegment_xd support: +cmake flags can be combined. Use flags `-DLDMRS=0 -DSCANSEGMENT_XD=0` to build **without LDMRS** and **without scansegment_xd support**: ``` catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -DSCANSEGMENT_XD=0 -Wno-dev ``` +### Summary for the different build options: + +* **Without LDMRS-support** and **without Multiscan136 support** +``` + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -DSCANSEGMENT_XD=0 -Wno-dev +``` +* **Without LDMRS-support** and **with Multiscan136 support** +``` + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -Wno-dev +``` +* **with LDMRS-support** and **without Multiscan136 support** +``` + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DSCANSEGMENT_XD=0 -Wno-dev +``` +* **with LDMRS-support** and **with Multiscan136 support** +``` + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -Wno-dev +``` + Note: To create source code documentation by doxygen, run + ``` cd ./doxygen doxygen ./docs/Doxyfile diff --git a/INSTALL-ROS2.md b/INSTALL-ROS2.md index 98134876..b907a5f3 100644 --- a/INSTALL-ROS2.md +++ b/INSTALL-ROS2.md @@ -16,6 +16,7 @@ Run the following steps to build sick_scan_xd on Linux with ROS 2: git clone https://github.com/SICKAG/msgpack11.git git clone https://github.com/SICKAG/sick_scan_xd.git popd + rm -rf ./build ./build_isolated/ ./devel ./devel_isolated/ ./install ./install_isolated/ ./log/ # remove any files from a previous build ``` 3. Build sick_generic_caller: @@ -24,6 +25,7 @@ Run the following steps to build sick_scan_xd on Linux with ROS 2: colcon build --packages-select libsick_ldmrs --event-handlers console_direct+ source ./install/setup.bash colcon build --packages-select msgpack11 --cmake-args " -DMSGPACK11_BUILD_TESTS=0" --event-handlers console_direct+ + source ./install/setup.bash colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" --event-handlers console_direct+ source ./install/setup.bash ``` @@ -33,12 +35,12 @@ Note: libsick_ldmrs is only required to support LDMRS sensors. If you do not nee ``` colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" --event-handlers console_direct+ ``` -Note: msgpack11 is only required to support Multiscan136/sick_scansegment_xd. If you do not need or want to support Multiscan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without Multiscan136/sick_scansegment_xd support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`: +Note: msgpack11 is only required to support multiScan136. If you do not need or want to support multiScan136, you can skip building msgpack. To build sick_generic_caller without multiScan136 support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`: ``` colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ ``` -cmake flags can be combined. Use flags `-DLDMRS=0 -DSCANSEGMENT_XD=0` to build without LDMRS and scansegment_xd support: +cmake flags can be combined. Use flags `-DLDMRS=0 -DSCANSEGMENT_XD=0` to build **without LDMRS** and **without scansegment_xd support**: ``` colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ ``` @@ -52,6 +54,27 @@ sudo apt install ros-${ROS_DISTRO}-diagnostic-msgs # sudo apt install ros-foxy-diagnostic-msgs ``` +### Summary for the different build options: + +* **Without LDMRS-support** and **without Multiscan136 support** + +``` +colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ +``` +* **Without LDMRS-support** and **with Multiscan136 support** +``` +colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" --event-handlers console_direct+ +``` +* **with LDMRS-support** and **without Multiscan136 support** +``` +colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ +``` +* **with LDMRS-support** and **with Multiscan136 support** +``` + colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " --event-handlers console_direct+ +``` + + Note: To create source code documentation by doxygen, run ``` cd ./doxygen @@ -72,23 +95,40 @@ To install sick_scan_xd on Windows with ROS-2, follow the steps below: 3. Clone repositories https://github.com/SICKAG/msgpack11.git and https://github.com/SICKAG/sick_scan_xd: ``` - mkdir ./src - pushd ./src + mkdir .\src + pushd .\src git clone https://github.com/SICKAG/msgpack11.git git clone https://github.com/SICKAG/sick_scan_xd.git popd ``` -4. Build sick_generic_caller: +4. Set the ROS-2 and Visual-Studio environment: + ``` + call "%ProgramFiles(x86)%\Microsoft Visual Studio\2019\Community\Common7\Tools\VsDevCmd.bat" -arch=amd64 -host_arch=amd64 + call C:\opt\ros\foxy\x64\setup.bat + ``` + Note: This step depends on your local ROS-2 and Visual-Studio installation. Please replace `C:\opt\ros\foxy\x64\setup.bat` with your ROS-2 version and adapt the path to the Visual Studio folder if your installation is different. + +5. Cleanup to insure a complete rebuild: + ``` + rmdir /s/q .\build + rmdir /s/q .\install + rmdir /s/q .\log + del /f/q .\src\CMakeLists.txt + ``` + Note: This step is only required for a complete rebuild. A complete rebuild is recommended e.g. after an update of the sick_scan_xd sources. + +6. Build sick_generic_caller: ``` colcon build --packages-select msgpack11 --cmake-args " -DMSGPACK11_BUILD_TESTS=0" --event-handlers console_direct+ + call .\install\setup.bat colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" --event-handlers console_direct+ call .\install\setup.bat ``` Note: LDMRS sensors are currently not supported on Windows. -Note: msgpack11 is only required to support Multiscan136/sick_scansegment_xd. If you do not need or want to support Multiscan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without Multiscan136/sick_scansegment_xd support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`: +Note: msgpack11 is only required to support multiScan136/sick_scansegment_xd. If you do not need or want to support multiScan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without multiScan136/sick_scansegment_xd support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`: ``` colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ ``` diff --git a/README.md b/README.md index 7926f77a..2d33e1ca 100644 --- a/README.md +++ b/README.md @@ -18,6 +18,7 @@ This project provides a driver for the SICK LiDARs and Radar sensors mentioned [ - [IMU-Support](#imu-Support) - [Radar](doc/radar.md) - [multiScan100](doc/sick_scan_segment_xd.md) +- [NAV350](doc/nav350.md) - [Software PLL](#software-pll) - [Field Evaluation Information](#field-extensions) - [SLAM-Support](doc/slam.md) @@ -79,11 +80,11 @@ Further information on the implementation and use of the experimental Imu suppor ## Radar support -See [radar documentation](doc/radar.md) for RMS1xxx and RMS3xx support. +See [radar documentation](doc/radar.md) for RMSxxxx support. -## Multiscan100 support +## multiScan100 support -See [sick_scan_segment_xd](doc/sick_scan_segment_xd.md) for Multiscan100 support. +See [sick_scan_segment_xd](doc/sick_scan_segment_xd.md) for multiScan100 support. ## Software PLL @@ -115,8 +116,7 @@ MRS6000 MRS6124 RMS1xxx RMS1000 -RMS3xx -RMS320 +RMSxxxx ROS LiDAR SICK LiDAR SICK Laser @@ -139,4 +139,9 @@ NAV245 NAV310 LDMRS LRS4000 +LD-LRS3600 +LD-LRS3601 +LD-LRS3611 +LD-OEM1500 +LD-OEM1501 multiScan100 diff --git a/REQUIREMENTS.md b/REQUIREMENTS.md index b4c0e498..9c034df2 100644 --- a/REQUIREMENTS.md +++ b/REQUIREMENTS.md @@ -15,6 +15,11 @@ This driver works with all of the following products. | LMS1xx | [e.g. 1041114](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/lms1xx/c/g91901) | | LMS4000 | [e.g. 1091423](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/lms4000/lms4111r-13000/p/p578044?ff_data) | | LDMRS | [e.g. 1115128](https://www.sick.com/de/de/p/p662073) | +| LD-LRS3600 | [1060831](https://www.sick.com/no/en/lidar-sensors/2d-lidar-sensors/ld-lrs/ld-lrs3600/p/p362656) | +| LD-LRS3601 | [1060832](https://www.sick.com/no/en/lidar-sensors/2d-lidar-sensors/ld-lrs/ld-lrs3601/p/p362657) | +| LD-LRS3611 | [1067186](https://www.sick.com/no/en/lidar-sensors/2d-lidar-sensors/ld-lrs/ld-lrs3611/p/p362658) | +| LD-OEM1500 | [1060828](https://www.sick.com/no/en/lidar-sensors/2d-lidar-sensors/ld-oem/ld-oem1500/p/p362654) | +| LD-OEM1501 | [1060829](https://www.sick.com/no/en/lidar-sensors/2d-lidar-sensors/ld-oem/ld-oem1501/p/p362655) | | LRS4000 | [1098855](https://www.sick.com/no/en/detection-and-ranging-solutions/2d-lidar-sensors/lrs4000/c/g555594) | | NAV310 | [e.g. 1060834](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/nav3xx/nav310-3211/p/p349345) | | NAV210+NAV245 | [e.g. 1074308](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/nav2xx/c/g356151) | diff --git a/USAGE.md b/USAGE.md index fa169bcd..dc63ecaa 100644 --- a/USAGE.md +++ b/USAGE.md @@ -127,6 +127,12 @@ Use the following commands to run the sick_scan_xd driver for a specific scanner * Windows native: `sick_generic_caller sick_lrs_36x1_upside_down.launch` * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x1_upside_down.launch`
For upside down mounted devices, the pointcloud is rotated by 180 deg about the x axis (180 deg roll angle). This additional rotation is configured in the launch file using parameter `add_transform_xyz_rpy` with value `"0,0,0,3.141592,0,0"`. +- For LD-OEM15xx: + * Linux native: `sick_generic_caller sick_oem_15xx.launch` + * Linux ROS-1: `roslaunch sick_scan sick_oem_15xx.launch` + * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_oem_15xx.launch` + * Windows native: `sick_generic_caller sick_oem_15xx.launch` + * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_oem_15xx.launch` - For NAV210 and NAV245: * Linux native: `sick_generic_caller sick_nav_2xx.launch` * Linux ROS-1: `roslaunch sick_scan sick_nav_2xx.launch` @@ -134,29 +140,29 @@ Use the following commands to run the sick_scan_xd driver for a specific scanner * Windows native: `sick_generic_caller sick_nav_2xx.launch` * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_2xx.launch` - For NAV310: - * Linux native: `sick_generic_caller sick_nav_3xx.launch` - * Linux ROS-1: `roslaunch sick_scan sick_nav_3xx.launch` - * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_3xx.launch` - * Windows native: `sick_generic_caller sick_nav_3xx.launch` - * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_3xx.launch` -- For RMS1xxx-family: - * Linux native: `sick_generic_caller sick_rms_1xxx.launch` - * Linux ROS-1: `roslaunch sick_scan sick_rms_1xxx.launch` - * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_1xxx.launch` - * Windows native: `sick_generic_caller sick_rms_1xxx.launch` - * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_1xxx.launch` -- For RMS2xx-family: - * Linux native: `sick_generic_caller sick_rms_2xxx.launch` - * Linux ROS-1: `roslaunch sick_scan sick_rms_2xxx.launch` - * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_2xxx.launch` - * Windows native: `sick_generic_caller sick_rms_2xxx.launch` - * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_2xxx.launch` -- For Multiscan130 (sick_scansegement_xd): - * Linux native: `sick_generic_caller sick_scansegment_xd.launch hostname:= udp_receiver_ip:=` - * Linux ROS-1: `roslaunch sick_scan sick_scansegment_xd.launch hostname:= udp_receiver_ip:=` - * Linux ROS-2: `ros2 launch sick_scan sick_scansegment_xd.launch.py hostname:= udp_receiver_ip:=` - * Windows native: `sick_generic_caller sick_scansegment_xd.launch hostname:= udp_receiver_ip:=` - * Windows ROS-2: `ros2 launch sick_scan sick_scansegment_xd.launch.py hostname:= udp_receiver_ip:=` + * Linux native: `sick_generic_caller sick_nav_31x.launch` + * Linux ROS-1: `roslaunch sick_scan sick_nav_31x.launch` + * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_31x.launch` + * Windows native: `sick_generic_caller sick_nav_31x.launch` + * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_31x.launch` +- For NAV350: + * Linux native: `sick_generic_caller sick_nav_350.launch` + * Linux ROS-1: `roslaunch sick_scan sick_nav_350.launch` + * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_350.launch` + * Windows native: `sick_generic_caller sick_nav_350.launch` + * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_350.launch` +- For RMSxxxx-family (RMS1xxx, RMS2xxx): + * Linux native: `sick_generic_caller sick_rms_xxxx.launch` + * Linux ROS-1: `roslaunch sick_scan sick_rms_xxxx.launch` + * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_xxxx.launch` + * Windows native: `sick_generic_caller sick_rms_xxxx.launch` + * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_xxxx.launch` +- For multiScan136 (sick_scansegement_xd): + * Linux native: `sick_generic_caller sick_multiscan.launch hostname:= udp_receiver_ip:=` + * Linux ROS-1: `roslaunch sick_scan sick_multiscan.launch hostname:= udp_receiver_ip:=` + * Linux ROS-2: `ros2 launch sick_scan sick_multiscan.launch.py hostname:= udp_receiver_ip:=` + * Windows native: `sick_generic_caller sick_multiscan.launch hostname:= udp_receiver_ip:=` + * Windows ROS-2: `ros2 launch sick_scan sick_multiscan.launch.py hostname:= udp_receiver_ip:=` * `hostname` is the ip-address of the lidar, `udp_receiver_ip` is the ip-address of the receiver (i.e. the ip of the computer running sick_generic_caller). Common commandline options are @@ -274,7 +280,7 @@ ros2 run sick_scan sick_generic_caller sick_tim_5xx.launch hostname:=192.168.0.7 - **Field monitoring**: The **LMS1xx**, **LMS5xx**, **TiM7xx** and **TiM7xxS** families have [extended settings for field monitoring](./doc/field_monitoring_extensions.md). -- **Radar devices**: For radar devices (RMS-1xxx, RMS-3xx), radar raw targets or radar objects or both can be tracked and transmitted. You can activate parameter transmit_raw_targets, transmit_objects or both in the launchfile: +- **Radar devices**: For radar devices (RMSxxxx), radar raw targets or radar objects or both can be tracked and transmitted. You can activate parameter transmit_raw_targets, transmit_objects or both in the launchfile: ``` @@ -313,7 +319,7 @@ ros2 service call /SCsoftreset sick_scan/srv/SCsoftresetSrv "{}" # save curr Use ros service `SickScanExit` to stop the scanner and driver: ``` -rosservice call /sick_nav_3xx/SickScanExit "{}" # stop scanner and driver on ROS-1 +rosservice call /sick_nav_31x/SickScanExit "{}" # stop scanner and driver on ROS-1 ros2 service call /SickScanExit sick_scan/srv/SickScanExitSrv "{}" # stop scanner and driver on ROS-2 ``` diff --git a/cfg/SickScan.cfg b/cfg/SickScan.cfg index 930ddbc4..2ccca5b9 100755 --- a/cfg/SickScan.cfg +++ b/cfg/SickScan.cfg @@ -86,7 +86,7 @@ gen.add("time_offset", double_t, 0, "An offset to add to the time stamp befor gen.add("sw_pll_only_publish",bool_t,0, "Publishes datagrams only if the software time synchronization is locked. This prevents leaps in the scan timestamps.") gen.add("use_generation_timestamp",bool_t, 1, "Use the lidar generation timestamp (1) or send timestamp (0) for the software pll converted message timestamp.") gen.add("auto_reboot", bool_t, 0, "Whether or not to reboot laser if it reports an error.", True) -gen.add("filter_echos", int_t, 0, "Bitmask to filter first [0], all [1], or last echos [2]", 1, 0, 2) +gen.add("filter_echos", int_t, 0, "Bitmask to filter first [0], all [1], or last echos [2]", 2, 0, 2) gen.add("powerOnCount", int_t ,0, "Read only Power On counter at start up.", 0,0,65536) gen.add("operationHours", double_t ,0, "Read only operationg hours [h].", 0,0,6553.6) gen.add("locationName", str_t,0, "Device Location Name",""), diff --git a/doc/20210929-tokenized-msgpacks-mrs100-rviz.png b/doc/20210929-tokenized-msgpacks-multiscan-rviz.png similarity index 100% rename from doc/20210929-tokenized-msgpacks-mrs100-rviz.png rename to doc/20210929-tokenized-msgpacks-multiscan-rviz.png diff --git a/doc/addDevice.md b/doc/addDevice.md index d55d1eff..c672a0ef 100644 --- a/doc/addDevice.md +++ b/doc/addDevice.md @@ -34,10 +34,10 @@ To create a new device, it is recommended to copy, rename and edit an existing l ``` sick_scan::SickGenericParser *parser = new sick_scan::SickGenericParser(scannerName); ``` -2. Add string constant like the constant SICK_SCANNER_RMS_3XX_NAME +2. Add string constant like the constant SICK_SCANNER_RMS_XXXX_NAME 3. Append this constant to allowedScannerNames - like allowedScannerNames.push_back(SICK_SCANNER_RMS_3XX_NAME); + like allowedScannerNames.push_back(SICK_SCANNER_RMS_XXXX_NAME); in the file sick_generic_parser.cpp 4. Add new parameter block like diff --git a/doc/combi_recording.md b/doc/combi_recording.md index 93722075..bb830418 100644 --- a/doc/combi_recording.md +++ b/doc/combi_recording.md @@ -1,4 +1,4 @@ -# Checking driver in combination of MRS6xxx and RMS3xx +# Checking driver in combination of MRS6xxx and RMSxxxx 1. Setup environment * 230 V Voltage converter diff --git a/doc/combination_rms_1xxx_lms_1xx/tutorial_rms1_lms1_combination.md b/doc/combination_rms_1xxx_lms_1xx/tutorial_rms1_lms1_combination.md index 07578997..92ad0c20 100644 --- a/doc/combination_rms_1xxx_lms_1xx/tutorial_rms1_lms1_combination.md +++ b/doc/combination_rms_1xxx_lms_1xx/tutorial_rms1_lms1_combination.md @@ -6,10 +6,10 @@ To demonstrate the lidar/radar combination, a RMS_1xxx and a LMS_1xxx device wer Run the following steps: -1. Connect RMS_1xxx and LMS_1xxx and start sick_scan_xd with launchfiles sick_lms_1xxx.launch and sick_rms_1xxx.launch: +1. Connect RMS_1xxx and LMS_1xxx and start sick_scan_xd with launchfiles sick_lms_1xxx.launch and sick_rms_xxxx.launch: ``` roslaunch sick_scan sick_lms_1xxx.launch - roslaunch sick_scan sick_rms_1xxx.launch + roslaunch sick_scan sick_rms_xxxx.launch ``` Make sure, that different ros node names and different IP-addresses are used. diff --git a/doc/driverComponentsDiagram1.txt b/doc/driverComponentsDiagram1.txt index cc6f01fd..f7bcdb74 100644 --- a/doc/driverComponentsDiagram1.txt +++ b/doc/driverComponentsDiagram1.txt @@ -76,10 +76,10 @@ end note note bottom of sick_scansegment_xd sick_scansegment_xd: - * SOPAS startup (MRS100) + * SOPAS startup (multiScan100) * UDP receiver thread * Process msgpacks - * Publish pointcloud + cloud_360 + * Publish pointcloud + cloud_fullframe end note note bottom of sick_scan_services diff --git a/doc/driverComponentsDiagram2.txt b/doc/driverComponentsDiagram2.txt index b47d304e..e66199f1 100644 --- a/doc/driverComponentsDiagram2.txt +++ b/doc/driverComponentsDiagram2.txt @@ -13,13 +13,13 @@ package "sick_scansegment_xd" { note top of MsgPackThreads Init and run all sick_scansegment_xd components - * SOPAS startup (MRS100, TiMTwo) + * SOPAS startup (multiScan) * Run UDP receiver thread * Parse and convert msgpacks * Collect scan segments * Validate msgpacks and scansegments * Publish pointcloud (single segments) - * Publish cloud_360 (360 pointcloud) + * Publish cloud_fullframe (full frame pointcloud) end note @enduml diff --git a/doc/messageSequenceDiagram2.txt b/doc/messageSequenceDiagram2.txt index 4a0af17e..82260b59 100644 --- a/doc/messageSequenceDiagram2.txt +++ b/doc/messageSequenceDiagram2.txt @@ -1,6 +1,6 @@ @startuml -"MRS100" -> "UDP receiver thread": UDP datagram +"multiScan100" -> "UDP receiver thread": UDP datagram "UDP receiver thread" -> "UDP receiver thread": recv message header "UDP receiver thread" -> "UDP receiver thread": recv message payload "UDP receiver thread" -> "UDP receiver thread": check CRC @@ -21,6 +21,6 @@ "MsgPackPublisher" -> "MsgPackPublisher": collect scan segments "MsgPackPublisher" -> "ROS": pointcloud -"MsgPackPublisher" -> "ROS": 360 degree pointcloud +"MsgPackPublisher" -> "ROS": full frame pointcloud @enduml diff --git a/doc/nav350.md b/doc/nav350.md new file mode 100644 index 00000000..a3321b40 --- /dev/null +++ b/doc/nav350.md @@ -0,0 +1,84 @@ +# NAV350 + +NAV-350 devices are supported by sick_scan_xd since 2023. Since they support navigation and use a different communication mode, this chapter gives an overview of the NAV-350 support in sick_scan_xd. Please refer to the manuals for further information. + +## Process loop + +Scan data, landmarks and poses of NAV-350 devices are queried by SOPAS commands with polling. Therefore the sick_scan_xd process loop runs as followed: + +1. Initialization and setup + +2. Main loop (polling):
+ 1 . Send data request "sMN mNPOSGetData 1 2"
+ 2 . Receive and parse response
+ 3 . Convert and publish pointcloud, laserscan, landmarks, pose and transform
+ 4 . API: notify listeners and run their callback functions
+ 5 . Repeat from step 1
+ +3. In case of incoming odometry messages (asynchron):
+ 1 . Convert to SOPAS command
+ 2 . Send "sMN mNPOSSetSpeed " to NAV350
+ +## Initialization and setup + +After initialization, sick_scan_xd switches to navigation mode by default. Navigation requires mapping (i.e. a valid landmark layout), which can be done by +* SOPAS ET (recommended), or +* optional mapping with parameter `nav_do_initial_mapping:=True` using the landmarks detected at start, or +* using an optional imk-file. + +Configuration and setup using SOPAS ET is most powerful and recommended. + +The default sopas initialization sequence runs as followed: +``` +"sMN SetAccessMode 3 F4724744" # switch to access level authorized client +"sMN mNEVAChangeState 1" # switch to operation mode standby +"sWN NEVACurrLayer " # set layer configured by launchfile +"sWN NLMDLandmarkDataFormat 0 1 1" # set result format (landmark data) +"sWN NAVScanDataFormat 1 1" # set result format (scan data) +"sWN NPOSPoseDataFormat 1 1" # set result format (pose data) +"sMN mNEVAChangeState 4" # switch to navigation mode +"sMN mNPOSGetData 1 2" # query pose, landmark and scan data +``` + +If optional parameter `nav_do_initial_mapping` is true, a landmark layout is initialized using the reflectors detected at startup (sopas command "sMN mNMAPDoMapping"). The sopas initialization sequence for an initial mapping runs as followed: +``` +"sMN SetAccessMode 3 F4724744" # switch to access level authorized client +"sMN mNEVAChangeState 1" # switch to operation mode standby +"sWN NEVACurrLayer " # set layer configured by launchfile +"sWN NLMDLandmarkDataFormat 0 1 1" # set result format (landmark data) +"sWN NAVScanDataFormat 1 1" # set result format (scan data) +"sWN NPOSPoseDataFormat 1 1" # set result format (pose data) +"sMN mNEVAChangeState 2" # switch to mapping mode +"sMN mNLAYEraseLayout 1" # clear landmark layout +"sWN NMAPMapCfg ..." # configure mapping parameter +"sWN NLMDReflSize " # set reflector size configured by launchfile +"sMN mNMAPDoMapping" # detect landmarks and run mapping +"sMN mNLAYAddLandmark ..." # add all detected landmarks to the layout +"sMN mNLAYStoreLayout" # store landmark layout +"sMN mNEVAChangeState 4" # switch to navigation mode +"sMN mNPOSGetData 1 2" # query pose, landmark and scan data +``` + +The landmark layout stored in an imk-file can optionally loaded at startup with optional parameter `nav_set_landmark_layout_by_imk_file`. File [20230126_nav350_4reflectors_moving.imk](../test/emulator/scandata/20230126_nav350_4reflectors_moving.imk) shows an example. See the NAV-350 manual for details about imk-files. + +The settings are configured in launchfile [sick_nav_350.launch](../launch/sick_nav_350.launch). + +## Messages + +sick_scan_xd polls the NAV350 scan data, reflectors and poses in its main loop. Scan data are published by pointcloud messages (in topic "cloud" by default). Reflectors are published by type `sick_scan/NAVLandmarkData` on topic "/sick_nav_350/nav_landmark" and as MarkerArray on topic "/sick_nav_350/nav_reflectors" for easy visualization using rviz. Poses are published by type `sick_scan/NAVPoseData` on topic "/sick_nav_350/nav_pose" and as ros transform on topic "/tf". + +The following rviz-screenshot shows the pointcloud, landmarks and pose of a NAV-350: + +![nav350_ros1_screenshot2.jpg](nav350_ros1_screenshot2.jpg) + +### Odometry messages + +Odometry messages can be sent to the NAV-350 device using ROS messages `nav_msgs/Odometry` on topic "/sick_nav_350/odom" or `sick_scan/NAVOdomVelocity` on topic "/sick_nav_350/nav_odom_velocity". Odometry messages `sick_scan/NAVOdomVelocity` specify the velocity (vx, vy) in m/s in lidar coordinates. Odometry messages `nav_msgs/Odometry` specify the velocity (vx, vy) in m/s in ros coordinates. The angular velocity is expected in radians/s. + +Example odometry messages with vx = 1 m/s, vy = -1 m/s and omega: 0.5 rad/s: +``` +rostopic pub --rate 10 /sick_nav_350/nav_odom_velocity sick_scan/NAVOdomVelocity '{vel_x: 1.0, vel_y: -1.0, omega: 0.5, timestamp: 123456789, coordbase: 0}‘ +rostopic pub --rate 10 /sick_nav_350/odom nav_msgs/Odometry '{twist: { twist: { linear: {x: 1.0, y: -1.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.5}}}}' +``` + + diff --git a/doc/nav350_ros1_screenshot2.jpg b/doc/nav350_ros1_screenshot2.jpg new file mode 100644 index 0000000000000000000000000000000000000000..e7df0963712727b0435dee733224b7a35e5ce3f9 GIT binary patch literal 57947 zcmd3N2T)r{)~+0J#+V!oW1=mR0RtA%fgFb*M3S(HMj{Fj8D<Oa?U{{!((^HyYuh=cK&+x>Q%j~D)s4m`<_m>&pCb0*Czuf-vBqY)HKxq zXU+fsXHI{BlVQM9z{T?yE?zi)@#2MxbaWRlUAcMX%H_*fm~PNtyUEJL#>UFT!oqP| zfQN&VkBf!nCy}4{1cmMa@3HfU-WR=lU*PV&yMJSHhK`Qz%B3reSFSML3&Yd~!>Ny60GN8jHr30X&EfJ|3yxP^|J2Qz}}{T#OkyayG^|wGDI~#5U}- z~B&hVB{FG33^jOQU+ytipq<94^bk?)%9|C?B|GO%PnBk6GIF<3nhep2$pe ztO9g7(hf&BO5|5n7dEGK3nfyRYP>`5_qzCGE>|m&@_Bcq%xrk5mCfM6&~Mtz`R#S6 zyZX%NBHM(*_N!gXegfw66-ZB5PVQFPdq@9N+D`r0qEEKXpz(MFa&rkgd?YSGF;16> z4f|9gvjpNGWo0vUFYJyFe$FCbS7#vJX^wKdJBDV>d)961SvTYu+gZ4sRn%c&a4B)-nmL5%i=n$w0t+SSjM|kL(*9%MYt2Yr{BUI)@wC87$((Pjv_&a<8+}d>7 zoZg`KH7*!)iTo@`Qw94Z4h-G|RqFK-D2|Y9aTP<**6xPN)=cNX;baXSE*={-bE!9<1qFDfyFu%Rm3Uxnz845Mp9prw4-TCS^BcDo>FXS z>m6%N;x;@TQqWW(?yh!rj$uy>O_X&hzR>I>^8kuUh&47@>zQ#~ly8Gsds>45eG<;%53tC^{#n(Tmzm?RAsumA7nz>tnX} zXXz_m9R$8R8syoq1I&PU0>!^NtbSy|VQJmpF9t0#Jo-<=aC%?B-~?d0BgPw*a{|co zi~Gy6=X1Z$K)#;<9GP&+RrnJ?%?beh7p$#5t}PxHR(F36ib{hL2_sRmLuXv9=B&1I zA6qu%N_F{nwfy|Y>vTj(Q_H&TjK~NRVN(>@&I_UuMlFagS<}t-<9I`VksJ}E*P!-R zz<(Fag1DaTz8Y;6@vdmLpdanWvXbKnj&0B0byFF0pcp`K3QJ$>Cm&{CjwekHR7J>S zs}XS670<9aSTf*&D{_;Efpo|k7$UiLp|8fHVa=^q#@UgvG~uPn{-Q8hc9ih4mSucBdD<4q;fqO9=K}d@ixuxcu^r=Z!c< z;rN<*9DPxJ&Q9}# z66~5wWyy@o*MWyMU#`m~DgW9&;o2xB*>BDfYv&~9A0<6I=PF3X&gb`&O13#K%46<> zxKu}Q!cpz8XtGEluq9e$H9`GJ{t5%cF=J*?ZT29wtU|~S8XT=h6FULaIS18`Kc|gU zo&f$a{o0{>o0Le-w|Bl!8P;6PldoQntxS)45M?jD;)#^z>NBa#>ggpG#*m@%xvBA&v)VC+)E>FfWodSq?)K>>2@NR7G--KGCQu;Z&7 zVzLt&zD|za)=}{V$zO^6t!mgHZwRb0y=k6@OcSdB0<%iB9bUs&P|w}Nqdi12aoOzx zj`8$P;h7jg6Mp#0HdWS#`UnA=8Q;Sc3{)<2{~)A%s+9`7L(5-X7GeDC!DILJDK9uh z1J?Vd$V17b7#ltVL=wun6z{?ajXwAQ;g`)0Na;}}y^GDPq5a9l;z>ESrL85QV` zUg9ESMU6w~xXbuC*guEs**~f0q|vD{&fKs@+f=L>-qtc7zt}hPX#t#rpH@?&$~$Hb zA+q8e8aHUyWiLZSM&caxdRt*k=*zaa&8+$Oh{L|Sj~8w394lYp9T3t1j@r}8 z&W#&p*8ptMjXwUa)OhUlZm%dMRpB%3=V1N96F?&0vB+;+*?eo62iLy$@c8y;v%a(tMrSkI74tTW z^LiJgPpfS4dOp1ipaV|cxY+zuF-*cs*o?TJ;pzTp`^}0v$q(U-Fi)@nGeCmhWI#Xm zJT(7?e3A{wbNvWrFko%oluJXdA*$n!_|}(4 zz@N)Dz=F0c)6=n+`E^MGm=%8zX?ECvb=t^IBEI9N+Gl)domH#b5w`ez3{|9v$uHZF zmuzGQ)a~^*^o@IM%#6+Q8RwYwc@>iA@OI@nyyw%tq-5EhYwz_s>Q~fA9LL^5awN~$ zZ1-O#j@c%iF6c2;eX!yp{c2G%lQC?-myLEvPgKzJz~)dGK$iJX;P*h_JClT+^Mmh zN!MB`F1aNFf|q9YTY^)xX^E)cW zu+XsHrCBM_#B6jWn&1qJw#^fT&-bq|&o?R;e0)%URU73euJYDm&g5vc<1y<+_p5~g z>ekh#yP@J4cj}UHvu%2zwLR5DxXW&ZI}rw^N>Oe%K3xxc+2kPcfne(SMsk4AWsCUy zd?XLouBxs!C%98eZmzC?vRBP9JbxH?$8y((GeWZzD>|(6Hr0GwTrz8?WOX}Ag545j zAMNsdt+&RYCEsbs-YE~Px2-M>mb*Ua;z+X?yfN0PjC$d{=q|)ze_y2K8S6Q8{iwZ~ zL#NBVj1-M7Uv>^PeCQ~#ufrHBBs3e|x@AFWFoN8?*EjTBNZ6!%DD4`p3B-mndLAJS`_u?`V zqg9@;ay$WZ`bhSA0uGlxoBL&L-*I_=6x@)-$z9>a{o5T}-$nr}Mf# z`$v0lw@OHwLQCb7BR#N@i+%8Y*c;tsp9`@`3XCuvh5aSR{q$(JwTpB2W*%DaR1``y z$2Hlm)-&aM+{@UYaMXdRvS1Z3+#6ruZ8%!ZT*JhktzWw@@6noC81#Ceb)A}0X~k(C zMe6W3I<-go4u-O!=Zf)TuqM=2LFIkR>)bA4OpV3p4BPk^>)lm2DY&Gn(j-k9+M2(V zMN5}EBRV?rIuCIEmbrIz`oWFel8PS$Cb^mV<+-b!*w$YDT^?(g$}riy58Ulb)p&}6 z*k*d$>RrI2*j+MMtj8UsIQdY?p1CDrvG9&Iyf@_0Id*h=Pm5JcZ%%<(VR0sctG%1A z4->4f<=FICQ0L!d30qs-7qFsE^+1bOk=1469mzwRdjRfdhdIjG11ErX>-~Vz28{mWCxGN> z?VHteN?^H8#)zUm!K99% zm_dgT1756n@Il8CyIr9N zSM~5?&VBh;Mw}2=9ZZB?M1WOI(kpjLRH@!i3R_0RmsYjgr8{yaWql&QN8WFoSudc# zUX{No&+QKAnbtJmE~Hco=HyvB%S6tqdy7@XO5MXI^3<`Hx?l9W>gKLxW8Q?kCBsi# zNXM9B*Kl%*p3}N%OSywQUq+OUD(q5U*2~|GRL`u>=4fwAZtWyGA5{v-(?@vT`m*dL z5Mp*@uO@PRCZJLsg-%#pNfVb1>N+n;q6JSaAJ!F9$kb79E( z3C?B(hE6bsEWy#K_srB5h$ZHd#rnBP{o1;18)Cxr*v^r5%R^;5p+bZ{#txVfhTpFC$! zR?Bwv!$Gg;w+)Z3{QfJOy_l~0ZS&l$@2W>BjLf*-T0lB>oF(x@^Ib%d#xOA~i_9#? z0fMf$2gz>MyhXufG*LbRbGZ#mvum*WZ@FU~W4Yl?8!T$1yw@mP6d&8Z37dt?ZXJ@u z@F2IVLf2`iioOpW=@7YOQf56ZG4lbFP`9KIsm~HlS?U{F9om_9z5GjU*^TnpN%sY` zF*PjH9;JAAbR9`MdgBuyBX!vf@!Y+naHnsim05$65gaH7=XJ^pc0LrHS(!99qm6ft z|DN(ghW(LVv)7&gres+;*$J45zsCE;2NuTsLVrK7m#1erc(&;T5PolAtBuEtZu^jD zzestqH*FL?9v~ zKSV}t!4@}afYDbfP4Rkqf9!gKFN4G&hpZ_&bSFdwYz<6vg^qb`?MG@2EGx1v8{<;P zJD=iXuTWmB^kAs+BQbVP?$L@sd~pKn3V*Qf7BAi#2xLJ_KQtL>PguDJ z96+u#b1GxuLP#VjU_NXlc`;jVf&F1FUa}a{T{e+9`g z1CctIo=|VHYLU8mpjNdqf1wB}qRPMNXbj^9}a?8|~e&e_XA)PDVLL9778cG<;37=gx zR+S!>&LRj2gl2@bB&zs>EA_Qp+dS#7>>p+7U#e1vaYpS`=4o;?%*?t1X=%K)K()6) z-;-V!HlrN0_67zLh+ER~?}wu5j?_0qi@(t3R&9*7R}+laNDcj>^=BzEgbbx-XuB>6 z^?DkPNRLiqs?kN2^;Sd*jA?;*ey+IkGSj|zsU|DLypwZ!j$nTwGS5-im!q(e*iInY zy7c*PKQDT)1j$tM5tnQ$Wh2J&yTmff-wGygfGm!t@Wb)0{^K;i>&Tg5R?$}bI-+NHW&G;u(04UvZea$4qsvZe@sDbF7jo~i$V64_wcY++{(9v4~DvoDl$ zR#XfazxV0!udhI%+5?A3(WZoInPIWv44eUTPMylo8s2LS8jy+dDF|~fpWlcz=(|10 zXT~>SI=j4+q2jHVEPqI=&Zx4}3b%f#hY(DJk2ojyfA=i4+3tEZL%CcCo! z9v>cSi}vZp*Uj>EtAM?Qlk(Mhh5VdZiAP_?&p4zdx95XPpUuuh+H2KmGFqsrKwQh; z&;~zj+(qmSA8Odn*(Db^g|*~1w^!#c5(;9%B0HuF`$YP);Tj)5Vbs&aVTt{56#1hD z|Jy>|qB(9RsL3_|4d{rr^Rs1K=CD_%vJD$R(c_m}S$j%lJ;C4k4UNa?`VU-B02ycR z{+r18Q}&FdRY&pMlb7W+$qpUAu!;C!H+)I+S0Tyq4UTXjGv6xOy(|!WRx{;SgTc9**NGm;<2;Z+F(hI$9eT<=?13GN7 zvr0|3u}NrjFE(Le^L?{g!S5rkQN^_Zy70c1c&7Rh4TOG_hkKNgGy>vL7Pl{kUjY_^ zu<~JS<5&7tzljXJ<6(}-@MW?7n5(uWqH66;;_A@sVgP}_V6cI73*|!Rg)OU)oxBiV zCx;cwd@tvUXv2$nmvTAfZ@A?3B$q&(!QRzxnvOV3qhd8_#St{?4lS+>)_dI=X0R9O z2+o`&+7@q`SQn%~lEye(+kKpYa^EQ%e#+i(r0qZ-|1Bb$)#UBX0iwmqodQQE(!ylN zj0#MRkc6*L+oUaZJIR#yi1L(gKcA>};caddjh&jVh;a5{%SqbOZmJ^8NQ2C&UQlPR zqNLO_1C!EqfqS+KGx2JUK&7TXW*USBKrGdLFwQWsH1uuOl!OY$06} z0~xK;k}QSC!h%nu?v7g}Ci}zM!3MspFim;BhC$Rc@#ot0Zd+#?;d&+ATZh(8;{Lu- zcl%5!>D!5m7`r&(=*+Nww8Z|$sYAs-fTMYSLLY;FvAY_BLTj;Y#L01-98f)TgHFS9FYZp zHBQ{7y6v&w-!pTXMQ(Op^VxTj5&SaGY-kry0hPv&Ix~P#$@D3gT&&3A?3%G}5JNiC z7|+`^P6MQCFl}hYkndr1P5D0St1!5Yd&=zWsD%Wk(q$A;4SeSy8pbtOw}%@u@|@O- zR2?=jf;M>nS`P2LQHw)8O>cSkO(n%fNj$w0#d<0iQAEW3kXwV7Z zmFdJVmqFwG;dX;b)z4E3aQHT!nt82dS7pb#8lr)&^hl`I_Vr9y)N0v#OwrTnEnhw# zub_5$G*)fV+(~~9VbF~-5Qe1Un3++tJ41r4U)}T&*IhQ<;uwoLBg*7g1Z(BF)#SPmnd^wd4A?&XuR72w}==6s^td>Oa5y=!kiTCvz?IfeZpxhmE$`;Z-u(s z!hKy0Tq{#VbdXkAydO~P!0!rTT}-WSoNH3=4$ZKQ$v$1vO!FtFd%^|<*rrIBH=b8; zw|A{uu&%^k!FkH{@ol5Ht2FgZ`-k+3F_StEKnk(E)iR_Un-sjdcLUe1V&!IZu`4hQ zkqtZEvXe*>N7Jr`Y?!7Dmu+wqnymQx`tEaJX8WlYR8((sTykkj#~VI>;l&?bf(`Z2 zGqB5558RD1LuASPd5Indkg@mN)IYolTb*yj&sKr!xo;MmwSklP9HE|Km{O;fX2#e{ zQ_ys4q<0~thWvA`&71Y><{NvDCS)7W>mcoO7R>8Z^|wECx@?5FG$@iJ22ko^9=vnv z;3zNK7hz>>--Dlr=F|WQ7IO+GfG@{q9yW5;KUBc9S}ITW7BT0yV9nz@P1J>9IlZO^ z%$|Myee^2sbXT4|jMT77skPbEf5TiT*N1A#Q(aRT@?3qwes}izl)_Q~kDFavh}{lE zz{PyRV)FLVH}K4sABT zPdlx>+b9i1*QvlNyS*9QVOKd{)_!|Fu^huB=@XV69~+>`!_+DXluzOJ0~Uw_dDF20 zsX2+^6G?OQ+L7Vx?>FLLtI;49koq9?jM$LK9mk z%w+1igHNrj4foK8WqH0h5Y{6d-z-=3KxKF~sOD3hu@w%Szm>GM)+*2QN{pci)+%>d zk`)TMgF|N^wK;t=)ChNGOz?z+{%u4Fnvl6B=Wx?4>s;vcoBd@OW&S+_3&wni6S@wz zt(&bk{ou&qWz%|<^2^*`n?9=xgpx#aRs{GxjpS53phK!{iJM%QArLdjsNh3E#g^!D zYc>7DR8ZWPdIk4lMT0>zxS%S}ZHG(~n$BLA^3{0esvHbhSM2@3-v8A84_pq{96qr+ z=3je<(T-}ZICklN%w>=lh$gW$o$3ZvlZHf9h&MjB_^&1w=IBB8J6OMCJ1#%2IHKo3 zn@N86IquT5xLxx3_^U+1`<8k`kN~Etn-BbclK=*{BmAU?RKeY}#JHCMGaWxoETM=n zdo{K--3u1EoHl`><~gBrg%xU*0nPbBDyrR1byEr~>Tf2Fn00QbT>*)x<~pl8-s4M) z@8(Nb?H02ge?RTg)s)+k9MI7D+48~h?QkK1$}#2l%@$s*Mlwuq&^ri=EH!jhPH68M z7`%wj$hQmfJUYBdvq%oU*Z}zH{XYzn|L2>gV32rj@Mi*BASDYw>aj~>RZ9BEWom4QY z61Mg2Ld&@{AJK0CV@BeyJB(j4H$Dy$YNF}%vc=k_*Y?4RZsSw z)5NravrU5u_?($;&oxm0tz~0pazC)Ym?_URDVB|>#=F$2qF1T^zVU4FYlqwMFHGEw z!o$ZjTPzB~n_A8Dmi*EjZBpZ~kQa%%;vZ2Va;p}ne%fErV=grLnnkfzMgf2h7P_yA`wc@FGpL> zrw)0yR2o+4qWh2xx_)Vv#L=xU1tuQO%#3(HR_&depS8j#1v6`s^_3#{9t-H&Rv&pM zvNCNjZidDzYATZ@K*SJG7k(En8%Kyf^)&hA5`VFK^7{U|@d822BS075hjVmppS3J_ zfSZea!972NX_}=FGFA0K{&#}<)$CjS>Y|(aH~BK4cp)&AmdnKPP_@-L@1SKxQ#5fJ zZ~+~EZJF(0pS3ZxSiF$b49?s#-bh-xnKGnuz)&pt~OEnsov= z(8$QGrqAC#^>Jz67T?rtJN@g9!M{#dWB>%w7ElU!v?Ut0S-t`%E^5}M5^PLiVf%(m^lt&V))alyWT zlC7+8by;TWnrky8%9Sv5YK#q)GR*j#WXvNz0e`#;j5A_y3%FtuSB0j<`DhUHbTqz{G82N z+o_cnTg)3}S7wO16WqiruQybY3Qk?6Y*-Ab312LYo}Tu=<*OJ&)3#ODfT=5n0SeJO z>!PkBRJjtb5hVm`Vu+^OAkgemx;NjrAxf*i@+-a%dN0pT;m%`qP;5hO?f&8M3?Iu( zf?M6*)SR`VoJc6mS9liwq2R&*ej&MOmRJvH6DkB z=YLR`yXtPD9u~km@Vg!IVPXloS2`Oz-hWkVD`a*YLS z)h%3Yu#{-;V7y;^r^|;9?!dfcSjYrJ+Ich(NE7@k9~$L!*?aXF{Eq# zPU?&Tvwnqa9d@c76|Nc{I!gu0*Fzh6>KxpTB+FqPcZDYuP^eB4;P>fw1yA$z?$ty2%tu)ATq zTj74}FD5@GRAI~*HZ)58S(TKGRaDg#A7HvY<@do!Mk1Thf2;(SCMLXDD#p$!uhKlX}kBt)hLz`$B49@@h7qa%9De|U)SG-R(^Py;f2)6w(nUmX>L9% zpv_qJBF~0QE-NK7fOF6}6<}GTkcmRF!#5wu%ZxA6Y2?s0b?f#ysxe_eRX9VFUHlsv zmUOC=P=pfQ+f7B?NAty7^!pacR^I@Z{#exiO#Yt-XMR3LS{a^hN(}D--)==6#2qjG zAz1=dz^h8P?ph{bVh8f3o$`lx@w~LGsu|lMeFwv@21qTJ)}YOYO86C2_LurUrr@8= zKGj`7*H=q{vL}FURRQhwK(7d`^2+cB43e7hOp@f#ZI>bHO8z+Sl4DO=!owjwMXNW9_L)ApUWc;t z9fauSp8CdfYaIie3d{K%WY!k!QdU`ZvT;l^+YVA<1xfOSW{$;u&E}X?pe(27)%ykY z%PF{O<$F#eeEIa6o5n&D7w5#Nh~9P|iN+CcCU;E#tY}f9R6pA?QvFKZ&hpeIuBD@k z74HpVO!Hh5rXMMwt%sig%1dexWpd5IL!RBWVtrA|C4=^K7Ganujaw>aY*bfV%hvdk>;y|-LezF?y}YK%q%13A(3{&ot48Z`!=N6i23nX)O!-hHrCN**1wCP1MUe0 zEa_>KjrF?J5qdIE_#T>XI*ghHvcSh-7Q=EhejFse%>{kFsQ?evMy2!#C6x3(yxq43RblmVN3#Ukg}h<(*2^R>BED{5T`i_wLRK zU}_a*IyT5gZI~rT>ZT`DsyQ`a-1$SZ6F)r zg<^=OGd%a`K@!TgOM{Z}0l+K*05Ifh4e$&r2ViUYsL%sFu;|`)&2g&7vHlj5ec*z_ z2|!r3@P^fd)E7a-xrFBR5+(++Wy$@$S zT@EsOm#`1!3e5@6RJGD#ypDLa=T~-l@{^C87BK{Ok8cjPf4iN7ytGMfAzCF3{`b`i zkcj^g4Zzt}-^yLI+ifre{1?bec6R5V!vuHf?lt@G>JtWD8=7d37X`xV{wmxG(E4~Z zU7dLXsL1&D(7>N$#t-t0ktr=5m+yhiJw@DYr{2?)=ZUS1EI@J?p$vxK6(xhrt5%kJ z;E8%Wt3Snr#H5@5O=R>F4ygUyCe|2(pRv`Q^-J?(?5Md@;8w$FU7j*R z+7Kz-39IT>Tql@zJ^>aa2Ld=BUZhj5_o-512|y3nWi<+gmjXLAdH6?1qj-Z6L9PfxJ&7IX=@mE18jL-ThM4 zRpQrI$Zr94Zjp2h5oJ$8Brk%=B_)Hh!gAKGpEZ9^#b8PT21eB|UAVRH-|0mp}pm~9BS$#NB3nvg6{U>hzO zSu6nsK#Ef|GG5`Ta`Lf6^|a0Hv4j1lR3EMf<@D~SV2zx;FC%&g$qM6m2!dCOxWX_F zl(#RaAMqdl=4K|sD5>l3vEEhrxRB2&En8OMhBdLYT0d0_9Zum?62c&6fOaO=^q}zy zaFU31W*DX@sksy{q|BR^wVm_b(;$CbKKbb9?$+d`kwbcbTd%1+TgcSdqS1ghl5a(4 z|F$G@QLhhHFso;>GrM)`w~1>4LHyB*usp2oG|Z9bcD#A>?bg<|`L2;jZ*e_4mv&|J z!_h%*?%9UMs5)D?{k&0`d8>JIx>K}z2@C?lQzb^{@o03G|CeAU%Lyml%Q>Yyz zgIAexHtNDy!k}Uf1q{hmCEjTG9MM$F8TqoEU~Hk<-ZT(vI*UmfiNJP1#Hz}C&B04^ z-E7xr%%Y4Ud~JFh{O)cmCzZav@l*@{#PqJ5c% zDu&c@(R-cLZQd1zUU4k(bxu2ka5JnN*F3Sr!C!G6xHM)1QdERqOVpbM>|(Lekkqx>y8D zVgP$KfLi(m`~0LCt}W!*ls8;Uy*XSzwPB{sVEk#ZRPws5BMf9dW(UVpxGRPPyYPvy zO)sUNLR>f#rQylTUOr|bQK~5J4igz^RG$TDK4&yLlle-So%ctXj^VZ@&}&QGMwE}; zLwvRD#Sht!r}FA4b{l1%iMGd(fY2RR7)(`nd(b6FIhT&jq(vLqSZ?Bj6ecZ_n4uy? zI-zwo$|T;jbc4&KsDSD5-$uPU9Q~4YSRC7ynC~QL2oU$#B2DdP<}hI83nTd4l6~f% ztGFNAtC1KTf~~)AYF-_vSnJ;W$P%DnBGO-M?a*pnMEEDIqY2HTi2QmoQ93WuS+qx2w z0q}jb`rC1;%?Ut8{m4vTKjptbhJLL;{T^tS;;&(w-9b$o&cdMU_^zoX_J*dqMi11^JW1ihoN-SoahmEEaO`~N>tnOWTGSCk z&nbD3=$V&Ok+;Lqd*-{`j^j`#X#&@>Zl0lZ)2uQq5R_A~75~+Y5U1A&(xK`csjFu*?M-8-J@&H!DgJ_tF(WUUl%Fe z(Jjq6BkV_{YFM6$bAu}2-V-ay}IbD5FG z*EhK9?47LhI|FG;q{sxh@>UaHhK16Vu7+vd>=t_oU;AlibF`fI$4HijU3~>%E)Txp z_G#Awy&2V0Am!_P$(}=T)%I3Jk2(4M;@IAGySTRu$Qyoc~9PSdLf)V=p^w9#f#hdDrJm)T1}b{dN%S1g9kl3K?X;M-C^DT;Q?%(Iv! zg_OOD*a-;OC1}1OMA6_}^#M*-`CfdH?CxldVBkT12>%T6D%Do@<#OJn5V>{uz zLd{Jz84WW5cidK`BwU&8{WoE`t;3BLtne@DZM`0-`6&;-v^3iLZurPT@w}F^lHc0m zvxW6IhG&}}IQJlvPR$lqZ}3i|W}+S(>lzqp^yQ%rwmgmXWsPnpeA5$(YCZBAtbIuc-lhXUCPfnm9FqLtM;Ro zQ-5gfXN#senLTxBhcDMH`%LfekQu_OQw+&13G>b+3s%i; zBilZ?B~a^Dfe=&|WLKLL*OV~3RIRFeRM{1OL8KJr%~I`e(ea%1+Qz60n;vrk_OqDi zC!c7^0?hzt+)hhs(-IbjS!nMFtF924tzwcHL$Et=I+n!0t!>n1wIhNPD52w}6eCNb zby+$VLhO^Clgl|3YQZ#E9;ScvJ0ZQ_Rq+(*gb74?Ncq-Hv+!UUF@N?`L z(Q(OJg^ptf@8;!)x)=T+dQ4h;G$s^aPwM64?K4Qp&CfSJi(ntEsYH?R4dU7HMk;Rn z!@puISEVImBo>?_%lzmD^ZI$oMQR-|2R2$N8$Rhd<27oFgJIr1 z{@LytGasX5?Hu8O`pp7ynYcK=u?nX>C+Au_;Dk#)PK(sj5`dH?;8@MKvyFBAblozq zY~+(Z^%R$yZB06d)@r?c6@_rLQYfpILk)idT<>`Hqu$dLFI#<&li$UI-#R^EmWX4P z$1&mSHV2cp%wjd|{Z=}$EpwnCFl=*xw}m#QOFL_vdhzp8#`k^(O~U|AI$|`nP}kY6p)6?~%~(>O^^W9i*hmmdY-LG7Mk2Txv3xl13{g&v6x0xVt5z z$-W$I+rCZ7^-`^jvtjGP&_P8Dj>1+8O%6w9>1@|d^2XG##phK$9W|E}NXT4kpVz?s z``S#>@U*f9PELRIN}Lg{L#+T=ne97&Q-A8qAj(!a+I4Q*9n*sdap5L!g!@ zli(7y${(HLV!O?|WT78Jd0S^&1Y_FU)d^;pmSAX9bVRv{9?jYrWCe3}?F&Cj@}+_2ts&0Xsm z@tH}|k*LB}?MuwYQ>G>gXlSl-Uqv10stfi0xbVoxLD;SA6|77=V6EZ!-tmpgufMr) zM2GHi=Y4<^bEALPw4UZ740Z||UT}(Z*)GuY-q)t)7PJe*?;&2=v_ms_uGJ0ijLX2P zO5quK$!1q{FxccSQd1ui?+Hun&d&PmtC$@$E;|!|RyDIoR`gDI`x34B^yR&$+}79X zU3_oD+OB!nX!I@S+kld0W?a8zjZhe_R{=%`w0;LX76`+dpwc0FremSik@+CHGw$~1 zUX?RH^gySOq)03;R(_6Rm9u!~F;cZrX4R6ui$*7|Eqd)wI~nmui3_tcj?8THB4Y!4 z^G^V$=WO3bA7x1U6N_AYCywsau{iE2|1Q}*g$xU+o_h+c)aGbqj4B@KQIs z4qJGliP)mKQmMq9GUaFg408Q*?fl& zVhjQDdhYirYf3g=wfjw*AmBNZmDF9C%plzoxlsL`H`_a7E>`TdUGcq)Lw`l8Z&58c zYRCY!ys{hEa1G$e_AjCKF9Y(ofApkt-Q`YvKze$LP%AV7qR8hb-8^qRuDWOkD|un$ z<&Y+wt_H>f-TFS9bISL>6FOne0yho0;X6{8ZdOKNfCRJ0l zjt_u#dg?~U;eyvI$*|-Ry!Dpz#NMYaS*CqEvIv1UFDx@G;poDapQ2LGsnTH7XTRI? zvojRIm7-su+A#VyQ=i_5j@>vW%2`n$q1_8HIA*g68c9(_w|WZ%!14Gxm@Xdr)Z z;2-gfj-VHFPc*?%pq8&S*U*c*ci1%)!20KrJty?OZLlW1@oRD+${5?snJ@}6fE;Si zQB@8ip4|hwCmKez+n2kDWK1UL|E%Txx$s~vXh|a@JAjji=)GD({*Tl2_whUX!>eHM2AdViE$0#cxYP?@e6PJxQ7z;&V8bV#!ly9feU$iFiNeA>8-qN! zBS%<<$~KE@Pr>_4X>F{G`IfZlt~}X5`*9}!G%+;F`2p#H6I7*k__KM-hMIMI)hjIL zxs2JD5Ml2OxKLEG5wWSWb9T!ICXiw(fvE#KCoI;P-Esh7pkiHKnG5q$ObR?Uj^mkj zWt-haG0B;JTV``;fqhP5YDTls1RQ*S-_i*=XeueG%}?}5Z0o~5sG>Yb73|Vm=+7h; z6!muwOlE80i+sgKku+%q*PAo9)i1oBIOsL3-Ow!)ULhZ0>`5)7K;m&=eDJx-!_Ob9 zk9~tfC&ASh8wLs@ZBa;OCxc&wRpBMgTm%lGZ)C5`?-y;}-tYUw7V6#$75**EGN-80 zzRCackLC2Y|8qR?w9M3_ysKE;EqZtq_wE-Oj2U#Z0Lhp4%BQSQ8>yw)DP z0?E(3=iCjEtBNG9G~g4g4m)bhGG))NNqZWmMVq`TRHvVst^XXB4R0&WQ#DkHb>t>O zC{PYEisD6r`Z20`hBNyfITo8H$P(CQrP(>kDCPtTIhgsfDraS-2oCoia8u)7t^GT7 zQR;_L6_xknn9HNTKK8BrS|m{V^jP0Nc7OlI???99+EueRQ5FQa`6pF-MbuX9kO(}v zWh^&yX?*hzt58zY^f++1geIe^F82T`0cKzf$nEia%67xyh0?`o8INd zkZ*d(i#~zyVr$+~O|H~mJ4q_=71Wy!p;p1G=R5>I?Jhu*MpI?QWsUW|i&eIvWU`)Q zWBlVIa^I~KbtW`Mn0i)ATv1t?a_sWrO~);R_uJ#g|uEn^K;YH3X_^f3naz; z@OKz7%3;j7vblZKny8+Un>Xc9`D#=BM0vmO=7$5bqz51Bo79T>q8m7+xI>dh!3@%0 zTWucJoQr7|y~6&`zBI9iff+;^Z^0*c_u`)w2GK96zOyY|u}T#ziLK;;H^Onawi#n< zAr+OO?4uRqqnC}8ya=#^qFugHQgo3n*t z_D|SmoW$TD8xuUSA}y<@C!eGBq0xgFYmB3>^Q1VB@rv0|;}q=#;2qoqZCww6O)FS7 z2^WSVr)A)L8F2obF~SH%iNa$1X;`z2;@U!mYz2Pqt24`}pt*kU--OP#VyBWU+2FMK z4C*M{m9^lS0l6SnUo9y#Csiy0<$(6+^RW^|HC!JcFsIw3c@~$VYU1ZpoV~hZHZDpj zCTE7Vs7InBZP@MX>@=NZ)LKRbkFvH^q0IKYlX^lENu^4=EL=`eDr1ePVeuE+NU;iU z!S@-@wPaA{bKYsyD~zqLMCJ@Auy`~0-jeG%P%ygiA>7z1fpXi|Nrjs3YdHhW2KN|% z8D~}n%0eYv3M1ddvF%HWdf+#C$^2?rRJ>rcl^90M5^}S*rKjDb_A}DgJy#3vTGWtt zLthT$2NeIP2vpDiVperqjFqHSWp?#Lquv66znKH5J&Q>0`E8HM{Mc-c;=bB0vh97x zBrk4bq)dq?iPSVQUSBR_ozblM-sYaVq!a3>6afiC385v7R7ofjdT5S{ z(vcF75*Xsl4f8YgupnI})eciz=Uu&)x77Mhl~d*up`<_pK8S25JZByo?zI{&=SAOu$aIH0H^;5?23E06A=8TR_LGD! zgDL7Oduy3{p0at@TZdg+aP<i}F7_$S9pe;wcW8&_S&^lz-5 zTW9`E-a!ZanZy1Jm6q{|T`eV6hYlk8{*HV0FDLxRrle8vQqrwbPC09asM!3~MF>Zr z*A}}@vYyO|m#yI^Id_Riy)#d;Pp_rJA!gOK->4y0@4s1<<_ zRi&Z0ih5`~ziNOd3jRt~Q;(_ik#`SA=APfNn(Yl6C(U6ay;iM zaA3_72|^l;wsfnkgoCVBL9q&$f}#|SZ=ntk)D54^6`7jKtLbn&LLT(wQw%{h$82iG zyl7T#*q!%I_OA7>%Bcn76j$26zkuI6zu54*PH1M3QBpl%QAoy@nJi_PUt~+;HO!U1npuzJ?5ABLU&zH8}#7iWJIXIa8Y87q`@p-42*MjS2$TI zAoTna;>f*$x;}cO*n-zqMpZs>CqhBSr8-RQ9qE`&{oiM;ykfz3r>uByzTykI@62w* zO&FRsd1$-Rvk)uV@6|EuUFBv0KUTK*T%d*3SUv@Ce;c27PDCe};K0mjY<-Z4b6pXp zn@~r{`8w;cw~3Dlan0lJREW+l(B^KgG<>!O+Ghv831yoddOkTa#IgLe3D;n)JeXy? zjpObEHZx&V_5u%Yv}?2ImRbG2J?K?!+{uE67a*J_@rX+~zm1YeAf~RUKt>H3JYxKz zV#Wo__}BXpS5(76x`dVzI{S7;B{Ip!x@N^}_gCZWNac2!2k)Ylwk65=T zDd4BNnL8ba)b0~gHyciCu36vbtZTa!ElJR zz|a`#Fdg2Od(Xzx3AV1=tOoiugN&@SiZU6G@2VTkebg}RzOy0G^h@rz?hZ~(UbrRV zCgdPs&Mb?(`I$s11^jsFPv!cH+4OfH!vC$w(YdED=r!HOQ<|ggpL+AF9Gl#5`{py0 z%3uB5qeLB`@6lmk)E3c$WtD%k_b4Z(&u#bzR_02l6QkxOg;nmdQ-Fx=4i{Zv<1E40 zVD*c2Jf`4;|M=Q**2ymyf5)7>KIu6H+&Kk2I0Za81tk1=dO`RV98&;nM zgkQ2=aGEQw6B>Szk}Z~zE*IpTjze(EPOUJNRcR1m<7 z((ll?bu-p86qiDiD;gj+W*-wznhY)vC-T>E%_bJ={MbU+h!Vh!&*6|nbQIsbFWB6H}`t_-2FVBfwhWzxaw)5FH}Yu z3)Yc_-%Y7u`WRAhUQKps7Qvs6&zXSvcx}7Q%ANdC+a%Fb;GJVVMtpr2z@c7a|JfgQ8fu0)RbWhsyAkp~dU(Xah zT+tZ3!`eT2N1^O1acFN_+6>p~T%hb7dr!2Suwi|y$wTp^wa2r(Hb2*C+NK%09GIL8CJ1jrR7~dq6L5N z&NU8_VbEhI^$wN48UG)CRF_j)s8b$1`JH~H=|}s7;LmGizP)Oq*4xT-J+e!1+3xVc zW5z@6D48iT{Q;ntD4TdlG*{`77zpXL%8W$ zz_&sDv>mCt&m$9^c6f8oWFl>04;xqBYdeh}hTg(V2d~f}RloR79I6KQDP0o_zXUEL zz?8}^)`mnDl=H4`_J??;wdWtkQ#E@;LP@k2Zxd637U(8)o7I= z)B5SY{2YJB{vhA<>?bx#s!gT-3T@v;($A}li#u(a3TAOl7w+NTHZMEXrDl3f+`paD zHH);WE&ZI|Vz!$}AxRZM(0e$^-<$WRPXU6~yZVQWE2<}~t|{%>1s_8z8Zjw;OJ(#1 zjaJ`;oC547ihcjK-_=JC=~W$bgb5x}SmC8NZ7cLR{xAMwn&g)ZW9gF${@}Y(7iN9J ztG-G1NJO5b4bf9Tee;RoH0eYzc;ELF(DuD$y{#LFr+{lW545A(_c8R!pkLw6*a#z8?@YS^R9rxQ=!@pn z02>49_W}nEHVGWxIW$9LbRK|osZ$6@$!UEslpX_9$w1cy^U<(jQ` znf&8nMXY6HuZOp_>(=V=k9d1c`v^M?QCe!EP`IL$Q2a!u8?T+X z&3kQO@0S!zzOA;@QU0o=v@k039f4!R%8Ivq10>}s9Fn#OF>hKtPBo~I?Vz+l+_b*X z2A(%Kqf`lrvS|MJN<)tB%|z(6NDGpwm6sfW{}9?xtPtCMWUuc8ofYe&i2A~>Z1Ou~ zG;+zrb~``WL-FlE7RHDfl~--AO)`qsE=l*FZgM)Rlka8$qR;e^0; zecTntEP~1H$5}4q*G-niBkU!8bJ&#GxQCqcLe*us)GMJ)gf$+kK)FiF?(!Lb{cnEc zQuF4GHobbNOfrMNs8I;PGcc+gOcG79!i5z<#;YXrX85O6_&$17p8t~Fl7}}O6)pOE))j~Y8dxW3F! zlVl;3LQ$Y@3d)6z8#~uvuc(_k6q_x41K$9+%<+8~^P^mD(*ej~(5^)FCYg$C9lDai zM#no(>|$-*uB*%8V;`4b{A zPMoK{lL9fn`=@?v7pzyuAX?OzBK-C8%CQf=J;sd zZP>T-RCLheTwLi*sSVLw@HIAkj5guxCx(Z?=$|G!!eS*{dCId{_M- zwceN%+g4Jp4a~+Ss6BGkpzdfa^$+yc%?_rXadJP#i~Wi#?A1Ifj54h*we`%;uZH8t zMv)Q_4@~%7$!W4w4CsB>|yP5VP9*cq45^uSIWWz z)h%7xoGeBXeJqse?|uK8iW|x*IEbeGQX`gNMq4Fg_=ay6v~27)w#^ zC~X{2lFn8O(r0CB%)z8}&*dSsFqXX5o$EGlm4njReOmpz zB?X+K>Xv{o^Ra{7I(fhaowk28yILl2O=oQUhjq4A*4Qj|El}w8oSQV#Y4QGZmfGL& z2*BBA008FAUcKrg>w3BpYw$q}bC=j= zonkf_MSZlZ&E-G3Llqt{e{oy4A&i@Sxt(uME{@z*XLRS>@=uKpmu{7C3BEGL-{cyO zMh+L`XlgFx7tFCuhLtSN)Fg079Dqfab9*Sw=>$TGY zaW7zuoyxW(Fl_RyZpq5vfm*HC_34-I`WB{)mn{-?&ZUoVH@EDPBA@K|+af$9<^GHkbtW3OK^*@3|q71 zE;pNv)|ca`gbyNN3k5}6QfaM+@tV&2m}NRqhLp$QBy6o#j=h)zmPIHUl6%WxPX;-t za_+0`Oopw6>1Z#!>vVHiq09?X@04Ssy>IgFMmO<`ypW-tZYL7s7fTo7plwB}pB-zq zDslI439RVdP|S{w;D6Y|#>Uf5)|NB?d9X~LPdB6z0@U1#8{)j<)k=-zMLCO1D_dr% z+XG2m`PLuY%WkwTOXqSZwTQVT_K^*5Yb^J}qv2RtG_!94w!dmqJBjU8TX(6v?c%=D zi$)Ko86qKB`VkVoAWEt>Bfzp^qlVprg3ID#zK*=HC9%>R_(|<@Fbf62i&LKd`W`^C z_ZmUJjJr+^OHo8M+v-$(CFxk56;V$%BuU~vxpTDc>~2{y{P zCmw<45+!gw`;xBb;Oz1JPy08f8{eupTxjskkWAW055DQFzf!I09S~M@yfe}9>gkzF z`rQgcqb}F;$45Q@3VsF9m)kS9Eg9n2as1iOwfKlA;39*N<13r?d8uS%nP}CVYTE)v z498!y*m;xzlQcPaGysM^U?nwG`HQyA5!+IX`rqad;N~m#4$;@&68%S8wg)m@AORWo zp0pYIgqdsV1Wj!l4h0VAE8mVPjOx&^3*UjzrFT{h8c#MKKbuTTOV;g^$XfNKw5=t_ z4%u?fXn4>SBDa7NC-x829_TAe8|%a9`s&ZV7xhoe{{%h#D-J<~j_wbM$^CB(OTg8P z8*r6Z5}lPqi^MU+CQVRVl{2kKV|h;Bt-DvN7~I$`hI-U8>wiN)ntyl@$R8g&wm5%> z9`(NTHi6*V;c5edwGd`~uoVB3;Dosx4qdSLn1WYgihW$g<9-W787s9kTxY(ha*nn+ z(D3sQRMfLS-P=Ezm%`RB^gR#;BG1o&XC}(FHckO6-WjlC5&HTRklS+W%x5(X0N|YA zq5z`WV%W{8Oyq}`ZYtUz_w3{J`Ya7?;IId0*yT*9e)^8^4C`dQD<2dF8k~4is3vH> z>=lR!HMhrW-XJw_PUtn;+yc~Dj>Md;f5cAV5{iGA0=9z*Yb@ML5`Tlar9DuZ?3+-0EO_xqJ&a47r)ae~d`A`WKJuK7QZ#e*SU?nB^n)qa)LNr=oK!$q zoV|tzcQmz9NoZu=K&|Sa-Q8EMj(>Xuk`}sS+g}Yg5O(VEqLrnelD; zY!j#|w0&{G2mKT2K<|d-)<{W9b7zr*R(RAx3OM7JoCn#1E7*rQ>E6|2gw^J1DWeK) z`Ug+1%PeMv;_H)Y^wdF3NMEHgB>i$H)Xd(ud>6HshhDDq#H9!bpEIEc(B7L1KJsj@ z^w;LAjM5y8@A0M_t39|M?quT}#u{Vua9lgfs-Po1Wc!^zPf(NEs63*zT}-#gq4!C6 zLD8(F3cGxlA<@sZ%Cr}4xmM?6Xg0N;Wa#N!F}FtK<%;Y~XKEADdWh)9{Av#4Hqy-> zx6{V~j}~5{tt68w_&JABT7$X62gVruPIA-6_A+ZA1wIUKHRmg=LZ*j@4{l~U$qhci z9)c-9eK4D{YUqw_e^cCmUQ1NK8&Z6FVGV{!I<1ObQl{gylLK|SE=i2G7BwP%GKn}p zaohSi3O@^T_Oj6?M|-42o6MPLz=Ttgi;I03{=YF_X0nP97((LYHdXl`8a6nW#X%fD z1h&Ju&8?IC+p34OH^y=_CKjIY&ikd7ovUZdc>RfbYgBBdp*<$XMB4?5peT$|)bC|IKLr3^7#gKv3HT+S zUN9%?K|*h0hYc~1!(w?bjMACd1bQFAG~uXv8T@d$_JXKC+yo3o`z+EngTb*5ko1O9 zQM1O_wjCskzJAem*xdauN>AS_J|>bzRx2$1m6^gaU^OB3jEbL$h7 zq!E4cS_DWR?#H4N|0E-S%v8c)dMl2LiS*D8L0&<-a7T=XzpDNe!1j95X|*#lWQ_b8 zxYbhA))d4K?U-1zRw{~009cZ3faKhg$BW!|OYCBoN9j%&y3KjcSGwY4A7z4sCMPsEr87^f zna+n>AS@6x!TGf=Z2QuaVGEBpl3d0+HI=kz2$l{)NGe3%8-Z0-2UX6Z@XMaTVq+16jcMomF(1)e`w)blft?olu2hvF!$eNO{d&_^?p;B{QPLQJfhLBpTGOvA7VOkm&`SmYUzZ#t1J=4TQiEkbvDfc zE3PE13}KLk3dv!dYtGBP$+f!6%StkootvM1$T_{bUOWn(MmchZ=5L2L`{Vh|6S6}# z8sS=-4KpSPS5;v!ZH*@V^Npl`a_xWF_@5|tfBnS5{1;r^4}4KNu3wz*@`(--Ayhh) zCn@feb+g8z%l)t`b}N;`+a_}Z$I3LRd|X5s?9Ln<4kJqJ5#>8_IIEo+3F&ypiQosm zZ87zw5yQ*1uz?-5GyFD(k~;UwC+&Xu&~$(A=P^)+fjWE-Y3KYxRNLE>irE~{+~nO| zgs7SPls{62$$0Ij^HI>Zn#en)D80G9tHu3*Wc+hvVQ6?wmxecHw4k@H=6RWT)@@Wj zmP^`0t0p2ZFGp2%@!Kzk}w1G0REG!h%98X|`#6Yt3A9A`s9WnfoOYc~bX7JeI*A`- zIZ3_sjB#t`=YQ=0H!b@8>7;lMjAtmF#R}Nhw~EAvK0=WMLxfgik80sN1h<~uxa8@# zh*E7g?gsmXO0z>Ii4mf*!e~L=<4`HETq$Bv1-sT+Cg*QL0(TaE25{Z@evu#Oo@Me= zt$j%PClTP@x4HtSzX5Bm_0UT}eLuX5BC z#7m=ExvJArC&-suoeQEIaQrTLj=pf0q<86D+f~YW>D_>S`mZ6du&7jFz;oa5! z3OXk6-givkFaN*n{;+g+72URQW_a8&!--z z;Dj3wKPI=(@I5*K%+VRq`y+DnxJ)^p=r7=Jh2etX4&sVw(WuG@KF=eluzUF&T}o&G zd0Vvudm}H-OIx3F3|rhrr=xWRdjeG)mN-vRAw+<7+iK~My|;A-AYmh z#&C=(^eyu(jkP3slK8`W6|&WFrIGbd(TeuZ8N15diknmkCQ;n^1XS*5k;xWNgt~6^ zL%pw90!K-LLpXXR;3+@NK;S8P;^miV|Ncl#swd+1X2uKJ^*jT9FoWYawq3k1?vS~b z7Xr7}(wY}FA0)mB>m#*;d9T)S(Un_?Dm&L1?{vEBZb-6D?c1*;HabJ}SigGkZ`uWr z1fn&)%V}EM+L97Is=!BI50gsY4@hUQ*wvt3-K$;Dj%%vtdFcoQ4tj77kgJD50tiX) ztTDg#$N8&0L^Nl=I9Z}3wws?MZw(|zbTsI&Yx*0qL5wlKR2w|oM`hpDW>jteGUj@Z zq~l;N;Pom`6h2?sHjfk52SL>C^C#BK2TPoM*qRTiIVAUe^fiJ|8dSKRRjL#}mQI{= z>FUyqsW8MAW1lMwo>Fe zz2lSRYF$Knw)P*n{bXzXKn5&|;mx0o)}Wz$Kx|5xbl|P-E9KX|l0VcBIF?3a5-Mbr zL1G3fiGrpDWK1B#5JBQnHS+#ypo~i-gmYIw5 z@c{zP=>Py%z<(rA(EFP~e-8hB`Cow6|B;gRuctn<1kB9LTr}jdJ>wb$R0gI`UFAbioDYovqL=5Ua9U#R-4NUbAp zkov>#Uq6CJCF+Z$p8(s}V*kG99|i|fcR{RChMVB&rk$Bdr!Ptz&HnyC2QO`0TBQ_( z>K98_R)~M~ka&;HTl2DNbe{pefGyESY=gokgFfyZj2U`dkufhtavI2LN{-s1xwctu zcJp-c@%A#f3F;e1_X3FKkmf1vEqd>9qD|DWb0H0d44(3ZT~TuPy7RHZ5%Ud`6u5?`k(wV03d5{an$_H2m4~Pm4LUn ztF2=BD~aa3~SpAgt)KdeJvjHX`@sV{dP$=-h=bkFWN zqo*D3-8oR~{qT&=3XCx>uO&Su_9;6elC@}hWjaYFrenMX8ttfFoXEu(H0>W-)9E7` zkK?2Fz?U?gt#oYnhv+U;Q2Cn7`%}QhxA??`zCf!eOK&%29szb6kkCz@5p zypE6@BfcqIs2m`hi)I-KZ2>(%vf(_wm<_#OBjKi?s_QSoVQ#yG3=4zYN zWK*jxOj~B*)gzXlF+~BYT$@N*DB8ZOcJVklbv?Q9K#i4LPiVB|igQ^u(zZFjMm9@k z$woB}S}jCDG+=8MiR`-XY0{2yrVC%=&Mtaz3!N~jG zIxJk)^NoVPKU<6kR5yi+lQdbdS>87$+MxTVCgFE1y_ffA%~arwPG1)*IVnVm_~dF$ z6KfHr>kQXkh~?;koRWCz-{z2?r?L)A=V2CQi6$nAnw~BuwB>!htyEW;murx&ry)av zg(Z(Sm~B`1t!R!9XF{0FXLj5+UDv2P;1ELhF}!DuMmRVHdj9UDxv%T{%k{m^>n{AV#Bg{e&v}9isgbHB5Z?@bGc0ACzJ|D^nSL znXG=vw#3ud_T}4!F@cznL?Hr?IU8E-LNkZ7GPMdp%qDEssf;9821ZAL?MsbrU>S;) zwD<=^44GDj-7v!t{k#?&Ov*~+K27pfNdl?@`=$w4vSpTNufzD6;np(M{H63A?rsMpQ}# zgN5~3OSDq`m)i#N+NvJhn>M&$@;)!>?&t%DE9>HNZc+VFwkvDo==m^Gun0l!7@fJQ zgqA(;|7;}IoBxWVvQf?>N{T^ZpInxyTzHWU8AneEhXC^Jtc^#Ng>bgZ-xsmAcxcv= z-Ven?5x7SB^3V>eP*^oV=m(Zn82Ldp+93Ef~wu73y+i`C!bIwqS z?NTUhjw-*H^-3oPV<=x!(liM2nl}Nza-JV&$#|AW*P1{D=+-DC7Y-R=8@2G<&8V%C zRkWy8MDF&mBslzmw;;b9G;#sf@oT{^cD70$nb1(_!Zz^_*@`mASo-$MwEp;==u|E0Wr(q4&iV_PlYn)##BWNI;?DW}c_4fiR13SoN z^Y8$}k&kmjBMowj>Y+<>W43EOMhS^?a3`x3sEnZ>vlqgnnUAB{PnbyiTB-56 zvP{hzPg`BJl6#A62HGH6uUe4!_d0f~9D_KWh#ID=%EtmbTisBXo+~~oJG?XQsLd2mmL%(}>B!^F(au2lZ3DPj$u&AVUXCAx zd6f-|6ko5BMcN8B{gTD3_EH( zrBh-p@TiU605B$^OcSk8SGP2n_r%tk3H;IHg>*z;F>5pH$}K z*x5>+ougBwjhsvGBDy~OUAGMrvsZuosIHz)I{v%$r^5Z4sqgq9KA5t4o5gmY*=>)4 zd+PamBL<;X@l9K)fA9!|{^{8Tzs;Zj+m;5uwJ zX5%!7o3M9T?K4Ji*P}D3#B<7KsRMTTt=LYs&>s0l6KZCqcWB0oBJAPv=)LuKk*}{c z58)tOt+04p)5LvA>kVK|V&m6EX>MoISY>?fzKc|A%P@`Vr8x+v@h4<-G;fuy~>ZmAkl z0Wwrp;L9X;A^a?nRuTP55~0osZZYXX`rMM3s1yx|En7=8N;oLKoAB=G6ly5`cB9I9 z+J$!c_7SL1LrIErF9jDJfsNyU%iOQm<4LumNR z1JGTP6B{=@Zs=64xmo>Qusk@->hxT{VD`P5siXHvVC_Q6Ib_IS_@48YC;V|p7 zC95M26K#WXqhrUcQK9P@Z`z@8zWWW74UxJtA;T)R<}wlE)pkwPeZB{qz`Z8= zc3R0V?fkh!U%w%h2yp|!oE!z40nMSsf}1X#GA8uK zS+!}cdH#5TgQQqo!ztjSu(t3!wx5-*UYI`ga#7@B#q~q_u5jY`@^V-L7woRuzPP*l z^m-(_^zq^*2a*~qT&fbjr^8nzhgzj)m8&w4c2a5T`WF{K-~3<4aQ~OMBK?eIPdQi> zUanczv>ASRzt0ZbR6m(*Kh8GL!KW$!o@O6erdSlB8;#D>^F$0BD)!b{0}q}lN&a>s z&^eC+!ta7WCIMJ&x=1CZ#I(6>B9WT)lnXfqKRZ5iP8vyekz>gnI{}zC5F#3o2 zH&n>Kw*$T2oC122-ZA{AQLN|9 zABL{qac{RN<+ri_M(WkB^b93}Q;*MC$$ju^^3}qFo0K~4Lf*%<+dS+qk(XCahINk- zgtO-fD00gjE-M}f^ou->mYGt%zV=gwjMw&opTA(N7xCw_8J7+o%Bc7!R~wG@&tQ!d z&=y$r54~cl$kFFbaCt$J`G@q5FC%ewRfYn5{Zw4R9LW(v7jaR!lG#h=s@(L2P`S)b5XtUz0LuDlar8 z-8JC7?~L^?ZX4*PkMCxK`8Jo6+EA!UHa6v5#fli<`YC{QeeMz&=Gq0fvmI~VEH6X$ zkd!mmlE;{o+=%d3T43pqjSXU-3=mIH%sOA1h7cVsGeH$?Ly#~oEKwR{BDTG<8RUIU z+~0rgK#I8T6`C5d`2{I2&CY{~6{-j_k9AQqnVpU0Y0oy@tiUkkhZRlj@B%&=PCYV3 zlnUFYxtwkOcvHzcAWBoK(ijGhg7QfmrcoFSZun|8*Vm^Rr75GJb5dT;pqo=hg<^@a zOW3UBAzbD`d2S;I`{Z==w95>?-P@&(^sRne?1yOA-sEqy+LFN%dpTh2FT#|ko5&Wq z>WbchhR7Jk-j)qU1xa^mtAv_}?elY{g%3$i+&lU%u-rusG?fU>_kDEo>l2n=P3S20pIG$@Z&TQ#X38)YTG8YyWOR<(=Gf5=G&ld^@^lVO?nRMQeV39A; z;YrhAB*O2EhB22|=4Pc9)#Mk=E~U&F$zOT)gO4d6GYF@M?z?DUXTS>3d|Anm)7(vnpuAa}yAY#%P$6r!)7x zruV!&DT*=*1EP!(6(0Q0b?6z)Jt)`vXB_rsm34a1(SgDI=_?eyQNemSU&Ivx(P6Be z!&w|JPoNaVIkDxB5k)Qf7W(x0_&jBDsIgo+kC( zU;fMY#>I-q8_`c7qXh!eGC@&`3~JDjWqXh7HAPyS?9$gOK-=?yJ~Tm;lh;vhQs)E( z=%SQ7=)a`?`x;W;@={X-W;&uq?X7=5Khd};8A0_%Dr|-~&PExhCOxS_EeF{RB#4c@ zXtf&RBdSNqBU!X|Y`L4V_)Yil5T6Rksds;4h5y+z|4)(iKcX7{DZarl!QgI+44ElD zx;o$M^-`;iU<>OAqXaQa<~+Iu7+2pXHr1WbW1IWFbuCUtEsJ&maT4P@P*5N+2k<=@ zzshE)U;pxbP)#2!SbeIiUUv{!SPBxL;}Q8S`;~~015c->Q$UEt+w_Z%UfsJ91hPN5 zUl=}XqfVzldTGGJs|U-rs`Qwk*Dh8uQeB6{wra7k!sYtMPSgBKa;b(xZl(mJV`AV) zr-EEmUU>U3KAOzawu0h!jHx_kg2JF^h{;5T#x2GrclYo}?C`{3!*sZlz}N>s<*mO~ z8*|CYesDq3zAInc->qkVuF-$7FBJz|QSUkhAl`%(2V7M@9Pg)xY)+U~wFiUg$eg!? zlQhc3pZ|(i`j;aCKiUV_S6^S|co&gE@Y28>wWQ%FV+!Q8t)JSO+~W}vyMw?a|o$)h3iJk=Y&o7y(f!w-d zL4$*_-9M2UznA=s-ZU>yhH{gN>;Xc)iI*O1akrXVR5pu^0z+U$J+RRTBB+XK62Kj$%}OvsPBK@wLx*F&@f z`s_W;fzSyhZT?&xOaA4qIigY}gd#}qkEyUsw?cPzc=?%g!Qz1e7G>`|H+Nm=8ER8u z!dRWpEkp03P2Di7+Cs;dLyVS2<7mAZ_br!~-Rj`JJ86iu^RU*ZNvz^QT%CPgBbjaP z62|gLrVd+Mq89VIGe|>Ye>nVPy10YXcyF0OM7AReQX(2#L(^Co+Y=hhZU35-QgX#| zt4o_F@6%93npX3aqN^)oQ}?;In@^H#0!$O>5_qN$Skn4Xs)nOWQ82zdSbYA+rq8}g z!3@+-$~O|SVq6dhcZ%*8cg&$G4%BK^p>iC$b9ul5>d>1!q5Bq0BSp(SQi5@%_K21y zNextmzXnVqAtIEXpa9kmDH!&C-!5Vr6g9aP2R1#xfX`dXYx)iz{dqglX8R%z0P=0E4=eRUGH#K73dN�kLnX` z6o2IH(U61Z=bZ5n(7;5-;-c5IC6dtkBK?Xl#Gz;C?2#5xYK|gI1=qBtj42>f{aTCZ z6>S$eAvzqb|BY4UWFo^oZ(?I5G(bJ(17}2S`jLg|86Yp{_a zl^-puD=(Rw{e@62{)ZeWlA!@^aG5m6e(g~1i$kAI<;c+zxM+5E{4mLXAPW13VBX7W zh+9>-`ERb}YD38Euwj0MB3JmaTkhfWel^c0s+{vCt^fMCf8YN9+a3Sq8e_$YnR`E~ zc%MI8np{3u9@N96EbsTNC?1D|1=mpZvp%knZ*`UHd33UdhQ?;8pIgG`XnQVT+iS7S zSn3-C#(Ct*yh|rRvfgj3rx#HbwMxc(OJWo%p2J%aP)>%P`2o^3#xNi~eWy_Kkeo1z zoL1Ef@lG+&!pPc04@1~2+*8Km*FeCJy{RuoR{-Zj{JMg&{lk}vPT`3AsO|rNDEV>L*V?Axmxk0hZGO)1swI53 zq}@Vy6h%(YMXQ^6u_c<$IO@d(ghO4GEjhX^pNf}=WAS#HfTBCu7^_&VJ-ne+ejkJ? zj6P~8PjtpXKkrA=^CLu?Agg-=X9Y&R&ebGB#GtQ>!|YEWM>XHJ6sS2ho}Z!BtmXKe zx16;mx-_`mV0Ko19V*(zRrZ(Q3Z%eZqD{0;j`PzJ>ciPyi@sspH>R^aI%kWbpG%YwlSZQo&)D@q>mgWAe0P;q2if5&3h~I;*hZhn4nPJFexp2#ODG zE7&J{wjknZgK?JIOa`K)=5Cv(u<0qFv8dTY3&7KS3jok9U9$+jH$hJq0Lei2eq;Q{ zw8RhJ{xUMS09?ZfOYdExOSwtodQC_b2s&k6T^rx`|XrO|LNqlPkEC+Z4nAY>Px zmCDc+#kP?}?uhb1yXjDT7G4$UdMgU)()7YnR?3Z);fYd17?kRv+)TQ6H}>b{md=?>LeI=%o}Uj zBdtt+SHi_RWHFSbovDha=Z*+i$PbU0Gm+|^biEk_lkWa#gH6!pr%v@oIn04)=Lh9a zV2Kf>u!_yPzzjcg(Yzryb+1trH^H2^FSj0_QXSX!h180|++e7%^YYeet|Vwg1cYUS zkDbYM854{zOE^hpq|`VZDURO!#S{0%eaC8-Olt1wM2WI>nOD*?7KTRkj3hBdS;#v{ zsHOCl%~$y_C(Uo>&xwgBjh~3)&bG-1aT}E}cTOy+N*+Y;fL7Bb z+*6cjd{N-*A3Em8>*SuKEHifojV8kTv{nNuaCC7KG;Ix-_bE+TH>av>T`^_HWx45g zc$%}_Fl`c|AjQj)VR}@Sd-yeX zQ2%T@Yi!eQKN7xKT4+b#S=PpSY3T z)oeIjcwR=%NAmUIaA5QP7`c3@R1}lnhiNP79U;#`tUnI>BqvJ{0rR|rGo{dC>eJcC z$ek6-z}KXQFiuxtRqxL1@Dxprg^16K3H!)%dOVId4X#8I?{^)ddmvGCtax$jV z?oFaOR23*dGE3m25{FHzwOKYjg|4rE5gRO93)J4`S#|1q>WjGU#(3jwGjt`f>#8tz zX-{=i)w$Z$BMpb2(^&3dVp8I{@yDAdArB%c-bc6=EfyHw-Ga>%+H3EeXs@lJeKFV3 zt7w$vbw`$d%(ebc{`G$(5a>=61+R&)(xI1sSpR(P^F3e3zD?zG<@81RLvxQtP6212 zGDj?Er5gh}f_{1C*B4U0T#t@r3s_xBv6er+oX7F-WA_y!5m;GQAwz=HPx0QRn(3l= zGy^`bHOn9AG<~UUHB+vw3QQRuD0>`L5rKobir(xZ=N1?$bLg}9!H2j*aji^N_>R!9 zIq)%Otm5T5=9jPmbNhO52jNXq@4i<`x60OhA%L--jywLo{11^VrIb^^kPNYH4e>^& z?Brs`d@$KSJv+D_mjCS(u+Y7QQJ+alxed7X^TA&)@%`_${BRr{(g>fqKbx`zN$xQZ ze`xRKFMuGo?uc+eOXR&PpKzsLIma0M>Z?gSTa=6WfHH@e*(`dMt#iaPEN48{r%rh0 z?f}>8Ja*nj9l^@S+(i@%lU^1~Yi<^~pF4GX+eC3P>g7DpNi}=8!XJtj9SOFm%^&vU zHo|Noj&`rVYUG~|h>W5|rOcy*da7x|2Vr}6cQ;OM9VO^lPzL19au#OH2K73#t?y?p z*JRj+J2yK@(qyG<@7TSKZ8s9iN{Q0KdxjU(DOV7D3VsV9sTLx~=jFn3GmCl(Hqlpa zEqCS#WLA%3Tl0of?)0%I*9IyxV81NbybnniQI_ueH8##e6vS+|fa}+sXbaIxODsn; zJ-Ec;UH}xeWJRN;@Ptec;T(mzga60edxkZYw(Y{185O$&2q;BHs`Q~tx6qLwC4`zN z9YT@bLB~cBFhD@M(n$!BE`-2{^eQEkP$CdQ3B7l}Y0^>i&C`+(@0tkQs!+@}ij^;625?j|xMDzO(2>(?a4o?;Wy zA8W%}<{p8c-*97`D0w@bZyDRbR8(Ffy)F$g7ez{?VtNYN8dZVlGlKs42jFH6bxrKf z##$q{74&AySQeS;v?n{Kc;It7fs4o=h-Do#I$%|J@CqGo{=~%|9pKP9aP9tZRDz~N zJMVxTz!|_IeI@KrT0dyS4ZbAyU{ldcmg55B2N6H#o88BXs7{r^U8<_IndP~UgE|8u zdO*|(kf*@qBq0@8+19_Lx_gfIdBHXLQ2OEJo+6B?7y}Y_oLA|oH{NL9^&qlJdvks~ zfz>B=^xU9uOQXF^4^sfZ5|^R>^H1ngO#-sMg9Dzexg**Q80F2p4UR9qJ(KM;$S!*Z zK#^8;vG}X?^%N!$@YzY)bi3 zoD!etk_y*>9sIInF2<2dD>=2(m=I%y_LSBKK&~0Jf55J?uvtzl@lCOG5i6q`CY>yv zWqJUp(SRuuh6-4#taC`7ee&+$7rhtdPL-GM$I%NL@wTe&ZT0qhsxjkLH9U&-=PGsu z(|mjGaI!uy)~@?u6O!G}rI>X-)j1r22{fK1xViV-hL zPbGT2+I4^1d`=bJVwGr_NlR+O!Hjda!hjm>5u$lCH#SDp(r)ij?zyCcEqFcu5AE^w z`$+9}QQb2JYj@^Y-*RPYHaKxxgy#&5rUiCXu+`s!Pd2X{z7lsX<&kVX_UZs5iZNDl zDJfUVc)((W(<6z!vYH+X1QE^X6szM<%9!5C}+66iL7bb*P-tT zHKKZSA~i%EqX)zsjPWJ+0+@z`dX_IwI9C>mzJSP@Vj0I9n3I!%`}EWcwJEMBKEWsw zz5Avs@evd#^YLLTodzJJTl(glXJLQ z?M-mvy+9tdY2&tjFy0A>SriPyi+k6wTDYAqeU5c!LEMU*N@U``Rvu>*vD)qulf=>kQT#&PncOO1 zap&qT_IPK#Vkg1TL+trj1PW1pnL%(c=t3ar5WNdCjLVI~*Wt9qCUHfuQvsu;#F13; z8@liyZj*_GDaV;>_vfg~J~$gmJ98#u)lPqK+G-^lgQU-Ignb4{2qw($=*5XVpP;cT zJ%6^iKq#2!VSppJ!dJ3TG*WqthHR^)Ait5(5}(a=e(8W9R|8k>!c+}4-tdZ%7vOqL zp6OAyr#286R!vPKbDCK(}E5sT)rakNq+a}*AMob?kh40v7N@n zOn{qukxN5#&qHh4S9Ck;O!|s7;%cDeWK#9!>TnatY20HCErlz>?;m3AA@td|@&#^g_mPTozEN^4c? z6+JQ%A979|N&y$cuERF1N)0NL7(;>s0@(DPpkFqM_$!!oy=z>^GYZK`Y*o|1C?uyb zDVOb)d7eG{8D#o=2W8n`>1x4pp8S)uO{fmsD!DZJa5?e-^urwczx1u0F%AhOZy~h| z_uFv$N4 zdQ3vaY(BL_4xJXPz+V}*65SCPJK81fQB4ld$@L)(01Nk^v9Z7?rBMl#BF}@7%dpK+ z7iY+TeCJ>*0=1mLq+V^x)LssD0aB(#tFPv`D+b+8n0#d#P;@^5;m+hQ1jRm6E0`=y zs!nnN5_2geTtd26j{|kG`fzbqa7}kJ5WeEQyMurBsYT~j$S)rn14On^WFMK9PH)R{ z%d#G^%PEbv4$4bn{OXH*f>{gDx2@c+)kUL3BVG2#LLX#=SDJtJd8L(GvGl8LzCMwu z+Y2VnOmWl2$oW@qBSNfdWhN}jTWh$9@9(BxE7FKcmifRSUQxwlY*%anI2!-F(`x-et0)_<ⅇql)!m#tJz=xlNlyQT5<3R)XMXt z74O6qzU0m%-e}AlTZnCRr_7AFv6m$R7avx4H3o<`E=n4Wz8|;Bjny(1ERhw8htAQ_Sl5yTj)bADdu!<;*@_eTdH^j!`qhn258e92n%CayqSuj0U zJLaPJmdBO(+i)i1G(Tabt)fPf20%AiVa$cI&(U?U$X}|4Js((YK2>{(>UQy7+QpcK zp^X+*tY;k)OA2}u{9JzJ>9bag$_aXewJUhUyV&J%>7y@a6lT1SZc$> z{b-9OaI~@jG(i?1Qbp2c)jV?h_v1UQilve~h2pZbvxxMi!)eH-Y1pD%TNwVjci|PQ zGpUluw#qQB>?JmwTGyM%=#95`(rj#yS{I|;Bm}+H^Ni>2Q2eg%z`5&L>>8<_7D_tw zBG;4mbgVi}VOv$~J7#0KLlK=>L1~WOC7q@tk(Pt&C6#W$hShZ!+=O;GY^&tYd&^X8 zLcIGSh2cV>iK<{*3T@g^euI4bctefnY&DJdzfj(ELKYcmQ>22CY49DQi*P zyGeS}S$sVecVTcMmGbV+Z`Nq41hXNB+3|z`Z)fQ_Rv%r`21L!c(kXRp?2Nuqi(j_s z;HS0jJkCqHvB9=k{4Yf8>kG9Qn84oilsWA726w1$s>hPsyTsV#HNfGZ@RZFP*Z*vc z?dM^PBoJV5re`4#T4hF=Em(Ww7a38`vEUu}>w6_XikqZWI#Yyt-kT01HK&6JZQl7? zfIIOogY-Mt^$*k?^b*~r-I-E@^W$2*yqZuwig7t@M>~?7ZM5zdd@lSC>p-8rt%BA> zECOTZ#%RHKUu$1_VfWyEdwb_tCy?Lv5#B0F=KF9YvgMzt&i&$WpmQio=lQxG7B(7B zX+hsWk#>msEe9i->y4R7x=%+jh zW_vR=EH9r&5C*(9Cd6F`x>|T;`)6CL4?`2F&c(M+7})XPETt{*yoa!QIy7H&TieW5vlI+r z*gHET&73t55PZ3(q`7~XG^}Ind|>jT>z)IwL5uO1sEoNwTW#YGmIUsqp!sSeJ^4(o z-Tsd+IwMBQd0A_Ra?CV+(%J!`-9x>c70Z~k=8RsIiJ~u2J@&i90Dha;Ezn84s?pNj?*yoyd;VzY?weE{ zQ#oe7X0`xAF-EH~t6=qmS6tnpqGMYRoyIKEieswt?`6Xf^#3~CrOe=%yrIpwV{ zqFIdg+97tT+Gs9hKK$q%pTzwcq@n4Z6AG^DO9rfGjQ0XJW#{pAt#e$wUZR_!hZPZQ>}reXrQ0&O8qYv|m;ybMc)mdk~@ zHPUq~HBII7<8~N}XbnE1Ooo|QsKE>6TNcLZY;PrG+nG0p=^+3NEiFe+%Tq29@Xy4N zrq$Hf%CX=mJvRt#GwygqludTu)6xJ1ve+4V*=H!e$>g*(GmJ?y=={_RAZ=c6S+H(- z{~Mn4i(mgQ|FQx6q&rlILOj~YahPN_hy_p8Z&_JYlY?C=H+l;k0mH#fkn)Pl_n;^2R6)3 z<^;Rit1pMG2(hXIbG@qJM9%Uxz$~(kTpx8zz%@!QOt{*KOtQUt%mn0*6p}P&l!`S6 zT6=9h1GqW_@mWI0H-2lSfw@e*K_#RjNR@#JbQnGw))$cxPTy)T8-Eu%!@$ zMC*YV7L@tPv=?bd&5GDc_{x$7Tj~4;uad z^ecJ&)ib5~YOVe&BL3{-;oK0je9T&&Am1UU{tUd@ZSO>xA?LTl&!A9~&FHO!Z{O(L)RXIiR|F>l*R4f8-H!wI=*ZE5>|#OR zawx&aHTB$+)(7^d-Cd%8bqfcw*C@Dia@lI8cD|p`(oCURji1uZ)_73UK?eMuucn>y z2A+=iE?KCq;*I$SeGy!Ec+L&(%sKtLEgwX8VU6>VbxC&|H<=9IwPD4=?E|F(lsI}N z-fNW=CB1BeJ3ruwm34JOWO_rS_kOLb@G9Xd2kcXWhOZw~@IAYIx?&$v;EIY*_TMN8 zZCe zR|mlo)BO|XUbqUe7%=fOy&VAWE)mU}yCr`F$;bTqhf;ntRv>F_Z~}yid=qaHnr9X9w~Eb_MnJiJgdXu|u2vJA#w>J%Q4Eag$w6L!-)9%%$xzckq{fwL9AMg#R2$ zlU)=9U+fgyi>ouOb4={`wa!3kwwd})40C1U-lO#W-p(K`a$$zU7eP1FRJm8#`sp5W zSv6?z_-5Ali<1ByUVt1f97ntAU32-Bk=lYu2G_giy(Vf$tG+My;6K)pHEIUl7avlo zA)@2~ir>PT-p2KUeB`hOXtrZzumgl zoBL|}7b1V(?UsTZRY4E3g9hm~q5homzXv;PC%4GB)i^IxFd@XywqWq_F8?X)J_JCl z*)6&-|7N{5=1V~jT~z$4>Kd?L%Bk~9!ZqRm>Ev$3nfXj*{~q9@iZx%RuI5g6X5E*U z$@_t-cqFz*+q#hQsTR#WL*U#*764GVm#t^q(_ue$_}9QU2N$GvN*paq1}Pd%GYai$ zM;i(c-m#9UoNCU3HasZxaWpqaIFF?vH{rdU{Yo4SUE%-Klaj>yquAP2GZS)!KU z+Hls1^+7dP{GL-6>F~0mlp9Rz0g;RzuVzjCxm_{!rTQ)>cC8qV!6lt?Yki%*W6%Q!`NhIcePImS+^(3c zzOKbDLF*xcvuRa!vBr~Q2NKsw*$GhB!z4edY1`e5%OOs{r{fnt`yh85-jS%RCOSpa zPfi;awKUt}vXVa;%j@e*@JU(Hlbu_iEuEAJ1;T?RF3^|ms_ee(sRy4HHTJh7q-{Ke zkv)fDFH+57N7XF_(V31)boJ0bE-I>rs)m-kkxU9)&lnCmV{wYbGRA9~F9OQ1kZ;il z%C}mD8tgYT_Kgp-zV5Sm;;Q);DFD_ieMS%GqWMODYO-`go&^(4N3OMxb92+jG@zrBVwJye>nH> z!pGMjkfSWY`vjpO2%_`6WGBL3j55)(73Dl%d&kv{4@u@)_uVbue9d<1$E7P#3+W2n zPUiz(Z6b-;Gaf!pTWk1C(L~2Sw18qyjS%&E%Jb_!N1o4TKJc1JCz#$N7?{Gjk)b72 z?FQAsoQuyEw4tqR7DEw|b{O02$Nr~RYHJME@vTwwL=QxOC5DY?7<~5P(FH%dpn3{9 z1|?kG?pjq?sc_Y6(a1xyn7daRpdoM?UuQiMYG2uizqZN*_mvpti8Xus$#F|_Xi9w@ zv+qch7wW4b>xYU=a9)lobt$o4>oa;y)d*SmRr;fWCz2&WjYM-7zI{|$t}K!h9Rj!2tw#gg^lo`n;hlx>F%B;Pa!ouqU;pbe|1c zp@-sj3Vn1Dr2Q3eneGp?Wi{R@#~hkc#L#cmYv?ShuTY2!Dz%?$T7=&37KA5@!?bEg zsnq2og|~_)WU4rF>f}yO&=r-2c?gn1tN&--a*Cn zeliOT2->#QxKW*jGJr(c>W zJ7T%fp)b9BdtiK{slf?@#)F5DeZ9!$suPk(W+pDPuWt>3u!3Xv#LUap*i_$Ok+TMn0`7*9q(DlT)mj>_8 zsEB;IaH?q+r`QvGXK*LEg;VJLA@8WN&ci+NB^kFm=kDpOL&F(zk^Nf7p`qDN(|#(s zKoxKQsDhSsP!@^-yszuaeUiIAAqChU)@nHWEI&|&E@WMqvH?qVEbnfK?Y}v&Px5f& zM}!!;kr4MX67DM}FY!}$IDvca?XPqOxrai|Hk}BO?rs)-}^5oV{`z8pRuuo_CaHwwD2#yXW~7kWl> zU9>~kr(Zpeq$ZDAdM(d9dA{~K@#y33&ZFyQXX9K=%zG(m(W}bu<&Z56@Qf4$)`7TY zaVjZC0?(^$w$`ys$%H{>LM8RhdjbSK`PDYkWS^GwyL%T$x7mJrx3lxCTd5Igfi5a4 zbf(n_C8p_*w$DnXhVW4Z13y{>=|~i_58l2XTONQj@?ykihT7!AH|erdb#L%_Q56rOMed z#2kg-06;2)iIKokwC5k+)#vQ7V_4ZxY~!Hzq*7b$5zSQ!qK0i7s*QN6?!yK0=IeIUd8h~F7et7hpE)Lw@SNE-0G8<%&0 zO>}#db6mGS0(0P&Veh#n>kK{jJ~}boUZSI|pathz#Y7JUF!J3ar`YnNev{u~IUhecpMDsJ{p{;3(f1Ca1{H9HdWHa?D|(ObZT5!HPQLy+R{+Gn`Y?yWo9sl2&C8a4&nt~2IhHE@ z8$#m#icIY3=4>zVEC^ARw0nr^a`q*-5X>8FG}yg<`LD91l zaPOsU(wd{Zl5}5+W^Vi(IB(TrxQ=K4#XKZRg|b@>bMZ|PDDqQ(KRoGrqVs;f@})(~ zqFHONw)_Uj2AZp=Hb+9L^j&*}4Gy-Tqre|qX*W&r%#H$Fq?vP;*+vi}5`%{?&1zQH zXM=!pFicwREsCRpb8Nv{3$1qTx3b|&k%SlLZ$Do&or1A-NN6%t$YAsNoT~gs=M0n~ zP6+MvTgy)AuIoB=p9r0kqvbm*iu@@#ab*c)J^=JK0szC>D%X$JsLbtQGS7j?j&feQ z=UEX1XF*0gZk9!vP~{9M8eDGkk_~MPdAwzXJFC9yTnsO=92BzR+*uy8bT(bSL%73v zn)M z@W~t)TjT$cPmf!VMQdwKh8t%|SVnqf$hOs8#pyQh$lfoxeAfGE4M`V~9rePKBgaEp zWD71H4)K4D960T#=V4R3>Aj$5Cs-xsoI@5;_|$qTgI0{z$sI^Mz{ zd#53(ZHVYK!{r7SI5%uj#$C>PrDTZD5_?u$jfdG>P_t-FrK~}%*E49E(xA($nqKqv zPB=r`miSyJD}Ttx_RWjE2Cjqcic#a|i~62GNZowtaBJ36Ope`-aq=fTE0~32{WX)F zd~pWtthhjv{*lyb)m>eg$q{IuA?I@;eiyH)A%jyTR!hxRe$1s~!q=jag=?sMf+2o} zYs+oXSXsj-ta^ITa!*Rt>6U+E?wvWKXNHk*tB1iJLO_hSpd5>KBvqM35=a&|Sq|p^ z7`jw&5f#1N8%%dJ|5qBw-`WABAAGLyC6&lu%0?FFNfNOT^}L+mV+#m(=kPcio}^_h zrlQ|uHjO6bXtN(K&>x3L7>;Lxu4{(=0FI%S+-)Xzd|uf zW4>lna=1%s5=8U2=N4c9oR;LZ?!lpZ!c`L2uJ+qHfd6~3z|82XAd<*^_Jl;y zd0b;w0R~L1^}o_!WSxo*BOL13G+B!&XE~Q-FBdJ&s~1BEES7U?ai$+kp}j5%#5iy$ zmpaxc)CWjUT#nCPHOflOP4F4?*SJOSm%Ao29^n1zo=#PCm2TKR#O`Utl9A_7e#(a1 zyn#e{OTR~E7PQKH-0^ag)2gq!MBa4;Ez=|zdpn-V%|mFQw6d!yQxHApjotQ!OT1AK zi?p4`wpt-0S@YIjHl-UvgPHW10ydX+&pD)kJ9hAIhj84?>bqJ+01T=au{5HVl^7z; z;<%$dl2J7`;SM(Oy;V)htz3=_94tP0S@V-ldV58=ow*UtFA0Ad-RS9Lm&`YVH;Svx>IAg za=1Piw-Qo&lpdFLPQM8(06P%N4RGtzNc<^-URPUCmo)0~!ICszI%zEcFQQ49plrMJ zn9a2Hway*Wu--+|cR*%qqQKEeg{`z{*hQ7sav4S$HP3oaddH>OC8Tcheln9vj*JXg zA0UT!h>yxOuuL@BxtcUrm|fcDJ&6_{?efg*^CymnEG<2wdy>HE^UibBjdMG&+!wEe zbKK!1%~0po5hS0FUc#Q8(p>?Yh07$*8itb6?tRDpwU&uk)sahLEQyG-2S&3LCpp6; zOpoCkm~q_ei6e9lr{4`U&l<$FU}4dCs+AI(K=_zE&2wX9Tpbn5+}tj*#^+=$La!Lx#C5- z8lBl>pW3XFa{W#N&6(0ySuA=T-JiKyP?-&bWDxAf-ik`UM)0F78-eE9dzpAaUs<>_aOkA z_sWcP3?h|WuWE@7qO`N-u&}UaToKqrpo*B!HEElh7O``3NcKH??I(_pZW(KZZ>tOW z;b$+tsig52^m57%TQGd0CuNqs*ib{$^(#uG>}OY@+am( z-}QkQ2HoUDwrHS_hn<1pZ3Z{;nl9nXW?kkzJ!C4|1U>TNFTSryI^jN|V^IRNYI zYZIbAlhU=6<=em57yQp0y#NR2H#P<(dH=|zl%uz+btzUyQm@%#|D1P^?;jl%(>NA| z|JjQ{%9*rxJHRj6O}n6WB?y#KqNY>f*hwO$YXW(C7)vrvF9)zVLR<9C>sM|K4rwW< zQNhu0<(`xGI(whsyh@GtSp_$_ZL-&VEeUqrI{YtMZP9oUa1H$uw42)>$piJ)&GepI zPY~+KGVA|UkJnt6@S~3hPg~PKU}7eox*>pjoU)-U+E3{%$A%0hY~pgC-)^e1HxR&& zZ!MTE1q7s=gI&Z+4>Zk)$B&jc&HLY-OBtz1YwaU=Sy@I^$AlLc`}&M5po*&ubbZ%0 zXq%i?ZSZGV=thqZ2sw0PI1kKHEC7MWrJ~u(ci_4`BouQpdXTipO%6c@k*|tJ3&@P< zVIYGUWTBml@ozhM;w%bl7AK+Lw0!4v9Wy&F&*n2tqZ`#L%O9M7phjOO+|ztjJu826^k^-ae_h=YL`@%{xft{7&%nQMCEa0C@=L1 zbRzg0TFUvy1&<8uR~S@q5zZxE<4Y+m^@aA#jqA!wp%<^UbqdW)?MVIX%;G-e=q^)o zX(72plhWq^?$m0-^?BiGeQ25ujQugaRexdENP4zX`cy36x__W*Z*F291fD-^l{wdz zsV4$S+pjz&rrDb1N3&v;z6evo@p;8RKZ ztn9^v$#=V4cFLI1l#q>RDum>_7<#gB75TVJblBb*wTKHQL(*EKuNat0le=f!F+bT( zy*M7iSqVPft1IUCqq2q;)^kx`zgl&!Z+f@WVemrOGrnYup1J{6KtJWl=8YdKnzqF) z2IS1r=EHl=;jF*nG$GZ~BKxwXCcR3~f

^sU4Kz2TXW0J}`K$S4io~x^r#l3X1u@oyzrmwq9!O z3U!H;v5bjHc~CyuIUhMnwDc|NX3TlZRD5RgV~Vbb{ATbn_wa0uH>=R6f#00;U|1Fp zG1$b}?8^dzgU_2x!n@g9@&^dD>6o5ifg_GPxeAYZ#xkl|m|{p-lTT9y;Z1<=2N^r% zXI&-ieRVY=zh84a##kXFc#$diS)B^gG*6IgwB{R%YgtQ=KD4)Hgw~^SF#Lp5$Lg1a zyX}_CU1KdY_t_w+5tr`Ydi>#pln*TDBC^ZOrG0O9)btE_DABjo1|Q@io;XjLzAQK= zH!mGFt=@PxYzZnTS8Gcn9Of~kL%XG7+m;-8JgVB~AeIHiJBq0Llc|Am-A*TtlFTwD z4f1Z9o%hHeA+8233qkXYKF!^V)xwCDRgJnXPYNRt5_(sPx_a1pN<0NcYi7fWmULab z52?&xsoY^3b?o!<6yrImScOh9kv_y|*1>pkK}7fb_AsI2@JwbamUC`F3PKrqS8%8k zk+`Demy$hd%aK>khZVk~A&5!d@Sy4Rnd!w9uqIsf@Y{6u)i3YqiWQt4w{>zcL~rkQ z-bI2JBM@9fR1apPWm->TCuqi~T0$!8GiXvNF`){_He#vWGuLVqczwE(eWP{8xukqR zN33=-Qd}fZqJFtPYQAx|$Lnsh2cf7}L5Cpb5$oZb-7*@SWXg7hqH5ff3=Yq{I@d>f zU>-0c*?|6puGG<)9)6cz#lrhs_|C$-(qiCzR$ZL0XtkiPnD6WH1Ol6cgtG-C$;_wr zlqH*7O`@b@Hh5Z1RTr^u*(}4?*E*VW>@D%_>}1>uQksl)Q96T@zv3gCHD=4AHe56_ zGB7aJ|GQVJLl-t+YpceqQ7VP41sj+sp!sZm17Y#>@cU~?7pv92gr}|PS|3Hd0oTPd*4J_3@ri82cS>cX77>q)GVAC(OX4LElQ&d5P zGp|M#3Ook+;8zo4=(wjMP>zJ+e(XD(lW)nz);ytE-{Fa{N<=tNbwZPW$Ksa z9kN49q3-?2?#U5|rwMg)d?P>d0{%h6ysqc~v(sDRPcZUyO*fA5TBqTeJ zj}}UE)!A19Fw`6)i>y%#76fxfD8`%IkEs_)np**zHEL!5=A#-h%C^pP&Z8Pfm(W)ddAk{zR zBfr7ebD<>AurRjB4 z)2TWL&mVMGJQdMog8&Gd)+^9e2xz)@5QG@G%T|;D>zodIqhEP#J;VM&$*)nTKkC?7 zOpi1j=DvxTJo74ux4h99GbK~HX6WWR$KF6a>fJRz~WDbL`@p!WkW z+QnDcJ-3c711GV(AIXk*lz8rU|3R&!R*E@~d#20+A&+Y-ij;TTCgZV#`TNSt$kLGY z%lrtANXB(twz?I(7A{2%eL$}j?`Bl_vK{_x)RFPPbS!qTbAgFjFA9xC#mAIRPpw;pzK6qRckx8&kBi=x#f;tSOIqlIlnSMXfe zv-&psK0}C#BI_)@g`ytmI&A@D)$i~2#SK;QanBkZ6ir2LTPK)uA3%$$^ZG5T-d{>u zeK+qt=9E}9kAVf0tZ6sG>(|N%NA9b?cG^`Rdai|9F)7~aS%M|`&om6anJUbAq3gzi z(HhoC61etzG}T!b;XSQ@2FA7cnEJaG#bT_bS6{ly^qO1emFjy9I5Jtb;g)p$_dgC_ z{@or*i2Nana>64xg+gU2D&U=2~0E ze+E&;@35SIF!jLAqZ!pU8fAj45c13#YS3#`bwYf!2j5Yhl8@H^IQOCc?$exn&?UAn zP_vTDIx4%SgfN5!q-pMXaePyby`zbHld4I3D(6{XEw?pPXx8^p2vC@ejeq6-3_dn8@>Mn%1IwfcgkTeMUTLe zr{5PFAKXa-i@tf;sS9u3fBjf@!Bz;PYj&U|ssRGh#sY^=_u1ZrW!UW=FhhB^`N}mv8d->FT#iZOe zErl-;txi1P8O+$&PKy3{%8jt#WVm`PNx@zlr{(WkK|=$XYs+NclrG1I19ckrSLx%O zn&!dFvUt&w_+oEIsZWcpC+AFSe9zfD@G2>mZOEw-vYN44D=ra(dU9>3x~8UpQ{Xo* z@PAtRuq3yGkWK@um#>sK*2Np6`G{U)2*jS015cBUvm^_j96OmQC|si4EQP8%uuUFy z*T`BmVlWa?u$<-%${_p7$iqaRnpc zrq@P@^Y;b&Wz3=!_eKsx&MtwEMCGLs_i383O zYY>YNQ`L|M$as*9eYKR^%;yRw1U@KO&ZKl(21V#G8khTee>ITHN2iWn zeuOQW7uM}{{CW(XFz_@`hSZ4sn8l;cMN86OG`dytxPay4{2D96P65{bBGs%Z%WR~y zoSsoL8QEeL6~lD%dX+16VI2FyJH>-Qq=0RmFjdu}eU8)ouQIj^A|iFH!{#!5H;i=i z<`j3mqSz&Om|XMNlZXAK(I3|^UgPKE$7K1viI-Jw>zF=xX z?HWZoy^8!TJK|aRIeUlID}2JwBw&6SJ)Q9SsBxbv#kOa62j6i&b$pP1RiJN;8j_~C zq$das2vOCO@UPQ;Xba>+F|%CdldBmtg=ySKtr^PW{`r4~tsvKp^EMK<;{>57bOkwxW447G*@H()k%l+1D& zbZYgy{}71{B0%ikA~e1|JboV4Q16K;URhsUg@>FNd$Q8WtPytYc7CkTz*IIV`N)`G zZgvcPkOKI#X?cbicV^vp7969IS&If`I{fyX?HvWw@kI41o%e}n3?{eTd4oL7gQ01K z3pkh4vD%AqH+}G`~-GW~0#uW6W`N z1tB4F24g}p=A>5V*pj&3%44mXv#GaGL|D;6jbRy$w7lY9K1(C`US?S>x>Ce_IZ(*D z;UMH<(hEvZ@@zywMP+C0(KEc*$IX%p6}$m3X+Ck zot%pGy%_qi!Ec3ZNpoFBmP*n^DIzOxdym9IElWzwbw+qfhfQKNw+;^(dL(*LqlHAeP$> zO)J}HZ42O^28Rm6I)VewmQawGVN7LP+F{_q2-m8{iRtClXL^#_%gtxrMY1nj zamf`q;q;JD)mzaVT8-_VT|mUpeEYKV5v2|5f7?+*v7{Tl+|2FKvKmdty0<`h>3ktA zocq!axg!cT?STN|Tm#iiLqy+O@&CsD>?X2RfkCsU&~rDL&`!zRka8xUoe`dKwK;4r zwPR1E&)&S>T7l2>kft};oR4vvlVMl6Fp||2FH(YTj(+?zDYXlQGbV7)4mM=)K0_ z+{~zbF-8(*5f=nyihaNtVMws?Xd81Otw_5z=YpAyTKi|>LR zP4_NS#-qX>u@@+7voy5ozj&sOuH_e}hnQy1htC@LtM~b;#g>%*U-!uUb3g`x82(!7 z{Lk_i>~?&g+jkkannV!IK3Y#Y_qE=i?^hQ>;&r(eeF$T;(G=Jd+5xXsj*MId+B_#9 zlk}_s>}f_-{|pk5+HlH>Lm8Sa8})Gh5Q{N<=9iwjCgg1S+ld?F02z_1dclW7Si9itauSANa~2Oy(WRw?+^-Z2tv%mqmtiRLp;5cg2t^!B zD!lI}K~>vKQtOcrlF&OVmIS!1c=TnUl9|3}IeI0bkIl``(njTE9D7yUdWK!pi`#0} zl+21?Ktg6%TEK2!uk(YMtfq+qA7+cu1Qp?t)f-r!bfUFBgr$m!SxRkVQRz60(bGhy z*WgXP1Et~C99B%qsc|tkR5B%JT{^!Bn0F@xz=S(@AWprc_)?c;dtcqVdC!}PIa+1{ zWr+q-4}S1$|6le0^$hAWs6BL_yn)n$A5;R=@}EIke|uVP{Nu47GX(_&U04VgK_~ki z44a!~XN_>ivSJbymwLi5b4IE>4|QDdB@8+|pkoXm5Odmc{?TIsRy5&H;o(2Z``6+3 zq0_&0_6p4PeQSsYQ4$Xitp^lHCSv66%@~tq$9vx^_v^X8qsDoy2^$$Vf$+G0%cJ@O zU&%andgE3`ZM}F1r$0rSJQO-m&stsVpkt@C$z;=aC=Kmn3vGCz)5^crgYRRdKkv*) zIGIk~6!r-)D4C+hfqu$gdHBs6*?KTq*?h3J&NB8Zxihoc3R^4z*Xjwn<$;gFtR887 zaLn^fyu#d#8;xH8@dYONj^?!Pl-2$cyQNfHlOF7*+U6crkygaucIy(T_-nBYb-QlB z9C8#ZARs?wxppUGoVfpObjUZge=GiPXFzBf>r4QNjc{jMz!wne)#mA^)|i*a!4OK* z4tTK6KPGZ$V^t>$xaV)kool34%J12DdEI5Bcx^hQVKP{r|Y- zzZ^0k_@&KT=bkct2I)Vsg6IW~Z3-NygMwSo>w_D@q4$3-=<+P@n6~7LLyQ_&KhK^s zyc-y!le5~>JUJ5{QGzO2&*8OVw%hxX0O}DfEIaprjlalk>D@orF8)igUl0Dw0kr%}_}*Qh?-MGv zZtH}aUcz;*hO!#=*(WK_xzV_pc5B6^w0~$338cE^*7o-J4A$E1Ar?ixTkO}9K&IH7 z^5f4SZ_VsN-`coYm2IOxU;Vp}@fvLt$Oah=_b;OKb}^1~&yOl_JD@*di@*B_=x^=h zADzn>)Y!-UnTz%nz3h9B{S{gA|A$>nBDj}4pf0o?^yS-on5tK8zSq(i;szwyJoGpN z+fA9nm6chRFDKO|@9W(7UhQAc{nu%zA}^;%ulGMB(is8h1myrCzvCAL;bD14xexz? zc49saA9{sWuL%gMJhtELIij_^{#rc{$@SlC|7U$Q!>e)=8F4@cVuO`r{Nt7qj!MMg zDU)CPcE5dXGb}EDG$LTB^51U93f=0g_YNZf1i;{Z&cOvBB>3NKf2sqi^{0w};P-!X zKj>I~^;98{P+%;5G;^43V5CAfya%ME?yUGrpv!D1I|E-#vfx>tq*ML0FgV zgF8BWuh_5We*Ckc#1?;5&cC@Cq@8tCJa3`Ks}E!hZi<5b>=oaC1VvYa4Xpw+g4u5y z+^_v7Fva&C`YRjZFX4N4fxaJ`_s!(5r<;!0&lWa`td|kIZikBUK3S7M&}RyCym7q~ z7u2~iNHXIaYB4vZOjlZ4wBx2Z=B{nunrT`H=^=l~j`>UeOE_ku1~34l51v{AHY9rT z>zc|%w4;lO(XI`=EU#Jkz{+j#?wQ~}uKBnfXoUPW6K%TFKrYJq-?@lsq#nd)pmShy z_EU@@)$u?9sJPAeA12pBwDpMkUw6PIW4!_WKX!NNB0F~lR3I8(cQ15zO;t=a{@C5P zA|^}8K7>>vNXJ@35vW6%8+i;ukd6PJG_=zhk3 zhU4WgWl51;>gjZA*`>9u{*z39b3@Ht8VWpkWz~KSmifi+YJ!fxx%-VD#t?L1^OkkG z71JBGw#9Gn$Mp;Aoa|vc;n-kBme+T?=SOww&fZV>$X!?UY59q@A)xIXirJ4o)ipj6 z+o!Mo0v~_Rhn;qHEdh+$M{@r8|4WfmPXAQ@SrN3)17*9HD+6#B)_LHXy=RV(%xvCd z)x6KD?*yrm{3{6BB?YB{bBFka_WbRt`D|!^1hnM>wrh(YzGW*3xMc;gYYVvD>o{;3 zj-dR{js;BNo!+4PD;_ETXE0QnYAL+^_UlDue!w*>NxwyoOsm=q51K|e~b4W(qK3l)+8^w{oi!?2j;)|{-pfm_}d~c z)BjcW8`r(BJHJT(KHpi@2?Z@?h zO@Zkpwl}}G{^bg#2IyIL*h-eD@_mbc@qOE}3(ml%iUIq%m-F9E{I}WQKf_Os`d-bK z&A=TkCH-G#w*O~1;SB7KSlrovZvXS3rk(Y3>(4F4YAjSx2v!mkS55IhSN*CFJZfRm jih4`^mG$pd5U8tG!2(V-?EBjD3+~@@f;FM!|K9`vsKegZ literal 0 HcmV?d00001 diff --git a/doc/radar.md b/doc/radar.md index 229c6ee0..fcc6353d 100644 --- a/doc/radar.md +++ b/doc/radar.md @@ -14,11 +14,11 @@ ## Introduction -This driver supports the radar type RMS3xx. This radar records raw targets and tracking objects. The tracking objects are determined on the basis of the raw targets. Two variants of a tracking method are already installed in the radar, which enables the radar to be put into operation quickly. +This driver supports the radar type RMSxxxx. This radar records raw targets and tracking objects. The tracking objects are determined on the basis of the raw targets. Two variants of a tracking method are already installed in the radar, which enables the radar to be put into operation quickly. ## Measuring Principle -The RMS3xx is based on FMCW radar. +The RMSxxxx is based on FMCW radar. With frequency-modulated continuous wave radar (FMCW radar), the transmission frequency is changed periodically. Triangle functions are usually used for distance measurement. While the transmission frequency changes as linearly as possible to the target object and back during the propagation time of the signal, @@ -38,7 +38,7 @@ Raw targets correspond to individual reflectors that are detected by the radar. * Doppler speed * Reflectivity of the target (aka rcs - radar cross section) -The radar RMS3xx does not resolve elevation angles. Therefore, the radar assumes the elevation values (z values) with 0.0. The error in distance estimation is usually negligible and is 0.4% (1.0 - cos(5°)) at an elevation angle of 5° compared to horizontal. +The radar RMSxxxx does not resolve elevation angles. Therefore, the radar assumes the elevation values (z values) with 0.0. The error in distance estimation is usually negligible and is 0.4% (1.0 - cos(5°)) at an elevation angle of 5° compared to horizontal. ## Tracking Objects @@ -171,9 +171,8 @@ An arrow of length 0.4 * 20[m] = 8[m] is displayed in rviz. The following launch files serve as examples for use: -* sick_rms_3xx.launch: Communication with the RMS3xx and sending of radar ROS messages after successful parsing of SOPAS telegrams coming directly from the radar. +* sick_rms_xxxx.launch: Communication with the RMSxxxx and sending of radar ROS messages after successful parsing of SOPAS telegrams coming directly from the radar. * radar_object_marker.launch : Conversion of radar messages to visualization messages -* sick_rms_3xx_emul.launch: Additionally an emulation was created, which allows testing the interface chain without a physical radar. ### Data visualization example video @@ -185,10 +184,10 @@ The following figure shows a viz-screenshot of the pointcloud: ## Parameter for Radar Usage The following parameters are support by the node **sick_generic_caller** in -combination with the RADAR RMS3xx: +combination with the RADAR RMSxxxx: * ~scanner_type (string, default: "")
- Must be set to **sick_rms_3xx** + Must be set to **sick_rms_xxxx** * ~range_max (double, default: 25.0)
Maximum range * ~hostname diff --git a/doc/radar_datagram.md b/doc/radar_datagram.md index 931152cb..09469415 100644 --- a/doc/radar_datagram.md +++ b/doc/radar_datagram.md @@ -30,9 +30,8 @@ rosmsg show sick_scan/RadarScan' The following is a short datagram showing the structure of the radar datagram. The position of the individual elements for the data of the PreHeader is explained below. -For further information you can look up in the attached [PDF](telegram_listing_RMS320_20171101.pdf), -which has been provided by SICK in a preliminary form and -may be updated again in the near future. +See the documenation on https://www.sick.com/de/en/radar-sensors/c/g575803?q=:Def_Type:ProductFamily +for further information. ###### Example of very short radar datagram: diff --git a/doc/sick_scan_segment_xd.md b/doc/sick_scan_segment_xd.md index 0de8922b..0815ddc6 100644 --- a/doc/sick_scan_segment_xd.md +++ b/doc/sick_scan_segment_xd.md @@ -1,18 +1,17 @@ -# MultiScan136/sick_scan_segment_xd +# multiScan136 -The MultiScan136 Beta is a new lidar from Sick with a total of 16 lidar units rotating around a vertical axis. -The rotation speed is 20 rounds per second. +The multiScan136 are new lidars from Sick. multiScan136 has a total of 16 lidar units rotating around a vertical axis. The rotation speed is 20 rounds per second. Scan data are transmitted in msgpack format over UDP. -MultiScan136 / sick_scan_segment_xd lidars are supported by sick_scan_xd. See [README](../README.md) for build and run instructions. +multiScan136 lidars are supported by sick_scan_xd. See [README](../README.md) for build and run instructions. The following describes the configuration, validation and test in more detail. ## Configuration -MultiScan136/sick_scan_segment_xd is configured by launch file [sick_scansegment_xd.launch](../launch/sick_scansegment_xd.launch). +multiScan136/sick_scan_segment_xd is configured by launch file [sick_multiscan.launch](../launch/sick_multiscan.launch). -Modify file [sick_scansegment_xd.launch](../launch/sick_scansegment_xd.launch) to change configuration. Note that the ip address of the udp receiver __must__ be configured on each system. This is the ip address of the computer running sick_scan_xd. +Modify file [sick_multiscan.launch](../launch/sick_multiscan.launch). Note that the ip address of the udp receiver __must__ be configured on each system. This is the ip address of the computer running sick_scan_xd. The ip address of the lidar and the udp receiver can be configured in the launch file by e.g. ``` @@ -22,11 +21,11 @@ The ip address of the lidar and the udp receiver can be configured in the launch or by command line by e.g. ``` # Run sick_scansegment_xd generic without ROS: -sick_generic_caller ./launch/sick_scansegment_xd.launch hostname:=192.168.0.1 udp_receiver_ip:=192.168.0.100 +sick_generic_caller ./launch/sick_multiscan.launch hostname:=192.168.0.1 udp_receiver_ip:=192.168.0.100 # Run sick_scansegment_xd on ROS-1: -roslaunch sick_scan sick_scansegment_xd.launch hostname:=192.168.0.1 udp_receiver_ip:=192.168.0.100 +roslaunch sick_scan sick_multiscan.launch hostname:=192.168.0.1 udp_receiver_ip:=192.168.0.100 # Run sick_scansegment_xd on ROS-2: -ros2 launch sick_scan sick_scansegment_xd.launch.py hostname:=192.168.0.1 udp_receiver_ip:=192.168.0.100 +ros2 launch sick_scan sick_multiscan.launch.py hostname:=192.168.0.1 udp_receiver_ip:=192.168.0.100 ``` ## SOPAS support @@ -45,7 +44,7 @@ The driver sends the following SOPAS start and stop sequence at program start re sMN SetAccessMode 3 F4724744 // set authorization level for writing settings sWN ScanDataEthSettings 1 +192 +168 +0 +1 +2115 // configure destination scan data output destination to 192.168.0.52 port 2115 sWN ScanDataFormat 1 // set scan data output format to MSGPACK -sWN ScanDataPreformatting 1 +sWN ScanDataPreformatting 1 // for multiscan136 only sWN ScanDataEnable 1 // enable scan data ouput sMN LMCstartmeas // start measurement sMN Run // apply the settings and logout @@ -61,30 +60,44 @@ sMN Run // apply the settings and logout ## Visualization -The Multiscan136 scans can be visualized by rviz. The following screenshots show two examples of a Multiscan136 pointcloud: +The multiScan136 scans can be visualized by rviz. The following screenshots show two examples of a multiScan136 pointcloud: ![msgpacks-emulator-rviz](20210929-tokenized-msgpacks-emulator-rviz.png) -![msgpacks-emulator-rviz](20210929-tokenized-msgpacks-mrs100-rviz.png) +![msgpacks-emulator-rviz](20210929-tokenized-msgpacks-multiScan-rviz.png) Note that sick_scan_xd publishes 2 pointclouds: * The pointcloud on topic `/cloud` is published for each scan segment. -* The pointcloud on topic `/cloud_360` collects all segments for a complete 360 degree full scan. +* The pointcloud on topic `/cloud_fullframe` collects all segments for a complete 360 degree full scan (360 degree for multiScan136). Pointcloud callbacks defined in the [API](sick_scan_api/sick_scan_api.md) are called the same way: A callback registered with SickScanApiRegisterPolarPointCloudMsg is called * with a segment_idx >= 0 for each scan segment, and * with segment_idx := -1 for the complete 360 degree full scan. +## Pointcloud memory layout + +The Multiscan136 scans with 12 segments and 16 layer. For test, development and debugging, knowledge the internal memory layout of the pointclouds can be helpful. + +The pointcloud on topic `/cloud` is published for each scan segment. Each pointcloud concatenates the layer of that segment. Each layer concatenates the points of that layer and segment. Each point concatenates the cartesian position (x, y, z) and the intensity i of a scan point. Each value of a point (x, y, z, i) is represented by a 4 byte float value. The pointcloud on topic `/cloud_fullframe` collects all segments of a complete 360 degree full scan. Therefore, a total of 13 cartesian pointclouds are published for a 360 degree full scan: + +* 12 segment pointclouds. Each segment pointcloud concatenates the points of each layer in this segment in a flat memory layout:
+ ![sick_scan_segment_xd_01.png](sick_scan_segment_xd_01.png) + +* 1 full scan pointcloud concatenating all 12 segments:
+ ![sick_scan_segment_xd_02.png](sick_scan_segment_xd_02.png) + +Note that segments and layer are not sorted in ascending order. They are published in the same order as they are received from the lidar. + ## Msgpack validation A msgpack validation can be activated. This validation checks 1. each incoming msgpack for scan data out of the expected values, and -2. missing scandata after collecting the msgpack data for a full scan (360 degree) +2. missing scandata after collecting the msgpack data for a full scan (360 degree for multiScan136) If a msgpack contains scan data out of expected values, the msgpack is discarded and an error message is printed. This should not happen in normal operation mode. If scan data are missing after a full 360 degree scan, an error message is printed. This might happen in case of udp packet drops. -By default, the full range of scan data is expected, i.e. all echos, all segments, all layers and azimuth values covering -180 up to +180 degree. If filters are activated (echo-, layer- or angle-range-filter to reduce network traffic), the msgpack validation should currently be deactivated or configured thoroughly to avoid error messages. In the next release, the filter configuration is queried from MultiScan136 Beta and validation settings are adopted to the MultiScan136 Beta filter settings. +By default, the full range of scan data is expected, i.e. all echos, all segments, all layers and azimuth values covering -180 up to +180 degree. If filters are activated (echo-, layer- or angle-range-filter to reduce network traffic), the msgpack validation should currently be deactivated or configured thoroughly to avoid error messages. In the next release, the filter configuration is queried from multiScan136 Beta and validation settings are adopted to the multiScan136 Beta filter settings. -The msgpack validation is configured in file [sick_scansegment_xd.launch](../launch/sick_scansegment_xd.launch). To activate or deactivate msgpack validation, set `msgpack_validator_enabled` to True (activated) resp. False (deactivated). +The msgpack validation is configured in file [sick_multiscan.launch](../launch/sick_multiscan.launch). To activate or deactivate msgpack validation, set `msgpack_validator_enabled` to True (activated) resp. False (deactivated). Msgpack validation leads to error messages in case of udp packet drops. Increase the value `msgpack_validator_check_missing_scandata_interval` to tolerate udp packet drops. Higher values increase the number of msgpacks collected for verification. @@ -141,41 +154,41 @@ Make sure you have only one network adapter activated with custom NAT: :white_check_mark: Run the following steps: * Install python msgpack package with `pip install msgpack` -* Play the pcapng-file using mrs100_pcap_player.py -* Receive and convert to msgpack using mrs100_receiver.py +* Play the pcapng-file using multiscan_pcap_player.py +* Receive and convert to msgpack using multiscan_receiver.py * Convert to json using online-converter https://toolslick.com/conversion/data/messagepack-to-json Linux example: ``` pushd sick_scan_xd/test/python -python3 python mrs100_receiver.py & -python3 mrs100_pcap_player.py --pcap_filename=../emulator/scandata/20210929_mrs100_token_udp.pcapng -mv ./mrs100_dump_12472.msgpack 20210929_mrs100_token_udp.msgpack -mv ./mrs100_dump_12472.msgpack.hex 20210929_mrs100_token_udp.msgpack.hex +python3 python multiscan_receiver.py & +python3 multiscan_pcap_player.py --pcap_filename=../emulator/scandata/20210929_multiscan_token_udp.pcapng +mv ./multiscan_dump_12472.msgpack 20210929_multiscan_token_udp.msgpack +mv ./multiscan_dump_12472.msgpack.hex 20210929_multiscan_token_udp.msgpack.hex popd ``` -Then paste the content of file `20210929_mrs100_token_udp.msgpack.hex` in https://toolslick.com/conversion/data/messagepack-to-json and save the json-output. +Then paste the content of file `20210929_multiscan_token_udp.msgpack.hex` in https://toolslick.com/conversion/data/messagepack-to-json and save the json-output. Windows example: ``` pushd sick_scan_xd\test\python python --version -REM Convert 20220915_mrs100_msgpack_output.pcapng (16-bit RSSI record) to msgpack resp. json -del /f/q mrs100_dump*.msgpack -del /f/q mrs100_dump*.msgpack.hex -start python mrs100_receiver.py -python mrs100_pcap_player.py --pcap_filename=../emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 -move /y .\mrs100_dump_23644.msgpack 20220915_mrs100_msgpack_output.msgpack -move /y .\mrs100_dump_23644.msgpack.hex 20220915_mrs100_msgpack_output.msgpack.hex -REM Convert 20210929_mrs100_token_udp.pcapng (8-bit RSSI record) to msgpack resp. json -del /f/q mrs100_dump*.msgpack -del /f/q mrs100_dump*.msgpack.hex -start python mrs100_receiver.py -python mrs100_pcap_player.py --pcap_filename=../emulator/scandata/20210929_mrs100_token_udp.pcapng --verbose=0 -move /y .\mrs100_dump_12472.msgpack 20210929_mrs100_token_udp.msgpack -move /y .\mrs100_dump_12472.msgpack.hex 20210929_mrs100_token_udp.msgpack.hex -del /f/q mrs100_dump*.msgpack -del /f/q mrs100_dump*.msgpack.hex +REM Convert 20220915_multiscan_msgpack_output.pcapng (16-bit RSSI record) to msgpack resp. json +del /f/q multiscan_dump*.msgpack +del /f/q multiscan_dump*.msgpack.hex +start python multiscan_receiver.py +python multiscan_pcap_player.py --pcap_filename=../emulator/scandata/20220915_multiscan_msgpack_output.pcapng --udp_port=2115 +move /y .\multiscan_dump_23644.msgpack 20220915_multiscan_msgpack_output.msgpack +move /y .\multiscan_dump_23644.msgpack.hex 20220915_multiscan_msgpack_output.msgpack.hex +REM Convert 20210929_multiscan_token_udp.pcapng (8-bit RSSI record) to msgpack resp. json +del /f/q multiscan_dump*.msgpack +del /f/q multiscan_dump*.msgpack.hex +start python multiscan_receiver.py +python multiscan_pcap_player.py --pcap_filename=../emulator/scandata/20210929_multiscan_token_udp.pcapng --verbose=0 +move /y .\multiscan_dump_12472.msgpack 20210929_multiscan_token_udp.msgpack +move /y .\multiscan_dump_12472.msgpack.hex 20210929_multiscan_token_udp.msgpack.hex +del /f/q multiscan_dump*.msgpack +del /f/q multiscan_dump*.msgpack.hex popd ``` -Then paste the content of files `20220915_mrs100_msgpack_output.msgpack.hex` resp. `20210929_mrs100_token_udp.msgpack.hex` in https://toolslick.com/conversion/data/messagepack-to-json and save the json-output. +Then paste the content of files `20220915_multiscan_msgpack_output.msgpack.hex` resp. `20210929_multiscan_token_udp.msgpack.hex` in https://toolslick.com/conversion/data/messagepack-to-json and save the json-output. diff --git a/doc/sick_scan_segment_xd_01.png b/doc/sick_scan_segment_xd_01.png new file mode 100644 index 0000000000000000000000000000000000000000..5e1bcd5a7d2dd5bcb313266e8855947dd8874b12 GIT binary patch literal 28071 zcmb@uby$>b*zP+PASem~BB`{1gruY|4BZ`4qNH>T4HnWMARr(hAdPgVbV&|~NJ)2t zH0*19zi;jDSo_<@T5BJB{+ag`n0e-Tp8LAb>vx{l1Sl&?UB@NEMIaE@Wu%d+2*kxY z1mZ#~&PDhaN>A!r2*h244Dug!*H5eChE}&+szlG)ME70?nLJ(9D|yoC@K910NiW6m zlz=h3Z1Ka^yKH7c{rIw=_kmaN6_Z`ZGAy=>L)!C8f=_bsnTgott~~SJ=N_ z?EkfFaSMIxMV4+raJ|0-1)J5iE?NC0ijMlAh1s2iP)ak3bA@eh{m-sKJF#Y_Vj{iY z7kYmNw$NOFW7FO_FBO1igtqK^%=2U7|NSSbxQ!rlZui~AenFRYZZ5Ky=}U`?q!bjM zhjTGcC~*+q2%p8<@^(m3xm|X<#A=(P`Ss}Rc;gA>75E94?XpqvNE`+3pndh9JJ1BmkKm zM6TO#w3FD-&@f)@!bo^kLZz8KYIIHV4+7DLzuae3XgyjM@TgKxQRRbJ@=ct}xiNhs zBX0Zf&Mq4$6iTyn<#>N1$S1j5$8#c5Hce7el1?TrHa0dfk)&RI$?0g^Wzt&q3j(1m zlW;7+PZ6rkDv1n0rqt0l;o2_^(9;NbpHy_sP1EE1%?xI#A)TfgeKjSq;3EwB+eDZo zQ6N+M*Vor^Pw%aws>f|^Q&-Z{M9HS9-tV7Z+jDQx%u??(E#4{)d3^U9%UiMJB9GPe z^-#%FV&k5Lon+EFHg=~aGTQ5y*o_N>e z^sekhmto(Lw-~;pHi_l0d@@0jN4RySVFpWk62>LD=OsuR%a`HtNzZbo%;0R=iDM9J$|iHQ(LY= zR7w`ERAEwR?xfx}>XwE;bPN6EH~-8W?Ib7>(6N6o(P6!Y^`>?6b;I5Ko7b>-6j+2Q zMAE3j{PuIQvst_JK2$VQRBmy!wYGL9h8%jYxYoC(=J`e$+9D84IXzi#@>Wh54b(;T zEba%UtO&4o%sJ=AMx9!ncC4fd>-$HUcWGO|xPj z`Q`1G<)xl?rU+4manc#IIYqfqCiR*zXE(Yrkpxxc_3l4uTey&Q3GujV=$`XUzLn{v z*M;d*M-#iZMPs_Q;!@J@#|x0M)EgJNo}IX0{9oA@dj|pJBkbcXU zx%-2Fovai5#*BcT4Eslc4_}EM2}!7Q+8E2f3y$KJdhs36nW)6pO!UBzJ^W~sAS5h| ze3&zh;Y3B~X52>R*Dv@qc=u0nF~TjvZM1;(c*3j5r9YIe{_EHy*u!`DlEG==` zOxy9RYbekWmfKF|abz2JML%c_i|4atNFnF5Q4eKIP`uTjt`MJlS(JxI0$Gh(bKs|F zolCJFm%VqfyxH;7>&11xBwDFZI>La734^5G;mJu`z4PjgI@%PmWTRyR4naXV$7dwD zTnW7if-Ve%SJg@sGt`EiRkn9`xe2_EmNJI(^lzfz|F8^5@MV7$g|FuuU`coxz<@gGV6>4W*fjLX+|t@=M^~XvkU%=V)>;I&npwb{pe9_??UDF{8<2ej_f&Y(uJo}b``rh zr7worD%?sdV_I@xr9T+_8IW0NGb4DMK2>*xq^R_yp|4D$0u@a@~< zH7WIEN8`DFSa_Hz#Tu2$SViP7B3`yySS|l{sHBr&Cc;e*6iVvl%VYTMM?ta^wKiIg z9;+=?5u(bd3`Dmd=-o@|ji+kg2|3`K^n9jg?q#eH)5Rkou)K4K>)SZj6%(-7FGZc^ zOJ({+H49F`C5nr$zO}cv=jm6AoS*DejWsw9`_-TA<(J+zA#l++-0orz!E2e*c5;AE zy*IEqY`E3j1nYFr=g{-)a3P72CaG8JnZmc;Yx1?!_!Rt)k&KF;ZXV()jVhPnx>V{F zv946D?;p1`$}>35wiD2}?+t0Sh$+(cBno%I4VDXAfbe^t`B*c5b0ee2s{&MEqFc?k zg7{0?s66D~#wcbelph>=CcaRM>r#ia7_hWtu_g4`K&_yfmX-W4OEe>xPs#0#Q${T!M+!fuaKnCaWU|jcS%c5T z#ntLY*<6;+i6=DwdcVa)Hlmqtu5~E5dfv8W_SQ}U%>;Wd>KhBWQYem6Zqxo;$~QeW zv$)3rc-;62OdcX`$yuB!PIupYy#9K@83*f4x(e6MDcOg!v(0xxe$B7N73E`kogP^} zpBpAeAUd<}JO94mntsnG&7tyBx`lDv-MkrvH}~93FK&gmJ&kL<9<8Va#TSYg(wDCF zX51err=&SLyih?~)aBCmLR#C}D&=J&Twh<#Yf->v37lRE3v}#g@LXt{+45I5(c$gk zczUhZ_2Qd`&-csU)5*lsYLXPc_#m8QqJ}L(Gh?Oi86iQ;CKJ&%!-+rqX=C`MH~-@g z!?S~dsR@D?!1bXI*OQ z8@t2#S5RDEO1yA2#5U(nG;A20+WqnM3HNXZ0mzBaWsdwyB_zG8CH6Ct`NP#uo3Ln( zj#lyyPtz895?WtFe?KQb{{h^*!)e@zm#7~#1SDZX z+c?`;+O3*jb@xv9C(8wJ5ij4pFj!X39~FFA=9`>|C?G5`;*~jZ^EJV;;6&syEizpF ze|_Ko_F%LVv&2P`l+sk;99=!$cCY1)e-PQh{nU_^2p5!)I`dG8FsN0WwI~5Q9;;bY zb!xwP=^TZKXGMAW{^nFu2+qLlM6CyM_jkh^hE+T`k8`>l@z1a5U586;$M0{BLh*)6 zLlqT0T~bO-P5sdoZlZJYgt&TK5$Uoq@f9U*E~3YZco}V;pMk&nwR|G(uUN8jCW^)U>c4;0HrUX@s^ z--1%afF@8fwI*)dhMIzc=+uD4kQKIt+-V^ay<}ZYTy>2o|A>y~ON~1Vy*q729PM*V zfhpP9i+votybQek+#&wl3S8^@Dnt@0!J?!=aHeLyx{9|Nc7Oja9f3p^u?7ieXA2m9 z4+<*KW@U(!L5rdD4$bf;l5vY%NjXlc>*x_Nm<*vyAG9vqL2XJ4l0%yt7}Q>#$uPAd1QiXo-0K z@Xj@?uGX4GoS->XY?XlRHa)gBVoTq_S9qL~PmaE(FwsO97cOqO2FG5PdP3Iv{d@2d z^3IJ&ys1XFZm4In$4He{R!dXUR;PQ&yt}-!&i?HmeM^+ALk2G(G(Vu^DGyBb$DWhF zAj{*Iv*oNr-OlNb`B$H`&&6O_=&9-!Muq4MehvyP(2fvTtm+vm&SJ{@!jggCnt>xp zF>!FAZsE#p|ydhPSBr^CG>R`N)~^*LtQd0JJX7NV&8Jogp&JE@$H2Y*0M zVkBXa-w?~1pty6Rh}ENZ;_S>jrmIDcfsrvldMpaJSLE1AAfe7}$8;`2;*r&8nRNh? zCcYcgeNTBu`J}g;qxEIYLDJ-Y*dQNCqqiSCDQh+^JSu%4{*vfsk_l)qR-g>zZ9Ucc6T?=`)8l*a|cV_Mc%1SFDU&3)f> zl7@42#2xqNGzU2wsm~AEf=2tM?5^FoVOa0&Wo7lVu&a%aB%W=L~fQl2a z*q1C0>yrq#a(o?0GqrPQ+3QE;b7CJ?&zV+t=N(UG>j1E@ z&xw^i&Ba9yYEIXPCUP>eheMND>axg(JUk}lqe@94UeFdoJropV`6X!CEcF2t#|C}J z!j2FlQbU%V1P-VHKDnzyhgfpo(9qEM_!_Eu4Rze!?oTH}4QEKdlaWS3rQ*4P>-yLu z8r;hl6*JIlD8YmtP`Wl}hE8w2yu1Qah;Xk=#GsdioEDWW%;YEb+S_q9s!~&*IXDzK zBy}q@4UUg@TVH?k=1t9ZPJW%`HTDEW+p!9JJG=M8Q?%0tvB+I$>sP8K`up#hKAFC_ zGJv}SYEmwu#CB4oe3TLw3i^k8wG{L>>55gUrhWa#;r82lK2gwu7*Y>^I6 zWx94UFqrxhN~!$uRFx6M0_73QifMa}Jw>+eB2_*ZacB$lzFqndjej#?!v*YSPDU>R40j@y)vu1(X5jwz<80z+9t?YuL{*V;`1%)uKUa_a-EID>mp zvQguy!z;vwQ6}_a@!h7u@9nM9Q-GI@+bkqf_nSL4a7iM!Tx+VUpX}b|+{^m@{Gma9 zOxIt1GD$?9bd#X|H>z5$mvN(AJM<^u8}`uzE+X83&8q6^nXQVg(JSmEE=1QaUqlk& zTz0)uNGl>>U|v2NKk|kc@mYa*q3}1{ytc12WkD??zhql3iOED9q~@(e(1j<e zB+C}VwWjDf2*SftBQKH5|_8qV$2!Sbw7}XcQ#34Dvgt! zO{|qAC@fvy(X|GNE;&@3IXK{_CZqNR-R~-tZc8sS-L&f}bka{s?gz^;-NJLgpNaHZ zXh=x6i-;SghK5@*YnSm;4pQ!SZCfmXDHzQ%`{p54&+FTA!_Erm;D0RRqU9M{I%4SV zlU{S!EMo0-Nr}r{KNr7%VAa*Z;1fNx|FR#;wWMCgY=Oei!Xpfz;QJ03#Ufx?nW!h~ zJy011;`+Rml!t5~ZuSIZ ztPeUT$l8Ply*su-uQyBOsRZ*)wp22swf8?LP##2=TPf(V%21L}CUYXT!dK*;@fh;1 z7WsY*eH{`KqSEmUfiJ1PnO4e9ow~IeC7e`a`0SmEJI+bJlY@L#KueIuJ?Bh22VETC zA|;*M)K69ZCoYnap6+iBL;~NhsY1rkkea~0RKFqm!INNWKHCX-BOr8T9Ksu`ufR=!=&>QGny*koEEo%1uAP;sXK^3 zfr*u-lb?E&pr}9W)+Z~Hv_R{=KkgE;qF3qQE88l|I&DnQ4=ZcZbp|M+-CSoB(3MRK zY=rhyw9oSA>}Agx4%>+u8Tu&M5awP!=i@)tG{4HM<68!nNuOE_O9UMq9Yu7^_NPi= zI0(i31me?Uy;}Dupo7RIe`4(Da*ISnuVTh?Xe$5-y&9Z$|%l1fTptX+lL-`7xnV)yDN zsc|k7NsLsx*oH`zSfClsl2>=Qx2&=x6A}{o%|BGDVu}px~?grxmPzzLZeVufxtkkx#YAlehZCM3VUCm`>=vaaYe_G>h;-f4>S~3??Er!#a87 z%5Z*$Z@LQXshAzBZe(QS@!gvtl3pT{&CSRMS=Su-DLj!u-k**2&CC`GpS4L!Nd+uU zo~=!i)e1tdS(c)P9n(eMRg|~lU2n(=^#!i^y%a4&uhQ0SxNGE)CF*=AK@@-gZp9 zaOde$cV)vWL8_PE@PI8S5-V4=9pV;}-V3X|3!2B(>NTB=UPpkleD!>i+Z^s299NUX zwU=7F_!~rROzHU?*C`TUg%6d2eiusI+S=+|h`b#qiTe1?!#|d}x#SigG%&1~^KBvY-81=&*5E-Gutp5t!nCA+}XmHez?5z_W!X@v2Y2Q_-bu-xPog-deaLX6fnxb~9 z&cxoLtV`IoxiyZJpW3yMZ_{?m?4k zwjwrsRcwSgF{m28_IB=9_LSY_ZQ1^t%c^AD_)1}3r$^4uXeETGCgI-oZD23BdvY6U zTx@y%rE>J8!AxbQ_9Cn0vCWwJ6W`8SrOOD!ugMxE$_ek|wO6lR)t{ekI8V|i3gj}` zl*gMPp_3dV-fFrbDxZOlpi4trJiih2?i_Y!=ACr>y-79w^MR?2dDZaVg-Ne{eZ||Z z0VzztH{Iu=w3c~#j7=JSpxYz{wIfG(hKG2Zle_5l*A~+K@7y6Wg?P{GIu9$|6F31k zL{IEk@~(lcRqX8KRI~TvNz@C;-~#P$v|&OHPzv6y1M&Fmm8)C2e7sSgUvN8aUPA3(W$0{dV#xDer+iugY=MRT$aJv0KY0FWn`c=6trNuD( zeWYh(n3iGBX&*s=ub$$WJ*Z%Gv9+l7giTf+0K#J`jv+OtMaWtwkn*@US zlKyP!_aRGAEvzRVP>5Gco~{o0qvyEP7*X`4uZ!S z`LE!yj}FFDfXY(&?fJt;lNR9q=;2deIX61ejGi;%$h07fF&(o2MM1gc_S=zOCIWbB zM*7bEP$&S~y-)yrp=(L8(;vnll~jwk-i4X4)~2jE<0xIH%$7<+T{Jr#;@fN1A77;@ zmkUw{Ecs*Q9flfx&=Ox!XD5dP&d5D3!)2o}J1}@+NOLF*y46`6YmKuH-ZU7Ee7sGW z%r9rcMWw>8^(smYJCC-@yX9Deh(4w(0Q2enTU%QXFuX@W>s8Z7D1r|-sTgcqWZq@I ze5632QTG!x4!_I2;0wbJg|ZI>2B*i;_^9|1FNxt;n%tM~N^1MxBTQMRASY*eGP6DD zRrt#@>rJ5mdyhtJ+J1E_eH!tTk&hA*)r&Z>_Q{~lYGZ%gzg}H(Qzjy~s=a%!1^S)! zPbi`E!AjUzS((W{+szOq=uZ^(kdHsvj;W`(b4SMxKeIcYk6hGdq==p*XuQffr;%aO z`}je3DS@`iv$vAq4l$%a9}pYu_>-6yE*LE>Ef_>X-X{mJK?@2BwrB}0&w|xai&~4* zcAaUx9!FlFOpHEM*Wd}4uFzvEeCwjmM079%oTZIgp_awe%q;79ZZi#=CZ+flg!#oU z_|B;ozhKMl^ls6}&mzn0hvT_@f&xE^L@wvqTc6Vv!`tQRGuNyBGYc@axw(0?x8k!C zKY97$D_F`qhl)ueYrs;Oz_e1~r(hemT>Dje($te6cq^b8{E_41W4Eg}Y)9(6j;E&y z_zl5~zKQd}yBd0+D{`5h(5OU{1Q(cN}hBaJv}VNKn$AY=!yMELy`W zn3JEdTl1MP`v)#cMz(j#h zo+0DkHUn1+xeHS7hqj0)7YA?V{_bvjQSM4rp_>RaRXx7S8gJuyt!YvK$IKH-r>TBI z;jqQXMbC*PbMW)G%WWUl46l40>whXDB;3l?DrdI?pXptu0$ zeSkU8tDn?%_7$e%<+ju@-;1w-T^QWOiWA+ z#Mo@+Nl8jpWQK-@c64-fdhx1deSmm^du_6^|AOFv^Z3G!y&-n~@CqEs(r#-AIW{)- zUtjJnHNk!-UCREXkKyoMXej=ST>FW)kG^GJnCO{y1f56|Me={4E*geoJZ~2gFCsGQ zf3-Pit4|8^KJaD{oW8K1Zts*9(Lr}(D78|BG`-b>O2vXAwRa^RYcVGD5ug!Jg5PWC zu$aj9nYxO47j79wg#ad(Kp3|&Qcc{KiO6F4c5qP9PAo*#`~Ca14g0!(0;=+YI#Lgq zWX-tA$jNUNQ`)(L^ln4tcM_rPP=GS)|mrM}+;pcQq`S=$(+9L&F#t^9R_x)A?sozN)$6(9E0Z>wwNlV{@_ z-fg1!x}}y-yP(%NUQ+T|b`tP#cjvYG**CDL2~IL3NKlxZ2o$HSEpVv0yurJAht5!{ z-)zL|#>ex=xej8bU>4xq%y9i#>5{WcANNOvB7GDnw8mNwOvr@MyUr1mkD!Z_zETD{ zOMTha=a`(}`t|DxLT*J-Dr|xzIYQj9(?1^70S)H4@kGRJdp1igBaY$*X>tR6LPYjU z0Pp|bo}LB(5h{BGsO0D!ES&P9(KWE1>Qn+6TjrHH4`0wK%#(CBS)zlAYUe zD+EW$pA`5e4b8^-dKncQ85ibcQwh0Zu7G76>i4$`Gg@F30W?t;N+(sDy(hXhR<)-N#NAGzR-KT-lsm@B0HO~9}M(mV)-4V{;4KE z4-bzU)FQ0WWA#2s7z&bZ0qtf1X%k$Q=0_l5e}I8&o;^J_p&8)rF40pV z3RTn4*edQ_;QHFS=kzc(*H*p85Zwr-&Qqqq#^bppkmvS1dH3bQ%=_bf0uQ9YkWR zjQSk@ijeT{dTE_V~;v>RsZwbLG zD=RCsH2T9YRx|D&DUq5yj6M8eyLdmPQZYPS`b(ogNazK`XN&g&B{9(A(mX`13%YLJ zC8*t>@Yu0#q&zCK9yQgigN~YJz_~Ws_0Q^vIQ@Pzq`JbT+l$6VMjlPRt1;Ya|K~T> z8QHOg&3KgtkG9?EyF{9}6{3#wGG;8q%XFRXEb*mrGcTS)JCMfN*;RRtZO77d5sDdJ zB1ub{LyO=Fo5TcVOqc$<-H?TlNlDt7`3@U+Sl4}J7E;fYQ*UZ&y5-1U2`nqkBCxMd z?YW9zGsLfM%kemCSEH!%J~LA9BTC=}TjU96WXEDvUbdz0kHDcEEk16#hK>F5 zru#)Hl*Z7l1f|6Z)SPU0P5PXBabZ{TqJ!A$+C?EQrH{t~29cxqo28YNNV@wZLH1nD z*wmhnMOQ;qvc*RKp_HHr2eW0%Ro6Q(*Bo;jEE{K~<9CgY6q^P3XYiEphinZFez3TD zBk1N1{?Kden-1|3kj(Yy-g;|Z6$r;j{aaCbYuf}0dPS{u9y*DYg!AF*6!<~E+DR{)@D z-I}?}e|}edw9OL>8~gUB$ZvBFTgdlPVN~=f8Hw(e4VT>hGvqP&VZ`cBdpL-V<(z!Y z2{3y{t$0N;i@?HYNykm@&oIxzTztVV_-k{(M5^+z-~6-A)(43sXVjd)3|x3jhr1L) z&IeZT-!0yN2KK<5zSwXu#G**zB45rA#9&OLqA1*WsaT-ld3kJrj#1SnIzKR*y+-?0 z(t6w#P-Ht_H(SYVZ-(cs*u3f7n6PTRudDp`)`NGq2DrZIL2ckRM_jl$-4cZ4=HO6% zD(=^053T!p_a9Q|viMAF4uCyt0Pd&T?mD3G4I+Ng@}_sq#@ z(7olms{v%PZs*og;QIlMCc?!N(d83Bt^NcZoQ)gc&!$(Q?8X}e(n|9Q1J5?m-ah70 z&Wl-_|11}z)PhWnpwr3fnBF>gjeayJ!A&;g?B(fMqmZss;vE?60$28;svrGQXA}#v z#QW5Jr93%YdiLz}7ythgORy zEgB?vVRG`lGuwyU+=I*7WNE9^Lr|chaBn%%p6O|y$l0)znKw44iPc=dW5h*#ru-!2 zwv9<@a&vP#ZO?YJ5JMmULaFK+2_HXZn%OxU7mqyG@rEAWd|ibNTOJ9qAdt2~U#?xg zs9sOjX&mK;1z8Afd)N{CmT!{$b`l6#tIyHOpO+#yiMZ?6)G+C=-1q!1EBS??g40+r zJQx3qt@Dz8Dc-j3aJ_niOx=|?e((4cTtdiz4j!Bo3D5v(1ZQP;{B4a8S;%&qr-&_c z!!9m47^hx%uIX;o;|6Y;df-HqkFKEJ@;rV6Vu+OsPm#lb-#(yB>Du)o(*R^)K@mUn#~oDD3SV%TLKuut8;Y5Aes6!5+cuyYK@=kP9tT#dLN zK9CkpaZhftD7qzx4A9Z6zXDT0{`8(}kuJKTk5UjdNM%_(v~s`AVaSFtn=)NCR~c?& z%aQL4vEW5R`lMLl-3Z-V+CQyfD^FI)82d z$8YW4UbBp92*4-L&?A_9W#)!W=31m-R753P3IMxW%`k2m9be5~oStB01-EH_m z@WIzUtvtPc=URe$u?!;5V&&%{@T7<5U}&6LS0}PA_eq*=nM#REiNNLKv`lyhJqGHDY154F76g#?ifu(n>K>e<>Ha@tItR#3?Zw$TmhQRmHrR+Hua9Vty zoxS9wE7bfV8O1NLcJmT~R6#+Z(#N00!+T>8?nz;Oeo|cDPl_4$uNu`K4dt8Xxox%F zHsto+`|quMewTGUF8tgEP;a+k^%svce=LimUz4o`M^Lrxp+gq70)=9l+b(PPsRNPuMv8v-ztn>Y|ZxrTMV z=_l&%>?(vKW$a5Qlg3$c`RK3A?DsnUL&fJIQ7IR#NoQ%u%s`9Z=S zf1ulti($LdwQ#Sih*e7wkNJCmW-26T$V7FQnENZ#kdFE!vVuz7TyR+eyXKfRJ@a>A zuaiGb>#Xw%Jik_Vw47s7PbkjU1+gy^YSZu7`Sq{2YYfndRl>%1kv71^8-B#uuv3kRs2Jx>A*$9kST0{Gfhv z5{fm}*n;Gi^Nr6BK?zCk4o1;s*$ZQ_5YdnHSJ6f%C$?FXjFD=Io<_;ohn$G>c2GpuptjFMfd zn)v*#Hv4ndMZ{+fOh@4<(rbO;HoL*<>Z*|A{I%|oUaOnl4am7iSM}IpQkY z*YQkXFk~UgC(&pA>OIgnU~&@E2-y98Jjd5y2r`fLyUe%z6zL3f5$luOf>Uo%m$2Wj z5`p&2Cjz> zX=Dai@@r^F3~F2pb8=o78GRO8wmU!bLYGKaPnIQf0S>XFxKi3BQe zN4#G7(6S?zovnKq9pnF>iZ0#l7t76C#+oaQzIah)#oEmuEUybooSvQ%6IRcI7VhJg zrqnC}t5PlEOQ%{|k*m;1;fZWyt=lw_bbtgC>St?B#O=oi#f?l8s_O5vi-enn*}p29a_r6jwxXI!LHk0w}rzDXi&p2B3pqLrp3R4`RkN z_0(rpI-eGg9AJN90t?j+G@4q5B0+y&UtdQg;}goBBICl&CLmVO7hJ(Wn*V>E3Z|Q9 z?Bly3S=vp#b6E2?Uov799RrjELNsA*Pi{Xu7Q>{DS$cq8V36@nn{EARnKBa*uJ35M z?M?(FLNzbl!W>b-w9whX4Dt&TuFqJBMI^*#=uT%DMW^T?ET4D8qi4t!UK zkN4ORY&GqBteS4-1@K4zC2?dfI6gk!djC7LVBMzV6cn>zvGT3_m&kwwMFNc#6snH> z*tm*m5S;dwHO@Xt)3tN_m!GQY$UiM}Tu`L>Rprd$1A$wPxI0i#uxVLIUJ_t%nV8XA z5Uq$o>i;#cyEyR)QrFfC|CQGT8pa=@lg^MJ3L$mtk$lBOA-Cnh zEQYpAlK;(mC7S>h!xUl0DaL$#h;PS_jq@rJ73pT^n=)vOQNjrtv*6wQ3UR=jz_jG4_F z%lvMAqIuM9{j21DtURbfL5ffTikBCpjD<*Mws8x$?ty1)#k;b9EWuYbkkUy;O`Y`d zFS4u&sh~=EgQ*daAQJbuLrF zt*0zDVs%bSs&Eki?qp|2(82M;U3I6rFJ{h;e|fa*0Zyo$LKsDo1+s=0@Bm$xoG_QF&P zNTWp1UAg#5otcM&Edze>NtmgIX}nUZUQ;e>q-uR45LJde`5c zj-v39sJUW;EzIYj5|H=TC$rX18Zr?@@1|wR&K=N(`xj1O9F8wXpd! zYW?O7f}KVtBI-xr72rGZ_!*Ccx3jIY3sN*<(hb%pYF}KV+V}@c`wwWE1UT&vAEq#M z&d$=gxEDOk{cL@@tW{5w5_FqAUG={HaIL{c1XXsM0`W%u`B{VrjKS#Mbv9>*9x-VF zq+Vfh@ec5PZa$IX>sxX(%wg_!m3n`Z5HL4K*srD}Je0eb4ZCAk-HsTbb7zZe^NTDo z=?M?^4lYFV?ZQie&K`0Wh9FMo#Gso}+!#gEtkz zH*puRcL;l{ki)F>8~p66G3m*eyMu%JUvL*394sBN+sTse?~9SB%gaEGGv5p3VwBm7 zA_%f~3f4jx)gtqC-pUxcJ=isxjRug9pCkUWKHep~#9G&miNA6y%|_5Ys5k`cbxU6v zQr6kSdJldM5A23YCH{UYV(nqUeA(2#6m>9k@#PODF^M%)3ix~k7I8mL^mk6ciir?) zp!}Sj9c6uZJ)|lx#@nR!{n50GFnRA6)~*P;Ifu#?;tC{t$TvnL(SUsVxp7bswPq*= zZYA6yBeQg7?Fs?-p+spEKH<5e0Fz(ytDP`7y|+Z6OHUR8P{S+x9m@1{pd=%WO7vTi z-{4c3X{?#_?%F2DGykQJ{!H=RFzN`&Z%&^=Tg#?Z<9*UQK%}0(ymbX8xc}W;@6(WxkqLk^HjN98 zmcG7zc6QkUohfg;LiAjWfqTHUuxD#1k7Q+jpEFM%pL$pGww>i4Iq!$8Urr;_j>^Z-d^EPP*fw&W9ccu0-`Fvm4Nhg$Nr-NayvGSK@ z27s9%Tr4JC<1JV-gh`MhzTNnudS5~%rGF8$1GmD?p=Y6XE=*z0rQrV80f%>0AS@SZv|z3Jnr$_*i^8%i>bwj(gxbW5#fcYoi-*%?~& zZ~O#!1Y1ak?%1p82Ai0Z@EaQj{h)2`dC)aOb};0Q-2L?;pP)ZYE~Xhb$@}O&f$+|E zt{PpMaAl&Fk2k^F!fY;lrTpe`WhL8KjcTATUltXOtPTziLNX&@5tLbLJ}f8?m@hHs zW;T@%lZ357_;5B7xeG(~1S-kO9D9Dh7p|w1_yI)Y-XtZvq(mA0QJ&#h{4>uzN4bj- z|IV|W3YSI_8T`7Ebv5|HHGKRZh0m+U*<|yc6$asavndY}WMqL>o;hlo?_;rvXDotr z-|mBn8_df5g%xj`p3dMxRCi_zhA`HN?u51B`Mw@rnZqU?Gx=wD@7s;7obiLXZ*pY7 zz~7;PtMMr0Y-;-JessSA)a6;mB*o?pp6E6xi38kyn`ddh>8go?&Lsy^^YQ zbS+(^7y5w(d^dqC&R;~2Uel9emGj?4Cp!2ul` z9rZZad|LSYjv)gA1@@aA|LWiI3fE;5Z=SqaJEz)alXL{W9&jsjMbhB8>IxGSbj6#`1GvXB={k z#>Oxxg|e~=jv=SkDKA!LQZKvDnAD5$(vjS}yu6SJ;^eg7U)TSezSVsg`J%K4GZRcA z-WmczQy@VT6RA>=mzP&kg5-mA1QEVp2yFw2*?+RTVeQZkhg$i*%Qu8~>uYx>+!hi5 zKb@ZK*Pkz@hN)%^0_n!smNEK2*HAP^e@1Ov2%m*XzoPcC@ABQMbr-r!o1hqf)-=C! z*vZAm3i&I|6cG9JgaHGXkD(p$1+4!dUl=pMz;L_6?Ya4PmC~?n0dq;~VmGV<3mno* zDARdb7?0QBIWKPp_IfMe^8U|Z=-~Z&F)`_6Wv1CZcR>LGXezbP4dvKCjeN;nLRx97K_KqrN-KSHC zjM1e)*v2xfL{HJ8Yva}Btzs4V#>vXkCw+h8@8D*AOt(N)dp& z9`7t%!}K739d6>XX`2^dyyN;cjY&PVv}7g;0^2aZ?x^zvw)!`hk|$+WoDUY*M0qAQ z-GmagSy6}ECNtx7lg64 z<>+{c0A3Re0sWNIy80s`4t6)7jXcH|nX!mqYCsM=T9i2Rj&ee!4rB1NG z;(H1awI6t~VZ$p4A5$&J!ZHdRp23`(v%V5TB>L}Tp*M*!W%6X6?}v-9;Y{A%ud2{r zCl4F8H#g4DHcpztymx8KN4=(-1I~AR&Uzu0_9y8aCKp_Ky4dT--b!yD3N3UBC5Wf) zx10-lZ$~C^yBp#)s`-_12?%5}$=BqGpf>Q@(+Kh+Bb$Z&h@OTsLENjmr)OIj+{u3@6o;MrA4c>pxNSTD z_j}C=g`yJm7P^VE`{xf~KV-663}w~CFNtmw-ynqUKT9jlUK8s~fA(E@D-1RON{wMR zoNv44$<;X?PNIIe2;-{?8lhW8ZWph~MC5YQ4EkdW!q=Rz+z9-5-w`vRAzc9F%B0E? z;$5_J$;H;rf_3U_w;tniw)T*-j2O3-AdRY&qyfst#^I=1kQ z|8nfH>W4pAnbqJFyyFoftu z2t8zABvcO8*vE8XS}E&^Ln>wjFP;A6B&quKkIY}`fdb5w?%!sd;(P>EZ(pyK|2Nja z7$v(^JqnY0^fEAKX|iBrdAW{&m9nSp#@yK!^?BhReY?4%25f3O>?N%3G<}k={bP)i zfwyxgB+3O}wc%s%>@dhYJ2MEzdcE6ikAx3lF!1jrPxrNSR7f4X4JLFzpVaGU?G7Zm zr>6(@>^?o`(aS=Ybd!}gk57&74f_|dvj22yWLM7ov}m`3gy|Wb;JCgAI={VO_<)m) z8ANl=9Y~kYEnJ4#4x93w%Z?^ccink3STBk$i$ePQBk!AA%FMCp>4T8kj$(7c|Mpx*XD0c5g`wC=@0y-w{2PTdyL%JzFEe4H1_j#dE;<4- zY2mKppYm4leq0XXGBoRj=;Zorq3vWH2#@m7T7X~Ltm#ahub*jS!dgs3%ubWXaO?Mq z3PCGotXp-&%d7E`Z~lMIRoJXQ6XfMBGxE4uFJjqXLB5almSM`@5Eo~-PYM9T=tD3V!rvLd^XkUYjyDH+L1R+LcIqpZq^^LqS# z=Xb{Sz0P%=bMAja?&rFFKKK3kyx*^N^GHVu=DPYQow!H1SV(B!D*txi;z_bk@|l#Q zf^2a43{F9+(ct3Doe$)@x=1R*;A-9V&+r?o$BvMKILf-iLXZuNZ1tsk zX9~{c&ju0Bm0RwjF)?vT@fWi|lL-gz{zKxPH0L*J7{oX#@bmK*vMQ2a_?KoF)N*M^ zaXqrDuT^7m6La6R=7*SDDM#sQL5C$`Yp8FQ)9a8z=D!k~YSlmQhYRQiV9pdJw-9Qe zirFG_=r9%Utp4AF?7ByGMHe+)s_4sSx?fRIQ5|@2$0h@hYgWqTw`iaV8aH`@EG(hU zqAqP(>#|;CzsxbAco2zYE1b!)zm`Drd_xAa?nZK(8Ih>hjwqRyXWQUrT{SphX}vTL zXi`pZaEri{`P1=RR)64QmecEn%H$fOLdgCd)TFJl#5mmc7VUCAp={)ElI5x_@ymPp zL;pli^Jm2=XWk{X%AtisH3wJRxIM`4f2MEP*2`cCu%_#`DH5c7EH6&`t_gUnUxx1ou8!yj{sB$Za(7QT#Y=O}c+o%w?mO);69l1_BS$7m=uKaF5@d0yU<$ zo+9LjgSFvBCSqJ0_bBG|OtA-2YXkHDgp`g|eEL_Q$0u9rCz~(;=Gad)nhCOHyl0qZ_m8?tJR{~J zh3S%Z?=7d>|6VozLv|Jx5y2K&v@-Y~jPhm$Co7U`ZdutCM@JnZaa9L^4*Ljk4rX>^ zDDda}CErq;zQD$KN!~4%)!s-1n1J-{0R149k1y;*>{!9L%2xpLSlb|f%Cw~q zs!>b*4jmW8ALL)DQ^!ug)(Vg3>#9EZICxqQ*`PuAz#>B(07=|Sx9tVPc&9j zLN)_m*fBl5O_4>6{lVKE0l4ZgpLX;wCH4`$(`tgM21pFyYNNm2XLtU%#HN z?E=#NLivt_dO=#6zx2=9Q9~~X?IlF@ltuUXD+#*-vIS=X-xlFW8vg@S!wSpq8ORMSN?A$c0zjUyFWW>RCTa%8Qg2J0Jj&E${~ znyjRX3d47OMrk8oy%0fy9^^?=$AM&RV>%iQA*~U8OCw@t#ptV70iXkUcx=bZdZ#-v zhLv9fnca?gRP{r)Roewgn`y?&;a%vfP9E_`9X~&oM|c8F&d&BdMgdIfyJ+edGY-ea zBep@?064v{TR~OTHCx*qJmCx8)hZEw9%zZ$RWtQQv$uMvu>zqv#kWDzGhTRi?$DzX z4|^%hv}*yT0kC9cS_CjNn$pHByIrNm*UL+(MmkmX>Vb3nGG@xif5Mlk9Knz9c?-u= zC?!TyjuT_al0q6k{JET+ofBN>9p-bGvYqQHqP&mj2V>TJ(K!zf567+^kpG@pmi61A zF`|FqlqnxFglIJv8{KSe(~F9Vva@&Z8_`J9Xz&vlrXy8VsrI!-87|Cggi#ljy$oPI z-@R2{8es9gZyz4PuYP{W?{xFK@4vFmN49(3JxnUUW7uus}sF%oyWtT#GUhi}1v&8YrpcJzg`xnvjNyOVyIT0riCTh7=#wNCtP zcYV6`)2GydcSLjZ@i{MDi(0+9&*Cik%y;B~?t${hU$HA_A^?=k4SUX$1B8!D#0yoCzRg5&ka z8UFaVIENf(K6ZgH)B_lno?L+uOJxM{_5JQ@T9DRE;95NhZo&fLUrPLq3NAm)dHq`3 zG9yaP?;N8*NP2fXmMwdNV*Y0hoi^bI;u;q&T;RL?Nbg+TpiMEsPk=roeduzI-jehh zr%UT34deFTdnGEs85+2JBLv}kO-&dQ7>{V4$FXl*2Pc`KA-gI_>|JyNGg-P(r;1GA z)E;dxsb*dMotdF}_AGAiW%P@$sp$(x8{bIPI&4&=tW%-~C2ifOq~SE0yM8UeXlaggofxB^5!#Nw0q?MeW+c;-?iK zFE7%2y6*JHOC9STFKwpLn5yzXHx#;MX+?DP2uVl4=^?@CPbiT$@f1W%khurBCOrMNPJU;@BO0v3(0;fYg` z3I0RJSc8V9tBqADCfPx*f$Qq@5QAz`aj zJ%5A_r;F8L)z`T|O=#66G|FTi7-Kf4rQXQ*cU)V9k^|0&?Gae^_8TA*SX>+ha&~dC zEGs=WM8jFAsk!Z-Vipib?02ZKeX$u&pT;e_Q0-V(zJI{d^`+9Er?@7FgG&;`C?OQb~dzTU}R+E89;v&Q>y|kV-CQ<$d$U~j)2FI z^k;qNd3q29rTxndQvaZzKYzmP`}FD4%uFH5RnIC*%J~C1c?%!RFN{W59a$ZNsHDA@ zzMxQJw1pH2~N*Zy@hlR zrDWiwarVy2{s?X4RRHIrZJ@JRL5jLQ=$j(vqT1P9`n+Y4iIa`ZKoGx=b(re41*66H z&`kjgUh-KP86h}NE&jPKKY36#Ual~B3_gy99c23BIm7F zFZc<3Syr93k0HkxD@F!ox;W^B^2)%xJm2M+)GEshq;3DcKO^EIDg)Ucx8DHE&%JRY z1okNCSfqv%NVdH7j?wV~{CWp&_F{W^C_oQhh%*!@ zA~>}fJ_6B@*ipG947e8K_0Mx&^BOHXRu|?U>?IBFsaE7NNVP;g%YduY-tDDCyctBJ zc_Di{SF3~3m2<|#j`wewMoscRP%|0AiV6w}z8o8S{*S$)^zq}*3&N!Cc9J1^zUcy@ z0Pq08vSqkjt?o9)VJF5DE>jD#GnNK03j zi3*?Zcz)T2&dNyUHiUOuH>RC@jpXDFfhE1aD80K6v*jzxtEs8&-Aka?LO}`6By#6L z*YyK~;@Sr9^NmI??!{yPJFXd!stP28ApHQ`_|T&M@$9Kbhc#JCSbBSV?M;7rRRP^W z11alqr*d^QGmoelz*KO%KZCyh{6GU0F8+RCQw;(gQm2L0TxEsGpDu`)ZZD^S^dsR@ zx<-o)HmW;JL}HbtWa>McagB#qh&1*$QHNBn)NA;8aOzsr|LBNIL%bQ7pu^ zdIRJVx%pyS;K#P7zx;E;^!A%USEcTSp&y>JUtTlT-d)XcDleH1PVd6)Ubxua)5Ei( zaIVNCY?6s|97R{_{FAxuqw`vHldER8(_7|RNf!sSTwG=X8MjQE zqWpx`exh4;@XZmmrs+o-q=^;

uh8Kj-E$H)?KJpbOH{2-4P80@(hdztGiZD%47< z7SWyETTqq5B8{qdsh~1FZw#Ld!(GD}^VNgfjFa&J3A>Z zs`&S{3#lTj!!dJ4uYAw3N$LZtw)2Afh@@;Nx%rFnF1xU5MwIa(>35Z!NbO=abUIN; z&*I#&#pUD~&4Y2nc*^3xydPrCptrhMCt0H8WRRbqk19Ddf>gQW*N(41K{3#-#U1OZ z;uNXsHA{#*)UvrHrc$nJ1X^wM#A9QQl%Z#vhrozaPc%I*@=&H&t?A{qgvoIdG0kOY zMsMD~7rA&k)OQ5T!)vlxJMCLzYLzsJum$UH-J`C)0`@zW zv^SP1HlYIU@9cK&@B@s}Y-@DZrtR}g^A>e}x=o1C(b<`OmBC(SY{5#Ha!BDAw{ymg z?6+5=>=V9;mYx4&6eSv_)YenpI%z>ju0=Y&v{<27+j)S z<5e2-J*zKj+VFn((r+U-Ql^84vyrhe3k%D5+lP|Fjw*UbjJYU5I}S%hid7;{DE7-^ ztuXmA!TKaV-ETP$ITw^WlNp^eRFp$AiZ)#vuR*+~ zi;GLcD;pvatK~?vqobqD-4iPE^75*x`H9)wB6ye@8nFjLw$n^;$=bD`6xfJ(gg<6S?~?xnN{U$ zo|teVa3Dw%c0l{EQJHc)Xb_nZLQ0an4<1}^{+;2Sz<15R)E^1ZgT>?wJu7KmvbG;w z=R;S?jF*5BEu}~)A|dIj@1cRN;aY?~Q(QUK-|ga@0cP*mFmkIDmM5(PUYPSKi8WB27XtKn zI$+Sr&OXQ*0At;0pToMkjCM{wJ}G?50JE&az<+zFyyFcaLrxdBob3(+PN3FxfgiC( zk4}u^6S^xOqnevebwF_|xBXu3&Y>-T^r@tRIk0jryM%)|WTdl)6Qx!qaI zue(v>TDxT}Fx3g_dq_iNH8x}2WE0rD8Qd3kw)^d5QnQk&i5 z0O63%4&`W$jnr1=(DdK^SHDAphVG_4Kh$h*dw#3djn-(%kAVKnNNb{nNAF2MpVGX@ z2r+PYCA3qg(kj%qF|VIft9x?99^s2>y$)YYf@(B(isufw*83oN+lYuLLpa?Yo8@y{ zKSiHE64leva$osL?)3Qd^oRojQZs|(>{Wf;Q4a(rmK;1t?4asc?3y+T9){)#PJk#u zgF0>Jji|~IFHu@5&lzfRXk3R4EKa45y z8y*!Fc3OsxT=VR7JY4-*UxW%}Q6*}PCs=8B_64|Bfo?-NvDD^`^nqHFy<4l@DQ04v zh8(Xc?%APQ%T)GuZlli^*hlsxQh-IZHto3hS^PBVb?h2;=4YZWCJQq%7-$mBH!?%T=P*}}xcdi<=a@bDF>$DD<4kxP7Ji%|Tr zkvof9DlcxX+j;x=wHIlEEj9)bc+Cixot$EFWW7T^72V4Ept~Ty1p9HzfrZf{yvLIE zMo!XG_4?DsOYeJnd)=z+0~^)aF^z(DVV7HT`JkpF4?HC7y1yK#A)t|9UowT&$5o@E zFC}er8Y{!RQgf<$=Z9~w`=ot}wfewR~B!t9B;*ref?TTZwkOy(7N4}Yx{{^<~ zu%f`gK;&E$g~fk`W~(KIL|Iw-re3TbJ^fs=0t%vadHPb3+;gV1YuA{@ZPuQrrfz(w zprGgs-&B`Gf1F5s&9w+6PAPA=cZ~fzBV#&LuGeUhY&Yt9{A35F=Ee!a-o_t{v6J2; z@&O_-w7B}BV%-#D#D;*Se`1rd!k`H8XyK7WX&7PQbNzvy;xUoQBWn*Jwc|7+X z=>XKow!qMwM}6oMhskGBDz6}XjX|B^}u0>GW%akr=T?7HhnulZ!1i=OKp z7b7m|k%~E)uFWClEu|<*FgX3t%qi$muwUD~iOAflsll)T?8nK1%13H>c_M`xJQgx) zYDZbzqUnXMF8!9){Ura+Z~#s9x3;byCz>u;9Fdi!?O3->U)f^6Ok&f4gjnFO57a9G zQ{~ky%uP)02Q39{FxncxY`;Hl z7_rovtP+-$bqaW)$K$z6`Uz9D3_RwCa;&UaX)Ww_&FQyDZfxu6QNoKx2A|?BIf#{q zfbQ~*pr+H8DJX%P+xjQE<2GLzxv??*=Py1a z^8ao;Yd7t*WR%boot%<_vO42zE|(qO?K41^%i{_xt8^?xkPk%OQaYJ2k^aHQQ5fsR z)P{`I*x3X?PdDr~0rQOY3~D*%{A_5A8?SUNt>$Nj9hkfw>kK&DOn262`@&mgmK=2+ zA&tp1Gc)yR&i2I9H?IrH&e1iK)(RaSqfj!{%`{mZbaI9o>Eb8N3nA~xttEJ8AKT5b z!N7rTv+%>iJD=VG-Wg-4<-Dn0wzlO@Z=Ik@bab>>)2w_wez=W`1+ zS0i>!>M11uHTSs);a`Wmpi0{H~_8<@&6r; z`~N`-{~k3CaL$y_c#ZcIWJ*vq%%f=?Hh%$${l|AxNXgeMUaJ+v1O+f~qpJ6rxHQ5| zAYy-zT=ur|HgfSfvgPo@#|-y7R-xwq;SvK@{z|N@X4r?FXU!Y}u{oept Cx2BT- literal 0 HcmV?d00001 diff --git a/doc/sick_scan_segment_xd_02.png b/doc/sick_scan_segment_xd_02.png new file mode 100644 index 0000000000000000000000000000000000000000..ce80e287073aace5211e262e5a7c82f04d87b63a GIT binary patch literal 5812 zcmchbXHZjXyM}`zL_vxoHFOY_rbw@Xfgn;ukS0~7S3{SkAP5@-BLdQ-Nr`MgK!O1h zP-=(>7!V;efe@*I&=Trd?wNCD&wSrLKh8IE@+0%kDzn}^dDea3*OPS16n6R)|0xg% zboz!N^fm~@;0vsK!N-8_pQjVDKp^gj8&F-#u)LLtPu>FVyUc4a9r!OviJGDIZ@xDN zJlI`M%i&aQ|A^>`)e?JB`s~C-85uPtX~ECmuDwdJ>z(<&%zBeYFKJoNd9>ZAcqC)} zg0JR~XK^Q;|J)EF{_`ffFCxl3Fy1UMv?NBg7+mG;rt&yYMk=E@5g938-93(|qQ#+H^egBUz%{P5+>K3(@}eUt@)MFZ@ao+k@;QK@y2dHv*@rMI;U3bM0rNSwJY=i=+T!m%_lF_EF; z^5uXMb0l6Y98D8l)SLbYaXsoOfu*swgJY*h>)l}=Z9u%CQCK7AqZ^n$J$dR$Q;ef^ zYhQ<=iJuRzfn1h%gWaM`wsAtDvWg0QVSfHRr&=n(s{xLN^n2u#*}GvJDN#JBvzrbj zo;f6Vct9VsRgb@sRS%^5B*r2dVMch$kvbcsE+|$fX#eLc_!~}h+cYtQ7f*_;lb-L&ly$@pV0%0`Vd1P&&0(Q zGa8x9Nac%0Xx7PgU!oO}(x^p>u(z?9W~S;B6o;uk#QFHZaEw2>&57H(=WarB5Cmot zl3|K$5oFshIUy*85Z}ik4`b=p-Z^FaVOhRIt({o4GdT#IL^wJxFPTSZH*AhwQt5RB=0ey8HAz@@tm!5GJ^z4mD03!b{E=+31(v6&B)Ubi9;6fN_1p}8I;;z z?Vrk!S=+PmdOD}o<}VEAFL{ozItj(bMoGAA!<-$fC7_5-0Bv8{|4=eXHI>6 z{qJsbLV7k5_t96hw5m{M5@#}9!{0PwF!{#kckcRJ%D7!aGd49nGq^$SXXdHLVojku z@{W))c;BgrECXokK1qI<2ffS?y+@onr1!_jahCh5p4z8`Mo7vN*L};=FgoId;F_+m$w~<2{3;Xm9=C9Gy z@+^+$yOreL91Rp`hQp2Vlw`A9`oQ!x*xjFNv#xqCaSuvw|NJ_epPw(F;wK|3%Q@K5 z!MGA5*Jhh-C{Z`+Q#su9XzSflQ|G;Pl~($5j+sw!%)6Ath&$Ka1AT^y64t(HR70}! z^0@REbvec=qo&9fMWX%M2A`REt{b#hoI9k=rY#cuTMw+FdW8p%fN~7+vH*^$15XAY@7K7u6Z)q-QB&FKA$U^*26UWwPc`~ z>DLnPky%=|uDb#s`YH{3)I4>N3bgZhkxttvI*{x^336x(UgGAA+8l%_@JCLCI)t_! zg!i11cfuM&*@d;|+^`h#2HmUSo!sKV&XiO!GrLgF9eG(UCX_RHT# zMy?AVVksL8EGwPg8@xxpfB!y*_>6c8Ucr=@Y11WMxjr|z-y^P_#D2gwabF>&v{aH6 zllILJQ-|qeQv4O9UNp$JuNri*ma>&IIZGc{oLgx0%qjEiy(ttm@^OlW5S4z#!+*N{ zR=50s8T;O_{zL8_UAaA9F-IgsQ%?so-GD}4tbuIwuwX?bd44PeJ?Kk;*;Mc-?E0BS zUkS>u*qK4dBuRhp)d-6B?s2jTrF`S{Z#z&hJ-cNtdQ(S*AMtBveSclTcMn-mBuMJ z^IH2ec5yj2M@iKhwF$torVh7N)ZYELoH9B(N^JBO9=;<0^1P-X0s}c;_$AEn`2N$s z`|`g!^?!T>0q&Sx|MBK0x!0{8UR_#RIxEfYY}tbVGwQafoC$@%qM|%KFXo#D-hO)< z8L3U#!{+VoO&v`>gkZgQ2<2vGO&*UK44NucQ=nkcr@x}|6J_wHFdjr@{}6(rnhfn> zIl(;l;ihV8U`p`u&8s4O0h;iXSP^in^Xp8YAETok!L?{K8ihi&w2UtV858WCoMQHA z>kmgx7M@g7;67YWi{ThN*xMmRg`Z98C^+1d65FNX>750IgmnPQ)|3CMofYq{Z!jB{cy%dal)ZY@EFyc+Zbn4OnXg1Cb$ zjR_LP_HtxZz=X;?zb|bKSP~S&-?{K^5`>;S!k9w!z(kZ__fMvA?64P(DcjfptP9O_ zT|X|`@%i&Hofq;sWytSVfFUzvW#JyskYi?_0&(he22NQSnSeMrT4wK7zSiP$(IMhM zSykS4LiLyMlM2>T&k=UQCns@%X!|K-OF@N&Mn0R=33q5CkrZ`z)gj+hYN!NPIh~(J ze#!DP^5-p&20MskZf2QhvLvX|Je}L*vNi`r;Vl>NUh9!^G4Z(u+d_9X=|ap;Omz!YmO#RK+mdqn+$84 znv9^$&CTLf7cz`+l4l2-DcGCPEIEhUMEj2cx(?LPsVEp!KtP~+DIxQF1WCV-U#tmE z`hy(45|W$l9p>KuMi8mh_AyBWmgo^4PT%O)kG3ekBlgWqUztH(vuQTEK_IRAHh?A39R)AkCdzz2t~_ zqy&zIr3o$YgwUz3W92(8?(VP{dR+Whd}89Pu^;#2buJPO0Tn~^xzS&`BQ0mFY4=-q z?EdT+NiiYh0sdD|AkTG5FVU{5OG_|kGJ4OK|A5g*cn5;_dG8^qEeU!=qP5rJrG^yuH0OhxDetd|~CxFe)7=(ruoYM_d&DX$*1O z?B1iannBrxG+!+FHWGmxAzIuVN@05w>gkyT>rSx~YBLAQ8J1$JT@dQfXd^@AIEilj z($|+=W(#B$LqGoL#~QHJPvTEzjA^4zD&L;PjV*%?8`C zH5>@}4z{-01qDvFwzhZgmSChs$o&O6?|B_#mp@4s`zY_t=lVylck*Ny#n%klWH!8i zFTzszB&vF`GYgO^pzvM|i{U~AXx8^FEz3=-Sx(WoD6uk6po>CjB`M|_p$w_t3`k1HONk`M12oAAy93Ws7OZ=H*7=?X%FLwOJNA+vp zc-LS87E)K~!mF5c&~%292w#`$J{Xke_%B5GAN$OIBj(_oVXX~7W&k!8DDywJkx^Ds zk`w?uC`?oPiy(?Y1U@*|)76!*e*+rHvI^wM=rIq)zx*O1LgOVB=I)+^_8LFHVlaVJ36KSjLb3$JZB7IJ&GX z=st@YJY{BGmGhzEQ{dkOLL;u+AG5{H$(QiSOzN+Xz;gqnr8>fh;e!Y))trOzI;ti5 zKpnes!c=}U4fMF$1KzQ75UDNSEC6)>FX?=Tk^G#T8Ki}U#m$>HizF3u`v9O_8uU;M z^OE5XbZDIL!@G_8phkTX=KIJSSjvurY8FpgcLWirY>@iO9(Ji|TqeUv_V+JZ2(sHQ z+73Z(&&1Y`E_)C;osEMcSYHQO(kg8N(Rs6~DBs*N+nSN<7AAx_-I z$wt>rfP)Vme+^s;lOm(%^i-Wxx$LUAVYvBuHxF}ahF>Fouq3ks&#BkL|M9sC?_vbs zi=iG))!?oxux+ANn7_5oesjc6F`bo&9UFYzkm~du0O#bM@c`R0_7!;zMY1T&rdza|BmxSa#5N_}XWu#T0x zt%HMPF(4-hD?m=?UV2hix&P6V)?MN7+fk1Ibn4$2TM1Ffh9FHJ6XbH1uy{PaAm$Kw zN>lho8OO}w!Jg*p8DP*bUU=)m=GWlWakXe?+e~tJpIiFQgQ*>Wio}3W|KnL(999!B zBYDLnb``b{7|e6X6KfO25pO{M03X^0SRo5f3e$AFz!kQ+)0xbJ)}3m92ZYRxp}UQK zoPz)&{Lxvw)MQ=bcsh1^umrXnzg~qN(l$p@0u(@n9^GjYXOeuBo9pWIpqc?!5r%8s z(Ub<8q#C~5GBphi4ILe)3tUe*8thgK<#MnG+eF3PlU!}me)e5Oxa5lvX5pCqhZVX2 zt1-(vwoD(HAuTUOzI~5Eg&gc|iC?_$=9Z9XRC*>&5iz!+oMy~-(tU<=a;0U$Z$oe> z&FcvWCKe0%dBu$$IT<{0ueK*nesvOqw;<2Y8xw#=Mq2g;JB$Q}cZ~Jrs-Qp3&As&{ z4tw1Zj0T2C$(=KgOJ0itI0}qh7(Z!HTH4RS8xmaB`o6$EXM5zhspo6I%#VqP$fXok zw_bQG&ZVof)X~wgxcF|LUGe|uBI3pM?)_&5=^JErnS?Bnh=8OExqVmw1f3$+^?@R2 zeXsj7KK_I)==5?R=}!IMd?YQLPjlq*iFEif(`7k>CL>L@unuc~>kGmo6|?ucxlgNH zdK^_tVc6#eM6RkT*{#ms1B5cq zXxh(bQWm0ZU<36>d}jgRIb;{&JSnP>ZCquQ+*Iv?oRE$ZSVIde3BG0LNMeZjQ0vz} zLhd;VpyV@xYW`%v1?=tu@G=e%%s);4{z-iQ?WpiyFV;^N@4R<~JNaC){%>!%SNmgV z=#@`fXuI}j4WHv}>q+Nf%F4=fW@0mU18Qn&HV6hKpF-OwCnrCKSp+cXDTovV1zgHgBao*{%u*w%IH?-U_SpK!aZve}?5pmFv~Hbktk6b static sick_scan::SickCallbackHandler s_ldmrsobjectarray_callback_handler; static sick_scan::SickCallbackHandler s_radarscan_callback_handler; static sick_scan::SickCallbackHandler s_visualizationmarker_callback_handler; +static sick_scan::SickCallbackHandler s_navposelandmark_callback_handler; namespace sick_scan { @@ -225,4 +226,24 @@ namespace sick_scan return s_visualizationmarker_callback_handler.isListenerRegistered(handle, listener); } + void addNavPoseLandmarkListener(rosNodePtr handle, NAV350mNPOSDataCallback listener) + { + s_navposelandmark_callback_handler.addListener(handle, listener); + } + + void notifyNavPoseLandmarkListener(rosNodePtr handle, NAV350mNPOSData* msg) + { + s_navposelandmark_callback_handler.notifyListener(handle, msg); + } + + void removeNavPoseLandmarkListener(rosNodePtr handle, NAV350mNPOSDataCallback listener) + { + s_navposelandmark_callback_handler.removeListener(handle, listener); + } + + bool isNavPoseLandmarkListenerRegistered(rosNodePtr handle, NAV350mNPOSDataCallback listener) + { + return s_navposelandmark_callback_handler.isListenerRegistered(handle, listener); + } + } // namespace sick_scan diff --git a/driver/src/sick_generic_caller.cpp b/driver/src/sick_generic_caller.cpp index c6b0436d..d0937ee6 100644 --- a/driver/src/sick_generic_caller.cpp +++ b/driver/src/sick_generic_caller.cpp @@ -116,9 +116,7 @@ int main(int argc, char** argv) if (argc == 1) // just for testing without calling by roslaunch { - // recommended call for internal debugging as an example: __name:=sick_rms_320 __internalDebug:=1 - // strcpy(nameTagVal, "__name:=sick_rms_3xx"); // sick_rms_320 -> radar - strcpy(nameTagVal, "__name:=sick_tim_5xx"); // sick_rms_320 -> radar + strcpy(nameTagVal, "__name:=sick_tim_5xx"); strcpy(logTagVal, "__log:=/tmp/tmp.log"); strcpy(internalDebugTagVal, "__internalDebug:=1"); // strcpy(sensorEmulVal, "__emulSensor:=1"); diff --git a/driver/src/sick_generic_field_mon.cpp b/driver/src/sick_generic_field_mon.cpp index f980b40a..5cec80c4 100644 --- a/driver/src/sick_generic_field_mon.cpp +++ b/driver/src/sick_generic_field_mon.cpp @@ -219,6 +219,10 @@ namespace sick_scan int SickScanFieldMonSingleton::parseBinaryDatagram(std::vector datagram, float rectFieldAngleRefPointOffsetRad) { + if (datagram.size() < 41) // at least 41 byte required to read the field configuration + { + return 0; + } int exitCode = ExitSuccess; int fieldNumberFromCMD=0; std::string sDatagramm( datagram.begin()+8, datagram.end() ); diff --git a/driver/src/sick_generic_laser.cpp b/driver/src/sick_generic_laser.cpp index 6f89bcb7..0453adcc 100644 --- a/driver/src/sick_generic_laser.cpp +++ b/driver/src/sick_generic_laser.cpp @@ -92,8 +92,8 @@ #include #define SICK_GENERIC_MAJOR_VER "2" -#define SICK_GENERIC_MINOR_VER "8" -#define SICK_GENERIC_PATCH_LEVEL "15" +#define SICK_GENERIC_MINOR_VER "9" +#define SICK_GENERIC_PATCH_LEVEL "0" #define DELETE_PTR(p) if(p){delete(p);p=0;} @@ -432,7 +432,7 @@ void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNo exit_code = sick_scansegment_xd::run(nhPriv, scannerName); return; #else - ROS_ERROR(SICK_SCANNER_SCANSEGMENT_XD_NAME " not supported. Please build sick_scan with option SCANSEGMENT_XD_SUPPORT"); + ROS_ERROR_STREAM("SCANSEGMENT_XD_SUPPORT deactivated, " << scannerName << " not supported. Please build sick_scan with option SCANSEGMENT_XD_SUPPORT"); exit_code = sick_scan::ExitError; return; #endif @@ -685,3 +685,44 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP mainGenericLaserInternal(argc, argv, nodeName, nhPriv, true, result); return result; } + +// Send odometry data to NAV350 +#include "softwarePLL.h" +#include "sick_scan_api.h" +#include "sick_scan/sick_nav_scandata_parser.h" +int32_t SickScanApiNavOdomVelocityImpl(SickScanApiHandle apiHandle, SickScanNavOdomVelocityMsg* src_msg) // odometry data in nav coordinates +{ + if(s_scanner) + { + sick_scan_msg::NAVOdomVelocity nav_msg; + nav_msg.vel_x = src_msg->vel_x; + nav_msg.vel_y = src_msg->vel_y; + nav_msg.omega = src_msg->omega; + nav_msg.timestamp = src_msg->timestamp; + nav_msg.coordbase = src_msg->coordbase; + s_scanner->messageCbNavOdomVelocity(nav_msg); + return SICK_SCAN_API_SUCCESS; + } + return SICK_SCAN_API_ERROR; +} +int32_t SickScanApiOdomVelocityImpl(SickScanApiHandle apiHandle, SickScanOdomVelocityMsg* src_msg) // odometry data in system coordinates +{ + if(s_scanner && s_scanner->getCurrentParamPtr() && SoftwarePLL::instance().IsInitialized()) + { + sick_scan_msg::NAVOdomVelocity nav_msg; + nav_msg.vel_x = src_msg->vel_x; + nav_msg.vel_y = src_msg->vel_y; + double angle_shift = -1.0 * s_scanner->getCurrentParamPtr()->getScanAngleShift(); + sick_scan::rotateXYbyAngleOffset(nav_msg.vel_x, nav_msg.vel_y, angle_shift); // Convert to velocity in lidar coordinates in m/s + nav_msg.omega = src_msg->omega; // angular velocity in radians/s + nav_msg.coordbase = 0; // 0 = local coordinate system of the NAV350 + SoftwarePLL::instance().convSystemtimeToLidarTimestamp(src_msg->timestamp_sec, src_msg->timestamp_nsec, nav_msg.timestamp); + s_scanner->messageCbNavOdomVelocity(nav_msg); + return SICK_SCAN_API_SUCCESS; + } + else + { + ROS_WARN_STREAM("## ERROR SickScanCommon::messageCbRosOdom(): SoftwarePLL not yet ready, timestamp can not be converted from system time to lidar time, odometry message ignored."); + } + return SICK_SCAN_API_ERROR; +} diff --git a/driver/src/sick_generic_monitoring.cpp b/driver/src/sick_generic_monitoring.cpp index 9155a7b2..cc3fe886 100644 --- a/driver/src/sick_generic_monitoring.cpp +++ b/driver/src/sick_generic_monitoring.cpp @@ -230,7 +230,11 @@ void sick_scan::PointCloudMonitor::runMonitoringThreadCb(void) pointcloud_subscriber2 = m_nh->subscribe(std::string("/") + m_ros_cloud_topic, 1, &sick_scan::PointCloudMonitor::messageCbPointCloud, this); #elif defined __ROS_VERSION && __ROS_VERSION == 2 rclcpp::Subscription::SharedPtr pointcloud_subscriber1, pointcloud_subscriber2; - pointcloud_subscriber1 = m_nh->create_subscription(m_ros_cloud_topic,10,std::bind(&sick_scan::PointCloudMonitor::messageCbPointCloudROS2, this, std::placeholders::_1)); + rosQoS qos = rclcpp::SystemDefaultsQoS(); + overwriteByOptionalQOSconfig(m_nh, qos); + QoSConverter qos_converter; + ROS_INFO_STREAM("PointCloudMonitor: subscribing to topic " << m_ros_cloud_topic << ", qos=" << qos_converter.convert(qos)); + pointcloud_subscriber1 = m_nh->create_subscription(m_ros_cloud_topic, qos, std::bind(&sick_scan::PointCloudMonitor::messageCbPointCloudROS2, this, std::placeholders::_1)); if(m_ros_cloud_topic[0] != '/') pointcloud_subscriber2 = m_nh->create_subscription(std::string("/") + m_ros_cloud_topic,10,std::bind(&sick_scan::PointCloudMonitor::messageCbPointCloudROS2, this, std::placeholders::_1)); #else diff --git a/driver/src/sick_generic_parser.cpp b/driver/src/sick_generic_parser.cpp index 1b457169..164e2499 100644 --- a/driver/src/sick_generic_parser.cpp +++ b/driver/src/sick_generic_parser.cpp @@ -263,22 +263,27 @@ namespace sick_scan } /*! \brief flag to mark the device as radar (instead of laser scanner) - \param _deviceIsRadar: false for laserscanner, true for radar (like rms_3xx) + \param _deviceIsRadar: false for laserscanner, true for radar (like rms_xxxx) \sa getDeviceIsRadar */ - void ScannerBasicParam::setDeviceIsRadar(bool _deviceIsRadar) + void ScannerBasicParam::setDeviceIsRadar(RADAR_TYPE_ENUM _radar_type) { - deviceIsRadar = _deviceIsRadar; + deviceRadarType = _radar_type; } /*! \brief flag to mark the device as radar (instead of laser scanner) - \param _deviceIsRadar: false for laserscanner, true for radar (like rms_3xx) + \param _deviceIsRadar: false for laserscanner, true for radar (like rms_xxxx) \sa getDeviceIsRadar */ bool ScannerBasicParam::getDeviceIsRadar(void) { - return (deviceIsRadar); + return (deviceRadarType != NO_RADAR); + } + + RADAR_TYPE_ENUM ScannerBasicParam::getDeviceRadarType(void) + { + return deviceRadarType; } /*! @@ -287,7 +292,7 @@ namespace sick_scan */ bool ScannerBasicParam::getTrackingModeSupported(void) { - return (deviceIsRadar && trackingModeSupported); + return (getDeviceIsRadar() && trackingModeSupported); } void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) { @@ -439,7 +444,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) */ ScannerBasicParam::ScannerBasicParam() : numberOfLayers(0), numberOfShots(0), numberOfMaximumEchos(0), elevationDegreeResolution(0), angleDegressResolution(0), expectedFrequency(0), - useBinaryProtocol(false), IntensityResolutionIs16Bit(false), deviceIsRadar(false), useSafetyPasWD(false), encoderMode(0), + useBinaryProtocol(false), IntensityResolutionIs16Bit(false), deviceRadarType(NO_RADAR), useSafetyPasWD(false), encoderMode(0), CartographerCompatibility(false), scanMirroredAndShifted(false), useEvalFields(EVAL_FIELD_UNSUPPORTED), maxEvalFields(0), rectFieldAngleRefPointOffsetRad(0), imuEnabled (false), scanAngleShift(0), useScancfgList(false), useWriteOutputRanges(false) { @@ -501,10 +506,8 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) allowedScannerNames.push_back(SICK_SCANNER_MRS_6XXX_NAME); allowedScannerNames.push_back(SICK_SCANNER_LMS_4XXX_NAME); allowedScannerNames.push_back(SICK_SCANNER_LRS_4XXX_NAME); - allowedScannerNames.push_back(SICK_SCANNER_RMS_1XXX_NAME); // Radar scanner - allowedScannerNames.push_back(SICK_SCANNER_RMS_2XXX_NAME); // Radar scanner - allowedScannerNames.push_back(SICK_SCANNER_RMS_3XX_NAME); // Radar scanner - allowedScannerNames.push_back(SICK_SCANNER_NAV_3XX_NAME); + allowedScannerNames.push_back(SICK_SCANNER_RMS_XXXX_NAME); // Radar scanner + allowedScannerNames.push_back(SICK_SCANNER_NAV_31X_NAME); allowedScannerNames.push_back(SICK_SCANNER_NAV_350_NAME); allowedScannerNames.push_back(SICK_SCANNER_NAV_2XX_NAME); allowedScannerNames.push_back(SICK_SCANNER_TIM_4XX_NAME); @@ -516,7 +519,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) for (int i = 0; i < (int) basicParams.size(); i++) // set specific parameter for each scanner type - scanner type is identified by name { - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setScannerName(allowedScannerNames[i]); // set scanner type for this parameter object @@ -530,7 +533,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setElevationDegreeResolution(2.5); // in [degree] basicParams[i].setExpectedFrequency(50.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -555,7 +558,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setElevationDegreeResolution(0.0); // in [degree] basicParams[i].setExpectedFrequency(50.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -579,7 +582,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(1.00000); basicParams[i].setExpectedFrequency(15.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -603,7 +606,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.3333); basicParams[i].setExpectedFrequency(15.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -626,7 +629,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.0833);// basicParams[i].setExpectedFrequency(600.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -649,7 +652,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.3333); basicParams[i].setExpectedFrequency(15.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -672,7 +675,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.3333); basicParams[i].setExpectedFrequency(15.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(true); // Safety scanner basicParams[i].setEncoderMode(-1); // Default @@ -695,7 +698,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.5); basicParams[i].setExpectedFrequency(15.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -718,7 +721,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.5); basicParams[i].setExpectedFrequency(25.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -741,7 +744,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.5); basicParams[i].setExpectedFrequency(15.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -764,7 +767,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.5); basicParams[i].setExpectedFrequency(15.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -788,7 +791,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setElevationDegreeResolution(1.25); // in [degree] basicParams[i].setExpectedFrequency(50.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -811,7 +814,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.05); basicParams[i].setExpectedFrequency(12.5); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -826,8 +829,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setWaitForReady(false); basicParams[i].setFREchoFilterAvailable(true); // (false) // LRS4XXX uses echo filter settings to configure 1 echo, use filter_echos = 0 (first echo) for LRS4xxx } - if (basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_2XXX_NAME) == 0 - || basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_3XX_NAME) == 0) // Radar + if (basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_XXXX_NAME) == 0) // Radar { basicParams[i].setNumberOfMaximumEchos(1); basicParams[i].setNumberOfLayers(0); // for radar scanner @@ -836,8 +838,8 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setElevationDegreeResolution(0.00); // in [degree] basicParams[i].setExpectedFrequency(0.00); basicParams[i].setUseBinaryProtocol(false); // use ASCII-Protocol - basicParams[i].setDeviceIsRadar(true); // Device is a radar - basicParams[i].setTrackingModeSupported(true); // Default + basicParams[i].setDeviceIsRadar(RADAR_3D); // Default: Device is a 3D radar (RMS-1xxx switched to 1D radar after device type query) + basicParams[i].setTrackingModeSupported(true); // Default: tracking mode enabled (not supported by RMS-1xxx, disabled for RMS-1 after device type query) basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default basicParams[i].setImuEnabled(false);// Default @@ -851,31 +853,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setWaitForReady(false); basicParams[i].setFREchoFilterAvailable(false); } - if (basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_1XXX_NAME) == 0) // Radar - { - basicParams[i].setNumberOfMaximumEchos(1); - basicParams[i].setNumberOfLayers(0); // for radar scanner - basicParams[i].setNumberOfShots(65); - basicParams[i].setAngularDegreeResolution(0.00); - basicParams[i].setElevationDegreeResolution(0.00); // in [degree] - basicParams[i].setExpectedFrequency(0.00); - basicParams[i].setUseBinaryProtocol(false); // use ASCII-Protocol - basicParams[i].setDeviceIsRadar(true); // Device is a radar - basicParams[i].setTrackingModeSupported(false); // RMS 1xxx does not support selection of tracking modes - basicParams[i].setUseSafetyPasWD(false); // Default - basicParams[i].setEncoderMode(-1); // Default - basicParams[i].setImuEnabled(false);// Default - basicParams[i].setScanAngleShift(0); - basicParams[i].setScanMirroredAndShifted(false); - basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); - basicParams[i].setMaxEvalFields(0); - basicParams[i].setRectEvalFieldAngleRefPointOffsetRad(0); - basicParams[i].setUseScancfgList(false); - basicParams[i].setUseWriteOutputRanges(true); // default: use "sWN LMPoutputRange" if scan configuration not set by ScanCfgList-entry - basicParams[i].setWaitForReady(false); - basicParams[i].setFREchoFilterAvailable(false); - } - if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) // Nav 3xx + if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) == 0) // Nav 3xx { basicParams[i].setNumberOfMaximumEchos(1); basicParams[i].setNumberOfLayers(1); @@ -883,7 +861,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.750); basicParams[i].setExpectedFrequency(55.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -898,21 +876,21 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setWaitForReady(true); basicParams[i].setFREchoFilterAvailable(false); } - if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0) // TODO: NAV-350 support + if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0) { basicParams[i].setNumberOfMaximumEchos(1); basicParams[i].setNumberOfLayers(1); - basicParams[i].setNumberOfShots(2880); + basicParams[i].setNumberOfShots(1440); basicParams[i].setAngularDegreeResolution(0.250); basicParams[i].setExpectedFrequency(8.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); basicParams[i].setTrackingModeSupported(false); basicParams[i].setUseSafetyPasWD(false); basicParams[i].setEncoderMode(-1); basicParams[i].setImuEnabled(false); basicParams[i].setScanAngleShift(-M_PI); - basicParams[i].setScanMirroredAndShifted(false); + basicParams[i].setScanMirroredAndShifted(true); basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED); basicParams[i].setMaxEvalFields(0); basicParams[i].setRectEvalFieldAngleRefPointOffsetRad(0); @@ -929,7 +907,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.750); basicParams[i].setExpectedFrequency(55.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -952,7 +930,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.5); basicParams[i].setExpectedFrequency(25.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -975,7 +953,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) basicParams[i].setAngularDegreeResolution(0.33333333333); basicParams[i].setExpectedFrequency(15.0); basicParams[i].setUseBinaryProtocol(true); - basicParams[i].setDeviceIsRadar(false); // Default + basicParams[i].setDeviceIsRadar(NO_RADAR); // (false); // Default basicParams[i].setTrackingModeSupported(false); // Default basicParams[i].setUseSafetyPasWD(false); // Default basicParams[i].setEncoderMode(-1); // Default @@ -992,7 +970,7 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) } if (basicParams[i].getScannerName().compare(SICK_SCANNER_SCANSEGMENT_XD_NAME) == 0) { - // SCANSEGMENT_XD MRS100 (Multiscan 136) handled by msgpack_converter and msgpack_exporter + // multiScan136 handled by msgpack_converter and msgpack_exporter } } diff --git a/driver/src/sick_generic_radar.cpp b/driver/src/sick_generic_radar.cpp index 699f29f0..61cdcd1d 100644 --- a/driver/src/sick_generic_radar.cpp +++ b/driver/src/sick_generic_radar.cpp @@ -101,7 +101,7 @@ namespace sick_scan rosDeclareParam(nh, "nodename", nodename); rosGetParam(nh, "nodename", nodename); - // just for debugging, but very helpful for the start + // publish radar targets and objects cloud_radar_rawtarget_pub_ = rosAdvertise(nh, nodename + "/cloud_radar_rawtarget", 100); cloud_radar_track_pub_ = rosAdvertise(nh, nodename + "/cloud_radar_track", 100); @@ -494,7 +494,7 @@ namespace sick_scan success = success && appendRadarDatagramField(datagram, datagram_length, 2, fields); // 2 byte Amount of 16 bit channels uint16_t num_16_bit_channels = 0; fields.back().toInteger(num_16_bit_channels); - // if(num_16_bit_channels > 4) // max 4 16-bit channel in case of RMS1xxx and RMS3xx + // if(num_16_bit_channels > 4) // max 4 16-bit channel in case of RMSxxxx // { // return std::vector(); // unrecoverable parse error // } @@ -894,24 +894,9 @@ namespace sick_scan #define OBJLEN_KEYWORD "OBLE1" #define OBJID_KEYWORD "OBID1" -#define RADAR_1D_DEVICE "sick_rms_1xxx" // this is a little bit redundant to parameter setting, -#define RADAR_3D_DEVICE "sick_rms_5xx" // but avoid "back-pointering" to overall calling instance - const int RAWTARGET_LOOP = 0; const int OBJECT_LOOP = 1; - // std::string nodename = parser_->getCurrentParamPtr()->getScannerName(); - // std::vector rawTargets; - // std::vector objectList; - - //printf("%s\n", this->getNameOfRadar().c_str()); - - enum RADAR_TYPE_ENUM {RADAR_1D_ENUM, RADAR_3D_ENUM, RADAR_NUM}; - RADAR_TYPE_ENUM enumRadarType = RADAR_3D_ENUM; - if (this->getNameOfRadar().compare(RADAR_1D_DEVICE) == 0) - { - enumRadarType = RADAR_1D_ENUM; - } for (int iLoop = 0; iLoop < 2; iLoop++) { keyWordList.clear(); @@ -925,15 +910,15 @@ namespace sick_scan keyWordList.push_back(MODE1_KEYWORD); break; case OBJECT_LOOP: - switch(enumRadarType) + switch(this->radarType) { - case RADAR_1D_ENUM: + case RADAR_1D: keyWordList.push_back(P3DX1_KEYWORD); keyWordList.push_back(V3DX1_KEYWORD); keyWordList.push_back(OBJLEN_KEYWORD); keyWordList.push_back(OBJID_KEYWORD); break; - case RADAR_3D_ENUM: + case RADAR_3D: keyWordList.push_back(P3DX1_KEYWORD); keyWordList.push_back(P3DY1_KEYWORD); keyWordList.push_back(V3DX1_KEYWORD); @@ -1548,9 +1533,9 @@ namespace sick_scan std::vector objectList; std::vector rawTargetList; - if (useBinaryProtocol && getNameOfRadar() == "sick_rms_3xx") // RMS-3xx is out of date and no longer available for order + if (useBinaryProtocol && getNameOfRadar() == SICK_SCANNER_RMS_XXXX_NAME) { - throw std::logic_error("Binary protocol for RMS-3xx currently not supported. Please use in your launchfile."); + throw std::logic_error("Binary protocol for RMSxxxx currently not supported. Please use in your launchfile."); } else { @@ -1747,6 +1732,9 @@ namespace sick_scan float v = sqrt(vx * vx + vy * vy); float heading = atan2(objectList[i].V3Dy(), objectList[i].V3Dx()); + radarMsg_.objects[i].id = objectList[i].ObjId(); + radarMsg_.objects[i].tracking_time = timeStamp; + radarMsg_.objects[i].velocity.twist.linear.x = objectList[i].V3Dx(); radarMsg_.objects[i].velocity.twist.linear.y = objectList[i].V3Dy(); radarMsg_.objects[i].velocity.twist.linear.z = 0.0; diff --git a/driver/src/sick_lmd_scandata_parser.cpp b/driver/src/sick_lmd_scandata_parser.cpp index c98763b7..30098d01 100644 --- a/driver/src/sick_lmd_scandata_parser.cpp +++ b/driver/src/sick_lmd_scandata_parser.cpp @@ -63,19 +63,67 @@ namespace sick_scan { - /* template static void readFromBuffer(const uint8_t* receiveBuffer, int& pos, int receiveBufferLength, T& value) + + /** Increments the number of packets received in the SoftwarePLL */ + void incSoftwarePLLPacketReceived() { - if(pos + sizeof(value) < receiveBufferLength) + SoftwarePLL::instance().packets_received++; + if (SoftwarePLL::instance().IsInitialized() == false) + { + if(SoftwarePLL::instance().packets_received <= 1) + { + ROS_INFO("Software PLL locking started, mapping ticks to system time."); + } + int packets_expected_to_drop = SoftwarePLL::instance().fifoSize - 1; + SoftwarePLL::instance().packets_dropped++; + size_t packets_dropped = SoftwarePLL::instance().packets_dropped; + size_t packets_received = SoftwarePLL::instance().packets_received; + if (packets_dropped < packets_expected_to_drop) + { + ROS_INFO_STREAM("" << packets_dropped << " / " << packets_expected_to_drop << " packets dropped. Software PLL not yet locked."); + } + else if (packets_dropped == packets_expected_to_drop) { - memcpy(&value, receiveBuffer + pos, sizeof(value)); - swap_endian((unsigned char *) &value, sizeof(value)); - pos += sizeof(value); + ROS_INFO("Software PLL is ready and locked now!"); } - else + else if (packets_dropped > packets_expected_to_drop && packets_received > 0) { - ROS_WARN_STREAM("readFromBuffer(): read pos = " << pos << " + sizeof(value) = " << sizeof(value) << " exceeds receiveBufferLength = " << receiveBufferLength); + double drop_rate = (double)packets_dropped / (double)packets_received; + ROS_WARN_STREAM("" << SoftwarePLL::instance().packets_dropped << " of " << SoftwarePLL::instance().packets_received << " packets dropped (" + << std::fixed << std::setprecision(1) << (100*drop_rate) << " perc.), maxAbsDeltaTime=" << std::fixed << std::setprecision(3) << SoftwarePLL::instance().max_abs_delta_time); + ROS_WARN_STREAM("More packages than expected were dropped!!\n" + "Check the network connection.\n" + "Check if the system time has been changed in a leap.\n" + "If the problems can persist, disable the software PLL with the option sw_pll_only_publish=False !"); } - } */ + } + } + + /** check angle values against +/- pi. + It is checked whether the angle is close to +pi or -pi. + In this case, the angle is minimally modified so that + the modified angle is safely within the interval [-pi,pi] + to avoid problems with angle wrapping. + If the angle value is modified, the function returns true else false. + */ + bool check_near_plus_minus_pi(float *angle_val) + { + bool angle_slightly_modified = false; + float pi_multiplier = *angle_val/M_PI; + float check_deviation_to_abs_one = fabs(pi_multiplier) - 1.0; + // check for a small deviation + if (check_deviation_to_abs_one < 10.0 * FLT_EPSILON ) + { + float factor = (*angle_val < 0.0) ? (-1.0) : (1.0); + *angle_val = factor * (1.0 - FLT_EPSILON) * M_PI; + angle_slightly_modified = true; + } + else + { + angle_slightly_modified =false; + } + return(angle_slightly_modified); + } /** Parse common result telegrams, i.e. parse telegrams of type LMDscandata received from the lidar */ @@ -143,37 +191,7 @@ namespace sick_scan //TODO Handle return values if (config_sw_pll_only_publish == true) { - SoftwarePLL::instance().packets_received++; - if (bRet == false) - { - if(SoftwarePLL::instance().packets_received <= 1) - { - ROS_INFO("Software PLL locking started, mapping ticks to system time."); - } - int packets_expected_to_drop = SoftwarePLL::instance().fifoSize - 1; - SoftwarePLL::instance().packets_dropped++; - size_t packets_dropped = SoftwarePLL::instance().packets_dropped; - size_t packets_received = SoftwarePLL::instance().packets_received; - if (packets_dropped < packets_expected_to_drop) - { - ROS_INFO_STREAM("" << packets_dropped << " / " << packets_expected_to_drop << " packets dropped. Software PLL not yet locked."); - } - else if (packets_dropped == packets_expected_to_drop) - { - ROS_INFO("Software PLL is ready and locked now!"); - } - else if (packets_dropped > packets_expected_to_drop && packets_received > 0) - { - double drop_rate = (double)packets_dropped / (double)packets_received; - ROS_WARN_STREAM("" << SoftwarePLL::instance().packets_dropped << " of " << SoftwarePLL::instance().packets_received << " packets dropped (" - << std::fixed << std::setprecision(1) << (100*drop_rate) << " perc.), maxAbsDeltaTime=" << std::fixed << std::setprecision(3) << SoftwarePLL::instance().max_abs_delta_time); - ROS_WARN_STREAM("More packages than expected were dropped!!\n" - "Check the network connection.\n" - "Check if the system time has been changed in a leap.\n" - "If the problems can persist, disable the software PLL with the option sw_pll_only_publish=False !"); - } - return false; - } + incSoftwarePLLPacketReceived(); } #ifdef DEBUG_DUMP_ENABLED @@ -482,13 +500,21 @@ namespace sick_scan msg.time_increment = fabs(parser_->getCurrentParamPtr()->getNumberOfLayers() * msg.scan_time * msg.angle_increment / (2.0 * M_PI)); } - if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) + if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) == 0) { msg.angle_min = (float)(-M_PI); msg.angle_max = (float)(+M_PI); msg.angle_increment *= -1.0; + if (msg.angle_increment < 0.0) + { + // angle_min corresponds to start angle + // i.e. if angle_increment is negative, + // the angle_min is greater than angle_max + msg.angle_min = (float)(+M_PI); + msg.angle_max = (float)(-M_PI); + } } - else if (parser_->getCurrentParamPtr()->getScanMirroredAndShifted()) // i.e. for SICK_SCANNER_LRS_36x0_NAME and SICK_SCANNER_NAV_3XX_NAME + else if (parser_->getCurrentParamPtr()->getScanMirroredAndShifted()) // i.e. for SICK_SCANNER_LRS_36x0_NAME and SICK_SCANNER_NAV_31X_NAME { /* TODO: Check this ... msg.angle_min -= (float)(M_PI / 2); @@ -500,6 +526,27 @@ namespace sick_scan msg.angle_max *= -1.0; } + + // Avoid 2*PI wrap around, if (msg.angle_max - msg.angle_min - 2*PI) is slightly above 0.0 due to floating point arithmetics + bool wrap_avoid = false; + bool ret = check_near_plus_minus_pi(&(msg.angle_min)); + if (ret) + { + wrap_avoid = true; + } + ret = check_near_plus_minus_pi(&(msg.angle_max)); + if (ret) + { + wrap_avoid = true; + } + + // in the case of slighlty modified min/max angles, + // we recalculate the angle_increment. + if (wrap_avoid) + { + msg.angle_increment = (msg.angle_max - msg.angle_min) / (numberOfItems - 1); + } + float *rangePtr = NULL; if (numberOfItems > 0) diff --git a/driver/src/sick_nav_scandata_parser.cpp b/driver/src/sick_nav_scandata_parser.cpp new file mode 100644 index 00000000..fccdc49b --- /dev/null +++ b/driver/src/sick_nav_scandata_parser.cpp @@ -0,0 +1,950 @@ +/* + * sick_nav_scandata_parser parses data telegrams from NAV-350. + * + * Copyright (C) 2023, Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2023, SICK AG, Waldkirch + * All rights reserved. + * +* 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 Osnabrueck University 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 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 + * + */ +#include +#include +#include + +#include "softwarePLL.h" +#include +#include +#include + +namespace sick_scan +{ + template static void appendToBuffer(std::vector& data_buffer, const T& value) + { + T dst_value = value; + swap_endian((unsigned char *) &dst_value, sizeof(dst_value)); + size_t pos = data_buffer.size(); + for (size_t n = 0; n < sizeof(dst_value); n++) + data_buffer.push_back(0); + memcpy(&data_buffer[pos], &dst_value, sizeof(dst_value)); + } + + template static bool readFromBuffer(const uint8_t* receiveBuffer, int& pos, int receiveBufferLength, T& value, const char* file, int line) + { + if(pos + sizeof(value) <= receiveBufferLength) + { + memcpy(&value, receiveBuffer + pos, sizeof(value)); + swap_endian((unsigned char *) &value, sizeof(value)); + pos += sizeof(value); + return true; + } + else + { + ROS_WARN_STREAM("readFromBuffer(): read pos = " << pos << " + sizeof(value) = " << sizeof(value) << " exceeds receiveBufferLength = " << receiveBufferLength << " (" << file << ":" << line << ")"); + return false; + } + } + + /** Serializes NAV350mNPOSData into a binary data buffer */ + static void writeNAV350BinaryPositionData(const NAV350mNPOSData& navdata, std::vector& data_buffer) + { + data_buffer.clear(); + uint32_t stx = 0x02020202; + uint32_t payload_size = 0; + appendToBuffer(data_buffer, stx); + appendToBuffer(data_buffer, payload_size); + std::string sopas_cmd = "sAN mNPOSGetData "; + std::copy(sopas_cmd.begin(), sopas_cmd.end(), std::back_inserter(data_buffer)); + appendToBuffer(data_buffer, navdata.version); + appendToBuffer(data_buffer, navdata.errorCode); + appendToBuffer(data_buffer, navdata.wait); + appendToBuffer(data_buffer, navdata.mask); + appendToBuffer(data_buffer, navdata.poseDataValid); + if (navdata.poseDataValid > 0) + { + appendToBuffer(data_buffer, navdata.poseData.x); + appendToBuffer(data_buffer, navdata.poseData.y); + appendToBuffer(data_buffer, navdata.poseData.phi); + appendToBuffer(data_buffer, navdata.poseData.optPoseDataValid); + if (navdata.poseData.optPoseDataValid > 0) + { + appendToBuffer(data_buffer, navdata.poseData.optPoseData.outputMode); + appendToBuffer(data_buffer, navdata.poseData.optPoseData.timestamp); + appendToBuffer(data_buffer, navdata.poseData.optPoseData.meanDev); + appendToBuffer(data_buffer, navdata.poseData.optPoseData.navMode); + appendToBuffer(data_buffer, navdata.poseData.optPoseData.infoState); + appendToBuffer(data_buffer, navdata.poseData.optPoseData.quantUsedReflectors); + } + } + appendToBuffer(data_buffer, navdata.landmarkDataValid); + if (navdata.landmarkDataValid > 0) + { + appendToBuffer(data_buffer, navdata.landmarkData.landmarkFilter); + appendToBuffer(data_buffer, navdata.landmarkData.numReflectors); + for(int reflectorCnt = 0; reflectorCnt < navdata.landmarkData.numReflectors; reflectorCnt++) + { + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].cartesianDataValid); + if (navdata.landmarkData.reflectors[reflectorCnt].cartesianDataValid > 0) + { + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].cartesianData.x); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].cartesianData.y); + } + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].polarDataValid); + if (navdata.landmarkData.reflectors[reflectorCnt].polarDataValid > 0) + { + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].polarData.dist); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].polarData.phi); + } + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorDataValid); + if (navdata.landmarkData.reflectors[reflectorCnt].optReflectorDataValid > 0) + { + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.localID); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.globalID); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.type); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.subType); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.quality); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.timestamp); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.size); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.hitCount); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.meanEcho); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.startIndex); + appendToBuffer(data_buffer, navdata.landmarkData.reflectors[reflectorCnt].optReflectorData.endIndex); + } + } + } + appendToBuffer(data_buffer, navdata.scanDataValid); + for (int channel = 0; channel < navdata.scanDataValid; channel++) + { + std::copy(navdata.scanData[channel].contentType.begin(), navdata.scanData[channel].contentType.end(), std::back_inserter(data_buffer)); + appendToBuffer(data_buffer, navdata.scanData[channel].scaleFactor); + appendToBuffer(data_buffer, navdata.scanData[channel].scaleOffset); + appendToBuffer(data_buffer, navdata.scanData[channel].startAngle); + appendToBuffer(data_buffer, navdata.scanData[channel].angleRes); + appendToBuffer(data_buffer, navdata.scanData[channel].timestampStart); + appendToBuffer(data_buffer, navdata.scanData[channel].numData); + for(int data_cnt = 0; data_cnt < navdata.scanData[channel].numData; data_cnt++) + { + appendToBuffer(data_buffer, navdata.scanData[channel].data[data_cnt]); + } + } + appendToBuffer(data_buffer, navdata.remissionDataValid); + if (navdata.remissionDataValid > 0) + { + std::copy(navdata.remissionData.contentType.begin(), navdata.remissionData.contentType.end(), std::back_inserter(data_buffer)); + appendToBuffer(data_buffer, navdata.remissionData.scaleFactor); + appendToBuffer(data_buffer, navdata.remissionData.scaleOffset); + appendToBuffer(data_buffer, navdata.remissionData.startAngle); + appendToBuffer(data_buffer, navdata.remissionData.angleRes); + appendToBuffer(data_buffer, navdata.remissionData.timestampStart); + appendToBuffer(data_buffer, navdata.remissionData.numData); + for(int data_cnt = 0; data_cnt < navdata.remissionData.numData; data_cnt++) + { + appendToBuffer(data_buffer, navdata.remissionData.data[data_cnt]); + } + } + // write payload size + payload_size = data_buffer.size() - 8; + std::vector payload_buffer; + appendToBuffer(payload_buffer, payload_size); + memcpy(&data_buffer[4], &payload_buffer[0], payload_buffer.size()); + data_buffer.push_back(0); // dummy crc + } + + /** Unittest for parseNAV350BinaryPositionData(): creates, serializes and deserializes NAV350 position data telegrams and checks the identity of results */ + bool parseNAV350BinaryUnittest() + { + NAV350mNPOSData navdata_src, navdata_dst; + std::vector data_buffer_src, data_buffer_dst; + data_buffer_src.reserve(32*1024); + data_buffer_dst.reserve(32*1024); + // Create dummy NAV350 position data + navdata_src.version = 1; + navdata_src.errorCode = 0; + navdata_src.wait = 1; + navdata_src.mask = 2; + navdata_src.poseDataValid = 1; + navdata_src.poseData.x = +1234; + navdata_src.poseData.y = -1234; + navdata_src.poseData.phi = M_PI / 2; + navdata_src.poseData.optPoseDataValid = 1; + navdata_src.poseData.optPoseData. outputMode = 1; + navdata_src.poseData.optPoseData. timestamp = 123456789; + navdata_src.poseData.optPoseData. meanDev = 789; + navdata_src.poseData.optPoseData. navMode = 3; + navdata_src.poseData.optPoseData. infoState = 1234; + navdata_src.poseData.optPoseData. quantUsedReflectors = 5; + navdata_src.landmarkDataValid = 1; + navdata_src.landmarkData.landmarkFilter = 1; + navdata_src.landmarkData.numReflectors = 25; + navdata_src.landmarkData.reflectors.resize(navdata_src.landmarkData.numReflectors); + for(int reflector_cnt = 0; reflector_cnt < navdata_src.landmarkData.numReflectors; reflector_cnt++) + { + navdata_src.landmarkData.reflectors[reflector_cnt].cartesianDataValid = 1; + navdata_src.landmarkData.reflectors[reflector_cnt].cartesianData.x = 2 * reflector_cnt + 0; + navdata_src.landmarkData.reflectors[reflector_cnt].cartesianData.y = 2 * reflector_cnt + 1; + navdata_src.landmarkData.reflectors[reflector_cnt].polarDataValid = 1; + navdata_src.landmarkData.reflectors[reflector_cnt].polarData.dist = 1.1415 * 2 * reflector_cnt; + navdata_src.landmarkData.reflectors[reflector_cnt].polarData.phi = M_PI * reflector_cnt / navdata_src.landmarkData.numReflectors; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorDataValid = 1; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.localID = 10 * reflector_cnt + 0; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.globalID = 10 * reflector_cnt + 1; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.type = 1; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.subType = 2; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.quality = 10 * reflector_cnt + 2; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.timestamp = 10 * reflector_cnt + 3; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.size = 10 * reflector_cnt + 4; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.hitCount = 10 * reflector_cnt + 5; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.meanEcho = 10 * reflector_cnt + 6; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.startIndex = 10 * reflector_cnt + 7; + navdata_src.landmarkData.reflectors[reflector_cnt].optReflectorData.endIndex = 10 * reflector_cnt + 8; + } + navdata_src.scanDataValid = 2; + navdata_src.scanData.resize(navdata_src.scanDataValid); + navdata_src.scanData[0].contentType = "DIST1"; + navdata_src.scanData[0].scaleFactor = 1; + navdata_src.scanData[0].scaleOffset = 0; + navdata_src.scanData[0].startAngle = 0; + navdata_src.scanData[0].angleRes = 250; + navdata_src.scanData[0].timestampStart = 123456789; + navdata_src.scanData[0].numData = 1024; + navdata_src.scanData[0].data.resize(navdata_src.scanData[0].numData); + for(int data_cnt = 0; data_cnt < navdata_src.scanData[0].numData; data_cnt++) + navdata_src.scanData[0].data[data_cnt] = data_cnt; + navdata_src.scanData[1].contentType = "ANGL1"; + navdata_src.scanData[1].scaleFactor = 1; + navdata_src.scanData[1].scaleOffset = 0; + navdata_src.scanData[1].startAngle = 0; + navdata_src.scanData[1].angleRes = 250; + navdata_src.scanData[1].timestampStart = 123456789; + navdata_src.scanData[1].numData = 1024; + navdata_src.scanData[1].data.resize(navdata_src.scanData[1].numData); + for(int data_cnt = 0; data_cnt < navdata_src.scanData[1].numData; data_cnt++) + navdata_src.scanData[1].data[data_cnt] = data_cnt * navdata_src.scanData[1].angleRes; + navdata_src.remissionDataValid = 1; + navdata_src.remissionData.contentType = "RSSI1"; + navdata_src.remissionData.scaleFactor = 1; + navdata_src.remissionData.scaleOffset = 0; + navdata_src.remissionData.startAngle = 0; + navdata_src.remissionData.angleRes = 250; + navdata_src.remissionData.timestampStart = 123456789; + navdata_src.remissionData.numData = 1024; + navdata_src.remissionData.data.resize(navdata_src.remissionData.numData); + for(int data_cnt = 0; data_cnt < navdata_src.remissionData.numData; data_cnt++) + navdata_src.remissionData.data[data_cnt] = data_cnt; + // Serialize to NAV350 position data to binary buffer + writeNAV350BinaryPositionData(navdata_src, data_buffer_src); + // Parse serialized to NAV350 position data + if (!parseNAV350BinaryPositionData(data_buffer_src.data(), data_buffer_src.size(), navdata_dst)) + { + ROS_ERROR_STREAM("## ERROR parseNAV350BinaryUnittest(): parseNAV350BinaryPositionData failed"); + return false; + } + // Re-serialize to NAV350 position data to binary buffer + writeNAV350BinaryPositionData(navdata_dst, data_buffer_dst); + // Result of serialization and deserialization must be identical + if (data_buffer_src.size() != data_buffer_dst.size() || memcmp(data_buffer_src.data(), data_buffer_dst.data(), data_buffer_src.size()) != 0) + { + ROS_ERROR_STREAM("## ERROR parseNAV350BinaryUnittest(): parseNAV350BinaryPositionData failed"); + return false; + } + ROS_DEBUG_STREAM("parseNAV350BinaryUnittest() passed (" << data_buffer_src.size() << " byte datagram)"); + return true; + } + + /** Parse binary NAV350 landmark data */ + bool parseNAV350BinaryLandmarkData(const uint8_t* receiveBuffer, int& receivePos, int receiveBufferLength, NAV350LandmarkData& landmarkData) + { + bool success = true; + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.landmarkFilter, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.numReflectors, __FILE__, __LINE__); + landmarkData.reflectors = std::vector(landmarkData.numReflectors); + for(int reflectorCnt = 0; reflectorCnt < landmarkData.numReflectors; reflectorCnt++) + { + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].cartesianDataValid, __FILE__, __LINE__); + if (landmarkData.reflectors[reflectorCnt].cartesianDataValid > 0) + { + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].cartesianData.x, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].cartesianData.y, __FILE__, __LINE__); + } + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].polarDataValid, __FILE__, __LINE__); + if (landmarkData.reflectors[reflectorCnt].polarDataValid > 0) + { + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].polarData.dist, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].polarData.phi, __FILE__, __LINE__); + } + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorDataValid, __FILE__, __LINE__); + if (landmarkData.reflectors[reflectorCnt].optReflectorDataValid > 0) + { + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.localID, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.globalID, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.type, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.subType, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.quality, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.timestamp, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.size, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.hitCount, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.meanEcho, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.startIndex, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.reflectors[reflectorCnt].optReflectorData.endIndex, __FILE__, __LINE__); + } + } + return success; + } + + + /** Parse binary NAV350 position data telegrams, i.e. parse NAV350 "sAN mNPOSGetData version errorCode wait mask poseData [x y phi optPoseData [outputMode timestamp meanDev navMode infoState quantUsedReflectors]] + ** landmarkData [landmarkFilter reflectors [cart [X Y] polar [D Phi] optLandmarkData [optLandmarkData…]]] scanData [contentType scaleFactor scaleOffset startAngle angleRes data [aData]]" + */ + bool parseNAV350BinaryPositionData(const uint8_t* receiveBuffer, int receiveBufferLength, NAV350mNPOSData& navdata) + { + // Init return value + navdata = NAV350mNPOSData(); + + // Parse header + if (receiveBuffer == 0 || receiveBufferLength < 17 + 9 || memcmp(receiveBuffer, "\x02\x02\x02\x02", 4) != 0) + { + ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): invalid telegram (" << __FILE__ << ":" << __LINE__ << ")"); + return false; + } + bool success = true; + int receivePos = 4; + uint32_t payload_size = 0; + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, payload_size, __FILE__, __LINE__); + if (!success || (int)(payload_size) + 9 > receiveBufferLength) + { + ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): invalid telegram size (" << __FILE__ << ":" << __LINE__ << ")"); + return false; + } + + // Parse "sAN mNPOSGetData " + if (strncmp((const char*)receiveBuffer + receivePos, "sAN mNPOSGetData ", 17) != 0) + { + ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): telegram does not start with \"sAN mNPOSGetData \""); + return false; + } + receivePos += 17; + + // Parse version errorCode wait mask poseData + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.version, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.errorCode, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.wait, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.mask, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseDataValid, __FILE__, __LINE__); + if (navdata.errorCode) + { + std::map errorCodeStr = { {0, "no error"}, {1, "wrong operating mode"}, {2, "asynchrony Method terminated"}, {3, "invalid data"}, {4, "no position available"}, {5, "timeout"}, {6, "method already active"}, {7, "general error"} }; + ROS_WARN_STREAM("## WARNING parseNAV350BinaryPositionData(): NAV350 mNPOSGetData errorCode = " << (int)navdata.errorCode << " (\"" << errorCodeStr[navdata.errorCode] << "\")"); + } + + // Parse poseData x y phi optPoseData + if (navdata.poseDataValid > 0) + { + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.x, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.y, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.phi, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseDataValid, __FILE__, __LINE__); + if (navdata.poseData.optPoseDataValid > 0) + { + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.outputMode, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.timestamp, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.meanDev, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.navMode, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.infoState, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.poseData.optPoseData.quantUsedReflectors, __FILE__, __LINE__); + } + } + + // Parse landmark data + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.landmarkDataValid, __FILE__, __LINE__); + if (navdata.landmarkDataValid > 0) + { + success &= parseNAV350BinaryLandmarkData(receiveBuffer, receivePos, receiveBufferLength, navdata.landmarkData); + } + + // Parse NAVScan data + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanDataValid, __FILE__, __LINE__); + if (navdata.scanDataValid > 2) + ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): \"ScanData\" = " << (int)navdata.scanDataValid << ", max. 2 channel DIST1 and ANGL1 expected"); + navdata.scanData.resize(navdata.scanDataValid); + for (int channel = 0; channel < navdata.scanDataValid; channel++) + { + if (strncmp((const char*)receiveBuffer + receivePos, "DIST1", 5) != 0 && strncmp((const char*)receiveBuffer + receivePos, "ANGL1", 5) != 0) + ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): \"DIST1\" resp. \"ANGL1\" not found"); + navdata.scanData[channel].contentType = std::string((const char*)receiveBuffer + receivePos, 5); + receivePos += 5; + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].scaleFactor, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].scaleOffset, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].startAngle, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].angleRes, __FILE__, __LINE__); + if (std::abs(navdata.scanData[channel].scaleFactor - 1.0f) > FLT_EPSILON || std::abs(navdata.scanData[channel].scaleOffset - 0.0f) > FLT_EPSILON || navdata.scanData[channel].startAngle != 0 || navdata.scanData[channel].angleRes != 250) + { + ROS_WARN_STREAM("## WARNING parseNAV350BinaryPositionData(): unexpected scan data parameter: scaleFactor=" << navdata.scanData[channel].scaleFactor << ", scaleOffset=" << navdata.scanData[channel].scaleOffset + << ", startAngle=" << navdata.scanData[channel].startAngle << ", angleRes=" << navdata.scanData[channel].angleRes); + } + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].timestampStart, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].numData, __FILE__, __LINE__); + navdata.scanData[channel].data = std::vector(navdata.scanData[channel].numData); + for(int data_cnt = 0; data_cnt < navdata.scanData[channel].numData; data_cnt++) + { + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.scanData[channel].data[data_cnt], __FILE__, __LINE__); + } + } + + // Parse remission data + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionDataValid, __FILE__, __LINE__); + if (navdata.remissionDataValid > 0) + { + if (strncmp((const char*)receiveBuffer + receivePos, "RSSI1", 5) != 0) + ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): \"RSSI1\" not found"); + navdata.remissionData.contentType = std::string((const char*)receiveBuffer + receivePos, 5); + receivePos += 5; + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.scaleFactor, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.scaleOffset, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.startAngle, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.angleRes, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.timestampStart, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.numData, __FILE__, __LINE__); + navdata.remissionData.data = std::vector(navdata.remissionData.numData); + for(int data_cnt = 0; data_cnt < navdata.remissionData.numData; data_cnt++) + { + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, navdata.remissionData.data[data_cnt], __FILE__, __LINE__); + } + } + + if (!success) + ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): parsing error"); + return success; + } + + /** Parse binary NAV350 landmark data received by response "sAN mNMAPDoMapping" after request "sMN mNMAPDoMapping" */ + bool parseNAV350BinaryLandmarkDataDoMappingResponse(const uint8_t* receiveBuffer, int receiveBufferLength, NAV350LandmarkDataDoMappingResponse& landmarkData) + { + // Init return value + landmarkData = NAV350LandmarkDataDoMappingResponse(); + // Parse header + if (receiveBuffer == 0 || receiveBufferLength < 19 + 9 || memcmp(receiveBuffer, "\x02\x02\x02\x02", 4) != 0) + { + ROS_ERROR_STREAM("## ERROR parseNAV350LandmarkDataDoMappingResponse(): invalid telegram (" << __FILE__ << ":" << __LINE__ << ")"); + return false; + } + bool success = true; + int receivePos = 4; + uint32_t payload_size = 0; + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, payload_size, __FILE__, __LINE__); + if (!success || (int)(payload_size) + 9 > receiveBufferLength) + { + ROS_ERROR_STREAM("## ERROR parseNAV350LandmarkDataDoMappingResponse(): invalid telegram size (" << __FILE__ << ":" << __LINE__ << ")"); + return false; + } + // Parse "sAN mNMAPDoMapping " + if (strncmp((const char*)receiveBuffer + receivePos, "sAN mNMAPDoMapping ", 19) != 0) + { + ROS_ERROR_STREAM("## ERROR parseNAV350LandmarkDataDoMappingResponse(): telegram does not start with \"sAN mNMAPDoMapping \""); + return false; + } + receivePos += 19; + // Parse errorCode + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.errorCode, __FILE__, __LINE__); + success &= readFromBuffer(receiveBuffer, receivePos, receiveBufferLength, landmarkData.landmarkDataValid, __FILE__, __LINE__); + if (landmarkData.errorCode) + { + std::map errorCodeStr = { {0, "no error"}, {1, "wrong operating mode"}, {2, "asynchrony Method terminated"}, {5, "timeout"}, {6, "method already active"}, {7, "general error"} }; + ROS_WARN_STREAM("## WARNING parseNAV350LandmarkDataDoMappingResponse(): NAV350 landmarkData.errorCode = " << (int)landmarkData.errorCode << " (\"" << errorCodeStr[landmarkData.errorCode] << "\")"); + } + // Parse landmarkData + if (landmarkData.landmarkDataValid > 0) + { + success &= parseNAV350BinaryLandmarkData(receiveBuffer, receivePos, receiveBufferLength, landmarkData.landmarkData); + } + return success; + } + + /** Creates and returns the sopas command "sMN mNLAYAddLandmark landmarkData {x y type subtype size layerID {ID}}" */ + std::vector createNAV350BinaryAddLandmarkRequest(const NAV350LandmarkData& landmarkData, int nav_curr_layer) + { + std::string sopas_start = "sMN mNLAYAddLandmark "; + std::vector request(sopas_start.begin(), sopas_start.end()); + appendToBuffer(request, (uint16_t)landmarkData.reflectors.size()); + for(int reflector_cnt = 0; reflector_cnt < landmarkData.reflectors.size(); reflector_cnt++) + { + if (landmarkData.reflectors[reflector_cnt].cartesianDataValid == 0) + ROS_ERROR_STREAM("## ERROR createNAV350BinaryAddLandmarkRequest(): " << (reflector_cnt + 1) << ". reflector has no valid cartesian data"); + if (landmarkData.reflectors[reflector_cnt].optReflectorDataValid == 0) + ROS_ERROR_STREAM("## ERROR createNAV350BinaryAddLandmarkRequest(): " << (reflector_cnt + 1) << ". reflector has no valid type and subtype"); + appendToBuffer(request, (int32_t)landmarkData.reflectors[reflector_cnt].cartesianData.x); + appendToBuffer(request, (int32_t)landmarkData.reflectors[reflector_cnt].cartesianData.y); + request.push_back((uint8_t)landmarkData.reflectors[reflector_cnt].optReflectorData.type); + request.push_back((uint8_t)landmarkData.reflectors[reflector_cnt].optReflectorData.subType); + appendToBuffer(request, (uint16_t)landmarkData.reflectors[reflector_cnt].optReflectorData.size); + appendToBuffer(request, (uint16_t)1); + appendToBuffer(request, (uint16_t)nav_curr_layer); + } + return request; + } + + /** Creates and returns the sopas command "sMN mNLAYAddLandmark landmarkData {x y type subtype size layerID {ID}}" */ + std::vector createNAV350BinaryAddLandmarkRequest(const std::vector landmarks) + { + std::string sopas_start = "sMN mNLAYAddLandmark "; + std::vector request(sopas_start.begin(), sopas_start.end()); + appendToBuffer(request, (uint16_t)landmarks.size()); + for(int reflector_cnt = 0; reflector_cnt < landmarks.size(); reflector_cnt++) + { + appendToBuffer(request, (int32_t)landmarks[reflector_cnt].x_mm); + appendToBuffer(request, (int32_t)landmarks[reflector_cnt].y_mm); + request.push_back((uint8_t)landmarks[reflector_cnt].type); + request.push_back((uint8_t)landmarks[reflector_cnt].subType); + appendToBuffer(request, (uint16_t)landmarks[reflector_cnt].size_mm); + appendToBuffer(request, (uint16_t)landmarks[reflector_cnt].layerID.size()); + for(int layer_cnt = 0; layer_cnt < landmarks[reflector_cnt].layerID.size(); layer_cnt++) + appendToBuffer(request, (uint16_t)landmarks[reflector_cnt].layerID[layer_cnt]); + } + return request; + } + + /** Creates and returns the sopas command "sMN mNPOSSetSpeed X Y Phi timestamp coordBase" */ + std::vector createNAV350BinarySetSpeedRequest(const sick_scan_msg::NAVOdomVelocity& msg) + { + std::string sopas_start = "sMN mNPOSSetSpeed "; + std::vector request(sopas_start.begin(), sopas_start.end()); + appendToBuffer(request, (int16_t)(1000.0 * msg.vel_x)); + appendToBuffer(request, (int16_t)(1000.0 * msg.vel_y)); + appendToBuffer(request, (int32_t)(1000.0 * msg.omega * 180.0 / M_PI)); + appendToBuffer(request, (uint32_t)(msg.timestamp)); + appendToBuffer(request, (uint8_t)(msg.coordbase)); + return request; + } + + /** Parse binary NAV350 position data telegrams, i.e. parse NAV350 "sAN mNPOSGetData ..." and converts the data to a ros_sensor_msgs::LaserScan message */ + bool parseNAV350BinaryPositionData(const uint8_t* receiveBuffer, int receiveBufferLength, short& elevAngleX200, double& elevationAngleInRad, rosTime& recvTimeStamp, + bool config_sw_pll_only_publish, double config_time_offset, SickGenericParser* parser_, int& numEchos, ros_sensor_msgs::LaserScan & msg, + sick_scan_msg::NAVPoseData& nav_pose_msg, sick_scan_msg::NAVLandmarkData& nav_landmark_msg, NAV350mNPOSData& navdata) + { + // Init return values + elevAngleX200 = 0; + elevationAngleInRad = 0; + numEchos = 1; + navdata = NAV350mNPOSData(); + double nav_angle_offset = parser_->getCurrentParamPtr()->getScanAngleShift(); + + // NAV-350 operation manual: scan frequency 8 Hz, angular resolution 0.25 deg, scanning 0 to 360 deg, 1440 points per scan + ROS_HEADER_SEQ(msg.header, elevAngleX200); + msg.range_min = parser_->get_range_min(); + msg.range_max = parser_->get_range_max(); + msg.angle_min = (float)(0.0 + nav_angle_offset); + msg.angle_max = (float)(2.0 * M_PI + nav_angle_offset); + msg.angle_increment = (float)(0.25 * M_PI / 180.0); + msg.scan_time = 1.0f / 8.0f; + msg.time_increment = msg.scan_time * msg.angle_increment / (float)(2.0 * M_PI); + uint32_t nav_timestampStart = 0; // timestamp of scan start in ms + + // Parse "sAN mNPOSGetData " + ROS_DEBUG_STREAM("NAV350: " << receiveBufferLength << " bytes received: " << DataDumper::binDataToAsciiString(&receiveBuffer[0], MIN(64, receiveBufferLength))); + if (!parseNAV350BinaryPositionData(receiveBuffer, receiveBufferLength, navdata)) + { + ROS_ERROR_STREAM("## ERROR parseNAV350BinaryPositionData(): parsing error"); + return false; + } + navdata.angleOffset = nav_angle_offset; + + // Convert NAV350PoseData to sick_scan_msg::NAVPoseData + nav_pose_msg = sick_scan_msg::NAVPoseData(); + nav_pose_msg.pose_valid = 0; + if (navdata.poseDataValid > 0) + { + nav_pose_msg.x = navdata.poseData.x; + nav_pose_msg.y = navdata.poseData.y; + nav_pose_msg.phi = navdata.poseData.phi; + nav_pose_msg.opt_pose_data_valid = navdata.poseData.optPoseDataValid; + if (navdata.poseData.optPoseDataValid > 0) + { + nav_pose_msg.output_mode = navdata.poseData.optPoseData.outputMode; + nav_pose_msg.timestamp = navdata.poseData.optPoseData.timestamp; + nav_pose_msg.mean_dev = navdata.poseData.optPoseData.meanDev; + nav_pose_msg.nav_mode = navdata.poseData.optPoseData.navMode; + nav_pose_msg.info_state = navdata.poseData.optPoseData.infoState; + nav_pose_msg.quant_used_reflectors = navdata.poseData.optPoseData.quantUsedReflectors; + } + // Convert pose into ros coordinates + convertNAVCartPos3DtoROSPos3D(nav_pose_msg.x, nav_pose_msg.y, nav_pose_msg.phi, nav_pose_msg.pose_x, nav_pose_msg.pose_y, nav_pose_msg.pose_yaw, nav_angle_offset); // position in ros coordinates in meter, yaw angle in radians + nav_pose_msg.pose_valid = 1; + ROS_DEBUG_STREAM("NAV350 PoseData: x=" << nav_pose_msg.pose_x << ", y=" << nav_pose_msg.pose_y << ", yaw=" << (nav_pose_msg.pose_yaw*180.0/M_PI) + << " (lidar: x=" << nav_pose_msg.x << ", y=" << nav_pose_msg.y << ", phi=" << nav_pose_msg.phi << ")"); + } + else + { + ROS_DEBUG_STREAM("NAV350: no PoseData"); + } + + // Convert NAV350LandmarkData to sick_scan_msg::NAVLandmarkData + nav_landmark_msg = sick_scan_msg::NAVLandmarkData(); + if (navdata.landmarkDataValid > 0) + { + ROS_DEBUG_STREAM("NAV350 LandmarkData: " << navdata.landmarkData.reflectors.size() << " reflectors"); + nav_landmark_msg.landmark_filter = navdata.landmarkData.landmarkFilter; + nav_landmark_msg.num_reflectors = navdata.landmarkData.numReflectors; + nav_landmark_msg.reflectors.resize(navdata.landmarkData.reflectors.size()); + for(int reflector_cnt = 0; reflector_cnt < navdata.landmarkData.reflectors.size(); reflector_cnt++) + { + nav_landmark_msg.reflectors[reflector_cnt].cartesian_data_valid = navdata.landmarkData.reflectors[reflector_cnt].cartesianDataValid; + nav_landmark_msg.reflectors[reflector_cnt].polar_data_valid = navdata.landmarkData.reflectors[reflector_cnt].polarDataValid; + nav_landmark_msg.reflectors[reflector_cnt].opt_reflector_data_valid = navdata.landmarkData.reflectors[reflector_cnt].optReflectorDataValid; + nav_landmark_msg.reflectors[reflector_cnt].pos_valid = 0; + if (navdata.landmarkData.reflectors[reflector_cnt].cartesianDataValid > 0) + { + nav_landmark_msg.reflectors[reflector_cnt].x = navdata.landmarkData.reflectors[reflector_cnt].cartesianData.x; + nav_landmark_msg.reflectors[reflector_cnt].y = navdata.landmarkData.reflectors[reflector_cnt].cartesianData.y; + // Convert landmark position into ros coordinates in meter + convertNAVCartPos2DtoROSPos2D(nav_landmark_msg.reflectors[reflector_cnt].x, nav_landmark_msg.reflectors[reflector_cnt].y, + nav_landmark_msg.reflectors[reflector_cnt].pos_x, nav_landmark_msg.reflectors[reflector_cnt].pos_y, nav_angle_offset); + nav_landmark_msg.reflectors[reflector_cnt].pos_valid = 1; + } + if (navdata.landmarkData.reflectors[reflector_cnt].polarDataValid > 0) + { + nav_landmark_msg.reflectors[reflector_cnt].dist = navdata.landmarkData.reflectors[reflector_cnt].polarData.dist; + nav_landmark_msg.reflectors[reflector_cnt].phi = navdata.landmarkData.reflectors[reflector_cnt].polarData.phi; + } + if (navdata.landmarkData.reflectors[reflector_cnt].optReflectorDataValid > 0) + { + nav_landmark_msg.reflectors[reflector_cnt].local_id = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.localID; + nav_landmark_msg.reflectors[reflector_cnt].global_id = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.globalID; + nav_landmark_msg.reflectors[reflector_cnt].type = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.type; + nav_landmark_msg.reflectors[reflector_cnt].sub_type = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.subType; + nav_landmark_msg.reflectors[reflector_cnt].quality = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.quality; + nav_landmark_msg.reflectors[reflector_cnt].timestamp = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.timestamp; + nav_landmark_msg.reflectors[reflector_cnt].size = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.size; + nav_landmark_msg.reflectors[reflector_cnt].hit_count = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.hitCount; + nav_landmark_msg.reflectors[reflector_cnt].mean_echo = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.meanEcho; + nav_landmark_msg.reflectors[reflector_cnt].start_index = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.startIndex; + nav_landmark_msg.reflectors[reflector_cnt].end_index = navdata.landmarkData.reflectors[reflector_cnt].optReflectorData.endIndex; + } + ROS_DEBUG_STREAM("NAV350 LandmarkData: reflector[" << reflector_cnt << "]" + << ": cartesian=" << (int)(nav_landmark_msg.reflectors[reflector_cnt].cartesian_data_valid) << ", x=" << nav_landmark_msg.reflectors[reflector_cnt].x << ", y=" << nav_landmark_msg.reflectors[reflector_cnt].y + << ", polar=" << (int)(nav_landmark_msg.reflectors[reflector_cnt].polar_data_valid) << ", dist=" << nav_landmark_msg.reflectors[reflector_cnt].dist << ", phi=" << nav_landmark_msg.reflectors[reflector_cnt].phi + << ", pos_valid=" << (int)(nav_landmark_msg.reflectors[reflector_cnt].pos_valid) << ", pos_x=" << nav_landmark_msg.reflectors[reflector_cnt].pos_x << ", pos_y=" << nav_landmark_msg.reflectors[reflector_cnt].pos_y + << ", optValid=" << (int)(navdata.landmarkData.reflectors[reflector_cnt].optReflectorDataValid) << ", local_id=" << nav_landmark_msg.reflectors[reflector_cnt].local_id << ", global_id=" << nav_landmark_msg.reflectors[reflector_cnt].global_id); + } + } + else + { + ROS_DEBUG_STREAM("NAV350: no LandmarkData"); + } + + // Convert scan data (NAV350ScanData) to LaserScan message + if (navdata.scanData.empty()) + ROS_WARN_STREAM("## WARNING NAV350: no scan data channel in mNPOSGetData message"); + for(int channel = 0; channel < navdata.scanData.size(); channel++) + ROS_DEBUG_STREAM("NAV350 scan data channel " << (channel + 1) << " of " << navdata.scanData.size() << ": " << navdata.scanData[channel].contentType << ", startAngle=" << navdata.scanData[channel].startAngle << ", angleRes=" << navdata.scanData[channel].angleRes << ", numPoints=" << navdata.scanData[channel].numData << ", timestampStart=" << navdata.scanData[channel].timestampStart); + msg.ranges.clear(); + msg.intensities.clear(); + if (navdata.scanDataValid > 0 && navdata.scanData.size() > 0) + { + msg.angle_min = (float)(0.001 * navdata.scanData[0].startAngle * M_PI / 180.0 + nav_angle_offset); + msg.angle_increment = (float)(0.001 * navdata.scanData[0].angleRes * M_PI / 180.0); + msg.angle_max = (float)(msg.angle_min + msg.angle_increment * navdata.scanData[0].numData); + nav_timestampStart = navdata.scanData[0].timestampStart; + NAV350ScanData* dist_scan_data = 0; + for(int channel = 0; dist_scan_data == 0 && channel < navdata.scanData.size(); channel++) + if (navdata.scanData[channel].contentType == "DIST1") + dist_scan_data = &navdata.scanData[channel]; + if (!dist_scan_data || dist_scan_data->numData <= 0) + { + ROS_ERROR_STREAM("## ERROR NAV350: No channel DIST1 found in scan data of mNPOSGetData message"); + } + else + { + float scaleFactor = dist_scan_data->scaleFactor; // multiplier, always 1 for NAV350 + float scaleOffset = dist_scan_data->scaleOffset; // offset, always 0 for NAV350 + msg.ranges.resize(dist_scan_data->numData); + if (std::fabs(scaleFactor - 1.0f) > FLT_EPSILON || std::fabs(scaleOffset) > FLT_EPSILON) + { + ROS_WARN_STREAM("## WARNING NAV350: using DIST1 scaleFactor=" << scaleFactor << " (expected: scaleFactor:=1) and scaleOffset=" << scaleOffset << " (expected: scaleOffset:=0) in mNPOSGetData message"); + for(int point_cnt = 0; point_cnt < dist_scan_data->numData; point_cnt++) + { + msg.ranges[point_cnt] = 0.001f * (scaleFactor * (float)dist_scan_data->data[point_cnt] + scaleOffset); // DIST in mm to range in m + } + } + else + { + for(int point_cnt = 0; point_cnt < dist_scan_data->numData; point_cnt++) + { + msg.ranges[point_cnt] = 0.001f * (float)dist_scan_data->data[point_cnt]; // DIST in mm to range in m + } + } + } + } + + // Convert scan intensities (NAV350RemissionData) to LaserScan message + if (navdata.remissionDataValid > 0) + { + ROS_DEBUG_STREAM("NAV350 remission data: " << navdata.remissionData.contentType << ", startAngle=" << navdata.remissionData.startAngle << ", angleRes=" << navdata.remissionData.angleRes << ", numPoints=" << navdata.remissionData.numData); + if (navdata.remissionData.contentType != "RSSI1" || navdata.remissionData.numData != msg.ranges.size()) + ROS_WARN_STREAM("## WARNING NAV350: remissionData.contentType=" << navdata.remissionData.contentType << " (expected: RSSI1), remissionData.numData=" << navdata.remissionData.numData << " (expected: " << msg.ranges.size() << " points) in mNPOSGetData message"); + msg.intensities.resize(navdata.remissionData.numData); + for(int point_cnt = 0; point_cnt < navdata.remissionData.numData; point_cnt++) + { + msg.intensities[point_cnt] = (float)navdata.remissionData.data[point_cnt]; + } + } + + // Update Software-PLL by sensor timestamp + if (nav_timestampStart > 0) + { + uint32_t recvTimeStampSec = (uint32_t)sec(recvTimeStamp), recvTimeStampNsec = (uint32_t)nsec(recvTimeStamp); + bool softwarePLLready = SoftwarePLL::instance().updatePLL(recvTimeStampSec, recvTimeStampNsec, nav_timestampStart); + if (softwarePLLready) + { + SoftwarePLL::instance().getCorrectedTimeStamp(recvTimeStampSec, recvTimeStampNsec, nav_timestampStart); + recvTimeStamp = rosTime(recvTimeStampSec, recvTimeStampNsec); + msg.header.stamp = recvTimeStamp + rosDurationFromSec(config_time_offset); // recvTimeStamp updated by software-pll + ROS_DEBUG_STREAM("NAV350: SoftwarePLL ready: NAV-timestamp=" << nav_timestampStart << " ms, ROS-timestamp=" << rosTimeToSeconds(recvTimeStamp) << " sec."); + } + else + { + ROS_DEBUG_STREAM("NAV350: SoftwarePLL still initializing, NAV-timestamp=" << nav_timestampStart << " ms, ROS-timestamp=" << rosTimeToSeconds(recvTimeStamp) << " sec."); + } + if (config_sw_pll_only_publish == true) + { + incSoftwarePLLPacketReceived(); + } + } + nav_pose_msg.header = msg.header; + nav_landmark_msg.header = msg.header; + + return true; + } + + /** Rotates a cartesian 2D position (x, y) by a given angle offset */ + void rotateXYbyAngleOffset(float& x, float& y, double angle_offset) + { + if (std::abs(angle_offset - (+M_PI)) <= FLT_EPSILON || std::abs(angle_offset - (-M_PI)) <= FLT_EPSILON) + { + x = (-x); + y = (-y); + } + else if (std::abs(angle_offset) <= FLT_EPSILON) + { + x = (+x); + y = (+y); + } + else + { + x = (float)(x * cos(angle_offset) - y * sin(angle_offset)); + y = (float)(x * sin(angle_offset) + y * cos(angle_offset)); + } + } + + /** Converts a cartesian 2D position from NAV to ROS coordinates */ + void convertNAVCartPos2DtoROSPos2D(int32_t nav_posx_mm, int32_t nav_posy_mm, float& ros_posx_m, float& ros_posy_m, double nav_angle_offset) + { + ros_posx_m = (float)(1.0e-3 * nav_posx_mm); + ros_posy_m = (float)(1.0e-3 * nav_posy_mm); + rotateXYbyAngleOffset(ros_posx_m, ros_posy_m, nav_angle_offset); + } + + /** Converts a cartesian 3D pose from NAV to ROS coordinates */ + void convertNAVCartPos3DtoROSPos3D(int32_t nav_posx_mm, int32_t nav_posy_mm, uint32_t nav_phi_mdeg, float& ros_posx_m, float& ros_posy_m, float& ros_yaw_rad, double nav_angle_offset) + { + convertNAVCartPos2DtoROSPos2D(nav_posx_mm, nav_posy_mm, ros_posx_m, ros_posy_m, nav_angle_offset); // position in ros coordinates in meter + ros_yaw_rad = (float)(1.0e-3 * nav_phi_mdeg * M_PI / 180.0 + nav_angle_offset); // yaw angle in radians + } + + /** Convert NAV350PoseData to ros transform */ + ros_geometry_msgs::TransformStamped convertNAVPoseDataToTransform(NAV350PoseData& poseData, rosTime recvTimeStamp, double config_time_offset, + const std::string& tf_parent_frame_id, const std::string& tf_child_frame_id, SickGenericParser* parser_) + { + double nav_angle_offset = parser_->getCurrentParamPtr()->getScanAngleShift(); // NAV350: -PI + ros_geometry_msgs::TransformStamped tf; + // Convert to ROS transform + float posx = 0, posy = 0, yaw = 0; + convertNAVCartPos3DtoROSPos3D(poseData.x, poseData.y, poseData.phi, posx, posy, yaw, nav_angle_offset); // position in ros coordinates in meter, yaw angle in radians + // Convert timestamp from lidar to system time + if (poseData.optPoseDataValid > 0 && poseData.optPoseData.timestamp > 0 && SoftwarePLL::instance().IsInitialized()) + { + uint32_t recvTimeStampSec = (uint32_t)sec(recvTimeStamp), recvTimeStampNsec = (uint32_t)nsec(recvTimeStamp); + SoftwarePLL::instance().getCorrectedTimeStamp(recvTimeStampSec, recvTimeStampNsec, poseData.optPoseData.timestamp); + tf.header.stamp = rosTime(recvTimeStampSec, recvTimeStampNsec) + rosDurationFromSec(config_time_offset); // recvTimeStamp updated by software-pll + // Check function convSystemtimeToLidarTimestamp inverse to getCorrectedTimeStamp (debugging only) + // uint32_t lidar_timestamp = 0; + // SoftwarePLL::instance().convSystemtimeToLidarTimestamp(recvTimeStampSec, recvTimeStampNsec, lidar_timestamp); + // if (poseData.optPoseData.timestamp != lidar_timestamp) + // ROS_ERROR_STREAM("## ERROR ticks_in=" << poseData.optPoseData.timestamp << ", time=" << rosTimeToSeconds(rosTime(recvTimeStampSec, recvTimeStampNsec)) << ", ticks_out=" << lidar_timestamp); + } + else + { + tf.header.stamp = recvTimeStamp; + } + // Convert to ros transform message + tf.header.frame_id = tf_parent_frame_id; + tf.child_frame_id = tf_child_frame_id; + tf.transform.translation.x = posx; + tf.transform.translation.y = posy; + tf.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + tf.transform.rotation.x = q.x(); + tf.transform.rotation.y = q.y(); + tf.transform.rotation.z = q.z(); + tf.transform.rotation.w = q.w(); + return tf; + } + + /** Converts NAV350 landmark data to a ROS VisualMarker message */ + ros_visualization_msgs::MarkerArray convertNAVLandmarkDataToMarker(const std::vector& reflectors, ros_std_msgs::Header& header, SickGenericParser* parser_) + { + double nav_angle_offset = parser_->getCurrentParamPtr()->getScanAngleShift(); // NAV350: -PI + ros_visualization_msgs::MarkerArray marker_array; + marker_array.markers.reserve(1 + 2 * reflectors.size()); + marker_array.markers.push_back(ros_visualization_msgs::Marker()); + marker_array.markers.back().header = header; + marker_array.markers.back().action = ros_visualization_msgs::Marker::DELETEALL; + for(int reflector_cnt = 0; reflector_cnt < reflectors.size(); reflector_cnt++) + { + if (reflectors[reflector_cnt].cartesianDataValid > 0) + { + float reflector_posx = 0, reflector_posy = 0; + convertNAVCartPos2DtoROSPos2D(reflectors[reflector_cnt].cartesianData.x, reflectors[reflector_cnt].cartesianData.y, reflector_posx, reflector_posy, nav_angle_offset); + float reflector_size = 0.001f * (float)((reflectors[reflector_cnt].optReflectorDataValid > 0) ? (reflectors[reflector_cnt].optReflectorData.size) : 80); + int32_t reflector_id = ((reflectors[reflector_cnt].optReflectorDataValid > 0) ? (reflectors[reflector_cnt].optReflectorData.localID) : (reflector_cnt + 1)); + ros_visualization_msgs::Marker marker_cylinder; + marker_cylinder.ns = "sick_scan"; + marker_cylinder.id = reflector_cnt; + marker_cylinder.type = ros_visualization_msgs::Marker::CYLINDER; + marker_cylinder.scale.x = reflector_size; + marker_cylinder.scale.y = reflector_size; + marker_cylinder.scale.z = reflector_size; + marker_cylinder.pose.position.x = reflector_posx; + marker_cylinder.pose.position.y = reflector_posy; + marker_cylinder.pose.position.z = 0.0; + marker_cylinder.pose.orientation.x = 0.0; + marker_cylinder.pose.orientation.y = 0.0; + marker_cylinder.pose.orientation.z = 0.0; + marker_cylinder.pose.orientation.w = 1.0; + marker_cylinder.action = ros_visualization_msgs::Marker::ADD; + marker_cylinder.color.r = 1.0f; + marker_cylinder.color.g = 0.0f; + marker_cylinder.color.b = 0.0f; + marker_cylinder.color.a = 0.5f; + marker_cylinder.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever + marker_cylinder.header = header; + marker_array.markers.push_back(marker_cylinder); + + ros_visualization_msgs::Marker marker_text; + marker_text.ns = "sick_scan"; + marker_text.id = reflector_cnt + reflectors.size(); + marker_text.type = ros_visualization_msgs::Marker::TEXT_VIEW_FACING; + marker_text.text = std::to_string(reflector_id); + marker_text.scale.x = reflector_size; + marker_text.scale.y = reflector_size; + marker_text.scale.z = 2.0f * reflector_size; + marker_text.pose.position.x = reflector_posx; + marker_text.pose.position.y = reflector_posy + 1.5f * reflector_size; + marker_text.pose.position.z = 0.0; + marker_text.pose.orientation.x = 0.0; + marker_text.pose.orientation.y = 0.0; + marker_text.pose.orientation.z = 0.0; + marker_text.pose.orientation.w = 1.0; + marker_text.action = ros_visualization_msgs::Marker::ADD; + marker_text.color.r = 1.0f; + marker_text.color.g = 1.0f; + marker_text.color.b = 1.0f; + marker_text.color.a = 1.0f; + marker_text.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever + marker_text.header = header; + marker_array.markers.push_back(marker_text); + + } + } + return marker_array; + } + + /** Import a NAV350 landmark layout from imk file. The NAV350 landmark layout can be stored in imk files using SOPAS ET. Each line in an imk file is formatted "globID x_mm y_mm type subtype size_mm layer1 layer2 layer3" */ + std::vector readNAVIMKfile(const std::string& nav_imk_file) + { + std::vector navImkLandmarks; + // TODO ... + std::ifstream imk_stream(nav_imk_file); + if(imk_stream.is_open()) + { + std::string line; + while (getline(imk_stream, line)) + { + // Ignore comments and headers + if (line[0] == '#' || strncmp(line.c_str(), "globID", 6) == 0) + continue; + // Split into arguments + std::stringstream ss(line); + std::string token; + std::vector args; + while (std::getline(ss, token, ' ')) + { + args.push_back(token); + } + // Parse arguments + if (args.size() < 7) + { + ROS_WARN_STREAM("## ERROR readNAVIMKfile(" << nav_imk_file << "): parse error, line \"" << line << "\" ignored."); + continue; + } + sick_scan::NAV350ImkLandmark landmark; + landmark.globID = (uint16_t)(atoi(args[0].c_str()) & 0xFFFF); + landmark.x_mm = (int32_t)(atoi(args[1].c_str()) & 0xFFFFFFFF); + landmark.y_mm = (int32_t)(atoi(args[2].c_str()) & 0xFFFFFFFF); + landmark.type = (uint8_t)(atoi(args[3].c_str()) & 0xFF); + landmark.subType = (uint16_t)(atoi(args[4].c_str()) & 0xFFFF); + landmark.size_mm = (uint16_t)(atoi(args[5].c_str()) & 0xFFFF); + for(int n = 6; n < args.size(); n++) + landmark.layerID.push_back((uint16_t)(atoi(args[n].c_str()) & 0xFFFF)); + navImkLandmarks.push_back(landmark); + } + } + return navImkLandmarks; + } + +} /* namespace sick_scan */ diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp index 3502bdfb..02dee22d 100644 --- a/driver/src/sick_scan_common.cpp +++ b/driver/src/sick_scan_common.cpp @@ -79,6 +79,7 @@ #include #include #include +#include #include "sick_scan/binScanf.hpp" #include "sick_scan/dataDumper.h" @@ -488,7 +489,7 @@ namespace sick_scan rosDeclareParam(nh, "nodename", nodename); rosGetParam(nh, "nodename", nodename); - if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) + if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) == 0) { // NAV-310 only supports min/max angles of -PI to +PI (resp. 0 to 360 degree in sensor coordinates). To avoid unexpected results, min/max angles can not be set by configuration. config_.min_ang = -M_PI; @@ -557,7 +558,7 @@ namespace sick_scan rosDeclareParam(nh, "read_timeout_millisec_startup", m_read_timeout_millisec_startup); rosGetParam(nh, "read_timeout_millisec_startup", m_read_timeout_millisec_startup); - if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) + if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) == 0) { // NAV-310 only supports min/max angles of -PI to +PI (resp. 0 to 360 degree in sensor coordinates). To avoid unexpected results, min/max angles can not be set by configuration. if(std::abs(config_.min_ang + M_PI) > FLT_EPSILON || std::abs(config_.max_ang - M_PI) > FLT_EPSILON) @@ -568,6 +569,33 @@ namespace sick_scan } } + publish_nav_pose_data_ = false; + publish_nav_landmark_data_ = false; + nav_tf_parent_frame_id_ = "map"; + nav_tf_child_frame_id_ = "nav"; + rosDeclareParam(nh, "nav_tf_parent_frame_id", nav_tf_parent_frame_id_); + rosGetParam(nh, "nav_tf_parent_frame_id", nav_tf_parent_frame_id_); + rosDeclareParam(nh, "nav_tf_child_frame_id", nav_tf_child_frame_id_); + rosGetParam(nh, "nav_tf_child_frame_id", nav_tf_child_frame_id_); + + if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0) + { + nav_pose_data_pub_ = rosAdvertise(nh, nodename + "/nav_pose", 100); + nav_landmark_data_pub_ = rosAdvertise(nh, nodename + "/nav_landmark", 100); + nav_reflector_pub_ = rosAdvertise(nh, nodename + "/nav_reflectors", 100); + publish_nav_pose_data_ = true; + publish_nav_landmark_data_ = true; +#if defined __ROS_VERSION && __ROS_VERSION == 1 + nav_tf_broadcaster_ = new tf2_ros::TransformBroadcaster(); + nav_odom_velocity_subscriber_ = nh->subscribe("nav_odom_velocity", 1, &sick_scan::SickScanCommon::messageCbNavOdomVelocity, this); + ros_odom_subscriber_ = nh->subscribe("odom", 1, &sick_scan::SickScanCommon::messageCbRosOdom, this); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + nav_tf_broadcaster_ = new tf2_ros::TransformBroadcaster(nh); + nav_odom_velocity_subscriber_ = nh->create_subscription("nav_odom_velocity", 10, std::bind(&sick_scan::SickScanCommon::messageCbNavOdomVelocityROS2, this, std::placeholders::_1)); + ros_odom_subscriber_ = nh->create_subscription("odom", 10, std::bind(&sick_scan::SickScanCommon::messageCbRosOdomROS2, this, std::placeholders::_1)); +#endif + } + cloud_marker_ = 0; publish_lferec_ = false; publish_lidoutputstate_ = false; @@ -1111,9 +1139,28 @@ namespace sick_scan if(reply) *reply = replyDummy; this->setProtocolType((SopasProtocol)prev_sopas_type); // restore previous sopas type + if (result != 0) // no answer + { + ROS_WARN_STREAM("## ERROR SickScanCommon: sendSopasAndCheckAnswer(\"" << sopasCmd << "\") failed"); + } return result; } + int SickScanCommon::get2ndSopasResponse(std::vector& sopas_response, const std::string& sopas_keyword) + { + int bytes_read = 0; + sopas_response.clear(); + sopas_response.resize(64*1024); + std::vector sopas_response_keywords = { sopas_keyword }; + if (readWithTimeout(getReadTimeOutInMs(), (char*)sopas_response.data(), (int)sopas_response.size(), &bytes_read, sopas_response_keywords) != ExitSuccess) + { + ROS_WARN_STREAM("## ERROR waiting for 2nd response \"" << sopas_keyword << "\" to request \"" << sopas_keyword << "\""); + return ExitError; + } + sopas_response.resize(bytes_read); + return ExitSuccess; + } + // Check Cola-Configuration of the scanner: // * Send "sRN DeviceState" with configured cola-dialect (Cola-B = useBinaryCmd) // * If lidar does not answer: @@ -1183,6 +1230,101 @@ namespace sick_scan return result; } + // NAV-350 data must be polled by sending sopas command "sMN mNPOSGetData wait mask" + int SickScanCommon::sendNAV350mNPOSGetData(void) + { + // "sMN mNPOSGetData wait mask" (Cola-A "sMN mNPOSGetData 1 2" or Cola-B "sMN mNPOSGetData 0102"): wait for next pose result and send pose+reflectors+scan + std::string sopas_cmd = "\x02sMN mNPOSGetData 1 2\x03"; // wait = 1 (wait for next pose result), mask = 2 (send pose+reflectors+scan) + std::vector sopas_request; + this->convertAscii2BinaryCmd(sopas_cmd.c_str(), &sopas_request); + // Send "sMN mNPOSGetData 1 2" + ROS_DEBUG_STREAM("NAV350: Sending: " << stripControl(sopas_request, -1)); + return sendSOPASCommand((const char*)sopas_request.data(), 0, sopas_request.size(), false); + } + + // Parse NAV-350 pose and scan data and send next "sMN mNPOSGetData" request (NAV-350 polling) + bool SickScanCommon::handleNAV350BinaryPositionData(const uint8_t* receiveBuffer, int receiveBufferLength, short& elevAngleX200, double& elevationAngleInRad, rosTime & recvTimeStamp, + bool config_sw_pll_only_publish, double config_time_offset, SickGenericParser * parser_, int& numEchos, ros_sensor_msgs::LaserScan & msg, NAV350mNPOSData & navdata) + { + // Parse NAV-350 pose and scan data and convert to LaserScan message + sick_scan_msg::NAVPoseData nav_pose_msg; + sick_scan_msg::NAVLandmarkData nav_landmark_msg; + if (!parseNAV350BinaryPositionData(receiveBuffer, receiveBufferLength, elevAngleX200, elevationAngleInRad, recvTimeStamp, config_sw_pll_only_publish, config_time_offset, parser_, numEchos, msg, nav_pose_msg, nav_landmark_msg, navdata)) + ROS_ERROR_STREAM("## ERROR NAV350: Error parsing mNPOSGetData response"); + // Send next "sMN mNPOSGetData" request (NAV-350 polling) + int result = sendNAV350mNPOSGetData(); + if (result != ExitSuccess) + { + ROS_ERROR_STREAM("## ERROR NAV350: Error sending sMN mNPOSGetData request, retrying ..."); + return false; + } + // Publish pose and landmark data + if (publish_nav_pose_data_ && navdata.poseDataValid > 0) + { + rosPublish(nav_pose_data_pub_, nav_pose_msg); +#if __ROS_VERSION > 0 + if (nav_tf_broadcaster_) + { + ros_geometry_msgs::TransformStamped nav_pose_transform = convertNAVPoseDataToTransform(navdata.poseData, recvTimeStamp, config_time_offset, nav_tf_parent_frame_id_, nav_tf_child_frame_id_, parser_); + nav_tf_broadcaster_->sendTransform(nav_pose_transform); + } +#endif + } + if (publish_nav_landmark_data_ && navdata.landmarkDataValid > 0) + { + rosPublish(nav_landmark_data_pub_, nav_landmark_msg); +#if __ROS_VERSION > 0 + if (navdata.landmarkData.reflectors.size() > 0) + { + ros_visualization_msgs::MarkerArray nav_reflector_marker_msg = convertNAVLandmarkDataToMarker(navdata.landmarkData.reflectors, msg.header, parser_); + rosPublish(nav_reflector_pub_, nav_reflector_marker_msg); + } +#endif + } + if (navdata.poseDataValid > 0 || navdata.landmarkDataValid > 0) + { + notifyNavPoseLandmarkListener(m_nh, &navdata); + } + + return true; + } + + void SickScanCommon::messageCbNavOdomVelocity(const sick_scan_msg::NAVOdomVelocity& msg) + { + ROS_DEBUG_STREAM("SickScanCommon::messageCbNavOdomVelocity(): vel_x=" << msg.vel_x << " m/s, vel_y=" << msg.vel_y << " m/s, omega=" << msg.omega << " rad/s, timestamp=" << msg.timestamp << ", coordbase=" << (int)msg.coordbase); + std::vector sopas_response; + std::vector setNAVSpeedRequestPayload = createNAV350BinarySetSpeedRequest(msg); // "sMN mNPOSSetSpeed X Y Phi timestamp coordBase" + std::vector setNAVSpeedRequest = { 0x02, 0x02, 0x02, 0x02, 0, 0, 0, 0 }; + setNAVSpeedRequest.insert(setNAVSpeedRequest.end(), setNAVSpeedRequestPayload.begin(), setNAVSpeedRequestPayload.end()); + setLengthAndCRCinBinarySopasRequest(&setNAVSpeedRequest); + if (sendSopasAndCheckAnswer(setNAVSpeedRequest, &sopas_response) != 0) + ROS_ERROR_STREAM("SickScanCommon::messageCbNavOdomVelocity(): sendSopasAndCheckAnswer() failed"); + } + +#if __ROS_VERSION > 0 + void SickScanCommon::messageCbRosOdom(const ros_nav_msgs::Odometry& msg) + { + sick_scan_msg::NAVOdomVelocity nav_odom_vel_msg; + nav_odom_vel_msg.vel_x = msg.twist.twist.linear.x; + nav_odom_vel_msg.vel_y = msg.twist.twist.linear.y; + double angle_shift = -1.0 * parser_->getCurrentParamPtr()->getScanAngleShift(); + rotateXYbyAngleOffset(nav_odom_vel_msg.vel_x, nav_odom_vel_msg.vel_y, angle_shift); // Convert to velocity in lidar coordinates in m/s + nav_odom_vel_msg.omega = msg.twist.twist.angular.z; // angular velocity of the NAV350 in radians/s, -2*PI ... +2*PI rad/s + nav_odom_vel_msg.coordbase = 0; // 0 = local coordinate system of the NAV350 + nav_odom_vel_msg.timestamp = (uint32_t)(1000.0 * rosTimeToSeconds(msg.header.stamp)); // millisecond timestamp of the Velocity vector related to the NAV350 clock + if (SoftwarePLL::instance().IsInitialized()) + { + SoftwarePLL::instance().convSystemtimeToLidarTimestamp(sec(msg.header.stamp), nsec(msg.header.stamp), nav_odom_vel_msg.timestamp); + messageCbNavOdomVelocity(nav_odom_vel_msg); + } + else + { + ROS_WARN_STREAM("## ERROR SickScanCommon::messageCbRosOdom(): SoftwarePLL not yet ready, timestamp can not be converted from system time to lidar time, odometry message ignored."); + ROS_WARN_STREAM("## ERROR SickScanCommon::messageCbRosOdom(): Send odometry messages after SoftwarePLL is ready (i.e. has finished initialization phase)."); + } + } +#endif + /*! \brief set timeout in milliseconds \param timeOutInMs in milliseconds @@ -1375,11 +1517,22 @@ namespace sick_scan sopasCmdVec[CMD_SET_LID_INPUTSTATE_ACTIVE] = "\x02sEN LIDinputstate 1\x03"; // TiM781S: activate LIDinputstate messages, send "sEN LIDinputstate 1" // NAV-350 commands - sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_0] = "\x02sMN mNEVAChangeState 0\x03"; // 0 = power down - sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_1] = "\x02sMN mNEVAChangeState 1\x03"; // 1 = standby - sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_2] = "\x02sMN mNEVAChangeState 2\x03"; // 2 = mapping - sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_3] = "\x02sMN mNEVAChangeState 3\x03"; // 3 = landmark detection - sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_4] = "\x02sMN mNEVAChangeState 4\x03"; // 4 = navigation + sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_0] = "\x02sMN mNEVAChangeState 0\x03"; // 0 = power down + sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_1] = "\x02sMN mNEVAChangeState 1\x03"; // 1 = standby + sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_2] = "\x02sMN mNEVAChangeState 2\x03"; // 2 = mapping + sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_3] = "\x02sMN mNEVAChangeState 3\x03"; // 3 = landmark detection + sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_4] = "\x02sMN mNEVAChangeState 4\x03"; // 4 = navigation + sopasCmdVec[CMD_SET_NAV_CURR_LAYER] = "\x02sWN NEVACurrLayer 0\x03"; // Set NAV curent layer + sopasCmdVec[CMD_SET_NAV_LANDMARK_DATA_FORMAT] = "\x02sWN NLMDLandmarkDataFormat 0 1 1\x03"; // Set NAV LandmarkDataFormat + sopasCmdVec[CMD_SET_NAV_SCAN_DATA_FORMAT] = "\x02sWN NAVScanDataFormat 1 1\x03"; // Set NAV ScanDataFormat + sopasCmdVec[CMD_SET_NAV_POSE_DATA_FORMAT] = "\x02sWN NPOSPoseDataFormat 1 1\x03"; // Set NAV PoseDataFormat + sopasCmdVec[CMD_SET_NAV_MAP_CFG] = "\x02sWN NMAPMapCfg 50 0 0 0 0\x03"; + sopasCmdVec[CMD_SET_NAV_REFL_SIZE] = "\x02sWN NLMDReflSize 80\x03"; + sopasCmdVec[CMD_SET_NAV_DO_MAPPING] = "\x02sMN mNMAPDoMapping\x03"; + sopasCmdVec[CMD_SET_NAV_ADD_LANDMARK] = "\x02sMN mNLAYAddLandmark 0\x03"; + sopasCmdVec[CMD_SET_NAV_ERASE_LAYOUT] = "\x02sMN mNLAYEraseLayout 1\x03"; + sopasCmdVec[CMD_SET_NAV_STORE_LAYOUT] = "\x02sMN mNLAYStoreLayout\x03"; + sopasCmdVec[CMD_SET_NAV_POSE] = "\x02sMN mNPOSSetPose 0 0 0\x03"; // Set NAV-350 start pose in navigation mode by "sMN mNPOSSetPose X Y Phi" // Supported by sick_generic_caller version 2.7.3 and above: sopasCmdVec[CMD_SET_LFPMEANFILTER] = "\x02sWN LFPmeanfilter 0 0 0\x03"; // MRS1xxx, LMS1xxx, LMS4xxx, LRS4xxx: "sWN LFPmeanfilter" + { 1 byte 0|1 active/inactive } + { 2 byte 0x02 ... 0x64 number of scans } + { 1 byte 0x00 } @@ -1500,6 +1653,17 @@ namespace sick_scan sopasCmdErrMsg[CMD_SET_NAV_OPERATIONAL_MODE_2] = "Error setting operational mode mapping \"sMN mNEVAChangeState 2\""; sopasCmdErrMsg[CMD_SET_NAV_OPERATIONAL_MODE_3] = "Error setting operational mode landmark detection \"sMN mNEVAChangeState 3\""; sopasCmdErrMsg[CMD_SET_NAV_OPERATIONAL_MODE_4] = "Error setting operational mode navigation \"sMN mNEVAChangeState 4\""; + sopasCmdErrMsg[CMD_SET_NAV_CURR_LAYER] = "Error setting NAV current layer \"sWN NEVACurrLayer ...\""; + sopasCmdErrMsg[CMD_SET_NAV_LANDMARK_DATA_FORMAT] = "Error setting NAV landmark data format \"sWN NLMDLandmarkDataFormat ...\""; + sopasCmdErrMsg[CMD_SET_NAV_SCAN_DATA_FORMAT] = "Error setting NAV scan data format \"sWN NAVScanDataFormat ...\""; + sopasCmdErrMsg[CMD_SET_NAV_POSE_DATA_FORMAT] = "Error setting NAV pose data format \"sWN NPOSPoseDataFormat ...\""; + sopasCmdErrMsg[CMD_SET_NAV_MAP_CFG] = "Error setting NAV mapping configuration \"sWN NMAPMapCfg ...\""; + sopasCmdErrMsg[CMD_SET_NAV_REFL_SIZE] = "Error setting NAV reflector size \"sWN NLMDReflSize ...\""; + sopasCmdErrMsg[CMD_SET_NAV_DO_MAPPING] = "Error setting NAV mapping \"sMN mNMAPDoMapping\""; + sopasCmdErrMsg[CMD_SET_NAV_ADD_LANDMARK] = "Error adding NAV landmark \"sMN mNLAYAddLandmark\""; + sopasCmdErrMsg[CMD_SET_NAV_ERASE_LAYOUT] = "Error erasing NAV layout \"sMN mNLAYEraseLayout\""; + sopasCmdErrMsg[CMD_SET_NAV_STORE_LAYOUT] = "Error storing NAV layout \"sMN mNLAYStoreLayout\""; + sopasCmdErrMsg[CMD_SET_NAV_POSE] = "Error setting start pose \"sMN mNPOSSetPose ...\""; // Supported by sick_generic_caller version 2.7.3 and above: sopasCmdErrMsg[CMD_SET_LFPMEANFILTER] = "Error setting sopas command \"sWN LFPmeanfilter ...\""; @@ -1538,7 +1702,7 @@ namespace sick_scan } //TODO add basicParam for this - if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0 || + if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) == 0 || parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LRS_36x0_NAME) == 0 || parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LRS_36x1_NAME) == 0 || parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_OEM_15XX_NAME) == 0) @@ -1555,7 +1719,7 @@ namespace sick_scan { isNav2xxOr3xx = true; } - if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) + if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) == 0) { isNav2xxOr3xx = true; } @@ -1582,7 +1746,7 @@ namespace sick_scan } tryToStopMeasurement = false; - // do not stop measurement for RMS320 - the RMS320 does not support the stop command + // do not stop measurement for RMSxxxx (not supported) } if (tryToStopMeasurement) { @@ -1619,17 +1783,18 @@ namespace sick_scan { sopasCmdChain.push_back(CMD_DEVICE_IDENT); } + sopasCmdChain.push_back(CMD_FIRMWARE_VERSION); // read firmware sopasCmdChain.push_back(CMD_DEVICE_STATE); // read device state sopasCmdChain.push_back(CMD_OPERATION_HOURS); // read operation hours sopasCmdChain.push_back(CMD_POWER_ON_COUNT); // read power on count sopasCmdChain.push_back(CMD_LOCATION_NAME); // read location name - // Support for "sRN LMPscancfg" and "sMN mLMPsetscancfg" for NAV_3xx and LRS_36x1 since version 2.4.4 + // Support for "sRN LMPscancfg" and "sMN mLMPsetscancfg" for NAV_31X and LRS_36x1 since version 2.4.4 // TODO: apply and test for LRS_36x0 and OEM_15XX, too - // if (this->parser_->getCurrentParamPtr()->getUseScancfgList() == true) // true for SICK_SCANNER_LRS_36x0_NAME, SICK_SCANNER_LRS_36x1_NAME, SICK_SCANNER_NAV_3XX_NAME, SICK_SCANNER_OEM_15XX_NAME + // if (this->parser_->getCurrentParamPtr()->getUseScancfgList() == true) // true for SICK_SCANNER_LRS_36x0_NAME, SICK_SCANNER_LRS_36x1_NAME, SICK_SCANNER_NAV_31X_NAME, SICK_SCANNER_OEM_15XX_NAME if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LRS_36x1_NAME) == 0 - || parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) + || parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) == 0) { sopasCmdChain.push_back(CMD_GET_SCANDATACONFIGNAV); // Read LMPscancfg by "sRN LMPscancfg" sopasCmdChain.push_back(CMD_SET_SCAN_CFG_LIST); // "sMN mCLsetscancfglist 1", set scan config from list for NAX310 LD-OEM15xx LD-LRS36xx @@ -1642,7 +1807,7 @@ namespace sick_scan sopasCmdChain.push_back(CMD_GET_SCANDATACONFIGNAV); // Read LMPscancfg by "sRN LMPscancfg" } /* - if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) + if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) == 0) { sopasCmdChain.push_back(CMD_SET_ACCESS_MODE_3); // re-enter authorized client level sopasCmdChain.push_back("\x02sWN LMDscandatacfg 01 00 1 0 0 0 00 0 0 0 1 1\x03"); @@ -1720,39 +1885,6 @@ namespace sick_scan } } - // Additional startup commands for NAV-350 support - if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0) - { - sopasCmdChain.push_back(CMD_SET_ACCESS_MODE_3); // re-enter authorized client level - sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_1); // "sMN mNEVAChangeState 1", 1 = standby - int nav_operation_mode = 4; - rosDeclareParam(nh, "nav_operation_mode", nav_operation_mode); - rosGetParam(nh, "nav_operation_mode", nav_operation_mode); - switch(nav_operation_mode) - { - case 0: - sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_0); - break; - case 1: - sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_1); - break; - case 2: - sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_2); - break; - case 3: - sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_3); - break; - case 4: - sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_4); - break; - default: - ROS_WARN_STREAM("Invalid parameter nav_operation_mode = " << nav_operation_mode << ", expected 0, 1, 2, 3 or 4, using default mode 4 (navigation)"); - sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_4); - break; - } - - } - return (0); } @@ -1926,7 +2058,7 @@ namespace sick_scan //TODO remove this and use getUseCfgList instead bool NAV3xxOutputRangeSpecialHandling=false; - if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0|| + if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) == 0|| this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LRS_36x0_NAME) == 0|| this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LRS_36x1_NAME) == 0) { @@ -2369,7 +2501,7 @@ namespace sick_scan // Compensate angle shift (min/max angle from config in ros-coordinate system) double start_ang_rad = (scancfg.sector_cfg[sector_cnt].start_angle / 10000.0) * (M_PI / 180.0); double stop_ang_rad = (scancfg.sector_cfg[sector_cnt].stop_angle / 10000.0) * (M_PI / 180.0); - if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0) // map ros start/stop angle to NAV-3xx logic + if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) == 0) // map ros start/stop angle to NAV-3xx logic { // NAV-310 only supports min/max angles 0 to 360 degree in sensor coordinates. To avoid unexpected results, min/max angles can not be set by configuration. start_ang_rad = 0; // this->config_.min_ang; @@ -2476,7 +2608,7 @@ namespace sick_scan if (this->parser_->getCurrentParamPtr()->getUseScancfgList()) { if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LRS_36x1_NAME) == 0 - || parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0 + || parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) == 0 || parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0) { // scanconfig handling with list now done above via sopasCmdChain, @@ -2957,7 +3089,7 @@ namespace sick_scan } if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0) { - int filter_echos = 0; + int filter_echos = 2; // rosDeclareParam(nh, "filter_echos", filter_echos); rosGetParam(nh, "filter_echos", filter_echos); switch (filter_echos) @@ -3329,7 +3461,7 @@ namespace sick_scan if (this->parser_->getCurrentParamPtr()->getFREchoFilterAvailable()) { char requestEchoSetting[MAX_STR_LEN]; - int filterEchoSetting = 0; + int filterEchoSetting = 2; rosDeclareParam(nh, "filter_echos", filterEchoSetting); // filter_echos rosGetParam(nh, "filter_echos", filterEchoSetting); // filter_echos @@ -3374,7 +3506,6 @@ namespace sick_scan // //----------------------------------------------------------------- std::vector startProtocolSequence; - bool deviceIsRadar = false; if (this->parser_->getCurrentParamPtr()->getDeviceIsRadar()) { bool transmitRawTargets = true; @@ -3390,21 +3521,12 @@ namespace sick_scan rosDeclareParam(nh, "transmit_objects", transmitObjects); rosGetParam(nh, "transmit_objects", transmitObjects); - if (this->parser_->getCurrentParamPtr()->getTrackingModeSupported()) - { - rosDeclareParam(nh, "tracking_mode", trackingMode); - rosGetParam(nh, "tracking_mode", trackingMode); - if ((trackingMode < 0) || (trackingMode >= numTrackingModes)) - { - ROS_WARN("tracking mode id invalid. Switch to tracking mode 0"); - trackingMode = 0; - } - ROS_INFO_STREAM("Raw target transmission is switched [" << (transmitRawTargets ? "ON" : "OFF") << "]"); - ROS_INFO_STREAM("Object transmission is switched [" << (transmitObjects ? "ON" : "OFF") << "]"); - ROS_INFO_STREAM("Tracking mode is set to id [" << trackingMode << "] [" << trackingModeDescription[trackingMode] << "]"); - } + rosDeclareParam(nh, "tracking_mode", trackingMode); + rosGetParam(nh, "tracking_mode", trackingMode); - deviceIsRadar = true; + ROS_INFO_STREAM("Raw target transmission is switched [" << (transmitRawTargets ? "ON" : "OFF") << "]"); + ROS_INFO_STREAM("Object transmission is switched [" << (transmitObjects ? "ON" : "OFF") << "]"); + ROS_INFO_STREAM("Tracking mode [" << trackingMode << "] [" << ((trackingMode >= 0 && trackingMode < numTrackingModes) ? trackingModeDescription[trackingMode] : "unknown") << "]"); // Asking some informational from the radar startProtocolSequence.push_back(CMD_DEVICE_TYPE); @@ -3438,21 +3560,17 @@ namespace sick_scan ROS_WARN("Both ObjectData and TargetData are disabled. Check launchfile, parameter \"transmit_raw_targets\" or \"transmit_objects\" or both should be activated."); } - if (this->parser_->getCurrentParamPtr()->getTrackingModeSupported()) + switch (trackingMode) { - switch (trackingMode) - { - case 0: - startProtocolSequence.push_back(CMD_SET_TRACKING_MODE_0); - break; - case 1: - startProtocolSequence.push_back(CMD_SET_TRACKING_MODE_1); - break; - default: - ROS_DEBUG("Tracking mode switching sequence unknown\n"); - break; - - } + case 0: + startProtocolSequence.push_back(CMD_SET_TRACKING_MODE_0); + break; + case 1: + startProtocolSequence.push_back(CMD_SET_TRACKING_MODE_1); + break; + default: + ROS_DEBUG("Tracking mode switching sequence unknown\n"); + break; } // leave user level @@ -3503,9 +3621,9 @@ namespace sick_scan else { startProtocolSequence.push_back(CMD_START_MEASUREMENT); + startProtocolSequence.push_back(CMD_RUN); // leave user level + startProtocolSequence.push_back(CMD_START_SCANDATA); } - startProtocolSequence.push_back(CMD_RUN); // leave user level - startProtocolSequence.push_back(CMD_START_SCANDATA); if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() == 4) // MRS1104 - start IMU-Transfer { @@ -3535,9 +3653,6 @@ namespace sick_scan for (it = startProtocolSequence.begin(); it != startProtocolSequence.end(); it++) { int cmdId = *it; - std::vector tmpReply; - // result = sendSopasAndCheckAnswer(sopasCmdVec[cmdId].c_str(), &tmpReply); - // RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, tmpReply); // No response, non-recoverable connection error (return error and do not try other commands) std::string sopasCmd = sopasCmdVec[cmdId]; std::vector replyDummy; @@ -3551,14 +3666,6 @@ namespace sick_scan result = sendSopasAndCheckAnswer(reqBinary, &replyDummy, cmdId); sopasReplyBinVec[cmdId] = replyDummy; RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, replyDummy); // No response, non-recoverable connection error (return error and do not try other commands) - - switch (cmdId) - { - case CMD_START_SCANDATA: - // ROS_DEBUG("Sleeping for a couple of seconds of start measurement\n"); - // rosSleep(10.0); - break; - } } else { @@ -3663,8 +3770,224 @@ namespace sick_scan } } } - tmpReply.clear(); + if (this->parser_->getCurrentParamPtr()->getDeviceIsRadar() && cmdId == CMD_DEVICE_TYPE) + { + std::string device_type_response = sopasReplyStrVec[cmdId], rms_type_str = ""; + size_t cmd_type_idx = device_type_response.find("DItype "); + if (cmd_type_idx != std::string::npos) + device_type_response = device_type_response.substr(cmd_type_idx + 7); + size_t radar_type_idx = device_type_response.find("RMS"); + if (radar_type_idx != std::string::npos) + rms_type_str = device_type_response.substr(radar_type_idx, 4); + ROS_INFO_STREAM("Radar device type response: \"" << sopasReplyStrVec[cmdId] << "\" -> device type = \"" << device_type_response << "\" (" << rms_type_str << ")"); + // Detect and switch between RMS-1xxx and RMS-2xxx + if (rms_type_str == "RMS1") + { + this->parser_->getCurrentParamPtr()->setDeviceIsRadar(RADAR_1D); // Device is a 1D radar + this->parser_->getCurrentParamPtr()->setTrackingModeSupported(false); // RMS 1xxx does not support selection of tracking modes + for (std::vector::iterator start_cmd_iter = startProtocolSequence.begin(); start_cmd_iter != startProtocolSequence.end(); start_cmd_iter++) + { + if (*start_cmd_iter == CMD_SET_TRACKING_MODE_0 || *start_cmd_iter == CMD_SET_TRACKING_MODE_1) + *start_cmd_iter = CMD_DEVICE_STATE; // Disable unsupported set tracking mode commands by overwriting with "sRN SCdevicestate" + } + ROS_INFO_STREAM("1D radar \"" << rms_type_str << "\" device detected, tracking mode disabled"); + } + else if (rms_type_str == "RMS2") + { + this->parser_->getCurrentParamPtr()->setDeviceIsRadar(RADAR_3D); // Device is a 3D radar + this->parser_->getCurrentParamPtr()->setTrackingModeSupported(true); // Default + ROS_INFO_STREAM("3D radar \"" << rms_type_str << "\" device detected, tracking mode enabled"); + } + else + { + ROS_ERROR_STREAM("## ERROR init_scanner(): Unexpected device type \"" << device_type_response << "\", expected device type starting with \"RMS1\" or \"RMS2\""); + } + } + } + + if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0) // TODO: move to sick_nav_init.cpp... + { + if (!parseNAV350BinaryUnittest()) + ROS_ERROR_STREAM("## ERROR NAV350: parseNAV350BinaryUnittest() failed."); + std::vector sopas_response; + // NAV-350 initialization sequence + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_ACCESS_MODE_3], &sopas_response, useBinaryCmd) != 0) // re-enter authorized client level + return ExitError; + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_1], &sopas_response, useBinaryCmd) != 0) // "sMN mNEVAChangeState 1", 1 = standby + return ExitError; + if (get2ndSopasResponse(sopas_response, "mNEVAChangeState") != ExitSuccess) + return ExitError; + // Set the current NAV Layer for Positioning and Mapping + int nav_curr_layer = 0; + rosDeclareParam(nh, "nav_curr_layer", nav_curr_layer); + rosGetParam(nh, "nav_curr_layer", nav_curr_layer); + sopasCmdVec[CMD_SET_NAV_CURR_LAYER] = std::string("\x02sWN NEVACurrLayer ") + std::to_string(nav_curr_layer) + "\x03"; // Set the current NAV Layer for Positioning and Mapping + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_CURR_LAYER], &sopas_response, useBinaryCmd) != 0) + return ExitError; + // Set NAV LandmarkDataFormat, ScanDataFormat and PoseDataFormat + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_LANDMARK_DATA_FORMAT], &sopas_response, useBinaryCmd) != 0) + return ExitError; + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_SCAN_DATA_FORMAT], &sopas_response, useBinaryCmd) != 0) + return ExitError; + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_POSE_DATA_FORMAT], &sopas_response, useBinaryCmd) != 0) // Set PoseDataFormat: "sWN NPOSPoseDataFormat 1 1" + return ExitError; + // Optionally do Mapping + bool nav_do_initial_mapping = false; + rosDeclareParam(nh, "nav_do_initial_mapping", nav_do_initial_mapping); + rosGetParam(nh, "nav_do_initial_mapping", nav_do_initial_mapping); + if (nav_do_initial_mapping) + { + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_ERASE_LAYOUT], &sopas_response, useBinaryCmd) != 0) // Erase mapping layout: "sMN mNLAYEraseLayout 1" + return ExitError; + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_2], &sopas_response, useBinaryCmd) != 0) // set operational mode mapping + return ExitError; + if (get2ndSopasResponse(sopas_response, "mNEVAChangeState") != ExitSuccess) + return ExitError; + // Configure Mapping: "sWN NMAPMapCfg mean negative x y phi" + int nav_map_cfg_mean = 50, nav_map_cfg_neg = 0, nav_map_cfg_x = 0, nav_map_cfg_y = 0, nav_map_cfg_phi = 0, nav_map_cfg_reflector_size = 80; + rosDeclareParam(nh, "nav_map_cfg_mean", nav_map_cfg_mean); + rosGetParam(nh, "nav_map_cfg_mean", nav_map_cfg_mean); + rosDeclareParam(nh, "nav_map_cfg_neg", nav_map_cfg_neg); + rosGetParam(nh, "nav_map_cfg_neg", nav_map_cfg_neg); + rosDeclareParam(nh, "nav_map_cfg_x", nav_map_cfg_x); + rosGetParam(nh, "nav_map_cfg_x", nav_map_cfg_x); + rosDeclareParam(nh, "nav_map_cfg_y", nav_map_cfg_y); + rosGetParam(nh, "nav_map_cfg_y", nav_map_cfg_y); + rosDeclareParam(nh, "nav_map_cfg_phi", nav_map_cfg_phi); + rosGetParam(nh, "nav_map_cfg_phi", nav_map_cfg_phi); + rosDeclareParam(nh, "nav_map_cfg_reflector_size", nav_map_cfg_reflector_size); + rosGetParam(nh, "nav_map_cfg_reflector_size", nav_map_cfg_reflector_size); + sopasCmdVec[CMD_SET_NAV_MAP_CFG] = std::string("\x02sWN NMAPMapCfg ") + std::to_string(nav_map_cfg_mean) + " " + std::to_string(nav_map_cfg_neg) + " " + std::to_string(nav_map_cfg_x) + " " + std::to_string(nav_map_cfg_y) + " " + std::to_string(nav_map_cfg_phi) + " " + "\x03"; // Configure Mapping + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_MAP_CFG], &sopas_response, useBinaryCmd) != 0) + return ExitError; + // Set reflector size: "sWN NLMDReflSize size" + sopasCmdVec[CMD_SET_NAV_REFL_SIZE] = std::string("\x02sWN NLMDReflSize ") + std::to_string(nav_map_cfg_reflector_size) + "\x03"; // Set reflector size + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_REFL_SIZE], &sopas_response, useBinaryCmd) != 0) + return ExitError; + // Do Mapping: "sMN mNMAPDoMapping" + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_DO_MAPPING], &sopas_response, useBinaryCmd) != 0) + return ExitError; + // Wait for response "sAN mNMAPDoMapping errorCode landmarkData[...]" (which is sent after the request acknowledge "sMA mNMAPDoMapping") + ROS_INFO_STREAM("1. response to mNMAPDoMapping request: " << stripControl(sopas_response, -1)); + if (get2ndSopasResponse(sopas_response, "mNMAPDoMapping") != ExitSuccess) + return ExitError; + ROS_INFO_STREAM("2. response to mNMAPDoMapping request: " << stripControl(sopas_response, -1)); + // Parse LandmarkData + sick_scan::NAV350LandmarkDataDoMappingResponse landmarkData; + if (!parseNAV350BinaryLandmarkDataDoMappingResponse(sopas_response.data(), (int)sopas_response.size(), landmarkData)) + { + ROS_WARN_STREAM("## ERROR parseNAV350BinaryLandmarkDataDoMappingResponse() failed"); + return ExitError; + } + landmarkData.print(); + // Add LandmarkData for mapping // "sMN mNLAYAddLandmark landmarkData {x y type subtype size layerID {ID}}" + if (landmarkData.landmarkDataValid > 0 && landmarkData.landmarkData.reflectors.size()) + { + std::vector addLandmarkRequestPayload = createNAV350BinaryAddLandmarkRequest(landmarkData.landmarkData, nav_curr_layer); + std::vector addLandmarkRequest = { 0x02, 0x02, 0x02, 0x02, 0, 0, 0, 0 }; + addLandmarkRequest.insert(addLandmarkRequest.end(), addLandmarkRequestPayload.begin(), addLandmarkRequestPayload.end()); + setLengthAndCRCinBinarySopasRequest(&addLandmarkRequest); + if (sendSopasAndCheckAnswer(addLandmarkRequest, &sopas_response) != 0) + return ExitError; + } + else + { + ROS_ERROR_STREAM("## ERROR parseNAV350BinaryLandmarkDataDoMappingResponse(): Not enough landmarks detected for initial mapping."); + return ExitError; + } + // Store mapping layout: "sMN mNLAYStoreLayout" + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_STORE_LAYOUT], &sopas_response, useBinaryCmd) != 0) + return ExitError; + } + // Optionally import landmark layout from imk file + std::string nav_set_landmark_layout_by_imk_file = ""; + rosDeclareParam(nh, "nav_set_landmark_layout_by_imk_file", nav_set_landmark_layout_by_imk_file); + rosGetParam(nh, "nav_set_landmark_layout_by_imk_file", nav_set_landmark_layout_by_imk_file); + if (!nav_set_landmark_layout_by_imk_file.empty()) + { + std::vector navImkLandmarks = readNAVIMKfile(nav_set_landmark_layout_by_imk_file); + if (navImkLandmarks.size() >= 3) // at least 3 reflectors required + { + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_ERASE_LAYOUT], &sopas_response, useBinaryCmd) != 0) // Erase mapping layout: "sMN mNLAYEraseLayout 1" + return ExitError; + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_2], &sopas_response, useBinaryCmd) != 0) // set operational mode mapping + return ExitError; + if (get2ndSopasResponse(sopas_response, "mNEVAChangeState") != ExitSuccess) + return ExitError; + std::vector addLandmarkRequestPayload = createNAV350BinaryAddLandmarkRequest(navImkLandmarks); + std::vector addLandmarkRequest = { 0x02, 0x02, 0x02, 0x02, 0, 0, 0, 0 }; + addLandmarkRequest.insert(addLandmarkRequest.end(), addLandmarkRequestPayload.begin(), addLandmarkRequestPayload.end()); + setLengthAndCRCinBinarySopasRequest(&addLandmarkRequest); + if (sendSopasAndCheckAnswer(addLandmarkRequest, &sopas_response) != 0) + return ExitError; + // Store mapping layout: "sMN mNLAYStoreLayout" + if (sendSopasAorBgetAnswer(sopasCmdVec[CMD_SET_NAV_STORE_LAYOUT], &sopas_response, useBinaryCmd) != 0) + return ExitError; + } + else if (navImkLandmarks.size() > 0) + { + ROS_ERROR_STREAM("## ERROR readNAVIMKfile(): Less than 3 landmarks configured in \"" << nav_set_landmark_layout_by_imk_file << "\". At least 3 landmarks required."); + return ExitError; + } + else + { + ROS_ERROR_STREAM("## ERROR readNAVIMKfile(): Can't read or parse imk file \"" << nav_set_landmark_layout_by_imk_file << "\"."); + return ExitError; + } + } + // Switch to final operation mode (navigation by default) + int nav_operation_mode = 4; + rosDeclareParam(nh, "nav_operation_mode", nav_operation_mode); + rosGetParam(nh, "nav_operation_mode", nav_operation_mode); + enum SOPAS_CMD sopas_op_mode_cmd = CMD_SET_NAV_OPERATIONAL_MODE_4; + switch(nav_operation_mode) + { + case 0: + sopas_op_mode_cmd = (CMD_SET_NAV_OPERATIONAL_MODE_0); + break; + case 1: + sopas_op_mode_cmd = (CMD_SET_NAV_OPERATIONAL_MODE_1); + break; + case 2: + sopas_op_mode_cmd = (CMD_SET_NAV_OPERATIONAL_MODE_2); + break; + case 3: + sopas_op_mode_cmd = (CMD_SET_NAV_OPERATIONAL_MODE_3); + break; + case 4: + sopas_op_mode_cmd = (CMD_SET_NAV_OPERATIONAL_MODE_4); + break; + default: + ROS_WARN_STREAM("Invalid parameter nav_operation_mode = " << nav_operation_mode << ", expected 0, 1, 2, 3 or 4, using default mode 4 (navigation)"); + nav_operation_mode = 4; + sopas_op_mode_cmd = (CMD_SET_NAV_OPERATIONAL_MODE_4); + break; + } + if (sendSopasAorBgetAnswer(sopasCmdVec[sopas_op_mode_cmd], &sopas_response, useBinaryCmd) != 0) + return ExitError; + if (get2ndSopasResponse(sopas_response, "mNEVAChangeState") != ExitSuccess) + return ExitError; + // NAV-350 data must be polled by sending sopas command "sMN mNPOSGetData wait mask" + bool nav_start_polling = true; + rosDeclareParam(nh, "nav_start_polling", nav_start_polling); + rosGetParam(nh, "nav_start_polling", nav_start_polling); + if (!nav_start_polling) + ROS_WARN_STREAM("NAV350 Warning: start polling deactivated by configuration, no data will be received unless data polling started externally by sopas command \"sMN mNPOSGetData 1 2\""); + for (int retry_cnt = 0; nav_start_polling == true && retry_cnt < 10 && rosOk(); retry_cnt++) + { + ROS_INFO_STREAM("NAV350: Sending: \"sMN mNPOSGetData 1 2\""); + if (sendNAV350mNPOSGetData() != ExitSuccess) + { + ROS_ERROR_STREAM("## ERROR NAV350: Error sending sMN mNPOSGetData request, retrying ..."); + rosSleep(1.0); + } + else + { + ROS_INFO_STREAM("NAV350: sMN mNPOSGetData request was sent"); + break; + } + } } return ExitSuccess; } @@ -3814,8 +4137,7 @@ namespace sick_scan supported = true; } - if (identStr.find("RMS3xx") != - std::string::npos) // received pattern contains 4 'x' but we check only for 3 'x' (MRS1104 should be MRS1xxx) + if (identStr.find("RMS1") != std::string::npos || identStr.find("RMS2") != std::string::npos) { ROS_INFO_STREAM("Deviceinfo " << identStr << " found and supported by this driver."); supported = true; @@ -3901,7 +4223,7 @@ namespace sick_scan do { const std::vector datagram_keywords = { // keyword list of datagrams handled here in loopOnce - "LMDscandata", "LMDscandatamon", + "LMDscandata", "LMDscandatamon", "mNPOSGetData", "LMDradardata", "InertialMeasurementUnit", "LIDoutputstate", "LIDinputstate", "LFErec" }; int result = get_datagram(nh, recvTimeStamp, receiveBuffer, 65536, &actual_length, useBinaryProtocol, &packetsInLoop, datagram_keywords); @@ -3944,12 +4266,10 @@ namespace sick_scan } - bool deviceIsRadar = this->parser_->getCurrentParamPtr()->getDeviceIsRadar(); - - if (true == deviceIsRadar) + if (true == this->parser_->getCurrentParamPtr()->getDeviceIsRadar()) { SickScanRadarSingleton *radar = SickScanRadarSingleton::getInstance(nh); - radar->setNameOfRadar(this->parser_->getCurrentParamPtr()->getScannerName()); + radar->setNameOfRadar(this->parser_->getCurrentParamPtr()->getScannerName(), this->parser_->getCurrentParamPtr()->getDeviceRadarType()); int errorCode = ExitSuccess; // parse radar telegram and send pointcloud2-debug messages errorCode = radar->parseDatagram(recvTimeStamp, (unsigned char *) receiveBuffer, actual_length, @@ -4049,9 +4369,14 @@ namespace sick_scan ROS_DEBUG_STREAM("SickScanCommon: received " << actual_length << " byte LMDscandatamon (ignored) ..."); // << DataDumper::binDataToAsciiString(&receiveBuffer[0], actual_length)); return errorCode; // return success to continue looping } + else if(memcmp(&receiveBuffer[8], "sMA mNPOSGetData", strlen("sMA mNPOSGetData")) == 0) // NAV-350: method acknowledge, indicates mNPOSGetData has started => wait for the "sAN mNPOSGetData" response + { + int errorCode = ExitSuccess; + ROS_DEBUG_STREAM("NAV350: received " << actual_length << " byte \"sMA mNPOSGetData\", waiting for \"sAN mNPOSGetData\" ..."); + return errorCode; // return success to continue looping + } else { - ros_sensor_msgs::LaserScan msg; sick_scan_msg::Encoder EncoderMsg; EncoderMsg.header.stamp = recvTimeStamp + rosDurationFromSec(config_.time_offset); @@ -4135,25 +4460,35 @@ namespace sick_scan } #endif // binary message - // if (lenVal < actual_length) - if (lenVal >= actual_length || actual_length < 64) // scan data message requires at least 64 byte, otherwise this can't be a scan data message + if (actual_length > 17 && memcmp(&receiveBuffer[8], "sAN mNPOSGetData ", 17) == 0) // NAV-350 pose and scan data { + NAV350mNPOSData navdata; // NAV-350 pose and scan data + success = handleNAV350BinaryPositionData(receiveBuffer, actual_length, elevAngleX200, elevationAngleInRad, recvTimeStamp, config_.sw_pll_only_publish, config_.time_offset, parser_, numEchos, msg, navdata); + if (!success) + ROS_ERROR_STREAM("## ERROR SickScanCommon::loopOnce(): handleNAV350BinaryPositionData() failed"); + } + else if (lenVal >= actual_length || actual_length < 64) // scan data message requires at least 64 byte, otherwise this can't be a scan data message + { + success = false; // warn about unexpected message and ignore all non-scandata messages ROS_WARN_STREAM("## WARNING in SickScanCommon::loopOnce(): " << actual_length << " byte message ignored (" << DataDumper::binDataToAsciiString(&receiveBuffer[0], MIN(actual_length, 64)) << (actual_length>64?"...":"") << ")"); } else { - if (!parseCommonBinaryResultTelegram(receiveBuffer, actual_length, elevAngleX200, elevAngleTelegramValToDeg, elevationAngleInRad, recvTimeStamp, - config_.sw_pll_only_publish, config_.use_generation_timestamp, parser_, FireEncoder, EncoderMsg, numEchos, vang_vec, msg)) - { - dataToProcess = false; - break; - } - msg.header.stamp = recvTimeStamp + rosDurationFromSec(config_.time_offset); // recvTimeStamp updated by software-pll - timeIncrement = msg.time_increment; - echoMask = (1 << numEchos) - 1; + success = parseCommonBinaryResultTelegram(receiveBuffer, actual_length, elevAngleX200, elevAngleTelegramValToDeg, elevationAngleInRad, recvTimeStamp, + config_.sw_pll_only_publish, config_.use_generation_timestamp, parser_, FireEncoder, EncoderMsg, numEchos, vang_vec, msg); + if (!success) + ROS_ERROR_STREAM("## ERROR SickScanCommon::loopOnce(): parseCommonBinaryResultTelegram() failed"); } + if (!success) + { + dataToProcess = false; + break; + } + msg.header.stamp = recvTimeStamp + rosDurationFromSec(config_.time_offset); // recvTimeStamp updated by software-pll + timeIncrement = msg.time_increment; + echoMask = (1 << numEchos) - 1; } } @@ -4445,7 +4780,6 @@ namespace sick_scan << ", msg.ranges.size()=" << msg.ranges.size() << ", msg.intensities.size()=" << msg.intensities.size() << ")"); } - if (publishPointCloud == true && numValidEchos > 0 && msg.ranges.size() > 0) { @@ -4513,10 +4847,9 @@ namespace sick_scan sinAlphaTable.resize(rangeNum); float mirror_factor = 1.0; float angleShift=0; - if (this->parser_->getCurrentParamPtr()->getScanMirroredAndShifted()) + if (this->parser_->getCurrentParamPtr()->getScanMirroredAndShifted()) // i.e. NAV3xx-series { -/**/ mirror_factor = -1.0; -// angleShift = +M_PI/2.0; // add 90 deg for NAV3xx-series + mirror_factor = -1.0; } size_t rangeNumPointcloudAllEchos = 0; @@ -4605,6 +4938,7 @@ namespace sick_scan { phi_used = angleCompensator->compensateAngleInRadFromRos(phi_used); } + // Cartesian pointcloud float phi2_used = phi_used + m_add_transform_xyz_rpy.azimuthOffset(); fptr[idx_x] = rangeCos * (float)cos(phi2_used) * mirror_factor; // copy x value in pointcloud fptr[idx_y] = rangeCos * (float)sin(phi2_used) * mirror_factor; // copy y value in pointcloud @@ -4612,6 +4946,7 @@ namespace sick_scan m_add_transform_xyz_rpy.applyTransform(fptr[idx_x], fptr[idx_y], fptr[idx_z]); + // Polar pointcloud (sick_scan_xd API) fptr_polar[idx_x] = range_meter; // range in meter fptr_polar[idx_y] = phi_used; // azimuth in radians fptr_polar[idx_z] = alpha; // elevation in radians @@ -4937,6 +5272,15 @@ namespace sick_scan std::string KeyWord16 = "sWN LMDscandatascalefactor"; // LRS4xxx: "sWN LMDscandatascalefactor" + { 4 byte float }, e.g. scalefactor 1.0f = 0x3f800000, scalefactor 2.0f = 0x40000000 std::string KeyWord17 = "sWN GlareDetectionSens"; // LRS4xxx: "sWN GlareDetectionSens" + { 1 byte sensitivity } + { 2 byte 0x03 } std::string KeyWord18 = "sWN ScanLayerFilter"; // MRS-1000 scan layer activation mask, "sWN ScanLayerFilter … ", default: all layer activated: "sWN ScanLayerFilter 4 1 1 1 1" + std::string KeyWord19 = "sMN mNPOSGetData"; // NAV-350 poll data: "sMN mNPOSGetData 1 2" (sopas arguments: wait = 1, i.e. wait for next pose result), mask = 2, i.e. send pose+reflectors+scan) + std::string KeyWord20 = "sMN mNPOSSetPose"; // Set NAV-350 start pose in navigation mode by "sMN mNPOSSetPose X Y Phi" + std::string KeyWord21 = "sWN NEVACurrLayer"; + std::string KeyWord22 = "sWN NMAPMapCfg"; + std::string KeyWord23 = "sWN NLMDReflSize"; + std::string KeyWord24 = "sWN NPOSPoseDataFormat"; + std::string KeyWord25 = "sWN NLMDLandmarkDataFormat"; + std::string KeyWord26 = "sWN NAVScanDataFormat"; + std::string KeyWord27 = "sMN mNLAYEraseLayout"; //BBB @@ -5090,7 +5434,7 @@ namespace sick_scan buffer[bufferLen] = (std::stoul(hex_str, nullptr, 16) & 0xFF); } #else - if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) != 0) + if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_31X_NAME) != 0) { { bufferLen = 18; @@ -5280,6 +5624,94 @@ namespace sick_scan buffer[bufferLen++] = (unsigned char) (0xFF & (scan_filter_cfg.scan_layer_activated[n])); // 1 byte } } + if (cmdAscii.find(KeyWord19) != std::string::npos && strlen(requestAscii) > KeyWord19.length() + 1)// NAV-350 poll data: "sMN mNPOSGetData 1 2" (sopas arguments: wait = 1, i.e. wait for next pose result), mask = 2, i.e. send pose+reflectors+scan) + { + // Convert ascii integer args to binary, e.g. "1 2" to 0x0102 + int args[2] = { 0, 0 }; + sscanf(requestAscii + KeyWord19.length() + 1, " %d %d", &(args[0]), &(args[1])); + buffer[0] = (unsigned char) (0xFF & args[0]); + buffer[1] = (unsigned char) (0xFF & args[1]); + bufferLen = 2; + } + if (cmdAscii.find(KeyWord20) != std::string::npos && strlen(requestAscii) > KeyWord20.length() + 1) // Set NAV-350 start pose in navigation mode by "sMN mNPOSSetPose X Y Phi", 3 arguments, each int32_t + { + int32_t args[3] = { 0, 0, 0 }; + sscanf(requestAscii + KeyWord20.length() + 1, " %d %d %d", &(args[0]), &(args[1]), &(args[2])); + bufferLen = 0; + for(int arg_cnt = 0; arg_cnt < 3; arg_cnt++) + { + buffer[bufferLen + 0] = (unsigned char) (0xFF & (args[arg_cnt] >> 24)); + buffer[bufferLen + 1] = (unsigned char) (0xFF & (args[arg_cnt] >> 16)); + buffer[bufferLen + 2] = (unsigned char) (0xFF & (args[arg_cnt] >> 8)); + buffer[bufferLen + 3] = (unsigned char) (0xFF & (args[arg_cnt] >> 0)); + bufferLen += 4; + } + } + if (cmdAscii.find(KeyWord21) != std::string::npos && strlen(requestAscii) > KeyWord21.length() + 1) // "sWN NEVACurrLayer 0" + { + int args[1] = { 0 }; + sscanf(requestAscii + KeyWord21.length() + 1, " %d", &(args[0])); + buffer[0] = (unsigned char) (0xFF & (args[0] >> 8)); + buffer[1] = (unsigned char) (0xFF & (args[0] >> 0)); + bufferLen = 2; + } + if (cmdAscii.find(KeyWord22) != std::string::npos && strlen(requestAscii) > KeyWord22.length() + 1) // "sWN NMAPMapCfg 50 0 0 0 0"; + { + int args[5] = { 0, 0, 0, 0, 0 }; + sscanf(requestAscii + KeyWord22.length() + 1, " %d %d %d %d %d", &(args[0]), &(args[1]), &(args[2]), &(args[3]), &(args[4])); + buffer[0] = (unsigned char) (0xFF & args[0]); + buffer[1] = (unsigned char) (0xFF & args[1]); + bufferLen = 2; + for(int arg_cnt = 2; arg_cnt < 5; arg_cnt++) + { + buffer[bufferLen + 0] = (unsigned char) (0xFF & (args[arg_cnt] >> 24)); + buffer[bufferLen + 1] = (unsigned char) (0xFF & (args[arg_cnt] >> 16)); + buffer[bufferLen + 2] = (unsigned char) (0xFF & (args[arg_cnt] >> 8)); + buffer[bufferLen + 3] = (unsigned char) (0xFF & (args[arg_cnt] >> 0)); + bufferLen += 4; + } + } + if (cmdAscii.find(KeyWord23) != std::string::npos && strlen(requestAscii) > KeyWord23.length() + 1) // "sWN NLMDReflSize 80"; + { + int args[1] = { 0 }; + sscanf(requestAscii + KeyWord23.length() + 1, " %d", &(args[0])); + buffer[0] = (unsigned char) (0xFF & (args[0] >> 8)); + buffer[1] = (unsigned char) (0xFF & (args[0] >> 0)); + bufferLen = 2; + } + if (cmdAscii.find(KeyWord24) != std::string::npos && strlen(requestAscii) > KeyWord24.length() + 1) // "NPOSPoseDataFormat 1 1"; + { + int args[2] = { 0, 0 }; + sscanf(requestAscii + KeyWord24.length() + 1, " %d %d", &(args[0]), &(args[1])); + buffer[0] = (unsigned char) (0xFF & (args[0])); + buffer[1] = (unsigned char) (0xFF & (args[1])); + bufferLen = 2; + } + + if (cmdAscii.find(KeyWord25) != std::string::npos && strlen(requestAscii) > KeyWord25.length() + 1) // "sWN NLMDLandmarkDataFormat 0 1 1" + { + int args[3] = { 0, 0, 0 }; + sscanf(requestAscii + KeyWord25.length() + 1, " %d %d %d", &(args[0]), &(args[1]), &(args[2])); + buffer[0] = (unsigned char) (0xFF & (args[0])); + buffer[1] = (unsigned char) (0xFF & (args[1])); + buffer[2] = (unsigned char) (0xFF & (args[2])); + bufferLen = 3; + } + if (cmdAscii.find(KeyWord26) != std::string::npos && strlen(requestAscii) > KeyWord26.length() + 1) // "sWN NAVScanDataFormat 1 1" + { + int args[2] = { 0, 0 }; + sscanf(requestAscii + KeyWord26.length() + 1, " %d %d", &(args[0]), &(args[1])); + buffer[0] = (unsigned char) (0xFF & (args[0])); + buffer[1] = (unsigned char) (0xFF & (args[1])); + bufferLen = 2; + } + if (cmdAscii.find(KeyWord27) != std::string::npos && strlen(requestAscii) > KeyWord27.length() + 1) // "sMN mNLAYEraseLayout 1" + { + int args[1] = { 0 }; + sscanf(requestAscii + KeyWord27.length() + 1, " %d", &(args[0])); + buffer[0] = (unsigned char) (0xFF & (args[0])); + bufferLen = 1; + } // copy base command string to buffer bool switchDoBinaryData = false; @@ -5318,28 +5750,34 @@ namespace sick_scan requestBinary->push_back(buffer[i]); } - msgLen = requestBinary->size(); - msgLen -= 8; - for (int i = 0; i < 4; i++) - { - unsigned char bigEndianLen = msgLen & (0xFF << (3 - i) * 8); - (*requestBinary)[i + 4] = ((unsigned char) (bigEndianLen)); // HIER WEITERMACHEN!!!! - } - unsigned char xorVal = 0x00; - xorVal = sick_crc8((unsigned char *) (&((*requestBinary)[8])), requestBinary->size() - 8); - requestBinary->push_back(xorVal); -#if 0 - for (int i = 0; i < requestBinary->size(); i++) - { - unsigned char c = (*requestBinary)[i]; - printf("[%c]%02x ", (c < ' ') ? '.' : c, c) ; - } - printf("\n"); -#endif + setLengthAndCRCinBinarySopasRequest(requestBinary); + return (0); }; + void SickScanCommon::setLengthAndCRCinBinarySopasRequest(std::vector* requestBinary) + { + + int msgLen = (int)requestBinary->size(); + msgLen -= 8; + for (int i = 0; i < 4; i++) + { + unsigned char bigEndianLen = msgLen & (0xFF << (3 - i) * 8); + (*requestBinary)[i + 4] = ((unsigned char) (bigEndianLen)); // HIER WEITERMACHEN!!!! + } + unsigned char xorVal = 0x00; + xorVal = sick_crc8((unsigned char *) (&((*requestBinary)[8])), requestBinary->size() - 8); + requestBinary->push_back(xorVal); +#if 0 + for (int i = 0; i < requestBinary->size(); i++) + { + unsigned char c = (*requestBinary)[i]; + printf("[%c]%02x ", (c < ' ') ? '.' : c, c) ; + } + printf("\n"); +#endif + } /*! \brief checks the current protocol type and gives information about necessary change diff --git a/driver/src/sick_scan_common_tcp.cpp b/driver/src/sick_scan_common_tcp.cpp index 11069113..763a91dd 100644 --- a/driver/src/sick_scan_common_tcp.cpp +++ b/driver/src/sick_scan_common_tcp.cpp @@ -128,7 +128,7 @@ namespace sick_scan answerList.push_back("sRA SCdevicestate 1"); keyWordList.push_back("sRN DItype"); - answerList.push_back("sRA DItype D RMS3xx-xxxxxx"); + answerList.push_back("sRA DItype F RMSxxxxx.xxxxxx"); keyWordList.push_back("sRN ODoprh"); answerList.push_back("sRA ODoprh 451"); diff --git a/driver/src/sick_scan_services.cpp b/driver/src/sick_scan_services.cpp index 2d3be280..aa4d32d7 100644 --- a/driver/src/sick_scan_services.cpp +++ b/driver/src/sick_scan_services.cpp @@ -349,7 +349,7 @@ bool sick_scan::SickScanServices::sendSopasCmdCheckResponse(const std::string& s /*! * Sends the multiScan136 start commands "sWN ScanDataFormat", "sWN ScanDataPreformatting", "sWN ScanDataEthSettings", "sWN ScanDataEnable 1", "sMN LMCstartmeas", "sMN Run" */ -bool sick_scan::SickScanServices::sendMRS100StartCmd(const std::string& hostname, int port) +bool sick_scan::SickScanServices::sendMultiScanStartCmd(const std::string& hostname, int port, const std::string& scanner_type) { std::stringstream ip_stream(hostname); std::string ip_token; @@ -360,7 +360,7 @@ bool sick_scan::SickScanServices::sendMRS100StartCmd(const std::string& hostname } if (ip_tokens.size() != 4) { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd() failed: can't split ip address \"" << hostname << "\" into 4 tokens, check ip address"); + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd() failed: can't split ip address \"" << hostname << "\" into 4 tokens, check ip address"); ROS_ERROR_STREAM("## ERROR parsing ip address, check configuration of parameter \"hostname\" (launch file or commandline)."); ROS_ERROR_STREAM("## In case of multiscan/sick_scansegment_xd lidars, check parameter \"udp_receiver_ip\", too."); return false; @@ -376,30 +376,32 @@ bool sick_scan::SickScanServices::sendMRS100StartCmd(const std::string& hostname eth_settings_cmd << port; if (!sendSopasCmdCheckResponse(eth_settings_cmd.str(), "sWA ScanDataEthSettings")) // configure destination scan data output destination , f.e. "sWN ScanDataEthSettings 1 +192 +168 +0 +52 +2115" (ip 192.168.0.52 port 2115) { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEthSettings 1\") failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEthSettings 1\") failed."); return false; } - - // if (!sendSopasCmdCheckResponse("sWN ScanDataFormatSettings 1 1", "sWA ScanDataFormatSettings")) // set scan data output format to MSGPACK - if (!sendSopasCmdCheckResponse("sWN ScanDataFormat 1", "sWA ScanDataFormat") // set scan data output format to MSGPACK - || !sendSopasCmdCheckResponse("sWN ScanDataPreformatting 1", "sWA ScanDataPreformatting")) + if (!sendSopasCmdCheckResponse("sWN ScanDataFormat 1", "sWA ScanDataFormat")) // set scan data output format to MSGPACK + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiscanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataFormat 1\") failed."); + return false; + } + if (scanner_type == SICK_SCANNER_SCANSEGMENT_XD_NAME && !sendSopasCmdCheckResponse("sWN ScanDataPreformatting 1", "sWA ScanDataPreformatting")) // ScanDataPreformatting for multiScan136 only { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataFormat 1\") and/or sendSopasCmdCheckResponse(\"sWN ScanDataPreformatting 1\") failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiscanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataPreformatting 1\") failed."); return false; } if (!sendSopasCmdCheckResponse("sWN ScanDataEnable 1", "sWA ScanDataEnable")) // enable scan data output { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEnable 1\") failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEnable 1\") failed."); return false; } if (!sendSopasCmdCheckResponse("sMN LMCstartmeas", "sAN LMCstartmeas")) // start measurement { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd(): sendSopasCmdCheckResponse(\"sMN LMCstartmeas\") failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sMN LMCstartmeas\") failed."); return false; } if (!sendSopasCmdCheckResponse("sMN Run", "sAN Run")) // apply the settings { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd(): sendSopasCmdCheckResponse(\"sMN Run\") failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sMN Run\") failed."); return false; } return true; @@ -410,16 +412,16 @@ bool sick_scan::SickScanServices::sendMRS100StartCmd(const std::string& hostname /*! * Sends the multiScan136 stop commands "sWN ScanDataEnable 0" and "sMN Run" */ -bool sick_scan::SickScanServices::sendMRS100StopCmd(void) +bool sick_scan::SickScanServices::sendMultiScanStopCmd(void) { if (!sendSopasCmdCheckResponse("sWN ScanDataEnable 0", "sWA ScanDataEnable")) // disble scan data output { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StopCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEnable 0\") failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStopCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEnable 0\") failed."); return false; } if (!sendSopasCmdCheckResponse("sMN Run", "sAN Run")) // apply the settings { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StopCmd(): sendSopasCmdCheckResponse(\"sMN Run\") failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStopCmd(): sendSopasCmdCheckResponse(\"sMN Run\") failed."); return false; } return true; @@ -497,13 +499,21 @@ std::string sick_scan::SickScanServices::convertFloatToHexString(float value, bo * @param[out] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise " ... " with 1 for enabled and 0 for disabled * @param[out] msgpack_validator_filter_settings; // filter settings for msgpack validator: required_echos, azimuth_start, azimuth_end. elevation_start, elevation_end, layer_filter */ -bool sick_scan::SickScanServices::queryMRS100Filtersettings(int& host_FREchoFilter, std::string& host_LFPangleRangeFilter, std::string& host_LFPlayerFilter, sick_scansegment_xd::MsgpackValidatorFilterConfig& msgpack_validator_filter_settings) +bool sick_scan::SickScanServices::queryMultiScanFiltersettings(int& host_FREchoFilter, std::string& host_LFPangleRangeFilter, std::string& host_LFPlayerFilter, + sick_scansegment_xd::MsgpackValidatorFilterConfig& msgpack_validator_filter_settings, const std::string& scanner_type) { std::vector> sopasRepliesBin; std::vector sopasRepliesString; // Query FREchoFilter, LFPangleRangeFilter and LFPlayerFilter settings - std::vector sopasCommands = { "FREchoFilter", "LFPangleRangeFilter", "LFPlayerFilter" }; + bool enableFREchoFilter = true, enableLFPangleRangeFilter = true, enableLFPlayerFilter = true; + std::vector sopasCommands; + if (enableFREchoFilter) + sopasCommands.push_back("FREchoFilter"); + if (enableLFPangleRangeFilter) + sopasCommands.push_back("LFPangleRangeFilter"); + if (enableLFPlayerFilter) + sopasCommands.push_back("LFPlayerFilter"); for(int n = 0; n < sopasCommands.size(); n++) { std::string sopasRequest = "sRN " + sopasCommands[n]; @@ -512,10 +522,10 @@ bool sick_scan::SickScanServices::queryMRS100Filtersettings(int& host_FREchoFilt std::string sopasReplyString; if(!sendSopasAndCheckAnswer(sopasRequest, sopasReplyBin, sopasReplyString) || sopasReplyString.find(sopasExpectedResponse) == std::string::npos) { - ROS_ERROR_STREAM("## ERROR SickScanServices::queryMRS100Filtersettings(): sendSopasAndCheckAnswer(\"" << sopasRequest << "\") failed or unexpected response: \"" << sopasReplyString << "\", expected: \"" << sopasExpectedResponse << "\""); + ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): sendSopasAndCheckAnswer(\"" << sopasRequest << "\") failed or unexpected response: \"" << sopasReplyString << "\", expected: \"" << sopasExpectedResponse << "\""); return false; } - ROS_DEBUG_STREAM("SickScanServices::queryMRS100Filtersettings(): request: \"" << sopasRequest << "\", response: \"" << sopasReplyString << "\""); + ROS_DEBUG_STREAM("SickScanServices::queryMultiScanFiltersettings(): request: \"" << sopasRequest << "\", response: \"" << sopasReplyString << "\""); sopasRepliesBin.push_back(sopasReplyBin); sopasRepliesString.push_back(sopasReplyString); } @@ -528,63 +538,75 @@ bool sick_scan::SickScanServices::queryMRS100Filtersettings(int& host_FREchoFilt std::vector parameterToken; sick_scansegment_xd::util::parseVector(parameterString, parameterToken, ' '); sopasTokens.push_back(parameterToken); - ROS_INFO_STREAM("SickScanServices::queryMRS100Filtersettings(): " << sopasCommands[n] << ": \"" << parameterString << "\" = {" << sick_scansegment_xd::util::printVector(parameterToken, ",") << "}"); + ROS_INFO_STREAM("SickScanServices::queryMultiScanFiltersettings(): " << sopasCommands[n] << ": \"" << parameterString << "\" = {" << sick_scansegment_xd::util::printVector(parameterToken, ",") << "}"); } - // Parse FREchoFilter - if(sopasCommands.size() > 0 && sopasTokens[0].size() == 1) - { - host_FREchoFilter = std::stoi(sopasTokens[0][0]); - } - else + std::vector multiscan_angles_deg; + std::vector layer_active_vector; + for(int sopasCommandCnt = 0; sopasCommandCnt < sopasCommands.size(); sopasCommandCnt++) { - ROS_ERROR_STREAM("## ERROR SickScanServices::queryMRS100Filtersettings(): parse error in FREchoFilter"); - return false; - } + const std::string& sopasCommand = sopasCommands[sopasCommandCnt]; + const std::vector& sopasToken = sopasTokens[sopasCommandCnt]; - // Parse LFPangleRangeFilter - std::vector mrs100_angles_deg; - if(sopasCommands.size() > 1 && sopasTokens[1].size() == 6) - { - std::stringstream parameter; - int filter_enabled = std::stoi(sopasTokens[1][0]); // - parameter << filter_enabled; - for(int n = 1; n < 5; n++) // + if (sopasCommand == "FREchoFilter") // Parse FREchoFilter { - float angle_deg = (convertHexStringToFloat(sopasTokens[1][n], SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN) * 180.0 / M_PI); - parameter << " " << angle_deg; - if(filter_enabled) - mrs100_angles_deg.push_back(angle_deg); + if(sopasToken.size() == 1) + { + host_FREchoFilter = std::stoi(sopasToken[0]); + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): parse error in FREchoFilter"); + return false; + } } - parameter << " " << sopasTokens[1][5]; // - host_LFPangleRangeFilter = parameter.str(); - } - else - { - ROS_ERROR_STREAM("## ERROR SickScanServices::queryMRS100Filtersettings(): parse error in LFPangleRangeFilter"); - return false; - } - // Parse LFPlayerFilter - std::vector layer_active_vector; - if(sopasCommands.size() > 2 && sopasTokens[2].size() == 17) - { - std::stringstream parameter; - int filter_enabled = std::stoi(sopasTokens[2][0]); // - parameter << filter_enabled; - for(int n = 1; n < sopasTokens[2].size(); n++) + if (sopasCommand == "LFPangleRangeFilter") // Parse LFPangleRangeFilter { - int layer_active = std::stoi(sopasTokens[2][n]); - if(filter_enabled) - layer_active_vector.push_back(layer_active); - parameter << " " << layer_active; + if(sopasToken.size() == 6) + { + std::stringstream parameter; + int filter_enabled = std::stoi(sopasToken[0]); // + parameter << filter_enabled; + for(int n = 1; n < 5; n++) // + { + float angle_deg = (convertHexStringToFloat(sopasToken[n], SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN) * 180.0 / M_PI); + parameter << " " << angle_deg; + if(filter_enabled) + multiscan_angles_deg.push_back(angle_deg); + } + parameter << " " << sopasToken[5]; // + host_LFPangleRangeFilter = parameter.str(); + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): parse error in LFPangleRangeFilter"); + return false; + } + } + + if (sopasCommand == "LFPlayerFilter") // Parse LFPlayerFilter + { + if(sopasToken.size() == 17) + { + std::stringstream parameter; + int filter_enabled = std::stoi(sopasToken[0]); // + parameter << filter_enabled; + for(int n = 1; n < sopasToken.size(); n++) + { + int layer_active = std::stoi(sopasToken[n]); + if(filter_enabled) + layer_active_vector.push_back(layer_active); + parameter << " " << layer_active; + } + host_LFPlayerFilter = parameter.str(); + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): parse error in LFPlayerFilter"); + return false; + } } - host_LFPlayerFilter = parameter.str(); - } - else - { - ROS_ERROR_STREAM("## ERROR SickScanServices::queryMRS100Filtersettings(): parse error in LFPlayerFilter"); - return false; } // Set filter settings for validation of msgpack data, i.e. set config.msgpack_validator_required_echos, config.msgpack_validator_azimuth_start, config.msgpack_validator_azimuth_end, @@ -599,16 +621,16 @@ bool sick_scan::SickScanServices::queryMRS100Filtersettings(int& host_FREchoFilt } else { - ROS_ERROR_STREAM("## ERROR SickScanServices::queryMRS100Filtersettings(): unexpected value of FREchoFilter = " << host_FREchoFilter + ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): unexpected value of FREchoFilter = " << host_FREchoFilter << ", expected 0: FIRST_ECHO (EchoCount=1), 1: ALL_ECHOS (EchoCount=3) or 2: LAST_ECHO (EchoCount=1)"); return false; } - if(mrs100_angles_deg.size() == 4) // otherwise LFPangleRangeFilter disabled (-> use configured default values) + if(multiscan_angles_deg.size() == 4) // otherwise LFPangleRangeFilter disabled (-> use configured default values) { - msgpack_validator_filter_settings.msgpack_validator_azimuth_start = (mrs100_angles_deg[0] * M_PI / 180); - msgpack_validator_filter_settings.msgpack_validator_azimuth_end = (mrs100_angles_deg[1] * M_PI / 180); - msgpack_validator_filter_settings.msgpack_validator_elevation_start = (mrs100_angles_deg[2] * M_PI / 180); - msgpack_validator_filter_settings.msgpack_validator_elevation_end = (mrs100_angles_deg[3] * M_PI / 180); + msgpack_validator_filter_settings.msgpack_validator_azimuth_start = (multiscan_angles_deg[0] * M_PI / 180); + msgpack_validator_filter_settings.msgpack_validator_azimuth_end = (multiscan_angles_deg[1] * M_PI / 180); + msgpack_validator_filter_settings.msgpack_validator_elevation_start = (multiscan_angles_deg[2] * M_PI / 180); + msgpack_validator_filter_settings.msgpack_validator_elevation_end = (multiscan_angles_deg[3] * M_PI / 180); } if(layer_active_vector.size() == 16) // otherwise LFPlayerFilter disabled (-> use configured default values) { @@ -617,10 +639,10 @@ bool sick_scan::SickScanServices::queryMRS100Filtersettings(int& host_FREchoFilt // Example: sopas.FREchoFilter = "1", sopas.LFPangleRangeFilter = "0 -180 180 -90.0002 90.0002 1", sopas.LFPlayerFilter = "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" // msgpack_validator_required_echos = { 0 }, msgpack_validator_angles = { -3.14159 3.14159 -1.5708 1.5708 } [rad], msgpack_validator_layer_filter = { 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 } - ROS_INFO_STREAM("SickScanServices::queryMRS100Filtersettings(): sopas.FREchoFilter = \"" << host_FREchoFilter + ROS_INFO_STREAM("SickScanServices::queryMultiScanFiltersettings(): sopas.FREchoFilter = \"" << host_FREchoFilter << "\", sopas.LFPangleRangeFilter = \"" << host_LFPangleRangeFilter << "\", sopas.LFPlayerFilter = \"" << host_LFPlayerFilter << "\""); - ROS_INFO_STREAM("SickScanServices::queryMRS100Filtersettings(): msgpack_validator_required_echos = { " << sick_scansegment_xd::util::printVector(msgpack_validator_filter_settings.msgpack_validator_required_echos) + ROS_INFO_STREAM("SickScanServices::queryMultiScanFiltersettings(): msgpack_validator_required_echos = { " << sick_scansegment_xd::util::printVector(msgpack_validator_filter_settings.msgpack_validator_required_echos) << " }, msgpack_validator_angles = { " << msgpack_validator_filter_settings.msgpack_validator_azimuth_start << " " << msgpack_validator_filter_settings.msgpack_validator_azimuth_end << " " << msgpack_validator_filter_settings.msgpack_validator_elevation_start << " " << msgpack_validator_filter_settings.msgpack_validator_elevation_end << " } [rad], msgpack_validator_layer_filter = { " << sick_scansegment_xd::util::printVector(msgpack_validator_filter_settings.msgpack_validator_layer_filter) << " }"); @@ -635,29 +657,32 @@ bool sick_scan::SickScanServices::queryMRS100Filtersettings(int& host_FREchoFilt * @param[in] host_LFPangleRangeFilter LFPangleRangeFilter settings, default: "0 -180.0 +180.0 -90.0 +90.0 1", otherwise " " with azimuth and elevation given in degree * @param[in] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise " ... " with 1 for enabled and 0 for disabled */ -bool sick_scan::SickScanServices::writeMRS100Filtersettings(int host_FREchoFilter, const std::string& host_LFPangleRangeFilter, const std::string& host_LFPlayerFilter) +bool sick_scan::SickScanServices::writeMultiScanFiltersettings(int host_FREchoFilter, const std::string& host_LFPangleRangeFilter, const std::string& host_LFPlayerFilter, const std::string& scanner_type) + { + bool enableFREchoFilter = true, enableLFPangleRangeFilter = true, enableLFPlayerFilter = true; + // Write FREchoFilter - if(host_FREchoFilter >= 0) // otherwise not configured + if(enableFREchoFilter && host_FREchoFilter >= 0) // otherwise not configured or supported { std::string sopasRequest = "sWN FREchoFilter " + std::to_string(host_FREchoFilter), sopasExpectedResponse = "sWA FREchoFilter"; if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse)) { - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); return false; } } // Write LFPangleRangeFilter - if(!host_LFPangleRangeFilter.empty()) // otherwise not configured + if(enableLFPangleRangeFilter && !host_LFPangleRangeFilter.empty()) // otherwise not configured or supported { // convert deg to rad and float to hex std::vector parameter_token; sick_scansegment_xd::util::parseVector(host_LFPangleRangeFilter, parameter_token, ' '); if(parameter_token.size() != 6) { - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings(): can't split host_LFPangleRangeFilter = \"" << host_LFPangleRangeFilter << "\", expected 6 values separated by space"); - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings() failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): can't split host_LFPangleRangeFilter = \"" << host_LFPangleRangeFilter << "\", expected 6 values separated by space"); + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings() failed."); return false; } int filter_enabled = std::stoi(parameter_token[0]); // @@ -674,18 +699,18 @@ bool sick_scan::SickScanServices::writeMRS100Filtersettings(int host_FREchoFilte std::string sopasRequest = "sWN LFPangleRangeFilter " + sopas_parameter.str(), sopasExpectedResponse = "sWA LFPangleRangeFilter"; if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse)) { - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); return false; } } // Write LFPlayerFilter - if(!host_LFPlayerFilter.empty()) // otherwise not configured + if(enableLFPlayerFilter && !host_LFPlayerFilter.empty()) // otherwise not configured or supported { std::string sopasRequest = "sWN LFPlayerFilter " + host_LFPlayerFilter, sopasExpectedResponse = "sWA LFPlayerFilter"; if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse)) { - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); return false; } } @@ -693,7 +718,7 @@ bool sick_scan::SickScanServices::writeMRS100Filtersettings(int host_FREchoFilte // Apply the settings if (!sendSopasCmdCheckResponse("sMN Run", "sAN Run")) { - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings(): sendSopasCmdCheckResponse(\"sMN Run\") failed."); + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"sMN Run\") failed."); return false; } return true; diff --git a/driver/src/sick_scan_xd_api/api_impl.cpp b/driver/src/sick_scan_xd_api/api_impl.cpp index 0fb22f48..42dd6d0a 100644 --- a/driver/src/sick_scan_xd_api/api_impl.cpp +++ b/driver/src/sick_scan_xd_api/api_impl.cpp @@ -6,6 +6,7 @@ #include #include +#include "softwarePLL.h" #include "sick_scan_api.h" #include "sick_scan_api_dump.h" #include "sick_scan/sick_generic_laser.h" @@ -25,6 +26,7 @@ static sick_scan::SickCallbackHandler s_callback_handler_radarscan_messages; static sick_scan::SickCallbackHandler s_callback_handler_ldmrsobjectarray_messages; static sick_scan::SickCallbackHandler s_callback_handler_visualizationmarker_messages; +static sick_scan::SickCallbackHandler s_callback_handler_navposelandmark_messages; #if __ROS_VERSION == 2 // workaround for missing imu quaternion operator << in ROS2 # define ROS_VECTOR3D_TO_STREAM(msg) ((msg).x) << "," << ((msg).y) << "," << ((msg).z) @@ -681,6 +683,7 @@ int32_t SickScanApiRelease(SickScanApiHandle apiHandle) s_callback_handler_radarscan_messages.clear(); s_callback_handler_ldmrsobjectarray_messages.clear(); s_callback_handler_visualizationmarker_messages.clear(); + s_callback_handler_navposelandmark_messages.clear(); for(int n = 0; n < s_malloced_resources.size(); n++) free(s_malloced_resources[n]); s_malloced_resources.clear(); @@ -1608,3 +1611,186 @@ int32_t SickScanApiFreeVisualizationMarkerMsg(SickScanApiHandle apiHandle, SickS } return SICK_SCAN_API_NOT_INITIALIZED; } + +/* +** NAV-350 support and messages +*/ + +static SickScanNavPoseLandmarkMsg convertNAV350mNPOSData(const sick_scan::NAV350mNPOSData& src_msg) +{ + SickScanNavPoseLandmarkMsg dst_msg; + memset(&dst_msg, 0, sizeof(dst_msg)); + dst_msg.pose_valid = src_msg.poseDataValid; + dst_msg.pose_nav_x = src_msg.poseData.x; + dst_msg.pose_nav_y = src_msg.poseData.y; + dst_msg.pose_nav_phi = src_msg.poseData.phi; + dst_msg.pose_opt_valid = src_msg.poseData.optPoseDataValid; + dst_msg.pose_opt_output_mode = src_msg.poseData.optPoseData.outputMode; + dst_msg.pose_opt_timestamp = src_msg.poseData.optPoseData.timestamp; + dst_msg.pose_opt_mean_dev = src_msg.poseData.optPoseData.meanDev; + dst_msg.pose_opt_nav_mode = src_msg.poseData.optPoseData.navMode; + dst_msg.pose_opt_info_state = src_msg.poseData.optPoseData.infoState; + dst_msg.pose_opt_quant_used_reflectors = src_msg.poseData.optPoseData.quantUsedReflectors; + if (dst_msg.pose_valid > 0) + sick_scan::convertNAVCartPos3DtoROSPos3D(dst_msg.pose_nav_x, dst_msg.pose_nav_y, dst_msg.pose_nav_phi, dst_msg.pose_x, dst_msg.pose_y, dst_msg.pose_yaw, src_msg.angleOffset); + if (dst_msg.pose_opt_valid > 0 && SoftwarePLL::instance().IsInitialized()) + SoftwarePLL::instance().getCorrectedTimeStamp(dst_msg.pose_timestamp_sec, dst_msg.pose_timestamp_nsec, dst_msg.pose_opt_timestamp); + + if (src_msg.landmarkDataValid && src_msg.landmarkData.reflectors.size() > 0) + { + dst_msg.reflectors.buffer = (SickScanNavReflector*)malloc(src_msg.landmarkData.reflectors.size() * sizeof(SickScanNavReflector)); + if (dst_msg.reflectors.buffer) + { + dst_msg.reflectors.size = src_msg.landmarkData.reflectors.size(); + dst_msg.reflectors.capacity = src_msg.landmarkData.reflectors.size(); + for(int reflector_cnt = 0; reflector_cnt < src_msg.landmarkData.reflectors.size(); reflector_cnt++) + { + const sick_scan::NAV350ReflectorData* src_reflector = &src_msg.landmarkData.reflectors[reflector_cnt]; + SickScanNavReflector* dst_reflector = &dst_msg.reflectors.buffer[reflector_cnt]; + dst_reflector->cartesian_valid = src_reflector->cartesianDataValid; + dst_reflector->cartesian_x = src_reflector->cartesianData.x; + dst_reflector->cartesian_y = src_reflector->cartesianData.y; + dst_reflector->polar_valid = src_reflector->polarDataValid; + dst_reflector->polar_dist = src_reflector->polarData.dist; + dst_reflector->polar_phi = src_reflector->polarData.phi; + dst_reflector->opt_valid = src_reflector->optReflectorDataValid; + dst_reflector->opt_local_id = src_reflector->optReflectorData.localID; + dst_reflector->opt_global_id = src_reflector->optReflectorData.globalID; + dst_reflector->opt_type = src_reflector->optReflectorData.type; + dst_reflector->opt_subtype = src_reflector->optReflectorData.subType; + dst_reflector->opt_quality = src_reflector->optReflectorData.quality; + dst_reflector->opt_timestamp = src_reflector->optReflectorData.timestamp; + dst_reflector->opt_size = src_reflector->optReflectorData.size; + dst_reflector->opt_hitcount = src_reflector->optReflectorData.hitCount; + dst_reflector->opt_meanecho = src_reflector->optReflectorData.meanEcho; + dst_reflector->opt_startindex = src_reflector->optReflectorData.startIndex; + dst_reflector->opt_endindex = src_reflector->optReflectorData.endIndex; + dst_reflector->pos_valid = src_reflector->cartesianDataValid; + if (src_reflector->cartesianDataValid) + sick_scan::convertNAVCartPos2DtoROSPos2D(src_reflector->cartesianData.x, src_reflector->cartesianData.y, dst_reflector->pos_x, dst_reflector->pos_y, src_msg.angleOffset); + if (src_reflector->optReflectorDataValid > 0 && SoftwarePLL::instance().IsInitialized()) + SoftwarePLL::instance().getCorrectedTimeStamp(dst_reflector->opt_timestamp_sec, dst_reflector->opt_timestamp_nsec, src_reflector->optReflectorData.timestamp); + } + } + } + return dst_msg; +} +static void freeNavPoseLandmarkMsg(SickScanNavPoseLandmarkMsg& msg) +{ + free(msg.reflectors.buffer); + memset(&msg, 0, sizeof(msg)); +} +static void nav_pose_landmark_callback(rosNodePtr node, const sick_scan::NAV350mNPOSData* msg) +{ + ROS_DEBUG_STREAM("api_impl nav_pose_landmark_callback: NAV350mNPOSData message"); + SickScanNavPoseLandmarkMsg export_msg = convertNAV350mNPOSData(*msg); + SickScanApiHandle apiHandle = castNodeToApiHandle(node); + s_callback_handler_navposelandmark_messages.notifyListener(apiHandle, &export_msg); + freeNavPoseLandmarkMsg(export_msg); +} +int32_t SickScanApiRegisterNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkCallback callback) +{ + try + { + if (apiHandle == 0) + { + ROS_ERROR_STREAM("## ERROR SickScanApiRegisterNavPoseLandmarkMsg(): invalid apiHandle"); + return SICK_SCAN_API_NOT_INITIALIZED; + } + s_callback_handler_navposelandmark_messages.addListener(apiHandle, callback); + rosNodePtr node = castApiHandleToNode(apiHandle); + sick_scan::addNavPoseLandmarkListener(node, nav_pose_landmark_callback); + return SICK_SCAN_API_SUCCESS; + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("## ERROR SickScanApiRegisterNavPoseLandmarkMsg(): exception " << e.what()); + } + catch(...) + { + ROS_ERROR_STREAM("## ERROR SickScanApiRegisterNavPoseLandmarkMsg(): unknown exception "); + } + return SICK_SCAN_API_ERROR; +} +int32_t SickScanApiDeregisterNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkCallback callback) +{ + try + { + if (apiHandle == 0) + { + ROS_ERROR_STREAM("## ERROR SickScanApiDeregisterNavPoseLandmarkMsg(): invalid apiHandle"); + return SICK_SCAN_API_NOT_INITIALIZED; + } + s_callback_handler_navposelandmark_messages.removeListener(apiHandle, callback); + rosNodePtr node = castApiHandleToNode(apiHandle); + sick_scan::removeNavPoseLandmarkListener(node, nav_pose_landmark_callback); + return SICK_SCAN_API_SUCCESS; + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("## ERROR SickScanApiDeregisterNavPoseLandmarkMsg(): exception " << e.what()); + } + catch(...) + { + ROS_ERROR_STREAM("## ERROR SickScanApiDeregisterNavPoseLandmarkMsg(): unknown exception "); + } + return SICK_SCAN_API_ERROR; +} +int32_t SickScanApiWaitNextNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkMsg* msg, double timeout_sec) +{ + int32_t ret_val = SICK_SCAN_API_ERROR; + try + { + if (apiHandle == 0) + { + ROS_ERROR_STREAM("## ERROR SickScanApiWaitNextNavPoseLandmarkMsg(): invalid apiHandle"); + return SICK_SCAN_API_NOT_INITIALIZED; + } + rosNodePtr node = castApiHandleToNode(apiHandle); + if (!sick_scan::isNavPoseLandmarkListenerRegistered(node, sick_scan::WaitForNAVPOSDataMessageHandler::messageCallback)) + sick_scan::addNavPoseLandmarkListener(node, sick_scan::WaitForNAVPOSDataMessageHandler::messageCallback); + sick_scan::WaitForNAVPOSDataMessageHandler wait_message_handler; + sick_scan::WaitForNAVPOSDataMessageHandler::addWaitForMessageHandlerHandler(&wait_message_handler); + sick_scan::NAV350mNPOSData navdata_msg; + if (wait_message_handler.waitForNextMessage(navdata_msg, timeout_sec) && (navdata_msg.poseDataValid > 0 || navdata_msg.landmarkDataValid > 0)) + { + ROS_INFO_STREAM("SickScanApiWaitNextNavPoseLandmarkMsg: NAV350mNPOSData message"); + *msg = convertNAV350mNPOSData(navdata_msg); + ret_val = SICK_SCAN_API_SUCCESS; + } + else + { + ret_val = SICK_SCAN_API_TIMEOUT; + } + sick_scan::WaitForNAVPOSDataMessageHandler::removeWaitForMessageHandlerHandler(&wait_message_handler); + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("## ERROR SickScanApiWaitNextNavPoseLandmarkMsg(): exception " << e.what()); + } + catch(...) + { + ROS_ERROR_STREAM("## ERROR SickScanApiWaitNextNavPoseLandmarkMsg(): unknown exception "); + } + return ret_val; +} +int32_t SickScanApiFreeNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkMsg* msg) +{ + if(apiHandle && msg) + { + freeNavPoseLandmarkMsg(*msg); + return SICK_SCAN_API_SUCCESS; + } + return SICK_SCAN_API_NOT_INITIALIZED; +} +// Send odometry data to NAV350 +int32_t SickScanApiNavOdomVelocityImpl(SickScanApiHandle apiHandle, SickScanNavOdomVelocityMsg* msg); +int32_t SickScanApiOdomVelocityImpl(SickScanApiHandle apiHandle, SickScanOdomVelocityMsg* src_msg); +int32_t SickScanApiNavOdomVelocityMsg(SickScanApiHandle apiHandle, SickScanNavOdomVelocityMsg* msg) +{ + return SickScanApiNavOdomVelocityImpl(apiHandle, msg); +} +int32_t SickScanApiOdomVelocityMsg(SickScanApiHandle apiHandle, SickScanOdomVelocityMsg* msg) +{ + return SickScanApiOdomVelocityImpl(apiHandle, msg); +} diff --git a/driver/src/sick_scansegment_xd/config.cpp b/driver/src/sick_scansegment_xd/config.cpp index ce8bbdb2..5cb760b6 100644 --- a/driver/src/sick_scansegment_xd/config.cpp +++ b/driver/src/sick_scansegment_xd/config.cpp @@ -51,7 +51,10 @@ * */ #include +#include #include "sick_scansegment_xd/config.h" +#include +#include #define ROS_DECL_GET_PARAMETER(node,name,value) do{rosDeclareParam((node),(name),(value));rosGetParam((node),(name),(value));}while(false) @@ -107,6 +110,19 @@ static bool setOptionalArgument(const std::map& key_va return false; } +/** Lookup a table of optional key value pairs and overwrite argument if key found in table */ +static bool setOptionalArgument(const std::map& key_value_pairs, const std::string& key, double& argument) +{ + std::string str_argument; + if (setOptionalArgument(key_value_pairs, key, str_argument) && !str_argument.empty()) + { + argument = std::stod(str_argument); + ROS_INFO_STREAM(key << "=" << argument << " set by commandline"); + return true; + } + return false; +} + /* * @brief Default constructor, initializes the configuration with default values */ @@ -116,8 +132,10 @@ sick_scansegment_xd::Config::Config() udp_sender = ""; // Use "" (default) to receive msgpacks from any udp sender, use "127.0.0.1" to restrict to localhost (loopback device), or use the ip-address of a multiScan136 lidar or multiScan136 emulator udp_port = 2115; // default udp port for multiScan136 resp. multiScan136 emulator is 2115 publish_topic = "/cloud"; // ros topic to publish received msgpack data converted top PointCloud2 messages, default: "/cloud" - publish_topic_all_segments = "/cloud_360"; // ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360" - segment_count = 12; // number of expected segments in 360 degree, multiScan136: 12 segments, 30 degree per segment + publish_topic_all_segments = "/cloud_fullframe"; // ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_fullframe" + //segment_count = 12; // number of expected segments in 360 degree, multiScan136: 12 segments, 30 degree per segment + all_segments_min_deg = -180; // angle range covering all segments: all segments pointcloud on topic publish_topic_all_segments is published, + all_segments_max_deg = +180; // if received segments cover angle range from all_segments_min_deg to all_segments_max_deg. -180...+180 for multiScan136 (360 deg fullscan) publish_frame_id = "world"; // frame id of ros PointCloud2 messages, default: "world" udp_input_fifolength = 20; // max. udp input fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length msgpack_output_fifolength = 20; // max. msgpack output fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length @@ -148,7 +166,7 @@ sick_scansegment_xd::Config::Config() host_LFPangleRangeFilter = "0 -180.0 +180.0 -90.0 +90.0 1"; // Optionally set LFPangleRangeFilter to " " with azimuth and elevation given in degree host_set_LFPangleRangeFilter = false; // If true, LFPangleRangeFilter is set at startup (default: false) host_LFPlayerFilter = "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"; // Optionally set LFPlayerFilter to " ... " with 1 for enabled and 0 for disabled - host_set_LFPlayerFilter = false; // If true, LFPlayerFilter is set at startup (default: false) + host_set_LFPlayerFilter = false; // If true LFPlayerFilter is set at startup (default: false) // msgpack validation default settings msgpack_validator_enabled = true; // true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation @@ -211,12 +229,15 @@ bool sick_scansegment_xd::Config::Init(rosNodePtr _node) { node = _node; + ROS_DECL_GET_PARAMETER(node, "scanner_type", scanner_type); ROS_DECL_GET_PARAMETER(node, "hostname", hostname); ROS_DECL_GET_PARAMETER(node, "udp_sender", udp_sender); ROS_DECL_GET_PARAMETER(node, "udp_port", udp_port); ROS_DECL_GET_PARAMETER(node, "publish_topic", publish_topic); ROS_DECL_GET_PARAMETER(node, "publish_topic_all_segments", publish_topic_all_segments); - ROS_DECL_GET_PARAMETER(node, "segment_count", segment_count); + // ROS_DECL_GET_PARAMETER(node, "segment_count", segment_count); + ROS_DECL_GET_PARAMETER(node, "all_segments_min_deg", all_segments_min_deg); + ROS_DECL_GET_PARAMETER(node, "all_segments_max_deg", all_segments_max_deg); ROS_DECL_GET_PARAMETER(node, "publish_frame_id", publish_frame_id); ROS_DECL_GET_PARAMETER(node, "udp_input_fifolength", udp_input_fifolength); ROS_DECL_GET_PARAMETER(node, "msgpack_output_fifolength", msgpack_output_fifolength); @@ -343,7 +364,9 @@ bool sick_scansegment_xd::Config::Init(int argc, char** argv) setOptionalArgument(cli_parameter_map, "udp_port", udp_port); setOptionalArgument(cli_parameter_map, "publish_topic", publish_topic); setOptionalArgument(cli_parameter_map, "publish_topic_all_segments", publish_topic_all_segments); - setOptionalArgument(cli_parameter_map, "segment_count", segment_count); + // setOptionalArgument(cli_parameter_map, "segment_count", segment_count); + setOptionalArgument(cli_parameter_map, "all_segments_min_deg", all_segments_min_deg); + setOptionalArgument(cli_parameter_map, "all_segments_max_deg", all_segments_max_deg); setOptionalArgument(cli_parameter_map, "publish_frame_id", publish_frame_id); setOptionalArgument(cli_parameter_map, "udp_input_fifolength", udp_input_fifolength); setOptionalArgument(cli_parameter_map, "msgpack_output_fifolength", msgpack_output_fifolength); @@ -405,11 +428,14 @@ bool sick_scansegment_xd::Config::Init(int argc, char** argv) void sick_scansegment_xd::Config::PrintConfig(void) { ROS_INFO_STREAM("sick_scansegment_xd configuration:"); + ROS_INFO_STREAM("scanner_type: " << scanner_type); ROS_INFO_STREAM("udp_sender: " << udp_sender); ROS_INFO_STREAM("udp_port: " << udp_port); ROS_INFO_STREAM("publish_topic: " << publish_topic); ROS_INFO_STREAM("publish_topic_all_segments: " << publish_topic_all_segments); - ROS_INFO_STREAM("segment_count: " << segment_count); + // ROS_INFO_STREAM("segment_count: " << segment_count); + ROS_INFO_STREAM("all_segments_min_deg: " << all_segments_min_deg); + ROS_INFO_STREAM("all_segments_max_deg: " << all_segments_max_deg); ROS_INFO_STREAM("publish_frame_id: " << publish_frame_id); ROS_INFO_STREAM("udp_input_fifolength: " << udp_input_fifolength); ROS_INFO_STREAM("msgpack_output_fifolength: " << msgpack_output_fifolength); diff --git a/driver/src/sick_scansegment_xd/msgpack_exporter.cpp b/driver/src/sick_scansegment_xd/msgpack_exporter.cpp index dd5204be..7dee67fb 100644 --- a/driver/src/sick_scansegment_xd/msgpack_exporter.cpp +++ b/driver/src/sick_scansegment_xd/msgpack_exporter.cpp @@ -197,7 +197,7 @@ bool sick_scansegment_xd::MsgPackExporter::RunCb(void) { int msg_cnt_delta = (int)msg_udp_received_counter - (int)msg_exported_counter; double packages_lost_rate = std::abs((double)msg_cnt_delta) / (double)msg_udp_received_counter; - if (m_verbose && msg_udp_received_counter != msg_exported_counter && msg_cnt_delta > msg_cnt_delta_max) // Test mode only, mrs100 emulator must be started after lidar3d_msr100_recv + if (m_verbose && msg_udp_received_counter != msg_exported_counter && msg_cnt_delta > msg_cnt_delta_max) // Test mode only, multiScan emulator must be started after lidar3d_multiscan_recv { ROS_INFO_STREAM("MsgPackExporter::Run(): " << msg_udp_received_counter << " udp messages received, " << msg_exported_counter << " messages exported, " << (100.0 * packages_lost_rate) << "% package lost"); msg_cnt_delta_max = msg_cnt_delta; diff --git a/driver/src/sick_scansegment_xd/msgpack_parser.cpp b/driver/src/sick_scansegment_xd/msgpack_parser.cpp index a398a9b1..b98ae8c1 100644 --- a/driver/src/sick_scansegment_xd/msgpack_parser.cpp +++ b/driver/src/sick_scansegment_xd/msgpack_parser.cpp @@ -399,6 +399,18 @@ class MsgPackToFloat32VectorConverter } return s.str(); } + float rad2deg(float angle) const { return angle * (float)(180.0 / M_PI); } + std::string printRad2Deg(void) + { + std::stringstream s; + if (!m_data.empty()) + { + s << rad2deg(m_data[0]); + for(int n = 1; n < m_data.size(); n++) + s << "," << rad2deg(m_data[n]); + } + return s.str(); + } std::vector& data(void) { return m_data; @@ -755,8 +767,6 @@ bool sick_scansegment_xd::MsgPackParser::Parse(std::istream& msgpack_istream, fi for (int pointIdx = 0; pointIdx < iPointCount; pointIdx++) { float dist = 0.001f * distValues[echoIdx].data()[pointIdx]; // convert distance to meter - if (range_filter.apply(dist)) // otherwise point dropped by range filter - { float intensity = rssiValues[echoIdx].data()[pointIdx]; float x = dist * cos_azimuth[pointIdx] * cos_elevation; float y = dist * sin_azimuth[pointIdx] * cos_elevation; @@ -769,8 +779,14 @@ bool sick_scansegment_xd::MsgPackParser::Parse(std::istream& msgpack_istream, fi msgpack_validator_data.update(echoIdx, segment_idx, azimuth_norm, elevation); msgpack_validator_data_collector.update(echoIdx, segment_idx, azimuth_norm, elevation); } + if (range_filter.apply(dist)) + { scanline.points.push_back(sick_scansegment_xd::MsgPackParserOutput::LidarPoint(x, y, z, intensity, dist, azimuth, elevation, groupIdx, echoIdx, pointIdx)); } + else // point dropped by range filter + { + scanline.points.push_back(sick_scansegment_xd::MsgPackParserOutput::LidarPoint(0, 0, 0, 0, 0, azimuth, elevation, groupIdx, echoIdx, pointIdx)); + } } } @@ -778,8 +794,10 @@ bool sick_scansegment_xd::MsgPackParser::Parse(std::istream& msgpack_istream, fi if (verbose) { ROS_INFO_STREAM((groupIdx + 1) << ". group: EchoCount = " << iEchoCount); - ROS_INFO_STREAM((groupIdx + 1) << ". group: phi = [" << channelPhi.print() << "], " << channelPhi.data().size() << " element"); - ROS_INFO_STREAM((groupIdx + 1) << ". group: theta = [" << channelTheta.print() << "], " << channelTheta.data().size() << " elements"); + ROS_INFO_STREAM((groupIdx + 1) << ". group: phi (elevation, rad) = [" << channelPhi.print() << "], " << channelPhi.data().size() << " element"); + ROS_INFO_STREAM((groupIdx + 1) << ". group: phi (elevation, deg) = [" << channelPhi.printRad2Deg() << "], " << channelPhi.data().size() << " element"); + ROS_INFO_STREAM((groupIdx + 1) << ". group: theta (azimuth, rad) = [" << channelTheta.print() << "], " << channelTheta.data().size() << " elements"); + ROS_INFO_STREAM((groupIdx + 1) << ". group: theta (azimuth, deg) = [" << channelTheta.printRad2Deg() << "], " << channelTheta.data().size() << " elements"); ROS_INFO_STREAM((groupIdx + 1) << ". group: timestampStart = " << u32TimestampStart << " = " << Timestamp(u32TimestampStart_sec, u32TimestampStart_nsec)); ROS_INFO_STREAM((groupIdx + 1) << ". group: timestampStop = " << u32TimestampStop << " = " << Timestamp(u32TimestampStop_sec, u32TimestampStop_nsec)); for (int n = 0; n < distValues.size(); n++) diff --git a/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp b/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp index 392e84a8..f9c20fcb 100644 --- a/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp +++ b/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp @@ -63,8 +63,9 @@ * @param[in] node_name name of the ros node * @param[in] config sick_scansegment_xd configuration, RosMsgpackPublisher uses * config.publish_topic: ros topic to publish received msgpack data converted to PointCloud2 messages, default: "/cloud" - * config.publish_topic_all_segments: ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360" - * config.segment_count: number of expected segments in 360 degree, multiScan136: 12 segments, 30 deg per segment + * config.publish_topic_all_segments: ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_fullframe" + * config.all_segments_min_deg, config.all_segments_min_deg: angle range covering all segments: all segments pointcloud on topic publish_topic_all_segments is published, + * if received segments cover angle range from all_segments_min_deg to all_segments_max_deg. -180...+180 for multiScan136 (360 deg fullscan) * config.publish_frame_id: frame id of ros PointCloud2 messages, default: "world" * @param[in] qos quality of service profile for the ros publisher, default: 1 */ @@ -77,12 +78,12 @@ sick_scansegment_xd::RosMsgpackPublisher::RosMsgpackPublisher(const std::string& m_frame_id = config.publish_frame_id; m_publish_topic = config.publish_topic; m_publish_topic_all_segments = config.publish_topic_all_segments; - m_segment_count = config.segment_count; + // m_segment_count = config.segment_count; + m_all_segments_min_deg = (float)config.all_segments_min_deg; + m_all_segments_max_deg = (float)config.all_segments_max_deg; m_node = config.node; m_laserscan_layer_filter = config.laserscan_layer_filter; - m_min_azimuth = (float)-M_PI; - m_max_azimuth = (float)+M_PI; #if defined __ROS_VERSION && __ROS_VERSION > 1 // ROS-2 publisher rosQoS qos = rclcpp::SystemDefaultsQoS(); QoSConverter qos_converter; @@ -91,7 +92,6 @@ sick_scansegment_xd::RosMsgpackPublisher::RosMsgpackPublisher(const std::string& rosGetParam(m_node, "ros_qos", qos_val); if (qos_val >= 0) qos = qos_converter.convert(qos_val); - m_points_collector = SegmentPointsCollector(m_segment_count); if(m_publish_topic != "") { m_publisher_cur_segment = create_publisher(m_publish_topic, qos); @@ -243,7 +243,8 @@ void sick_scansegment_xd::RosMsgpackPublisher::convertPointsToCloud(uint32_t tim pfdata_polar[data_cnt + 3] = lidar_points[echoIdx][pointIdx].i; int echo = lidar_points[echoIdx][pointIdx].echo; int layer = lidar_points[echoIdx][pointIdx].layer; - if (!is_cloud_360 && m_laserscan_layer_filter[layer]) + bool layer_enabled = (m_laserscan_layer_filter.empty() ? 1 : (m_laserscan_layer_filter[layer])); + if (!is_cloud_360 && layer_enabled) { // laser_scan_msg = laser_scan_msg_map[layer] ros_sensor_msgs::LaserScan& laser_scan_msg = laser_scan_msg_map[echo][layer]; @@ -289,8 +290,8 @@ void sick_scansegment_xd::RosMsgpackPublisher::convertPointsToCloud(uint32_t tim while (angle_diff < 0) angle_diff += (float)(2.0 * M_PI); laser_scan_msg.angle_increment = angle_diff / (float)(laser_scan_msg.ranges.size() - 1); - laser_scan_msg.range_min -= 1.0e-03; - laser_scan_msg.range_max += 1.0e-03; + laser_scan_msg.range_min -= 1.0e-03f; + laser_scan_msg.range_max += 1.0e-03f; laser_scan_msg.header = pointcloud_msg.header; laser_scan_msg.header.frame_id = m_frame_id + "_" + std::to_string(layer_idx); // scan_time = 1 / scan_frequency = time for a full 360-degree rotation of the sensor @@ -367,22 +368,19 @@ void sick_scansegment_xd::RosMsgpackPublisher::HandleMsgPackData(const sick_scan // iv. Konfiguration erfolgt über YAML-Datei. if(m_publish_topic_all_segments != "") { - // ROS_INFO_STREAM("RosMsgpackPublisher::HandleMsgPackData(): segment_idx=" << segment_idx << ", m_points_collector.segment_count=" << m_points_collector.segment_count << ", m_points_collector.total_point_count=" << m_points_collector.total_point_count); - // ROS_INFO_STREAM("RosMsgpackPublisher::HandleMsgPackData(): segment_idx=" << segment_idx << ", m_points_collector.segment_count=" << m_points_collector.segment_count - // << ", m_points_collector.total_point_count=" << m_points_collector.total_point_count << ", m_points_collector.azimuth=(" << m_points_collector.min_azimuth << "," << m_points_collector.max_azimuth - // << "), config.azimuth=(" << m_min_azimuth << "," << m_max_azimuth << ")"); - if (segment_idx == 0 || segment_idx < m_points_collector.segment_count) // start with new segment index, i.e. publish the collected pointcloud and start a new collection of all points (until N <= 12 segments collected) + float precheck_min_azimuth_deg = m_points_collector.min_azimuth * 180.0f / (float)M_PI; + float precheck_max_azimuth_deg = m_points_collector.max_azimuth * 180.0f / (float)M_PI; + bool publish_cloud_360 = (precheck_max_azimuth_deg - precheck_min_azimuth_deg + 1 >= m_all_segments_max_deg - m_all_segments_min_deg - 1) // fast pre-check + && m_points_collector.allSegmentsCovered(m_all_segments_min_deg, m_all_segments_max_deg); // all segments collected in m_points_collector + // ROS_INFO_STREAM("RosMsgpackPublisher::HandleMsgPackData(): segment_idx=" << segment_idx << ", m_points_collector.lastSegmentIdx=" << m_points_collector.lastSegmentIdx() + // << ", m_points_collector.total_point_count=" << m_points_collector.total_point_count << ", m_points_collector.allSegmentsCovered=" << publish_cloud_360); + if (m_points_collector.total_point_count <= 0 || m_points_collector.telegram_cnt <= 0 || publish_cloud_360 || m_points_collector.lastSegmentIdx() > segment_idx) { - // publish 360 degree point cloud if all segments collected - if (m_points_collector.total_point_count > 0) + // 1. publish 360 degree point cloud if all segments collected + // 2. start a new collection of all points (first time call, all segments covered, or segment index wrap around) + if (m_points_collector.total_point_count > 0 && m_points_collector.telegram_cnt > 0 && publish_cloud_360) { - bool publish_cloud_360 = false; - if (m_points_collector.segment_count + 1 == m_segment_count) // all segments collected in m_points_collector (m_segment_count:=12) - publish_cloud_360 = true; - else if (m_points_collector.min_azimuth - M_PI / 180.0 < m_min_azimuth && m_points_collector.max_azimuth + M_PI / 180.0 > m_max_azimuth) // all points within min and max azimuth collected in m_points_collector - publish_cloud_360 = true; - if (publish_cloud_360) // publish 360 degree point cloud - { + // publish 360 degree point cloud // scan_time = 1 / scan_frequency = time for a full 360-degree rotation of the sensor m_scan_time = (msgpack_data.timestamp_sec + 1.0e-9 * msgpack_data.timestamp_nsec) - (m_points_collector.timestamp_sec + 1.0e-9 * m_points_collector.timestamp_nsec); LaserScanMsgMap laser_scan_msg_map; // laser_scan_msg_map[echo][layer] := LaserScan message given echo (Multiscan136: max 3 echos) and layer index (Multiscan136: 16 layer) @@ -391,15 +389,13 @@ void sick_scansegment_xd::RosMsgpackPublisher::HandleMsgPackData(const sick_scan pointcloud_msg, pointcloud_msg_polar, laser_scan_msg_map, true); publish(m_node, m_publisher_all_segments, pointcloud_msg, pointcloud_msg_polar, m_publisher_laserscan_360, laser_scan_msg_map, std::max(1, (int)echo_count), -1); // pointcloud, number of echos, segment index (or -1 if pointcloud contains data from multiple segments) - // ROS_INFO_STREAM("RosMsgpackPublisher::HandleMsgPackData(): cloud_360 published, " << m_points_collector.total_point_count << " points, " << pointcloud_msg.data.size() << " byte, " + // ROS_INFO_STREAM("RosMsgpackPublisher::HandleMsgPackData(): cloud_fullframe published, " << m_points_collector.total_point_count << " points, " << pointcloud_msg.data.size() << " byte, " // << m_points_collector.segment_list.size() << " segments (" << sick_scansegment_xd::util::printVector(m_points_collector.segment_list, ",") << "), " // << m_points_collector.telegram_list.size() << " telegrams (" << sick_scansegment_xd::util::printVector(m_points_collector.telegram_list, ",") << "), " // << "min_azimuth=" << (m_points_collector.min_azimuth * 180.0 / M_PI) << ", max_azimuth=" << (m_points_collector.max_azimuth * 180.0 / M_PI) << " [deg]"); - m_points_collector = SegmentPointsCollector(m_segment_count, telegram_cnt); - } } // Start a new 360 degree collection - m_points_collector = SegmentPointsCollector(segment_idx, telegram_cnt); + m_points_collector = SegmentPointsCollector(telegram_cnt); m_points_collector.timestamp_sec = msgpack_data.timestamp_sec; m_points_collector.timestamp_nsec = msgpack_data.timestamp_nsec; m_points_collector.total_point_count = total_point_count; @@ -414,7 +410,7 @@ void sick_scansegment_xd::RosMsgpackPublisher::HandleMsgPackData(const sick_scan { if (m_points_collector.lidar_points.size() < lidar_points.size()) m_points_collector.lidar_points.resize(lidar_points.size()); - m_points_collector.segment_count = segment_idx; + // m_points_collector.segment_count = segment_idx; m_points_collector.telegram_cnt = telegram_cnt; m_points_collector.total_point_count += total_point_count; m_points_collector.appendLidarPoints(lidar_points, segment_idx, telegram_cnt); @@ -423,10 +419,10 @@ void sick_scansegment_xd::RosMsgpackPublisher::HandleMsgPackData(const sick_scan } else { - ROS_WARN_STREAM("## WARNING RosMsgpackPublisher::HandleMsgPackData(): current segment: " << segment_idx << ", last segment in collector: " << m_points_collector.segment_count + ROS_WARN_STREAM("## WARNING RosMsgpackPublisher::HandleMsgPackData(): current segment: " << segment_idx << ", last segment in collector: " << m_points_collector.lastSegmentIdx() << ", current telegram: " << telegram_cnt << ", last telegram in collector: " << m_points_collector.telegram_cnt << ", datagram(s) missing, 360-degree-pointcloud not published"); - m_points_collector = SegmentPointsCollector(m_segment_count, telegram_cnt); // reset pointcloud collector + m_points_collector = SegmentPointsCollector(telegram_cnt); // reset pointcloud collector } // ROS_INFO_STREAM("RosMsgpackPublisher::HandleMsgPackData(): segment_idx " << segment_idx << " of " << m_segment_count << ", " << m_points_collector.total_point_count << " points in collector"); } diff --git a/driver/src/sick_scansegment_xd/scansegment_threads.cpp b/driver/src/sick_scansegment_xd/scansegment_threads.cpp index 94b945e2..fa1382cf 100644 --- a/driver/src/sick_scansegment_xd/scansegment_threads.cpp +++ b/driver/src/sick_scansegment_xd/scansegment_threads.cpp @@ -73,25 +73,25 @@ int sick_scansegment_xd::run(rosNodePtr node, const std::string& scannerName) sick_scansegment_xd::Config config; if (!config.Init(node)) { - ROS_ERROR_STREAM("## ERROR " SICK_SCANNER_SCANSEGMENT_XD_NAME ": Config::Init() failed, using default values."); + ROS_ERROR_STREAM("## ERROR sick_scansegment_xd::run(" << config.scanner_type << "): Config::Init() failed, using default values."); return sick_scan::ExitError; } config.PrintConfig(); // Run sick_scansegment_xd (msgpack receive, convert and publish) - ROS_INFO_STREAM(SICK_SCANNER_SCANSEGMENT_XD_NAME " started."); + ROS_INFO_STREAM("sick_scansegment_xd (" << config.scanner_type << ") started."); sick_scansegment_xd::MsgPackThreads msgpack_threads; if(!msgpack_threads.start(config)) { - ROS_ERROR_STREAM("## ERROR " SICK_SCANNER_SCANSEGMENT_XD_NAME ": sick_scansegment_xd::MsgPackThreads::start() failed"); + ROS_ERROR_STREAM("## ERROR sick_scansegment_xd::run(" << config.scanner_type << "): sick_scansegment_xd::MsgPackThreads::start() failed"); return sick_scan::ExitError; } msgpack_threads.join(); // Close sick_scansegment_xd if(!msgpack_threads.stop()) { - ROS_ERROR_STREAM("## ERROR " SICK_SCANNER_SCANSEGMENT_XD_NAME ": sick_scansegment_xd::MsgPackThreads::stop() failed"); + ROS_ERROR_STREAM("## ERROR sick_scansegment_xd::run(" << config.scanner_type << "): sick_scansegment_xd::MsgPackThreads::stop() failed"); } - ROS_INFO_STREAM(SICK_SCANNER_SCANSEGMENT_XD_NAME " finished."); + ROS_INFO_STREAM("sick_scansegment_xd (" << config.scanner_type << ") finished."); return sick_scan::ExitSuccess; } @@ -227,8 +227,8 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) sick_scan::SickGenericParser parser = sick_scan::SickGenericParser(scannerName); sick_scan::ScannerBasicParam basic_param; basic_param.setScannerName(scannerName); - bool mrs100_write_filtersettings = m_config.host_set_FREchoFilter || m_config.host_set_LFPangleRangeFilter || m_config.host_set_LFPlayerFilter; - if (m_config.start_sopas_service || m_config.send_sopas_start_stop_cmd || m_config.host_read_filtersettings || mrs100_write_filtersettings) + bool multiscan_write_filtersettings = m_config.host_set_FREchoFilter || m_config.host_set_LFPangleRangeFilter || m_config.host_set_LFPlayerFilter; + if (m_config.start_sopas_service || m_config.send_sopas_start_stop_cmd || m_config.host_read_filtersettings || multiscan_write_filtersettings) { ROS_INFO_STREAM("MsgPackThreads: initializing sopas tcp (" << m_config.hostname << ":" << m_config.sopas_tcp_port << ", timeout:" << (0.001*m_config.sopas_timeout_ms) << ", binary:" << m_config.sopas_cola_binary << ")"); sopas_tcp = new sick_scan::SickScanCommonTcp(m_config.hostname, m_config.sopas_tcp_port, m_config.sopas_timeout_ms, m_config.node, &parser, m_config.sopas_cola_binary ? 'B' : 'A'); @@ -245,19 +245,22 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) } // Send SOPAS commands to read or optionally write filter settings for (FREchoFilter, LFPangleRangeFilter, LFPlayerFilter) - if ((m_config.host_read_filtersettings || mrs100_write_filtersettings) && sopas_tcp && sopas_service) + if ((m_config.host_read_filtersettings || multiscan_write_filtersettings) && sopas_tcp && sopas_service) { if (sopas_tcp->isConnected()) { // Optionally send SOPAS commands to write filter settings - if (mrs100_write_filtersettings) + if (multiscan_write_filtersettings) { sopas_service->sendAuthorization();//(m_config.client_authorization_pw); - sopas_service->writeMRS100Filtersettings((m_config.host_set_FREchoFilter ? m_config.host_FREchoFilter : -1), (m_config.host_set_LFPangleRangeFilter ? m_config.host_LFPangleRangeFilter : ""), (m_config.host_set_LFPlayerFilter ? m_config.host_LFPlayerFilter : "")); + sopas_service->writeMultiScanFiltersettings((m_config.host_set_FREchoFilter ? m_config.host_FREchoFilter : -1), + (m_config.host_set_LFPangleRangeFilter ? m_config.host_LFPangleRangeFilter : ""), + (m_config.host_set_LFPlayerFilter ? m_config.host_LFPlayerFilter : ""), + m_config.scanner_type); } // Send SOPAS commands to read filter settings sopas_service->sendAuthorization();//(m_config.client_authorization_pw); - sopas_service->queryMRS100Filtersettings(m_config.host_FREchoFilter, m_config.host_LFPangleRangeFilter, m_config.host_LFPlayerFilter, m_config.msgpack_validator_filter_settings); + sopas_service->queryMultiScanFiltersettings(m_config.host_FREchoFilter, m_config.host_LFPangleRangeFilter, m_config.host_LFPlayerFilter, m_config.msgpack_validator_filter_settings, m_config.scanner_type); } else { @@ -274,7 +277,6 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) m_config.msgpack_validator_verbose); msgpack_converter.SetValidator(msgpack_validator, m_config.msgpack_validator_enabled, m_config.msgpack_validator_discard_msgpacks_out_of_bounds, m_config.msgpack_validator_check_missing_scandata_interval); - ros_msgpack_publisher->SetFullScanAzimuthRange(m_config.msgpack_validator_filter_settings.msgpack_validator_azimuth_start, m_config.msgpack_validator_filter_settings.msgpack_validator_azimuth_end); ros_msgpack_publisher->SetActive(true); // Send SOPAS start command @@ -283,7 +285,7 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) if (sopas_tcp->isConnected()) { sopas_service->sendAuthorization();//(m_config.client_authorization_pw); - sopas_service->sendMRS100StartCmd(m_config.udp_receiver_ip, m_config.port); + sopas_service->sendMultiScanStartCmd(m_config.udp_receiver_ip, m_config.port, m_config.scanner_type); } else { @@ -316,7 +318,7 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) if(sopas_tcp && sopas_service && m_config.send_sopas_start_stop_cmd && sopas_tcp->isConnected()) { sopas_service->sendAuthorization();//(m_config.client_authorization_pw); - sopas_service->sendMRS100StopCmd(); + sopas_service->sendMultiScanStopCmd(); } // Stop SOPAS services DELETE_PTR(sopas_service); diff --git a/driver/src/softwarePLL.cpp b/driver/src/softwarePLL.cpp index ad4ae2f9..47686252 100644 --- a/driver/src/softwarePLL.cpp +++ b/driver/src/softwarePLL.cpp @@ -197,6 +197,25 @@ bool SoftwarePLL::getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32 return (true); } +// converts a system timestamp to lidar ticks, computes the inverse to getCorrectedTimeStamp(). +bool SoftwarePLL::convSystemtimeToLidarTimestamp(uint32_t systemtime_sec, uint32_t systemtime_nanosec, uint32_t& tick) +{ + if (IsInitialized() == false) + { + return (false); + } + double systemTimestamp = (double)systemtime_sec + 1.0e-9 * (double)systemtime_nanosec; // systemTimestamp := corrTime in getCorrectedTimeStamp + // getCorrectedTimeStamp(): corrTime = relTimeStamp + this->FirstTimeStamp() + // => inverse: relSystemTimestamp = systemTimestamp - this->FirstTimeStamp() + double relSystemTimestamp = systemTimestamp - this->FirstTimeStamp(); + // getCorrectedTimeStamp(): relSystemTimestamp = (tick - (uint32_t) (0xFFFFFFFF & FirstTick())) * this->InterpolationSlope() + //=> inverse: tick = (relSystemTimestamp / this->InterpolationSlope()) + (uint32_t) (0xFFFFFFFF & FirstTick()) + double relTicks = relSystemTimestamp / this->InterpolationSlope(); + uint32_t tick_offset = (uint32_t)(0xFFFFFFFF & FirstTick()); + tick = (uint32_t)std::round(relTicks + tick_offset); + return (true); +} + bool SoftwarePLL::nearSameTimeStamp(double relTimeStamp1, double relTimeStamp2, double& delta_time_abs) { delta_time_abs = fabs(relTimeStamp1 - relTimeStamp2); diff --git a/examples/ros2_example_application/README.md b/examples/ros2_example_application/README.md index 041b4e72..38a452f1 100644 --- a/examples/ros2_example_application/README.md +++ b/examples/ros2_example_application/README.md @@ -46,7 +46,7 @@ Run the following steps to build sick_scan_xd: colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" --event-handlers console_direct+ ``` - Note: msgpack11 is only required to support Multiscan136/sick_scansegment_xd. If you do not need or want to support Multiscan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without Multiscan136/sick_scansegment_xd support, run colcon with cmake option `-DSCANSEGMENT_XD=0`: + Note: msgpack11 is only required to support multiScan136. If you do not need or want to support multiScan136, you can skip building msgpack. To build sick_generic_caller without multiScan136 support, run colcon with cmake option `-DSCANSEGMENT_XD=0`: ``` colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ ``` diff --git a/include/sick_scan/sick_generic_callback.h b/include/sick_scan/sick_generic_callback.h index 739da2a3..2214888e 100644 --- a/include/sick_scan/sick_generic_callback.h +++ b/include/sick_scan/sick_generic_callback.h @@ -62,6 +62,7 @@ #include #include +#include // forward declaration of SickLdmrsObjectArray required for LdmrsObjectArray listener #if __ROS_VERSION == 2 // ROS-2 (Linux or Windows) @@ -90,6 +91,7 @@ namespace sick_scan typedef void(* SickLdmrsObjectArrayCallback)(rosNodePtr handle, const sick_scan_msg::SickLdmrsObjectArray* msg); typedef void(* RadarScanCallback)(rosNodePtr handle, const sick_scan_msg::RadarScan* msg); typedef void(* VisualizationMarkerCallback)(rosNodePtr handle, const ros_visualization_msgs::MarkerArray* msg); + typedef void(* NAV350mNPOSDataCallback)(rosNodePtr handle, const NAV350mNPOSData* msg); void addCartesianPointcloudListener(rosNodePtr handle, PointCloud2Callback listener); void notifyCartesianPointcloudListener(rosNodePtr handle, const PointCloud2withEcho* msg); @@ -131,6 +133,11 @@ namespace sick_scan void removeVisualizationMarkerListener(rosNodePtr handle, VisualizationMarkerCallback listener); bool isVisualizationMarkerListenerRegistered(rosNodePtr handle, VisualizationMarkerCallback listener); + void addNavPoseLandmarkListener(rosNodePtr handle, NAV350mNPOSDataCallback listener); + void notifyNavPoseLandmarkListener(rosNodePtr handle, NAV350mNPOSData* navdata); + void removeNavPoseLandmarkListener(rosNodePtr handle, NAV350mNPOSDataCallback listener); + bool isNavPoseLandmarkListenerRegistered(rosNodePtr handle, NAV350mNPOSDataCallback listener); + /* * Callback template for registration and deregistration of callbacks incl. notification of listeners */ @@ -261,7 +268,7 @@ namespace sick_scan static void removeWaitForMessageHandlerHandler(SickWaitForMessageHandlerPtr handler) { std::unique_lock lock(s_wait_for_message_handler_mutex); - for(typename std::list::iterator iter_handler = s_wait_for_message_handler_list.begin(); iter_handler != s_wait_for_message_handler_list.end(); iter_handler++) + for(typename std::list::iterator iter_handler = s_wait_for_message_handler_list.begin(); iter_handler != s_wait_for_message_handler_list.end(); ) { if (*iter_handler == handler) iter_handler = s_wait_for_message_handler_list.erase(iter_handler); @@ -301,6 +308,7 @@ namespace sick_scan typedef SickWaitForMessageHandler WaitForRadarScanMessageHandler; typedef SickWaitForMessageHandler WaitForLdmrsObjectArrayMessageHandler; typedef SickWaitForMessageHandler WaitForVisualizationMarkerMessageHandler; + typedef SickWaitForMessageHandler WaitForNAVPOSDataMessageHandler; } // namespace sick_scan #endif // __SICK_GENERIC_CALLBACK_H_INCLUDED diff --git a/include/sick_scan/sick_generic_parser.h b/include/sick_scan/sick_generic_parser.h index 988803dc..5cb9d5ec 100644 --- a/include/sick_scan/sick_generic_parser.h +++ b/include/sick_scan/sick_generic_parser.h @@ -70,10 +70,8 @@ #define SICK_SCANNER_LMS_1XX_NAME "sick_lms_1xx" #define SICK_SCANNER_MRS_6XXX_NAME "sick_mrs_6xxx" #define SICK_SCANNER_LMS_4XXX_NAME "sick_lms_4xxx" -#define SICK_SCANNER_RMS_1XXX_NAME "sick_rms_1xxx" -#define SICK_SCANNER_RMS_2XXX_NAME "sick_rms_2xxx" -#define SICK_SCANNER_RMS_3XX_NAME "sick_rms_3xx" -#define SICK_SCANNER_NAV_3XX_NAME "sick_nav_3xx" +#define SICK_SCANNER_RMS_XXXX_NAME "sick_rms_xxxx" // supports RMS-1xxx and RMS-2xxx +#define SICK_SCANNER_NAV_31X_NAME "sick_nav_31x" #define SICK_SCANNER_NAV_350_NAME "sick_nav_350" #define SICK_SCANNER_NAV_2XX_NAME "sick_nav_2xx" #define SICK_SCANNER_TIM_4XX_NAME "sick_tim_4xx" @@ -81,7 +79,8 @@ #define SICK_SCANNER_LRS_36x0_NAME "sick_lrs_36x0" #define SICK_SCANNER_LRS_36x1_NAME "sick_lrs_36x1" #define SICK_SCANNER_OEM_15XX_NAME "sick_oem_15xx" -#define SICK_SCANNER_SCANSEGMENT_XD_NAME "sick_scansegment_xd" +#define SICK_SCANNER_SCANSEGMENT_XD_NAME "sick_multiscan" // "sick_scansegment_xd", Multiscan126 + #include "abstract_parser.h" #include "sick_scan/sick_scan_common.h" @@ -99,6 +98,14 @@ namespace sick_scan USE_EVAL_FIELD_NUM // max number of eval field support types }; + enum RADAR_TYPE_ENUM + { + NO_RADAR = 0, // sensor is a not a radar (default) + RADAR_1D = 1, // sensor is a 1D radar + RADAR_3D = 2 // sensor is a 3D radar + }; + + class ScannerBasicParam { public: @@ -129,6 +136,7 @@ namespace sick_scan double getExpectedFrequency(void); bool getDeviceIsRadar(void); + RADAR_TYPE_ENUM getDeviceRadarType(void); bool getTrackingModeSupported(void); void setTrackingModeSupported(bool _trackingModeSupported); @@ -148,7 +156,7 @@ namespace sick_scan void setUseBinaryProtocol(bool _useBinary); - void setDeviceIsRadar(bool _deviceIsRadar); + void setDeviceIsRadar(RADAR_TYPE_ENUM _radar_type); void setIntensityResolutionIs16Bit(bool _IntensityResolutionIs16Bit); @@ -204,7 +212,7 @@ namespace sick_scan double scanAngleShift; bool useBinaryProtocol; bool IntensityResolutionIs16Bit; - bool deviceIsRadar = false; + RADAR_TYPE_ENUM deviceRadarType = NO_RADAR; bool trackingModeSupported = false; bool useSafetyPasWD; int8_t encoderMode; @@ -271,7 +279,7 @@ namespace sick_scan std::string scannerType; std::vector allowedScannerNames; std::vector basicParams; - ScannerBasicParam *currentParamSet; + ScannerBasicParam *currentParamSet = 0; RangeFilterResultHandling m_range_filter_handling = RANGE_FILTER_DEACTIVATED; }; diff --git a/include/sick_scan/sick_generic_radar.h b/include/sick_scan/sick_generic_radar.h index 225dd411..cefe97f1 100644 --- a/include/sick_scan/sick_generic_radar.h +++ b/include/sick_scan/sick_generic_radar.h @@ -181,6 +181,7 @@ namespace sick_scan bool emul = false; std::string radarName = "???"; // radar device type + RADAR_TYPE_ENUM radarType = NO_RADAR; rosNodePtr node; rosPublisher cloud_radar_rawtarget_pub_; @@ -207,9 +208,10 @@ namespace sick_scan int verboseLevel = 0); void simulateAsciiDatagram(unsigned char *receiveBuffer, int *actual_length); - void setNameOfRadar(std::string _radarName) + void setNameOfRadar(const std::string& _radarName, RADAR_TYPE_ENUM _radarType) { radarName = _radarName; + radarType = _radarType; } std::string getNameOfRadar() { diff --git a/include/sick_scan/sick_lmd_scandata_parser.h b/include/sick_scan/sick_lmd_scandata_parser.h index b4f5c938..70e8b610 100644 --- a/include/sick_scan/sick_lmd_scandata_parser.h +++ b/include/sick_scan/sick_lmd_scandata_parser.h @@ -71,8 +71,11 @@ namespace sick_scan /** Parse common result telegrams, i.e. parse telegrams of type LMDscandata received from the lidar */ bool parseCommonBinaryResultTelegram(const uint8_t* receiveBuffer, int receiveBufferLength, short& elevAngleX200, double elevAngleTelegramValToDeg, double& elevationAngleInRad, rosTime& recvTimeStamp, - bool config_sw_pll_only_publish, bool use_generation_timestamp, SickGenericParser* parser_, bool& FireEncoder, sick_scan_msg::Encoder& EncoderMsg, int& numEchos, + bool config_sw_pll_only_publish, bool use_generation_timestamp, SickGenericParser* parser_, bool& FireEncoder, sick_scan_msg::Encoder& EncoderMsg, int& numEchos, std::vector& vang_vec, ros_sensor_msgs::LaserScan & msg); + /** Increments the number of packets received in the SoftwarePLL */ + void incSoftwarePLLPacketReceived(); + } /* namespace sick_scan */ #endif /* SICK_LMD_SCANDATA_PARSER_H_ */ diff --git a/include/sick_scan/sick_nav_scandata.h b/include/sick_scan/sick_nav_scandata.h new file mode 100644 index 00000000..a0755c47 --- /dev/null +++ b/include/sick_scan/sick_nav_scandata.h @@ -0,0 +1,233 @@ +#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */ +/* + * sick_nav_scandata defines the data telegrams from NAV-350. + * + * Copyright (C) 2023, Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2023, SICK AG, Waldkirch + * All rights reserved. + * +* 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 Osnabrueck University 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 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 + * + */ + +#ifndef SICK_NAV_SCANDATA_H_ +#define SICK_NAV_SCANDATA_H_ + +#include +#include + +#include + +namespace sick_scan +{ + /** Container for NAV350 optional pose data */ + class NAV350OptPoseData + { + public: + uint8_t outputMode = 0; + uint32_t timestamp = 0; + int32_t meanDev = 0; + uint8_t navMode = 0; + uint32_t infoState = 0; + uint8_t quantUsedReflectors = 0; + }; + + /** Container for NAV350 pose data */ + class NAV350PoseData + { + public: + int32_t x = 0; // x-position in mm + int32_t y = 0; // y-position in mm + uint32_t phi = 0; // orientation in 0 ... 360000 mdeg + uint16_t optPoseDataValid = 0; + NAV350OptPoseData optPoseData; + }; + + /** Container for NAV350 Cartesian data */ + class NAV350CartesianData + { + public: + int32_t x = 0; + int32_t y = 0; + }; + + /** Container for NAV350 Polar data */ + class NAV350PolarData + { + public: + uint32_t dist = 0; + uint32_t phi = 0; + }; + + /** Container for NAV350 optional reflector data */ + class NAV350OptReflectorData + { + public: + uint16_t localID = 0; + uint16_t globalID = 0; + uint8_t type = 0; + uint16_t subType = 0; + uint16_t quality = 0; + uint32_t timestamp = 0; + uint16_t size = 0; + uint16_t hitCount = 0; + uint16_t meanEcho = 0; + uint16_t startIndex = 0; + uint16_t endIndex = 0; + }; + + /** Container for NAV350 Reflector data */ + class NAV350ReflectorData + { + public: + uint16_t cartesianDataValid = 0; + NAV350CartesianData cartesianData; + uint16_t polarDataValid = 0; + NAV350PolarData polarData; + uint16_t optReflectorDataValid = 0; + NAV350OptReflectorData optReflectorData; + std::string print() + { + std::stringstream s; + s << "cartesianDataValid=" << (int)cartesianDataValid << ", cartesianData.x=" << cartesianData.x << ", cartesianData.y=" << cartesianData.y + << ", polarDataValid=" << (int)polarDataValid << ", polarData.dist=" << polarData.dist << ", polarData.phi=" << polarData.phi + << ", optReflectorDataValid=" << (int)optReflectorDataValid << ", localID=" << optReflectorData.localID << ", globalID=" << optReflectorData.globalID << ", type=" << (int)optReflectorData.type << ", subType=" << (int)optReflectorData.subType + << ", quality=" << optReflectorData.quality << ", timestamp=" << optReflectorData.timestamp << ", size=" << optReflectorData.size << ", hitCount=" << optReflectorData.hitCount << ", meanEcho=" << optReflectorData.meanEcho + << ", startIndex=" << optReflectorData.startIndex << ", endIndex=" << optReflectorData.endIndex; + return s.str(); + } + }; + + /** Container for NAV350 Landmark data */ + class NAV350LandmarkData + { + public: + uint8_t landmarkFilter = 0; + uint16_t numReflectors = 0; + std::vector reflectors; + }; + + /** Container for NAV350 landmark data received by response "sAN mNMAPDoMapping" after request "sMN mNMAPDoMapping" */ + class NAV350LandmarkDataDoMappingResponse + { + public: + uint8_t errorCode = 0; + uint16_t landmarkDataValid = 0; + NAV350LandmarkData landmarkData; + void print() + { + ROS_INFO_STREAM("NAV350LandmarkDataDoMappingResponse: errorCode=" << (int)errorCode << ", landmarkDataValid=" << (int)landmarkDataValid << ", landmarkFilter=" << (int)landmarkData.landmarkFilter << ", numReflectors=" << (int)landmarkData.numReflectors); + for(int reflector_cnt = 0; reflector_cnt < landmarkData.reflectors.size(); reflector_cnt++) + ROS_INFO_STREAM("NAV350LandmarkDataDoMappingResponse: reflector[" << reflector_cnt << "]={" << landmarkData.reflectors[reflector_cnt].print() << "}"); + } + }; + + /** Container for NAV350 scan data */ + class NAV350ScanData + { + public: + std::string contentType = ""; // "DIST1" or "ANGL1" + float scaleFactor = 0; // multiplier, always 1 for NAV350 + float scaleOffset = 0; // offset, always 0 for NAV350 + int32_t startAngle = 0; // 0 ... +360000 mdeg + uint16_t angleRes = 0; // angular step, in 1/1000 deg, 250 mdeg (fix) + uint32_t timestampStart = 0; // timestamp of scan start in ms + uint16_t numData = 0; // number of scan data + std::vector data; // DIST in mm or ANGL in 1/10000 degree + }; + + /** Container for NAV350 remission data */ + class NAV350RemissionData + { + public: + std::string contentType = ""; // "RSSI1" + float scaleFactor = 0; // multiplier, always 1 for NAV350 + float scaleOffset = 0; // offset, always 0 for NAV350 + int32_t startAngle = 0; // 0 ... +360000 mdeg + uint16_t angleRes = 0; // angular step, in 1/1000 deg, 250 mdeg (fix) + uint32_t timestampStart = 0; // timestamp of scan start in ms + uint16_t numData = 0; // number of scan data + std::vector data; // remission data + }; + + /** Container for NAV350 position data telegrams, i.e. content of NAV350 mNPOSGetData telegrams */ + class NAV350mNPOSData + { + public: + uint16_t version = 0; + uint8_t errorCode = 0; + uint8_t wait = 0; + uint8_t mask = 0; + uint16_t poseDataValid = 0; + NAV350PoseData poseData; + uint16_t landmarkDataValid = 0; + NAV350LandmarkData landmarkData; + uint16_t scanDataValid = 0; + std::vector scanData; // no scan data (scanDataValid = 0) or DIST1 scan data (scanDataValid = 1) or DIST1 + ANGL1 scan data (scanDataValid = 2) + uint16_t remissionDataValid = 0; + NAV350RemissionData remissionData; + float angleOffset = -M_PI; + }; + + + /** Container for NAV350 landmark data imported from imk file. Each line in an imk file is formatted "globID x_mm y_mm type subtype size_mm layer1 layer2 layer3" */ + class NAV350ImkLandmark + { + public: + uint16_t globID = 0; + int32_t x_mm; + int32_t y_mm; + uint8_t type = 0; + uint16_t subType = 0; + uint16_t size_mm; + std::vector layerID; + }; + +} /* namespace sick_scan */ +#endif /* SICK_NAV_SCANDATA_H_ */ diff --git a/include/sick_scan/sick_nav_scandata_parser.h b/include/sick_scan/sick_nav_scandata_parser.h new file mode 100644 index 00000000..6d20695a --- /dev/null +++ b/include/sick_scan/sick_nav_scandata_parser.h @@ -0,0 +1,119 @@ +#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */ +/* + * sick_nav_scandata_parser parses data telegrams from NAV-350. + * + * Copyright (C) 2023, Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2023, SICK AG, Waldkirch + * All rights reserved. + * +* 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 Osnabrueck University 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 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 + * + */ + +#ifndef SICK_NAV_SCANDATA_PARSER_H_ +#define SICK_NAV_SCANDATA_PARSER_H_ + +#include +#include + +#include +#include +#include + +namespace sick_scan +{ + /** Parse binary NAV350 position data telegrams, i.e. parse NAV350 "sAN mNPOSGetData version errorCode wait mask poseData [x y phi optPoseData [outputMode timestamp meanDev navMode infoState quantUsedReflectors]] + ** landmarkData [landmarkFilter reflectors [cart [X Y] polar [D Phi] optLandmarkData [optLandmarkData…]]] scanData [contentType scaleFactor scaleOffset startAngle angleRes data [aData]]" + */ + bool parseNAV350BinaryPositionData(const uint8_t* receiveBuffer, int receiveBufferLength, NAV350mNPOSData& navdata); + + /** Parse binary NAV350 landmark data */ + bool parseNAV350BinaryLandmarkData(const uint8_t* receiveBuffer, int& receivePos, int receiveBufferLength, NAV350LandmarkData& landmarkData); + + /** Parse binary NAV350 landmark data received by response "sAN mNMAPDoMapping" after request "sMN mNMAPDoMapping" */ + bool parseNAV350BinaryLandmarkDataDoMappingResponse(const uint8_t* receiveBuffer, int receiveBufferLength, NAV350LandmarkDataDoMappingResponse& landmarkData); + + /** Creates and returns the sopas command "sMN mNLAYAddLandmark landmarkData {x y type subtype size layerID {ID}}" */ + std::vector createNAV350BinaryAddLandmarkRequest(const NAV350LandmarkData& landmarkData, int nav_curr_layer); + + /** Creates and returns the sopas command "sMN mNLAYAddLandmark landmarkData {x y type subtype size layerID {ID}}" */ + std::vector createNAV350BinaryAddLandmarkRequest(const std::vector landmarks); + + /** Creates and returns the sopas command "sMN mNPOSSetSpeed X Y Phi timestamp coordBase" */ + std::vector createNAV350BinarySetSpeedRequest(const sick_scan_msg::NAVOdomVelocity& msg); + + /** Parse binary NAV350 position data telegrams, i.e. parse NAV350 "sAN mNPOSGetData ..." and converts the data to a ros_sensor_msgs::LaserScan message */ + bool parseNAV350BinaryPositionData(const uint8_t* receiveBuffer, int receiveBufferLength, short& elevAngleX200, double& elevationAngleInRad, rosTime& recvTimeStamp, + bool config_sw_pll_only_publish, double config_time_offset, SickGenericParser* parser_, int& numEchos, ros_sensor_msgs::LaserScan & msg, + sick_scan_msg::NAVPoseData& nav_pose_msg, sick_scan_msg::NAVLandmarkData& nav_landmark_msg, NAV350mNPOSData& navdata); + + /** Convert NAV350PoseData to ros transform */ + ros_geometry_msgs::TransformStamped convertNAVPoseDataToTransform(NAV350PoseData& poseData, rosTime recvTimeStamp, double config_time_offset, + const std::string& tf_parent_frame_id, const std::string& tf_child_frame_id, SickGenericParser* parser_); + + /** Rotates a cartesian 2D position (x, y) by a given angle offset */ + void rotateXYbyAngleOffset(float& x, float& y, double angle_offset); + + /** Converts a cartesian 2D position from NAV to ROS coordinates */ + void convertNAVCartPos2DtoROSPos2D(int32_t nav_posx_mm, int32_t nav_posy_mm, float& ros_posx_m, float& ros_posy_m, double nav_angle_offset); + + /** Converts a cartesian 3D pose from NAV to ROS coordinates */ + void convertNAVCartPos3DtoROSPos3D(int32_t nav_posx_mm, int32_t nav_posy_mm, uint32_t nav_phi_mdeg, float& ros_posx_m, float& ros_posy_m, float& ros_yaw_rad, double nav_angle_offset); + + /** Converts NAV350 landmark data to a ROS VisualMarker message */ + ros_visualization_msgs::MarkerArray convertNAVLandmarkDataToMarker(const std::vector& reflectors, ros_std_msgs::Header& header, SickGenericParser* parser_); + + /** Import a NAV350 landmark layout from imk file. The NAV350 landmark layout can be stored in imk files using SOPAS ET. Each line in an imk file is formatted "globID x_mm y_mm type subtype size_mm layer1 layer2 layer3" */ + std::vector readNAVIMKfile(const std::string& nav_imk_file); + + /** Unittest for parseNAV350BinaryPositionData(): creates, serializes and deserializes NAV350 position data telegrams and checks the identity of results */ + bool parseNAV350BinaryUnittest(); + +} /* namespace sick_scan */ +#endif /* SICK_NAV_SCANDATA_PARSER_H_ */ diff --git a/include/sick_scan/sick_ros_wrapper.h b/include/sick_scan/sick_ros_wrapper.h index e99b78f0..ff43030c 100644 --- a/include/sick_scan/sick_ros_wrapper.h +++ b/include/sick_scan/sick_ros_wrapper.h @@ -120,6 +120,9 @@ template std::string paramToString(const T& param_value) #endif #include +#include +#include +#include #include #include #include @@ -128,9 +131,12 @@ template std::string paramToString(const T& param_value) #include #include #include +#include +#include typedef ros::NodeHandle* rosNodePtr; +#define ros_nav_msgs nav_msgs #define ros_sensor_msgs sensor_msgs #define ros_std_msgs std_msgs #define ros_geometry_msgs geometry_msgs @@ -160,6 +166,7 @@ inline uint32_t nsec(const rosTime& time) { return time.nsec; } // return nanose inline uint32_t sec(const rosDuration& time) { return time.sec; } // return seconds part of ros::Duration inline uint32_t nsec(const rosDuration& time) { return time.nsec; } // return nanoseconds part of ros::Duration inline uint64_t rosNanosecTimestampNow(void) { rosTime now = rosTimeNow(); return (((uint64_t)sec(now)) * (uint64_t)1000000000) + std::min((uint64_t)nsec(now), (uint64_t)1000000000); } +inline double rosTimeToSeconds(const rosTime& time) { return (double)sec(time) + 1.0e-9 * (double)nsec(time); } template class rosPublisher : public ros::Publisher { @@ -197,6 +204,9 @@ inline rosDuration rosDurationFromSec(double seconds) { return rosDuration(secon #include // generated by msg-generator #include // generated by msg-generator #include // generated by msg-generator +#include // generated by msg-generator +#include // generated by msg-generator +#include // generated by msg-generator #define sick_scan_msg sick_scan #include "sick_scan/ColaMsgSrv.h" // generated by srv-generator @@ -228,6 +238,9 @@ template class rosServiceServer : public ros::ServiceServer #include #include #include +#include +#include +#include #include #include #include @@ -236,9 +249,12 @@ template class rosServiceServer : public ros::ServiceServer #include #include #include +#include +#include typedef rclcpp::Node::SharedPtr rosNodePtr; +#define ros_nav_msgs nav_msgs::msg #define ros_sensor_msgs sensor_msgs::msg #define ros_std_msgs std_msgs::msg #define ros_geometry_msgs geometry_msgs::msg @@ -304,6 +320,7 @@ inline uint32_t nsec(const rosTime& time) { return (uint32_t)(time.nanoseconds() inline uint32_t sec(const rosDuration& time) { return (uint32_t)(time.nanoseconds() / 1000000000); } // return seconds part of rclcpp::Duration inline uint32_t nsec(const rosDuration& time) { return (uint32_t)(time.nanoseconds() - 1000000000 * sec(time)); } // return nanoseconds part of rclcpp::Duration inline uint64_t rosNanosecTimestampNow(void) { rosTime now = rosTimeNow(); return (((uint64_t)sec(now)) * (uint64_t)1000000000) + std::min((uint64_t)nsec(now), (uint64_t)1000000000); } +inline double rosTimeToSeconds(const rosTime& time) { return (double)sec(time) + 1.0e-9 * (double)nsec(time); } class QoSConverter { @@ -332,7 +349,7 @@ template class rosPublisher : public rclcpp::Publisher::SharedPtr rosPublisher() : rclcpp::Publisher::SharedPtr(0) {} template rosPublisher(U& _publisher) : rclcpp::Publisher::SharedPtr(_publisher) {} }; -template rosPublisher rosAdvertise(rosNodePtr nh, const std::string& topic, uint32_t queue_size = 10, rosQoS qos = rclcpp::SystemDefaultsQoS()) +inline void overwriteByOptionalQOSconfig(rosNodePtr nh, rosQoS& qos) { QoSConverter qos_converter; int qos_val = -1; @@ -340,6 +357,11 @@ template rosPublisher rosAdvertise(rosNodePtr nh, const std::string rosGetParam(nh, "ros_qos", qos_val); if (qos_val >= 0) qos = qos_converter.convert(qos_val); +} +template rosPublisher rosAdvertise(rosNodePtr nh, const std::string& topic, uint32_t queue_size = 10, rosQoS qos = rclcpp::SystemDefaultsQoS()) +{ + overwriteByOptionalQOSconfig(nh, qos); + QoSConverter qos_converter; ROS_INFO_STREAM("Publishing on topic \"" << topic << "\", qos=" << qos_converter.convert(qos)); auto publisher = nh->create_publisher(topic, qos); return rosPublisher(publisher); @@ -358,6 +380,9 @@ inline rosDuration rosDurationFromSec(double seconds) { return rosDuration(std:: #include // generated by msg-generator #include // generated by msg-generator #include // generated by msg-generator +#include // generated by msg-generator +#include // generated by msg-generator +#include // generated by msg-generator #define sick_scan_msg sick_scan::msg #include "sick_scan/srv/cola_msg_srv.hpp" // generated by rosidl-generator diff --git a/include/sick_scan/sick_scan_common.h b/include/sick_scan/sick_scan_common.h index bc3ef458..867df9ab 100644 --- a/include/sick_scan/sick_scan_common.h +++ b/include/sick_scan/sick_scan_common.h @@ -82,8 +82,8 @@ #include #include "sick_scan/sick_generic_parser.h" +#include "sick_scan/sick_nav_scandata_parser.h" #include "sick_scan/sick_scan_common_nw.h" - #include "sick_scan/sick_generic_field_mon.h" #include "sick_scan/sick_scan_marker.h" @@ -188,11 +188,22 @@ namespace sick_scan CMD_SET_SCAN_CFG_LIST, // "sMN mCLsetscancfglist %d", set scan config from list for NAX310 LD-OEM15xx LD-LRS36xx // NAV-350 commands - CMD_SET_NAV_OPERATIONAL_MODE_0, // "sMN mNEVAChangeState 0", 0 = power down - CMD_SET_NAV_OPERATIONAL_MODE_1, // "sMN mNEVAChangeState 1", 1 = standby - CMD_SET_NAV_OPERATIONAL_MODE_2, // "sMN mNEVAChangeState 2", 2 = mapping - CMD_SET_NAV_OPERATIONAL_MODE_3, // "sMN mNEVAChangeState 3", 3 = landmark detection - CMD_SET_NAV_OPERATIONAL_MODE_4, // "sMN mNEVAChangeState 4", 4 = navigation + CMD_SET_NAV_OPERATIONAL_MODE_0, // "sMN mNEVAChangeState 0", 0 = power down + CMD_SET_NAV_OPERATIONAL_MODE_1, // "sMN mNEVAChangeState 1", 1 = standby + CMD_SET_NAV_OPERATIONAL_MODE_2, // "sMN mNEVAChangeState 2", 2 = mapping + CMD_SET_NAV_OPERATIONAL_MODE_3, // "sMN mNEVAChangeState 3", 3 = landmark detection + CMD_SET_NAV_OPERATIONAL_MODE_4, // "sMN mNEVAChangeState 4", 4 = navigation + CMD_SET_NAV_CURR_LAYER, // "sWN NEVACurrLayer 0" + CMD_SET_NAV_LANDMARK_DATA_FORMAT, // "sWN NLMDLandmarkDataFormat 0 1 1" + CMD_SET_NAV_SCAN_DATA_FORMAT, // "sWN NAVScanDataFormat 1 1" + CMD_SET_NAV_POSE_DATA_FORMAT, // "sWN NPOSPoseDataFormat 1 1" + CMD_SET_NAV_MAP_CFG, // "sWN NMAPMapCfg 50 0 0 0 0" + CMD_SET_NAV_REFL_SIZE, // "sWN NLMDReflSize 80" + CMD_SET_NAV_DO_MAPPING, // "sMN mNMAPDoMapping" + CMD_SET_NAV_ADD_LANDMARK, // "sMN mNLAYAddLandmark landmarkData {x y type subtype size layerID {ID}}" + CMD_SET_NAV_ERASE_LAYOUT, // "sMN mNLAYEraseLayout 1" + CMD_SET_NAV_STORE_LAYOUT, // "sMN mNLAYStoreLayout" + CMD_SET_NAV_POSE, // Set NAV-350 start pose in navigation mode by "sMN mNPOSSetPose X Y Phi" // Supported by sick_generic_caller version 2.7.3 and above: CMD_SET_LFPMEANFILTER, // MRS1xxx, LMS1xxx, LMS4xxx, LRS4xxx: "sWN LFPmeanfilter" + { 1 byte 0|1 active/inactive } + { 2 byte 0x02 ... 0x64 number of scans } + { 1 byte 0x00 } @@ -235,10 +246,19 @@ namespace sick_scan int sendSopasAorBgetAnswer(const std::string& request, std::vector *reply, bool useBinaryCmd); + int get2ndSopasResponse(std::vector& sopas_response, const std::string& sopas_keyword); + ExitCode checkColaTypeAndSwitchToConfigured(bool useBinaryCmd); bool sendSopasRunSetAccessMode(bool useBinaryCmd); + // NAV-350 data must be polled by sending sopas command "sMN mNPOSGetData wait mask" + int sendNAV350mNPOSGetData(void); + + // Parse NAV-350 pose and scan data and send next "sMN mNPOSGetData" request (NAV-350 polling) + bool handleNAV350BinaryPositionData(const uint8_t* receiveBuffer, int receiveBufferLength, short& elevAngleX200, double& elevationAngleInRad, rosTime& recvTimeStamp, + bool config_sw_pll_only_publish, double config_time_offset, SickGenericParser* parser_, int& numEchos, ros_sensor_msgs::LaserScan& msg, NAV350mNPOSData& navdata); + int setAligmentMode(int _AligmentMode); int setMeanFilter(bool _active, int _numberOfScans); @@ -283,6 +303,8 @@ namespace sick_scan int convertAscii2BinaryCmd(const char *requestAscii, std::vector *requestBinary); + void setLengthAndCRCinBinarySopasRequest(std::vector* requestBinary); + int init_cmdTables(rosNodePtr nh); /// Send a SOPAS command to the scanner that should cause a soft reset @@ -305,6 +327,11 @@ namespace sick_scan return (&config_); } + ScannerBasicParam* getCurrentParamPtr() + { + return parser_ ? parser_->getCurrentParamPtr() : 0; + } + /// Converts reply from sendSOPASCommand to string /** * \param [in] reply reply from sendSOPASCommand @@ -326,6 +353,9 @@ namespace sick_scan rosPublisher Encoder_pub; ros_sensor_msgs::PointCloud2 cloud_; ros_sensor_msgs::PointCloud2 cloud_polar_; + + void messageCbNavOdomVelocity(const sick_scan_msg::NAVOdomVelocity& msg); + ////// // Dynamic Reconfigure SickScanConfig config_; @@ -414,6 +444,28 @@ namespace sick_scan bool publish_lidoutputstate_; SickScanMarker* cloud_marker_; + rosPublisher nav_pose_data_pub_; + bool publish_nav_pose_data_; + rosPublisher nav_landmark_data_pub_; + rosPublisher nav_reflector_pub_; + bool publish_nav_landmark_data_; + + std::string nav_tf_parent_frame_id_; + std::string nav_tf_child_frame_id_; +#if __ROS_VERSION > 0 + tf2_ros::TransformBroadcaster* nav_tf_broadcaster_; + void messageCbRosOdom(const ros_nav_msgs::Odometry& msg); +#endif +#if __ROS_VERSION == 1 + ros::Subscriber nav_odom_velocity_subscriber_; + ros::Subscriber ros_odom_subscriber_; +#elif __ROS_VERSION == 2 + rclcpp::Subscription::SharedPtr nav_odom_velocity_subscriber_; + void messageCbNavOdomVelocityROS2(const std::shared_ptr msg) { messageCbNavOdomVelocity(*msg); } + rclcpp::Subscription::SharedPtr ros_odom_subscriber_; + void messageCbRosOdomROS2(const std::shared_ptr msg) { messageCbRosOdom(*msg); } +#endif + // Diagnostics #if defined USE_DIAGNOSTIC_UPDATER #if __ROS_VERSION == 1 @@ -433,7 +485,7 @@ namespace sick_scan dynamic_reconfigure::Server dynamic_reconfigure_server_; #endif // Parser - SickGenericParser *parser_; + SickGenericParser *parser_ = 0; std::vector sopasCmdVec; std::vector sopasCmdMaskVec; std::vector sopasReplyVec; diff --git a/include/sick_scan/sick_scan_services.h b/include/sick_scan/sick_scan_services.h index 943edb51..43f63271 100644 --- a/include/sick_scan/sick_scan_services.h +++ b/include/sick_scan/sick_scan_services.h @@ -141,16 +141,17 @@ namespace sick_scan #if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 /*! - * Sends the MRS100 start commands "sWN ScanDataFormat", "sWN ScanDataPreformatting", "sWN ScanDataEthSettings", "sWN ScanDataEnable 1", "sMN LMCstartmeas", "sMN Run" + * Sends the multiScan start commands "sWN ScanDataFormat", "sWN ScanDataPreformatting", "sWN ScanDataEthSettings", "sWN ScanDataEnable 1", "sMN LMCstartmeas", "sMN Run" * @param[in] hostname IP address of multiScan136, default 192.168.0.1 * @param[in] port IP port of multiScan136, default 2115 + * @param[in] scanner_type type of scanner, currently only multiScan136 */ - bool sendMRS100StartCmd(const std::string& hostname, int port); + bool sendMultiScanStartCmd(const std::string& hostname, int port, const std::string& scanner_type); /*! - * Sends the MRS100 stop commands "sWN ScanDataEnable 0" and "sMN Run" + * Sends the multiScan stop commands "sWN ScanDataEnable 0" and "sMN Run" */ - bool sendMRS100StopCmd(void); + bool sendMultiScanStopCmd(void); /*! * Sends the SOPAS command to query multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter, host_LFPlayerFilter) @@ -159,7 +160,7 @@ namespace sick_scan * @param[out] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise " ... " with 1 for enabled and 0 for disabled * @param[out] msgpack_validator_filter_settings; // filter settings for msgpack validator: required_echos, azimuth_start, azimuth_end. elevation_start, elevation_end, layer_filter */ - bool queryMRS100Filtersettings(int& host_FREchoFilter, std::string& host_LFPangleRangeFilter, std::string& host_LFPlayerFilter, sick_scansegment_xd::MsgpackValidatorFilterConfig& msgpack_validator_filter_settings); + bool queryMultiScanFiltersettings(int& host_FREchoFilter, std::string& host_LFPangleRangeFilter, std::string& host_LFPlayerFilter, sick_scansegment_xd::MsgpackValidatorFilterConfig& msgpack_validator_filter_settings, const std::string& scanner_type); /*! * Sends the SOPAS command to write multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter, host_LFPlayerFilter) @@ -167,7 +168,7 @@ namespace sick_scan * @param[in] host_LFPangleRangeFilter LFPangleRangeFilter settings, default: "0 -180.0 +180.0 -90.0 +90.0 1", otherwise " " with azimuth and elevation given in degree * @param[in] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise " ... " with 1 for enabled and 0 for disabled */ - bool writeMRS100Filtersettings(int host_FREchoFilter, const std::string& host_LFPangleRangeFilter, const std::string& host_LFPlayerFilter); + bool writeMultiScanFiltersettings(int host_FREchoFilter, const std::string& host_LFPangleRangeFilter, const std::string& host_LFPlayerFilter, const std::string& scanner_type); #endif // defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 diff --git a/include/sick_scan/softwarePLL.h b/include/sick_scan/softwarePLL.h index 2aac8732..49cd57a2 100644 --- a/include/sick_scan/softwarePLL.h +++ b/include/sick_scan/softwarePLL.h @@ -34,6 +34,8 @@ class SoftwarePLL bool getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32_t tick); + bool convSystemtimeToLidarTimestamp(uint32_t systemtime_sec, uint32_t systemtime_nanosec, uint32_t& tick); + bool getDemoFileData(std::string fileName, std::vector &tickVec, std::vector &secVec, std::vector &nanoSecVec); diff --git a/include/sick_scan_xd_api/sick_scan_api.h b/include/sick_scan_xd_api/sick_scan_api.h index d107204f..e87d608b 100644 --- a/include/sick_scan_xd_api/sick_scan_api.h +++ b/include/sick_scan_xd_api/sick_scan_api.h @@ -108,12 +108,12 @@ typedef struct SickScanPointFieldMsgType // equivalent to ros::sensor_msgs::Poin { // SickScanPointFieldArray is an array of SickScanPointFieldMsg, which defines the structure of the binary data of a SickScanPointCloudMsg. // SickScanPointFieldMsg for pointclouds in cartesian coordinates with fields (x, y, z, intensity): - // [ SickScanPointFieldMsg(name="x", offset=0, datatype=FLOAT32, count=1), + // [ SickScanPointFieldMsg(name="x", offset=0, datatype=FLOAT32, count=1), // SickScanPointFieldMsg(name="y", offset=4, datatype=FLOAT32, count=1), // SickScanPointFieldMsg(name="z", offset=8, datatype=FLOAT32, count=1), // SickScanPointFieldMsg(name="intensity", offset=12, datatype=FLOAT32, count=1) ] // SickScanPointFieldMsg for pointclouds in polar coordinates with fields (range, azimuth, elevation, intensity): - // [ SickScanPointFieldMsg(name="range", offset=0, datatype=FLOAT32, count=1), + // [ SickScanPointFieldMsg(name="range", offset=0, datatype=FLOAT32, count=1), // SickScanPointFieldMsg(name="azimuth", offset=4, datatype=FLOAT32, count=1), // SickScanPointFieldMsg(name="elevation", offset=8, datatype=FLOAT32, count=1), // SickScanPointFieldMsg(name="intensity", offset=12, datatype=FLOAT32, count=1) ] @@ -355,6 +355,83 @@ typedef struct SickScanVisualizationMarkerMsgType // equivalent to visualization SickScanVisualizationMarkerBuffer markers; // Array of SickScanVisualizationMarkers } SickScanVisualizationMarkerMsg; +typedef struct SickScanNavReflectorType // NAV-350 reflector +{ + uint16_t pos_valid; + float pos_x; // reflector x-position in m, if pos_valid > 0 + float pos_y; // reflector y-position in m, if pos_valid > 0 + uint16_t cartesian_valid; + int32_t cartesian_x; // cartesian x in mm, if cartesian_valid > 0 + int32_t cartesian_y; // cartesian y in mm, if cartesian_valid > 0 + uint16_t polar_valid; + uint32_t polar_dist; // polar dist in mm, if polar_valid > 0 + uint32_t polar_phi; // polar phi in mdeg, if polar_valid > 0 + uint16_t opt_valid; + // Optional reflector data, if opt_valid > 0 + uint16_t opt_local_id; + uint16_t opt_global_id; + uint8_t opt_type; + uint16_t opt_subtype; + uint16_t opt_quality; + uint32_t opt_timestamp; // lidar timestamp in milliseconds + uint16_t opt_size; + uint16_t opt_hitcount; + uint16_t opt_meanecho; + uint16_t opt_startindex; + uint16_t opt_endindex; + uint32_t opt_timestamp_sec; // timestamp converted to system time (seconds part, 0 if timestamp not valid) + uint32_t opt_timestamp_nsec; // timestamp converted to system time (nanoseconds part, 0 if timestamp not valid) +} SickScanNavReflector; + +typedef struct SickScanNavReflectorBufferType // Array of SickScanNavReflectors +{ + uint64_t capacity; // Number of allocated elements, i.e. max. number of elements in buffer, allocated buffer size is capacity * sizeof(SickScanNavReflector) + uint64_t size; // Number of currently used elements in the buffer + SickScanNavReflector* buffer; // Memory, data in plain order and system endianess (buffer == 0, if size == 0 && capacity == 0, otherwise allocated memory), allocation/deallocation always managed by the caller. +} SickScanNavReflectorBuffer; + +typedef struct SickScanNavPoseLandmarkMsgType // NAV-350 pose and landmark message +{ + uint16_t pose_valid; + // NAV pose, if pose_valid > 0: + float pose_x; // x-position in ros coordinates in m + float pose_y; // y-position in ros coordinates in m + float pose_yaw; // yaw angle in ros coordinates in radians + uint32_t pose_timestamp_sec; // timestamp of pose converted to system time (seconds part, 0 if timestamp not valid) + uint32_t pose_timestamp_nsec; // timestamp of pose converted to system time (nanoseconds part, 0 if timestamp not valid) + int32_t pose_nav_x; // x-position in lidar coordinates in mm + int32_t pose_nav_y; // y-position in lidar coordinates in mm + uint32_t pose_nav_phi; // orientation in lidar coordinates in 0 ... 360000 mdeg + uint16_t pose_opt_valid; + // Optional NAV pose data, if pose_opt_valid > 0: + uint8_t pose_opt_output_mode; + uint32_t pose_opt_timestamp; // lidar timestamp in milliseconds + int32_t pose_opt_mean_dev; + uint8_t pose_opt_nav_mode; + uint32_t pose_opt_info_state ; + uint8_t pose_opt_quant_used_reflectors; + // NAV reflectors: + SickScanNavReflectorBuffer reflectors; // Array of SickScanNavReflectors +} SickScanNavPoseLandmarkMsg; + +typedef struct SickScanNavOdomVelocityMsgType // NAV350 velocity/odometry data, see NAVOdomVelocity.msg +{ + float vel_x; // x-component of velocity in the coordinate system defined by coordbase (i.e. in lidar coordinate for coordbase=0) in m/s, -32.0 ... +32.0 m/s + float vel_y; // y-component of velocity in the coordinate system defined by coordbase (i.e. in lidar coordinate for coordbase=0) in m/s, -32.0 ... +32.0 m/s + float omega; // angular velocity of the NAV350 in radians/s, -2*PI ... +2*PI rad/s + uint32_t timestamp; // timestamp of the Velocity vector related to the NAV350 clock + uint8_t coordbase; // coordinate system of the velocity vector (local or global), 0 = local coordinate system of the NAV350, 1 = absolute coordinate system +} SickScanNavOdomVelocityMsg; + +typedef struct SickScanOdomVelocityMsgType // Velocity/odometry data in system coordinates and time +{ + float vel_x; // x-component of velocity in ros coordinates in m/s + float vel_y; // y-component of velocity in ros coordinates in m/s + float omega; // angular velocity in radians/s + uint32_t timestamp_sec; // seconds part of system timestamp of the odometry data + uint32_t timestamp_nsec; // nanoseconds part of system timestamp of the odometry data +} SickScanOdomVelocityMsg; + /* * Callback declarations */ @@ -367,6 +444,7 @@ typedef void(* SickScanLIDoutputstateMsgCallback)(SickScanApiHandle apiHandle, c typedef void(* SickScanRadarScanCallback)(SickScanApiHandle apiHandle, const SickScanRadarScan* msg); typedef void(* SickScanLdmrsObjectArrayCallback)(SickScanApiHandle apiHandle, const SickScanLdmrsObjectArray* msg); typedef void(* SickScanVisualizationMarkerCallback)(SickScanApiHandle apiHandle, const SickScanVisualizationMarkerMsg* msg); +typedef void(* SickScanNavPoseLandmarkCallback)(SickScanApiHandle apiHandle, const SickScanNavPoseLandmarkMsg* msg); /* * Functions to initialize and close the API and a lidar @@ -433,6 +511,10 @@ SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiDeregisterLdmrsObjectArrayMsg(S SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiRegisterVisualizationMarkerMsg(SickScanApiHandle apiHandle, SickScanVisualizationMarkerCallback callback); SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiDeregisterVisualizationMarkerMsg(SickScanApiHandle apiHandle, SickScanVisualizationMarkerCallback callback); +// Register / deregister a callback for SickScanNavPoseLandmark messages +SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiRegisterNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkCallback callback); +SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiDeregisterNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkCallback callback); + /* * Polling functions */ @@ -466,6 +548,14 @@ SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiFreeLdmrsObjectArrayMsg(SickSca SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiWaitNextVisualizationMarkerMsg(SickScanApiHandle apiHandle, SickScanVisualizationMarkerMsg* msg, double timeout_sec); SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiFreeVisualizationMarkerMsg(SickScanApiHandle apiHandle, SickScanVisualizationMarkerMsg* msg); +// Wait for and return the next SickScanNavPoseLandmark message. Note: SickScanApiWait...Msg() allocates a message. Use function SickScanApiFree...Msg() to deallocate it after use. +SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiWaitNextNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkMsg* msg, double timeout_sec); +SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiFreeNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkMsg* msg); + +// Send odometry data to NAV350 +SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiNavOdomVelocityMsg(SickScanApiHandle apiHandle, SickScanNavOdomVelocityMsg* msg); // odometry data in nav coordinates +SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiOdomVelocityMsg(SickScanApiHandle apiHandle, SickScanOdomVelocityMsg* msg); // odometry data in system coordinates + /* * Error codes, return values of SickScanApi-functions */ diff --git a/include/sick_scansegment_xd/config.h b/include/sick_scansegment_xd/config.h index a1113cc3..880ee255 100644 --- a/include/sick_scansegment_xd/config.h +++ b/include/sick_scansegment_xd/config.h @@ -64,7 +64,7 @@ namespace sick_scansegment_xd { /* - * @brief Container for filter settings for msgpack validator, returned from by queryMRS100Filtersettings() + * @brief Container for filter settings for msgpack validator, returned from by queryMultiScanFiltersettings() */ class MsgpackValidatorFilterConfig { @@ -120,12 +120,16 @@ namespace sick_scansegment_xd * sick_scansegment_xd configuration */ - std::string udp_sender; // = ""; // Use "" (default) to receive msgpacks from any udp sender, use "127.0.0.1" to restrict to localhost (loopback device), or use the ip-address of a MRS100 lidar or MRS100 emulator + std::string scanner_type; // currently supported: "sick_multiscan" + + std::string udp_sender; // = ""; // Use "" (default) to receive msgpacks from any udp sender, use "127.0.0.1" to restrict to localhost (loopback device), or use the ip-address of a Multiscan136 lidar or Multiscan136 emulator int udp_port; // = 2115; // default udp port for multiScan136 resp. multiScan136 emulator is 2115 std::string publish_topic; // = "/cloud"; // ros topic to publish received msgpack data converted top PointCloud2 messages, default: "/cloud" - std::string publish_topic_all_segments; // = "/cloud_360" // ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360" - int segment_count; // 12 // number of expected segments in 360 degree, multiScan136: 12 segments, 30 degree per segment + std::string publish_topic_all_segments; // = "/cloud_fullframe" // ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_fullframe" + // int segment_count; // 12 // number of expected segments in 360 degree, multiScan136: 12 segments, 30 degree per segment + double all_segments_min_deg; // -180 // angle range covering all segments: all segments pointcloud on topic publish_topic_all_segments is published, + double all_segments_max_deg; // +180 // if received segments cover angle range from all_segments_min_deg to all_segments_max_deg. -180...+180 for MultiScan136 (360 deg fullscan) std::string publish_frame_id; // = "world"; // frame id of ros PointCloud2 messages, default: "world" int udp_input_fifolength; // = 20; // max. udp input fifo length(-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length @@ -156,8 +160,8 @@ namespace sick_scansegment_xd bool host_set_FREchoFilter; // False // If true, FREchoFilter is set at startup (default: false) std::string host_LFPangleRangeFilter; // "0 -180.0 +180.0 -90.0 +90.0 1" // Optionally set LFPangleRangeFilter to " " with azimuth and elevation given in degree bool host_set_LFPangleRangeFilter; // False // If true, LFPangleRangeFilter is set at startup (default: false) - std::string host_LFPlayerFilter; // "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" // Optionally set LFPlayerFilter to " ... " with 1 for enabled and 0 for disabled - bool host_set_LFPlayerFilter; // False // If true, LFPlayerFilter is set at startup (default: false) + std::string host_LFPlayerFilter; // "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" (multiScan136 only) // Optionally set LFPlayerFilter to " ... " with 1 for enabled and 0 for disabled + bool host_set_LFPlayerFilter; // False // If true LFPlayerFilter is set at startup (default: false) // msgpack validation bool msgpack_validator_enabled; // true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation diff --git a/include/sick_scansegment_xd/ros_msgpack_publisher.h b/include/sick_scansegment_xd/ros_msgpack_publisher.h index 7ea632d7..aadbf714 100644 --- a/include/sick_scansegment_xd/ros_msgpack_publisher.h +++ b/include/sick_scansegment_xd/ros_msgpack_publisher.h @@ -101,8 +101,9 @@ namespace sick_scansegment_xd * @param[in] node_name name of the ros node * @param[in] config sick_scansegment_xd configuration, RosMsgpackPublisher uses * config.publish_topic: ros topic to publish received msgpack data converted to PointCloud2 messages, default: "/cloud" - * config.publish_topic_all_segments: ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360" - * config.segment_count: number of expected segments in 360 degree, multiScan136: 12 segments, 30 deg per segment + * config.publish_topic_all_segments: ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_fullframe" + * config.all_segments_min_deg, config.all_segments_min_deg: angle range covering all segments: all segments pointcloud on topic publish_topic_all_segments is published, + * if received segments cover angle range from all_segments_min_deg to all_segments_max_deg. -180...+180 for MultiScan136 (360 deg fullscan) * config.publish_frame_id: frame id of ros PointCloud2 messages, default: "world" * @param[in] qos quality of service profile for the ros publisher, default: 1 */ @@ -125,15 +126,6 @@ namespace sick_scansegment_xd */ virtual sick_scansegment_xd::MsgPackExportListenerIF* ExportListener(void); - /* - * Sets min and max azimuth of a full scan in radians, default: -M_PI, +M_PI - */ - virtual void SetFullScanAzimuthRange(float min_azimuth = -M_PI, float max_azimuth = +M_PI) - { - m_min_azimuth = min_azimuth; - m_max_azimuth = max_azimuth; - } - /* * Activates resp. deactivates publishing */ @@ -152,22 +144,85 @@ namespace sick_scansegment_xd class SegmentPointsCollector { public: - SegmentPointsCollector(int segment_idx = 0, int telegram_idx = 0) : timestamp_sec(0), timestamp_nsec(0), segment_count(segment_idx), telegram_cnt(telegram_idx), min_azimuth(0), max_azimuth(0), total_point_count(0), lidar_points() + SegmentPointsCollector(int telegram_idx = 0) : timestamp_sec(0), timestamp_nsec(0), telegram_cnt(telegram_idx), min_azimuth(0), max_azimuth(0), total_point_count(0), lidar_points() { segment_list.reserve(12); telegram_list.reserve(12); + segment_coverage.clear(); } void appendLidarPoints(const std::vector>& points, int32_t segment_idx, int32_t telegram_cnt) { for (int echoIdx = 0; echoIdx < points.size() && echoIdx < lidar_points.size(); echoIdx++) + { lidar_points[echoIdx].insert(lidar_points[echoIdx].end(), points[echoIdx].begin(), points[echoIdx].end()); + for (int n = 0; n < points[echoIdx].size(); n++) + { + const sick_scansegment_xd::PointXYZRAEI32f& point = points[echoIdx][n]; + float elevation_deg = point.elevation * 180.0f / (float)M_PI; + float azimuth_fdeg = point.azimuth * 180.0f / (float)M_PI; + int elevation_mdeg = (int)(1000.0f * elevation_deg); + int azimuth_ideg = (int)(azimuth_fdeg); + segment_coverage[elevation_mdeg][azimuth_ideg] += 1; + if (azimuth_fdeg - azimuth_ideg > 0.5f) + segment_coverage[elevation_mdeg][azimuth_ideg + 1] += 1; + if (azimuth_fdeg - azimuth_ideg < -0.5f) + segment_coverage[elevation_mdeg][azimuth_ideg - 1] += 1; + } + } segment_list.push_back(segment_idx); telegram_list.push_back(telegram_cnt); + // for (std::map>::iterator segment_coverage_elevation_iter = segment_coverage.begin(); segment_coverage_elevation_iter != segment_coverage.end(); segment_coverage_elevation_iter++) + // { + // const int& elevation_deg = segment_coverage_elevation_iter->first; + // std::map& azimuth_histogram = segment_coverage_elevation_iter->second; + // for (std::map::iterator segment_coverage_azimuth_iter = azimuth_histogram.begin(); segment_coverage_azimuth_iter != azimuth_histogram.end(); segment_coverage_azimuth_iter++) + // { + // const int& azimuth_deg = segment_coverage_azimuth_iter->first; + // int cnt = segment_coverage_azimuth_iter->second; + // std::cout << "seg[" << elevation_deg << "][" << azimuth_deg << "]=" << cnt << " "; + // } + // std::cout << std::endl; + // } + } + // Returns the last segment index appended by appendLidarPoints + int32_t lastSegmentIdx() + { + return segment_list.empty() ? -1 : segment_list.back(); + } + // Returns true, if all scans in all elevation angles cover azimuth from all_segments_min_deg to all_segments_max_deg (otherwise false) + bool allSegmentsCovered(float all_segments_min_deg, float all_segments_max_deg) + { + int azimuth_min = (int)all_segments_min_deg; + int azimuth_max = (int)all_segments_max_deg; + for (std::map>::iterator segment_coverage_elevation_iter = segment_coverage.begin(); segment_coverage_elevation_iter != segment_coverage.end(); segment_coverage_elevation_iter++) + { + int azimuth_deg_first = 999, azimuth_deg_last = -999; + const int& elevation_deg = segment_coverage_elevation_iter->first; + std::map& azimuth_histogram = segment_coverage_elevation_iter->second; + for (std::map::iterator segment_coverage_azimuth_iter = azimuth_histogram.begin(); segment_coverage_azimuth_iter != azimuth_histogram.end(); segment_coverage_azimuth_iter++) + { + const int& azimuth_deg = segment_coverage_azimuth_iter->first; + int azimuth_cnt = segment_coverage_azimuth_iter->second; + if (azimuth_cnt > 0) + azimuth_deg_first = MIN(azimuth_deg_first, azimuth_deg); + } + for(azimuth_deg_last = azimuth_deg_first; azimuth_deg_last <= azimuth_deg_first + 360; azimuth_deg_last++) + { + if (azimuth_histogram[azimuth_deg_last] <= 0) + break; + } + bool success = (azimuth_deg_last - azimuth_deg_first >= all_segments_max_deg - all_segments_min_deg); + // ROS_INFO_STREAM("SegmentPointsCollector::allSegmentsCovered(): lastSegmentIdx=" << lastSegmentIdx() << ", total_point_count=" << total_point_count + // << ", cur_elevation=" << elevation_deg << ", azimuth=(" << azimuth_deg_first << "," << azimuth_deg_last << "), " << "ret=" << success); + if (!success) + return false; + } + return true; // all scans in all elevation angles cover azimuth from all_segments_min_deg to all_segments_max_deg } uint32_t timestamp_sec; // seconds part of timestamp of the first segment uint32_t timestamp_nsec; // nanoseconds part of timestamp of the first segment - int32_t segment_count; // number of segments collected + // int32_t segment_count; // number of segments collected int32_t telegram_cnt; // telegram counter (must be continuously incremented) float min_azimuth; // min azimuth of all points in radians float max_azimuth; // max azimuth of all points in radians @@ -175,6 +230,7 @@ namespace sick_scansegment_xd std::vector> lidar_points; // list of PointXYZRAEI32f: lidar_points[echoIdx] are the points of all segments of an echo (idx echoIdx) std::vector segment_list; // list of all collected segment indices std::vector telegram_list; // list of all collected telegram counters + std::map> segment_coverage; // segment histogram: segment_coverage[elevation][azimuth] > 0: elevation in mdeg and azimuth in deg covered (otherwise no hits) }; /* @@ -203,12 +259,12 @@ namespace sick_scansegment_xd bool m_active; // activate publishing rosNodePtr m_node; // ros node handle std::string m_frame_id; // frame id of ros PointCloud2 messages, default: "world" - int m_segment_count = 12; // number of expected segments in 360 degree, multiScan136: 12 segments, 30 deg per segment - float m_min_azimuth; // min azimuth of a full scan in radians, default: -M_PI - float m_max_azimuth; // max azimuth of a full scan in radians, default: +M_PI + // int m_segment_count = 12; // number of expected segments in 360 degree, multiScan136: 12 segments, 30 deg per segment + float m_all_segments_min_deg = -180; // angle range covering all segments: all segments pointcloud on topic publish_topic_all_segments is published, + float m_all_segments_max_deg = +180; // if received segments cover angle range from all_segments_min_deg to all_segments_max_deg. -180...+180 for multiScan136 (360 deg fullscan) SegmentPointsCollector m_points_collector; // collects all points of 12 segments (12 segments * 30 deg = 360 deg) std::string m_publish_topic; // ros topic to publish received msgpack data converted to PointCloud2 messages, default: "/cloud" - std::string m_publish_topic_all_segments; // ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360" + std::string m_publish_topic_all_segments; // ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_fullframe" PointCloud2MsgPublisher m_publisher_cur_segment; // ros publisher to publish PointCloud2 messages of the current segment PointCloud2MsgPublisher m_publisher_all_segments; // ros publisher to publish PointCloud2 messages of all segments (360 degree) LaserscanMsgPublisher m_publisher_laserscan_segment; // ros publisher to publish LaserScan messages of the current segment diff --git a/launch/demo_roscon.launch b/launch/demo_roscon.launch index dacc83e7..2433430d 100644 --- a/launch/demo_roscon.launch +++ b/launch/demo_roscon.launch @@ -1,7 +1,7 @@ diff --git a/launch/laser_radar_combo.launch b/launch/laser_radar_combo.launch index 6d3b543d..cb3f0acd 100644 --- a/launch/laser_radar_combo.launch +++ b/launch/laser_radar_combo.launch @@ -1,8 +1,7 @@ @@ -14,9 +13,9 @@ --> - + - + diff --git a/launch/sick_lms_5xx.launch b/launch/sick_lms_5xx.launch index f7745711..d499c92c 100644 --- a/launch/sick_lms_5xx.launch +++ b/launch/sick_lms_5xx.launch @@ -61,7 +61,7 @@ Check IP-address, if you scanner is not found after roslaunch. - + diff --git a/launch/sick_mrs_1xxx.launch b/launch/sick_mrs_1xxx.launch index dd14189a..58094898 100644 --- a/launch/sick_mrs_1xxx.launch +++ b/launch/sick_mrs_1xxx.launch @@ -26,7 +26,7 @@ default max_angle for this scanner type is +137.5 degree. - + diff --git a/launch/sick_scansegment_xd.launch b/launch/sick_multiscan.launch similarity index 95% rename from launch/sick_scansegment_xd.launch rename to launch/sick_multiscan.launch index 8fec96a0..5fdecd7e 100644 --- a/launch/sick_scansegment_xd.launch +++ b/launch/sick_multiscan.launch @@ -1,30 +1,30 @@ - + - - - + + + - + - + - - + + - + diff --git a/launch/sick_scansegment_xd.launch.py b/launch/sick_multiscan.launch.py similarity index 92% rename from launch/sick_scansegment_xd.launch.py rename to launch/sick_multiscan.launch.py index 7185fdd3..688d5e14 100644 --- a/launch/sick_scansegment_xd.launch.py +++ b/launch/sick_multiscan.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): ld = LaunchDescription() sick_scan_pkg_prefix = get_package_share_directory('sick_scan') launchfile = os.path.basename(__file__)[:-3] # convert ".launch.py" to ".launch" - launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/' + launchfile) # 'launch/sick_scansegment_xd.launch') + launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/' + launchfile) # 'launch/sick_multiscan.launch') node_arguments=[launch_file_path] # append optional commandline arguments in name:=value syntax @@ -18,7 +18,7 @@ def generate_launch_description(): if len(arg.split(":=")) == 2: node_arguments.append(arg) node_arguments.append("publish_topic:=/cloud") - node_arguments.append("publish_topic_all_segments:=/cloud_360") + node_arguments.append("publish_topic_all_segments:=/cloud_fullframe") node_arguments.append("publish_frame_id:=world") node_arguments.append("add_transform_xyz_rpy:=0,0,0,0,0,0") diff --git a/launch/sick_nav_3xx.launch b/launch/sick_nav_31x.launch similarity index 99% rename from launch/sick_nav_3xx.launch rename to launch/sick_nav_31x.launch index cb2319ff..05c9882b 100644 --- a/launch/sick_nav_3xx.launch +++ b/launch/sick_nav_31x.launch @@ -9,12 +9,12 @@ - + - + diff --git a/launch/sick_rms_1xxx.launch.py b/launch/sick_nav_31x.launch.py similarity index 96% rename from launch/sick_rms_1xxx.launch.py rename to launch/sick_nav_31x.launch.py index f536b87d..2919d54c 100644 --- a/launch/sick_rms_1xxx.launch.py +++ b/launch/sick_nav_31x.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): ld = LaunchDescription() sick_scan_pkg_prefix = get_package_share_directory('sick_scan') launchfile = os.path.basename(__file__)[:-3] # convert ".launch.py" to ".launch" - launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/' + launchfile) # 'launch/sick_rms_1xx.launch') + launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/' + launchfile) # 'launch/sick_nav_31x.launch') node_arguments=[launch_file_path] # append optional commandline arguments in name:=value syntax diff --git a/launch/sick_nav_350.launch b/launch/sick_nav_350.launch new file mode 100644 index 00000000..4e69ef95 --- /dev/null +++ b/launch/sick_nav_350.launch @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/sick_rms_3xx.launch.py b/launch/sick_nav_350.launch.py similarity index 96% rename from launch/sick_rms_3xx.launch.py rename to launch/sick_nav_350.launch.py index e31f8ef1..f02b62ea 100644 --- a/launch/sick_rms_3xx.launch.py +++ b/launch/sick_nav_350.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): ld = LaunchDescription() sick_scan_pkg_prefix = get_package_share_directory('sick_scan') launchfile = os.path.basename(__file__)[:-3] # convert ".launch.py" to ".launch" - launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/' + launchfile) # 'launch/sick_rms_3xx.launch') + launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/' + launchfile) # 'launch/sick_nav_350.launch') node_arguments=[launch_file_path] # append optional commandline arguments in name:=value syntax @@ -36,4 +36,3 @@ def generate_launch_description(): ld.add_action(node) return ld - diff --git a/launch/sick_nav_3xx_ascii.launch b/launch/sick_nav_3xx_ascii.launch deleted file mode 100644 index 1aa6bb24..00000000 --- a/launch/sick_nav_3xx_ascii.launch +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/sick_rms_1xxx.launch b/launch/sick_rms_1xxx.launch deleted file mode 100644 index 393581f2..00000000 --- a/launch/sick_rms_1xxx.launch +++ /dev/null @@ -1,77 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/sick_rms_1xxx_ascii.launch b/launch/sick_rms_1xxx_ascii.launch deleted file mode 100644 index 6dadef1f..00000000 --- a/launch/sick_rms_1xxx_ascii.launch +++ /dev/null @@ -1,74 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/sick_rms_2xxx.launch b/launch/sick_rms_2xxx.launch deleted file mode 100644 index 1b1abd6c..00000000 --- a/launch/sick_rms_2xxx.launch +++ /dev/null @@ -1,81 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/sick_rms_2xxx.launch.py b/launch/sick_rms_2xxx.launch.py deleted file mode 100644 index 8063e39d..00000000 --- a/launch/sick_rms_2xxx.launch.py +++ /dev/null @@ -1,39 +0,0 @@ -import os -import sys -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument - -def generate_launch_description(): - - ld = LaunchDescription() - sick_scan_pkg_prefix = get_package_share_directory('sick_scan') - launchfile = os.path.basename(__file__)[:-3] # convert ".launch.py" to ".launch" - launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/' + launchfile) # 'launch/sick_rms_2xxx.launch') - node_arguments=[launch_file_path] - - # append optional commandline arguments in name:=value syntax - for arg in sys.argv: - if len(arg.split(":=")) == 2: - node_arguments.append(arg) - - ROS_DISTRO = os.environ.get('ROS_DISTRO') # i.e. 'eloquent', 'foxy', etc. - if ROS_DISTRO[0] <= "e": # ROS versions eloquent and earlier require "node_executable", ROS foxy and later use "executable" - node = Node( - package='sick_scan', - node_executable='sick_generic_caller', - output='screen', - arguments=node_arguments - ) - else: # ROS versions eloquent and earlier require "node_executable", ROS foxy and later use "executable" - node = Node( - package='sick_scan', - executable='sick_generic_caller', - output='screen', - arguments=node_arguments - ) - - ld.add_action(node) - return ld - diff --git a/launch/sick_rms_3xx_emul.launch b/launch/sick_rms_3xx_emul.launch deleted file mode 100644 index d5361fae..00000000 --- a/launch/sick_rms_3xx_emul.launch +++ /dev/null @@ -1,69 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/sick_rms_3xx.launch b/launch/sick_rms_xxxx.launch similarity index 92% rename from launch/sick_rms_3xx.launch rename to launch/sick_rms_xxxx.launch index ab0a4887..b7691d25 100644 --- a/launch/sick_rms_3xx.launch +++ b/launch/sick_rms_xxxx.launch @@ -1,21 +1,20 @@ - + - + @@ -36,13 +35,15 @@ - + + + diff --git a/launch/sick_nav_3xx.launch.py b/launch/sick_rms_xxxx.launch.py similarity index 96% rename from launch/sick_nav_3xx.launch.py rename to launch/sick_rms_xxxx.launch.py index b174d4fc..1120efe8 100644 --- a/launch/sick_nav_3xx.launch.py +++ b/launch/sick_rms_xxxx.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): ld = LaunchDescription() sick_scan_pkg_prefix = get_package_share_directory('sick_scan') launchfile = os.path.basename(__file__)[:-3] # convert ".launch.py" to ".launch" - launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/' + launchfile) # 'launch/sick_nav_3xx.launch') + launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/' + launchfile) # 'launch/sick_rms_xxxx.launch') node_arguments=[launch_file_path] # append optional commandline arguments in name:=value syntax diff --git a/launch/test_002_combi_live.launch b/launch/test_002_combi_live.launch index 33143e9a..79508b7a 100644 --- a/launch/test_002_combi_live.launch +++ b/launch/test_002_combi_live.launch @@ -9,7 +9,7 @@ - + @@ -19,10 +19,10 @@ - + - + diff --git a/launch/test_002_combi_live_traffic.launch b/launch/test_002_combi_live_traffic.launch index 4e93ad2e..2aafa3a5 100644 --- a/launch/test_002_combi_live_traffic.launch +++ b/launch/test_002_combi_live_traffic.launch @@ -10,7 +10,7 @@ - + @@ -20,10 +20,10 @@ - + - + diff --git a/launch/test_006_radar_emul_vis.launch b/launch/test_006_radar_emul_vis.launch index aa74c4b2..01d1656c 100644 --- a/launch/test_006_radar_emul_vis.launch +++ b/launch/test_006_radar_emul_vis.launch @@ -1,7 +1,7 @@ @@ -13,9 +13,9 @@ type="static_transform_publisher" name="map_radar_broadcaster" args="0 0 0 0 0 0 map radar 100"/> - + - + diff --git a/msg/NAVLandmarkData.msg b/msg/NAVLandmarkData.msg new file mode 100644 index 00000000..204a53c8 --- /dev/null +++ b/msg/NAVLandmarkData.msg @@ -0,0 +1,6 @@ +# ROS message of NAV350LandmarkData, see sick_nav_scandata_parser.h and NAV-350 telegram listing for details. +std_msgs/Header header + +uint8 landmark_filter +uint16 num_reflectors +sick_scan/NAVReflectorData[] reflectors diff --git a/msg/NAVOdomVelocity.msg b/msg/NAVOdomVelocity.msg new file mode 100644 index 00000000..4cfa3a05 --- /dev/null +++ b/msg/NAVOdomVelocity.msg @@ -0,0 +1,7 @@ +# ROS message for NAV350 velocity/odometry data, see sick_nav_scandata_parser.h and NAV-350 telegram listing for details ("sMN mNPOSSetSpeed"). + +float32 vel_x # x-component of velocity in the coordinate system defined by coordbase (i.e. in lidar coordinate for coordbase=0) in m/s, -32.0 ... +32.0 m/s +float32 vel_y # y-component of velocity in the coordinate system defined by coordbase (i.e. in lidar coordinate for coordbase=0) in m/s, -32.0 ... +32.0 m/s +float32 omega # angular velocity of the NAV350 in radians/s, -2*PI ... +2*PI rad/s +uint32 timestamp # Timestamp of the Velocity vector related to the NAV350 clock +uint8 coordbase # Coordinate system of the velocity vector (local or global), 0 = local coordinate system of the NAV350, 1 = absolute coordinate system diff --git a/msg/NAVPoseData.msg b/msg/NAVPoseData.msg new file mode 100644 index 00000000..b412495c --- /dev/null +++ b/msg/NAVPoseData.msg @@ -0,0 +1,21 @@ +# ROS message of NAV350PoseData, see sick_nav_scandata_parser.h and NAV-350 telegram listing for details. +std_msgs/Header header + +# pose in lidar coordinates in mm and mdeg +int32 x +int32 y +uint32 phi +# optional pose data +uint16 opt_pose_data_valid +uint8 output_mode +uint32 timestamp +int32 mean_dev +uint8 nav_mode +uint32 info_state +uint8 quant_used_reflectors + +# pose in ros coordinates +int8 pose_valid # pose_x, pose_y and pose_yaw are valid if pose_valid > 0 +float32 pose_x # x position in ros coordinates in m +float32 pose_y # y position in ros coordinates in m +float32 pose_yaw # yaw angle in radians diff --git a/msg/NAVReflectorData.msg b/msg/NAVReflectorData.msg new file mode 100644 index 00000000..97eb8aef --- /dev/null +++ b/msg/NAVReflectorData.msg @@ -0,0 +1,30 @@ +# ROS message of NAV350ReflectorData, see sick_nav_scandata_parser.h and NAV-350 telegram listing for details. + +# cartesian data in lidar coordinates in mm +uint16 cartesian_data_valid +int32 x +int32 y + +# polar data in lidar coordinates in mm and mdeg +uint16 polar_data_valid +uint32 dist +uint32 phi + +# optional reflector data +uint16 opt_reflector_data_valid +uint16 local_id +uint16 global_id +uint8 type +uint16 sub_type +uint16 quality +uint32 timestamp +uint16 size +uint16 hit_count +uint16 mean_echo +uint16 start_index +uint16 end_index + +# reflector position in ros coordinates +int8 pos_valid # pose_x, pose_y and pose_yaw are valid if pose_valid > 0 +float32 pos_x # x position in ros coordinates in m +float32 pos_y # y position in ros coordinates in m diff --git a/msg/RadarObject.msg b/msg/RadarObject.msg index e85f0985..412f7e46 100644 --- a/msg/RadarObject.msg +++ b/msg/RadarObject.msg @@ -1,8 +1,8 @@ int32 id # ROS-1: -time tracking_time # since when the object is tracked -time last_seen +time tracking_time +time last_seen # not set # ROS-2: #builtin_interfaces/Time tracking_time # since when the object is tracked diff --git a/package.xml b/package.xml index 271f8292..5790f642 100644 --- a/package.xml +++ b/package.xml @@ -18,7 +18,6 @@ message_generation rosidl_default_generators - diagnostic_updater dynamic_reconfigure libsick_ldmrs roscpp @@ -26,6 +25,7 @@ tf diagnostic_msgs + diagnostic_updater geometry_msgs nav_msgs sensor_msgs diff --git a/python/api/sick_scan_api.py b/python/api/sick_scan_api.py index 8fbf9025..cd6af624 100644 --- a/python/api/sick_scan_api.py +++ b/python/api/sick_scan_api.py @@ -712,6 +712,110 @@ class SickScanVisualizationMarkerMsg(ctypes.Structure): ("markers", SickScanVisualizationMarkerBuffer) # Array of SickScanVisualizationMarkers ] +class SickScanNavReflector(ctypes.Structure): + """ + NAV-350 reflector equivalent to SickScanNavReflector defined in sick_scan_api.h + """ + _fields_ = [ + ("pos_valid", ctypes.c_uint16), + # reflector position in [m] in ros coordinates, if pos_valid > 0: + ("pos_x", ctypes.c_float), + ("pos_y", ctypes.c_float), + ("cartesian_valid", ctypes.c_uint16), + # reflector position in [mm] in lidar coordinates, if cartesian_valid > 0: + ("cartesian_x", ctypes.c_int32), + ("cartesian_y", ctypes.c_int32), + ("polar_valid", ctypes.c_uint16), + # reflector position in [mm] and [mdeg] in polar lidar coordinates, if polar_valid > 0: + ("polar_dist", ctypes.c_uint32), + ("polar_phi", ctypes.c_uint32), + ("opt_valid", ctypes.c_uint16), + # Optional reflector data, if opt_valid > 0 + ("opt_local_id", ctypes.c_uint16), + ("opt_global_id", ctypes.c_uint16), + ("opt_type", ctypes.c_uint8), + ("opt_subtype", ctypes.c_uint16), + ("opt_quality", ctypes.c_uint16), + ("opt_timestamp", ctypes.c_uint32), # lidar timestamp in milliseconds + ("opt_size", ctypes.c_uint16), + ("opt_hitcount", ctypes.c_uint16), + ("opt_meanecho", ctypes.c_uint16), + ("opt_startindex", ctypes.c_uint16), + ("opt_endindex", ctypes.c_uint16), + ("opt_timestamp_sec", ctypes.c_uint32), # timestamp converted to system time (seconds part, 0 if timestamp not valid) + ("opt_timestamp_nsec", ctypes.c_uint32) # timestamp converted to system time (nanoseconds part, 0 if timestamp not valid) + ] + +class SickScanNavReflectorBuffer(ctypes.Structure): + """ + Array of NAV-350 reflectors equivalent to SickScanNavReflectorBuffer defined in sick_scan_api.h + + Attributes + ---------- + capacity : ctypes.c_uint64 + Number of allocated elements, i.e. max. number of elements in buffer, allocated buffer size is capacity * sizeof(SickScanNavReflector) + size : ctypes.c_uint64 + Number of currently used elements in the buffer + buffer : ctypes.POINTER(SickScanNavReflector) + Memory, data in plain order and system endianess (buffer == 0, if size == 0 && capacity == 0, otherwise allocated memory), allocation/deallocation always managed by the caller. + """ + _fields_ = [ + ("capacity", ctypes.c_uint64), # Number of allocated elements, i.e. max. number of elements in buffer, allocated buffer size is capacity * sizeof(SickScanVisualizationMarker) + ("size", ctypes.c_uint64), # Number of currently used elements in the buffer + ("buffer", ctypes.POINTER(SickScanNavReflector)) # Memory, data in plain order and system endianess (buffer == 0, if size == 0 && capacity == 0, otherwise allocated memory), allocation/deallocation always managed by the caller. + ] + +class SickScanNavPoseLandmarkMsg(ctypes.Structure): + """ + NAV-350 pose and landmark message equivalent to SickScanNavPoseLandmarkMsg defined in sick_scan_api.h + """ + _fields_ = [ + ("pose_valid", ctypes.c_uint16), + # NAV pose, if pose_valid > 0: + ("pose_x", ctypes.c_float), # x-position in ros coordinates in m + ("pose_y", ctypes.c_float), # y-position in ros coordinates in m + ("pose_yaw", ctypes.c_float), # yaw angle in ros coordinates in radians + ("pose_timestamp_sec", ctypes.c_uint32), # timestamp of pose converted to system time (seconds part, 0 if timestamp not valid) + ("pose_timestamp_nsec", ctypes.c_uint32), # timestamp of pose converted to system time (nanoseconds part, 0 if timestamp not valid) + ("pose_nav_x", ctypes.c_int32), # x-position in lidar coordinates in mm + ("pose_nav_y", ctypes.c_int32), # y-position in lidar coordinates in mm + ("pose_nav_phi", ctypes.c_uint32), # orientation in lidar coordinates in 0 ... 360000 mdeg + ("pose_opt_valid", ctypes.c_uint16), + # Optional NAV pose data, if pose_opt_valid > 0: + ("pose_opt_output_mode", ctypes.c_uint8), + ("pose_opt_timestamp", ctypes.c_uint32), # lidar timestamp in milliseconds + ("pose_opt_mean_dev", ctypes.c_int32), + ("pose_opt_nav_mode", ctypes.c_uint8), + ("pose_opt_info_state", ctypes.c_uint32), + ("pose_opt_quant_used_reflectors", ctypes.c_uint8), + # NAV reflectors: + ("reflectors", SickScanNavReflectorBuffer) # Array of SickScanNavReflectors + ] + +class SickScanNavOdomVelocityMsg(ctypes.Structure): + """ + NAV-350 velocity/odometry data in nav coordinates, see NAVOdomVelocity.msg + """ + _fields_ = [ + ("vel_x", ctypes.c_float), # x-component of velocity in the coordinate system defined by coordbase (i.e. in lidar coordinate for coordbase=0) in m/s, -32.0 ... +32.0 m/s + ("vel_y", ctypes.c_float), # y-component of velocity in the coordinate system defined by coordbase (i.e. in lidar coordinate for coordbase=0) in m/s, -32.0 ... +32.0 m/s + ("omega", ctypes.c_float), # angular velocity of the NAV350 in radians/s, -2*PI ... +2*PI rad/s + ("timestamp", ctypes.c_uint32), # timestamp of the Velocity vector related to the NAV350 clock + ("coordbase", ctypes.c_uint8) # coordinate system of the velocity vector (local or global), 0 = local coordinate system of the NAV350, 1 = absolute coordinate system + ] + +class SickScanOdomVelocityMsg(ctypes.Structure): + """ + Velocity/odometry data in ros coordinates + """ + _fields_ = [ + ("vel_x", ctypes.c_float), # x-component of velocity in ros coordinates in m/s + ("vel_y", ctypes.c_float), # y-component of velocity in ros coordinates in m/s + ("omega", ctypes.c_float), # angular velocity in radians/s + ("timestamp_sec", ctypes.c_uint32), # seconds part of system timestamp of the odometry data + ("timestamp_nsec", ctypes.c_uint32) # nanoseconds part of system timestamp of the odometry data + ] + class SickScanApiErrorCodes(Enum): # """ Error codes, return values of SickScanApi-functions @@ -747,6 +851,7 @@ def __str__(self): SickScanRadarScanCallback = ctypes.CFUNCTYPE(None, ctypes.c_void_p, ctypes.POINTER(SickScanRadarScan)) # sick_scan_api.h: typedef void(* SickScanRadarScanCallback)(SickScanApiHandle apiHandle, const SickScanRadarScan* msg); SickScanLdmrsObjectArrayCallback = ctypes.CFUNCTYPE(None, ctypes.c_void_p, ctypes.POINTER(SickScanLdmrsObjectArray)) # sick_scan_api.h: typedef void(* SickScanLdmrsObjectArrayCallback)(SickScanApiHandle apiHandle, const SickScanLdmrsObjectArray* msg); SickScanVisualizationMarkerCallback = ctypes.CFUNCTYPE(None, ctypes.c_void_p, ctypes.POINTER(SickScanVisualizationMarkerMsg)) # sick_scan_api.h: typedef void(* SickScanVisualizationMarkerCallback)(SickScanApiHandle apiHandle, const SickScanVisualizationMarkerMsg* msg); +SickScanNavPoseLandmarkCallback = ctypes.CFUNCTYPE(None, ctypes.c_void_p, ctypes.POINTER(SickScanNavPoseLandmarkMsg)) # sick_scan_api.h: typedef void(* SickScanNavPoseLandmarkCallback)(SickScanApiHandle apiHandle, const SickScanNavPoseLandmarkMsg* msg); """ Functions to initialize and close the API and a lidar @@ -881,6 +986,24 @@ def SickScanApiLoadLibrary(paths, lib_filname): # sick_scan_api.h: int32_t SickScanApiFreeVisualizationMarkerMsg(SickScanApiHandle apiHandle, SickScanVisualizationMarkerMsg* msg); sick_scan_library.SickScanApiFreeVisualizationMarkerMsg.argtypes = [ctypes.c_void_p, ctypes.POINTER(SickScanVisualizationMarkerMsg)] sick_scan_library.SickScanApiFreeVisualizationMarkerMsg.restype = ctypes.c_int + # sick_scan_api.h: int32_t SickScanApiRegisterNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkCallback callback); + sick_scan_library.SickScanApiRegisterNavPoseLandmarkMsg.argtypes = [ctypes.c_void_p, SickScanNavPoseLandmarkCallback] + sick_scan_library.SickScanApiRegisterNavPoseLandmarkMsg.restype = ctypes.c_int + # sick_scan_api.h: int32_t SickScanApiDeregisterNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkCallback callback); + sick_scan_library.SickScanApiDeregisterNavPoseLandmarkMsg.argtypes = [ctypes.c_void_p, SickScanNavPoseLandmarkCallback] + sick_scan_library.SickScanApiDeregisterNavPoseLandmarkMsg.restype = ctypes.c_int + # sick_scan_api.h: int32_t SickScanApiWaitNextNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkMsg* msg, double timeout_sec); + sick_scan_library.SickScanApiWaitNextNavPoseLandmarkMsg.argtypes = [ctypes.c_void_p, ctypes.POINTER(SickScanNavPoseLandmarkMsg), ctypes.c_double] + sick_scan_library.SickScanApiWaitNextNavPoseLandmarkMsg.restype = ctypes.c_int + # sick_scan_api.h: int32_t SickScanApiFreeNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkMsg* msg); + sick_scan_library.SickScanApiFreeNavPoseLandmarkMsg.argtypes = [ctypes.c_void_p, ctypes.POINTER(SickScanNavPoseLandmarkMsg)] + sick_scan_library.SickScanApiFreeNavPoseLandmarkMsg.restype = ctypes.c_int + # sick_scan_api.h: int32_t SickScanApiNavOdomVelocityMsg(SickScanApiHandle apiHandle, SickScanNavOdomVelocityMsg* msg); + sick_scan_library.SickScanApiNavOdomVelocityMsg.argtypes = [ctypes.c_void_p, ctypes.POINTER(SickScanNavOdomVelocityMsg)] + sick_scan_library.SickScanApiNavOdomVelocityMsg.restype = ctypes.c_int + # sick_scan_api.h: int32_t SickScanApiOdomVelocityMsg(SickScanApiHandle apiHandle, SickScanOdomVelocityMsg* msg); + sick_scan_library.SickScanApiOdomVelocityMsg.argtypes = [ctypes.c_void_p, ctypes.POINTER(SickScanOdomVelocityMsg)] + sick_scan_library.SickScanApiOdomVelocityMsg.restype = ctypes.c_int return sick_scan_library def SickScanApiUnloadLibrary(sick_scan_library): @@ -1016,6 +1139,18 @@ def SickScanApiDeregisterVisualizationMarkerMsg(sick_scan_library, api_handle, l """ return sick_scan_library.SickScanApiDeregisterVisualizationMarkerMsg(api_handle, ldmrsobjectarray_callback) +def SickScanApiRegisterNavPoseLandmarkMsg(sick_scan_library, api_handle, callback): + """ + Register a callback for SickScanNavPoseLandmarkMsg messages + """ + return sick_scan_library.SickScanApiRegisterNavPoseLandmarkMsg(api_handle, callback) + +def SickScanApiDeregisterNavPoseLandmarkMsg(sick_scan_library, api_handle, callback): + """ + Deregister a callback for SickScanNavPoseLandmarkMsg messages + """ + return sick_scan_library.SickScanApiDeregisterNavPoseLandmarkMsg(api_handle, callback) + """ Polling functions """ @@ -1109,3 +1244,27 @@ def SickScanApiFreeVisualizationMarkerMsg(sick_scan_library, api_handle, msg): Deallocate a VisualizationMarker message, use after SickScanApiWaitNextVisualizationMarkerMsg """ return sick_scan_library.SickScanApiFreeVisualizationMarkerMsg(api_handle, msg) + +def SickScanApiWaitNextNavPoseLandmarkMsg(sick_scan_library, api_handle, msg, timeout_sec): + """ + Wait for and return the next SickScanNavPoseLandmarkMsg message + """ + return sick_scan_library.SickScanApiWaitNextNavPoseLandmarkMsg(api_handle, msg, timeout_sec) + +def SickScanApiFreeNavPoseLandmarkMsg(sick_scan_library, api_handle, msg): + """ + Deallocate a SickScanNavPoseLandmarkMsg message, use after SickScanApiWaitNextNavPoseLandmarkMsg + """ + return sick_scan_library.SickScanApiFreeNavPoseLandmarkMsg(api_handle, msg) + +def SickScanApiNavOdomVelocityMsg(sick_scan_library, api_handle, msg): + """ + Send NAV350 velocity/odometry data in nav coordinates + """ + return sick_scan_library.SickScanApiNavOdomVelocityMsg(api_handle, msg) + +def SickScanApiOdomVelocityMsg(sick_scan_library, api_handle, msg): + """ + Send velocity/odometry data in ros coordinates + """ + return sick_scan_library.SickScanApiOdomVelocityMsg(api_handle, msg) diff --git a/roswrap/src/include/sick_scan/NAVLandmarkData.h b/roswrap/src/include/sick_scan/NAVLandmarkData.h new file mode 100644 index 00000000..82a6f271 --- /dev/null +++ b/roswrap/src/include/sick_scan/NAVLandmarkData.h @@ -0,0 +1,286 @@ +#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */ +// Generated by gencpp from file sick_scan/NAVLandmarkData.msg +// DO NOT EDIT! + + +#ifndef SICK_SCAN_MESSAGE_NAVLANDMARKDATA_H +#define SICK_SCAN_MESSAGE_NAVLANDMARKDATA_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace sick_scan +{ +template +struct NAVLandmarkData_ +{ + typedef NAVLandmarkData_ Type; + + NAVLandmarkData_() + : header() + , landmark_filter(0) + , num_reflectors(0) + , reflectors() { + } + NAVLandmarkData_(const ContainerAllocator& _alloc) + : header(_alloc) + , landmark_filter(0) + , num_reflectors(0) + , reflectors(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint8_t _landmark_filter_type; + _landmark_filter_type landmark_filter; + + typedef uint16_t _num_reflectors_type; + _num_reflectors_type num_reflectors; + + typedef std::vector< ::sick_scan::NAVReflectorData_ , typename std::allocator_traits::template rebind_alloc< ::sick_scan::NAVReflectorData_ >> _reflectors_type; + _reflectors_type reflectors; + + + + + + typedef std::shared_ptr< ::sick_scan::NAVLandmarkData_ > Ptr; + typedef std::shared_ptr< ::sick_scan::NAVLandmarkData_ const> ConstPtr; + +}; // struct NAVLandmarkData_ + +typedef ::sick_scan::NAVLandmarkData_ > NAVLandmarkData; + +typedef std::shared_ptr< ::sick_scan::NAVLandmarkData > NAVLandmarkDataPtr; +typedef std::shared_ptr< ::sick_scan::NAVLandmarkData const> NAVLandmarkDataConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sick_scan::NAVLandmarkData_ & v) +{ +ros::message_operations::Printer< ::sick_scan::NAVLandmarkData_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sick_scan::NAVLandmarkData_ & lhs, const ::sick_scan::NAVLandmarkData_ & rhs) +{ + return lhs.header == rhs.header && + lhs.landmark_filter == rhs.landmark_filter && + lhs.num_reflectors == rhs.num_reflectors && + lhs.reflectors == rhs.reflectors; +} + +template +bool operator!=(const ::sick_scan::NAVLandmarkData_ & lhs, const ::sick_scan::NAVLandmarkData_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sick_scan + +namespace roswrap +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sick_scan::NAVLandmarkData_ > + : TrueType + { }; + +template +struct IsMessage< ::sick_scan::NAVLandmarkData_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sick_scan::NAVLandmarkData_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sick_scan::NAVLandmarkData_ const> + : FalseType + { }; + +template +struct HasHeader< ::sick_scan::NAVLandmarkData_ > + : TrueType + { }; + +template +struct HasHeader< ::sick_scan::NAVLandmarkData_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sick_scan::NAVLandmarkData_ > +{ + static const char* value() + { + return "6978f0aa2a911a9f94328887e912f9fb"; + } + + static const char* value(const ::sick_scan::NAVLandmarkData_&) { return value(); } + static const uint64_t static_value1 = 0x6978f0aa2a911a9fULL; + static const uint64_t static_value2 = 0x94328887e912f9fbULL; +}; + +template +struct DataType< ::sick_scan::NAVLandmarkData_ > +{ + static const char* value() + { + return "sick_scan/NAVLandmarkData"; + } + + static const char* value(const ::sick_scan::NAVLandmarkData_&) { return value(); } +}; + +template +struct Definition< ::sick_scan::NAVLandmarkData_ > +{ + static const char* value() + { + return "# ROS message of NAV350LandmarkData, see sick_nav_scandata_parser.h and NAV-350 telegram listing for details.\n" +"std_msgs/Header header\n" +"\n" +"uint8 landmark_filter\n" +"uint16 num_reflectors\n" +"sick_scan/NAVReflectorData[] reflectors\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +"\n" +"================================================================================\n" +"MSG: sick_scan/NAVReflectorData\n" +"# ROS message of NAV350ReflectorData, see sick_nav_scandata_parser.h and NAV-350 telegram listing for details.\n" +"\n" +"# cartesian data in lidar coordinates in mm\n" +"uint16 cartesian_data_valid\n" +"int32 x\n" +"int32 y\n" +"\n" +"# polar data in lidar coordinates in mm and mdeg\n" +"uint16 polar_data_valid\n" +"uint32 dist\n" +"uint32 phi\n" +"\n" +"# optional reflector data\n" +"uint16 opt_reflector_data_valid\n" +"uint16 local_id\n" +"uint16 global_id\n" +"uint8 type \n" +"uint16 sub_type\n" +"uint16 quality\n" +"uint32 timestamp\n" +"uint16 size\n" +"uint16 hit_count\n" +"uint16 mean_echo\n" +"uint16 start_index\n" +"uint16 end_index\n" +"\n" +"# reflector position in ros coordinates\n" +"int8 pos_valid # pose_x, pose_y and pose_yaw are valid if pose_valid > 0\n" +"float32 pos_x # x position in ros coordinates in m\n" +"float32 pos_y # y position in ros coordinates in m\n" +; + } + + static const char* value(const ::sick_scan::NAVLandmarkData_&) { return value(); } +}; + +} // namespace message_traits +} // namespace roswrap + +namespace roswrap +{ +namespace serialization +{ + + template struct Serializer< ::sick_scan::NAVLandmarkData_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.landmark_filter); + stream.next(m.num_reflectors); + stream.next(m.reflectors); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct NAVLandmarkData_ + +} // namespace serialization +} // namespace roswrap + +namespace roswrap +{ +namespace message_operations +{ + +template +struct Printer< ::sick_scan::NAVLandmarkData_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sick_scan::NAVLandmarkData_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "landmark_filter: "; + Printer::stream(s, indent + " ", v.landmark_filter); + s << indent << "num_reflectors: "; + Printer::stream(s, indent + " ", v.num_reflectors); + s << indent << "reflectors[]" << std::endl; + for (size_t i = 0; i < v.reflectors.size(); ++i) + { + s << indent << " reflectors[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::sick_scan::NAVReflectorData_ >::stream(s, indent + " ", v.reflectors[i]); + } + } +}; + +} // namespace message_operations +} // namespace roswrap + +#endif // SICK_SCAN_MESSAGE_NAVLANDMARKDATA_H diff --git a/roswrap/src/include/sick_scan/NAVOdomVelocity.h b/roswrap/src/include/sick_scan/NAVOdomVelocity.h new file mode 100644 index 00000000..2d8948f7 --- /dev/null +++ b/roswrap/src/include/sick_scan/NAVOdomVelocity.h @@ -0,0 +1,238 @@ +#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */ +// Generated by gencpp from file sick_scan/NAVOdomVelocity.msg +// DO NOT EDIT! + + +#ifndef SICK_SCAN_MESSAGE_NAVODOMVELOCITY_H +#define SICK_SCAN_MESSAGE_NAVODOMVELOCITY_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace sick_scan +{ +template +struct NAVOdomVelocity_ +{ + typedef NAVOdomVelocity_ Type; + + NAVOdomVelocity_() + : vel_x(0.0) + , vel_y(0.0) + , omega(0.0) + , timestamp(0) + , coordbase(0) { + } + NAVOdomVelocity_(const ContainerAllocator& _alloc) + : vel_x(0.0) + , vel_y(0.0) + , omega(0.0) + , timestamp(0) + , coordbase(0) { + (void)_alloc; + } + + + + typedef float _vel_x_type; + _vel_x_type vel_x; + + typedef float _vel_y_type; + _vel_y_type vel_y; + + typedef float _omega_type; + _omega_type omega; + + typedef uint32_t _timestamp_type; + _timestamp_type timestamp; + + typedef uint8_t _coordbase_type; + _coordbase_type coordbase; + + + + + + typedef std::shared_ptr< ::sick_scan::NAVOdomVelocity_ > Ptr; + typedef std::shared_ptr< ::sick_scan::NAVOdomVelocity_ const> ConstPtr; + +}; // struct NAVOdomVelocity_ + +typedef ::sick_scan::NAVOdomVelocity_ > NAVOdomVelocity; + +typedef std::shared_ptr< ::sick_scan::NAVOdomVelocity > NAVOdomVelocityPtr; +typedef std::shared_ptr< ::sick_scan::NAVOdomVelocity const> NAVOdomVelocityConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sick_scan::NAVOdomVelocity_ & v) +{ +ros::message_operations::Printer< ::sick_scan::NAVOdomVelocity_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sick_scan::NAVOdomVelocity_ & lhs, const ::sick_scan::NAVOdomVelocity_ & rhs) +{ + return lhs.vel_x == rhs.vel_x && + lhs.vel_y == rhs.vel_y && + lhs.omega == rhs.omega && + lhs.timestamp == rhs.timestamp && + lhs.coordbase == rhs.coordbase; +} + +template +bool operator!=(const ::sick_scan::NAVOdomVelocity_ & lhs, const ::sick_scan::NAVOdomVelocity_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sick_scan + +namespace roswrap +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sick_scan::NAVOdomVelocity_ > + : TrueType + { }; + +template +struct IsMessage< ::sick_scan::NAVOdomVelocity_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sick_scan::NAVOdomVelocity_ > + : TrueType + { }; + +template +struct IsFixedSize< ::sick_scan::NAVOdomVelocity_ const> + : TrueType + { }; + +template +struct HasHeader< ::sick_scan::NAVOdomVelocity_ > + : FalseType + { }; + +template +struct HasHeader< ::sick_scan::NAVOdomVelocity_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sick_scan::NAVOdomVelocity_ > +{ + static const char* value() + { + return "9ae8fb3e58991f0b8bd5ceefb3e1ca1d"; + } + + static const char* value(const ::sick_scan::NAVOdomVelocity_&) { return value(); } + static const uint64_t static_value1 = 0x9ae8fb3e58991f0bULL; + static const uint64_t static_value2 = 0x8bd5ceefb3e1ca1dULL; +}; + +template +struct DataType< ::sick_scan::NAVOdomVelocity_ > +{ + static const char* value() + { + return "sick_scan/NAVOdomVelocity"; + } + + static const char* value(const ::sick_scan::NAVOdomVelocity_&) { return value(); } +}; + +template +struct Definition< ::sick_scan::NAVOdomVelocity_ > +{ + static const char* value() + { + return "# ROS message for NAV350 velocity/odometry data, see sick_nav_scandata_parser.h and NAV-350 telegram listing for details (\"sMN mNPOSSetSpeed\").\n" +"\n" +"float32 vel_x # x-component of velocity in the coordinate system defined by coordbase in m/s, -32.0 ... +32.0 m/s\n" +"float32 vel_y # y-component of velocity in the coordinate system defined by coordbase in m/s, -32.0 ... +32.0 m/s\n" +"float32 omega # angular velocity of the NAV350 in radians/s, -2*PI ... +2*PI rad/s\n" +"uint32 timestamp # Timestamp of the Velocity vector related to the NAV350 clock\n" +"uint8 coordbase # Coordinate system of the velocity vector (local or global), 0 = local coordinate system of the NAV350, 1 = absolute coordinate system\n" +; + } + + static const char* value(const ::sick_scan::NAVOdomVelocity_&) { return value(); } +}; + +} // namespace message_traits +} // namespace roswrap + +namespace roswrap +{ +namespace serialization +{ + + template struct Serializer< ::sick_scan::NAVOdomVelocity_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.vel_x); + stream.next(m.vel_y); + stream.next(m.omega); + stream.next(m.timestamp); + stream.next(m.coordbase); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct NAVOdomVelocity_ + +} // namespace serialization +} // namespace roswrap + +namespace roswrap +{ +namespace message_operations +{ + +template +struct Printer< ::sick_scan::NAVOdomVelocity_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sick_scan::NAVOdomVelocity_& v) + { + s << indent << "vel_x: "; + Printer::stream(s, indent + " ", v.vel_x); + s << indent << "vel_y: "; + Printer::stream(s, indent + " ", v.vel_y); + s << indent << "omega: "; + Printer::stream(s, indent + " ", v.omega); + s << indent << "timestamp: "; + Printer::stream(s, indent + " ", v.timestamp); + s << indent << "coordbase: "; + Printer::stream(s, indent + " ", v.coordbase); + } +}; + +} // namespace message_operations +} // namespace roswrap + +#endif // SICK_SCAN_MESSAGE_NAVODOMVELOCITY_H diff --git a/roswrap/src/include/sick_scan/NAVPoseData.h b/roswrap/src/include/sick_scan/NAVPoseData.h new file mode 100644 index 00000000..12abe701 --- /dev/null +++ b/roswrap/src/include/sick_scan/NAVPoseData.h @@ -0,0 +1,360 @@ +#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */ +// Generated by gencpp from file sick_scan/NAVPoseData.msg +// DO NOT EDIT! + + +#ifndef SICK_SCAN_MESSAGE_NAVPOSEDATA_H +#define SICK_SCAN_MESSAGE_NAVPOSEDATA_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sick_scan +{ +template +struct NAVPoseData_ +{ + typedef NAVPoseData_ Type; + + NAVPoseData_() + : header() + , x(0) + , y(0) + , phi(0) + , opt_pose_data_valid(0) + , output_mode(0) + , timestamp(0) + , mean_dev(0) + , nav_mode(0) + , info_state(0) + , quant_used_reflectors(0) + , pose_valid(0) + , pose_x(0.0) + , pose_y(0.0) + , pose_yaw(0.0) { + } + NAVPoseData_(const ContainerAllocator& _alloc) + : header(_alloc) + , x(0) + , y(0) + , phi(0) + , opt_pose_data_valid(0) + , output_mode(0) + , timestamp(0) + , mean_dev(0) + , nav_mode(0) + , info_state(0) + , quant_used_reflectors(0) + , pose_valid(0) + , pose_x(0.0) + , pose_y(0.0) + , pose_yaw(0.0) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef int32_t _x_type; + _x_type x; + + typedef int32_t _y_type; + _y_type y; + + typedef uint32_t _phi_type; + _phi_type phi; + + typedef uint16_t _opt_pose_data_valid_type; + _opt_pose_data_valid_type opt_pose_data_valid; + + typedef uint8_t _output_mode_type; + _output_mode_type output_mode; + + typedef uint32_t _timestamp_type; + _timestamp_type timestamp; + + typedef int32_t _mean_dev_type; + _mean_dev_type mean_dev; + + typedef uint8_t _nav_mode_type; + _nav_mode_type nav_mode; + + typedef uint32_t _info_state_type; + _info_state_type info_state; + + typedef uint8_t _quant_used_reflectors_type; + _quant_used_reflectors_type quant_used_reflectors; + + typedef int8_t _pose_valid_type; + _pose_valid_type pose_valid; + + typedef float _pose_x_type; + _pose_x_type pose_x; + + typedef float _pose_y_type; + _pose_y_type pose_y; + + typedef float _pose_yaw_type; + _pose_yaw_type pose_yaw; + + + + + + typedef std::shared_ptr< ::sick_scan::NAVPoseData_ > Ptr; + typedef std::shared_ptr< ::sick_scan::NAVPoseData_ const> ConstPtr; + +}; // struct NAVPoseData_ + +typedef ::sick_scan::NAVPoseData_ > NAVPoseData; + +typedef std::shared_ptr< ::sick_scan::NAVPoseData > NAVPoseDataPtr; +typedef std::shared_ptr< ::sick_scan::NAVPoseData const> NAVPoseDataConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sick_scan::NAVPoseData_ & v) +{ +ros::message_operations::Printer< ::sick_scan::NAVPoseData_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sick_scan::NAVPoseData_ & lhs, const ::sick_scan::NAVPoseData_ & rhs) +{ + return lhs.header == rhs.header && + lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.phi == rhs.phi && + lhs.opt_pose_data_valid == rhs.opt_pose_data_valid && + lhs.output_mode == rhs.output_mode && + lhs.timestamp == rhs.timestamp && + lhs.mean_dev == rhs.mean_dev && + lhs.nav_mode == rhs.nav_mode && + lhs.info_state == rhs.info_state && + lhs.quant_used_reflectors == rhs.quant_used_reflectors && + lhs.pose_valid == rhs.pose_valid && + lhs.pose_x == rhs.pose_x && + lhs.pose_y == rhs.pose_y && + lhs.pose_yaw == rhs.pose_yaw; +} + +template +bool operator!=(const ::sick_scan::NAVPoseData_ & lhs, const ::sick_scan::NAVPoseData_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sick_scan + +namespace roswrap +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sick_scan::NAVPoseData_ > + : TrueType + { }; + +template +struct IsMessage< ::sick_scan::NAVPoseData_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sick_scan::NAVPoseData_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sick_scan::NAVPoseData_ const> + : FalseType + { }; + +template +struct HasHeader< ::sick_scan::NAVPoseData_ > + : TrueType + { }; + +template +struct HasHeader< ::sick_scan::NAVPoseData_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sick_scan::NAVPoseData_ > +{ + static const char* value() + { + return "09a85c46d8eba44e26ad07869c2bbc20"; + } + + static const char* value(const ::sick_scan::NAVPoseData_&) { return value(); } + static const uint64_t static_value1 = 0x09a85c46d8eba44eULL; + static const uint64_t static_value2 = 0x26ad07869c2bbc20ULL; +}; + +template +struct DataType< ::sick_scan::NAVPoseData_ > +{ + static const char* value() + { + return "sick_scan/NAVPoseData"; + } + + static const char* value(const ::sick_scan::NAVPoseData_&) { return value(); } +}; + +template +struct Definition< ::sick_scan::NAVPoseData_ > +{ + static const char* value() + { + return "# ROS message of NAV350PoseData, see sick_nav_scandata_parser.h and NAV-350 telegram listing for details.\n" +"std_msgs/Header header\n" +"\n" +"# pose in lidar coordinates in mm and mdeg\n" +"int32 x\n" +"int32 y\n" +"uint32 phi\n" +"# optional pose data\n" +"uint16 opt_pose_data_valid\n" +"uint8 output_mode\n" +"uint32 timestamp\n" +"int32 mean_dev\n" +"uint8 nav_mode\n" +"uint32 info_state\n" +"uint8 quant_used_reflectors\n" +"\n" +"# pose in ros coordinates\n" +"int8 pose_valid # pose_x, pose_y and pose_yaw are valid if pose_valid > 0\n" +"float32 pose_x # x position in ros coordinates in m\n" +"float32 pose_y # y position in ros coordinates in m\n" +"float32 pose_yaw # yaw angle in radians\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +; + } + + static const char* value(const ::sick_scan::NAVPoseData_&) { return value(); } +}; + +} // namespace message_traits +} // namespace roswrap + +namespace roswrap +{ +namespace serialization +{ + + template struct Serializer< ::sick_scan::NAVPoseData_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.x); + stream.next(m.y); + stream.next(m.phi); + stream.next(m.opt_pose_data_valid); + stream.next(m.output_mode); + stream.next(m.timestamp); + stream.next(m.mean_dev); + stream.next(m.nav_mode); + stream.next(m.info_state); + stream.next(m.quant_used_reflectors); + stream.next(m.pose_valid); + stream.next(m.pose_x); + stream.next(m.pose_y); + stream.next(m.pose_yaw); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct NAVPoseData_ + +} // namespace serialization +} // namespace roswrap + +namespace roswrap +{ +namespace message_operations +{ + +template +struct Printer< ::sick_scan::NAVPoseData_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sick_scan::NAVPoseData_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "phi: "; + Printer::stream(s, indent + " ", v.phi); + s << indent << "opt_pose_data_valid: "; + Printer::stream(s, indent + " ", v.opt_pose_data_valid); + s << indent << "output_mode: "; + Printer::stream(s, indent + " ", v.output_mode); + s << indent << "timestamp: "; + Printer::stream(s, indent + " ", v.timestamp); + s << indent << "mean_dev: "; + Printer::stream(s, indent + " ", v.mean_dev); + s << indent << "nav_mode: "; + Printer::stream(s, indent + " ", v.nav_mode); + s << indent << "info_state: "; + Printer::stream(s, indent + " ", v.info_state); + s << indent << "quant_used_reflectors: "; + Printer::stream(s, indent + " ", v.quant_used_reflectors); + s << indent << "pose_valid: "; + Printer::stream(s, indent + " ", v.pose_valid); + s << indent << "pose_x: "; + Printer::stream(s, indent + " ", v.pose_x); + s << indent << "pose_y: "; + Printer::stream(s, indent + " ", v.pose_y); + s << indent << "pose_yaw: "; + Printer::stream(s, indent + " ", v.pose_yaw); + } +}; + +} // namespace message_operations +} // namespace roswrap + +#endif // SICK_SCAN_MESSAGE_NAVPOSEDATA_H diff --git a/roswrap/src/include/sick_scan/NAVReflectorData.h b/roswrap/src/include/sick_scan/NAVReflectorData.h new file mode 100644 index 00000000..ef698415 --- /dev/null +++ b/roswrap/src/include/sick_scan/NAVReflectorData.h @@ -0,0 +1,405 @@ +#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */ +// Generated by gencpp from file sick_scan/NAVReflectorData.msg +// DO NOT EDIT! + + +#ifndef SICK_SCAN_MESSAGE_NAVREFLECTORDATA_H +#define SICK_SCAN_MESSAGE_NAVREFLECTORDATA_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace sick_scan +{ +template +struct NAVReflectorData_ +{ + typedef NAVReflectorData_ Type; + + NAVReflectorData_() + : cartesian_data_valid(0) + , x(0) + , y(0) + , polar_data_valid(0) + , dist(0) + , phi(0) + , opt_reflector_data_valid(0) + , local_id(0) + , global_id(0) + , type(0) + , sub_type(0) + , quality(0) + , timestamp(0) + , size(0) + , hit_count(0) + , mean_echo(0) + , start_index(0) + , end_index(0) + , pos_valid(0) + , pos_x(0.0) + , pos_y(0.0) { + } + NAVReflectorData_(const ContainerAllocator& _alloc) + : cartesian_data_valid(0) + , x(0) + , y(0) + , polar_data_valid(0) + , dist(0) + , phi(0) + , opt_reflector_data_valid(0) + , local_id(0) + , global_id(0) + , type(0) + , sub_type(0) + , quality(0) + , timestamp(0) + , size(0) + , hit_count(0) + , mean_echo(0) + , start_index(0) + , end_index(0) + , pos_valid(0) + , pos_x(0.0) + , pos_y(0.0) { + (void)_alloc; + } + + + + typedef uint16_t _cartesian_data_valid_type; + _cartesian_data_valid_type cartesian_data_valid; + + typedef int32_t _x_type; + _x_type x; + + typedef int32_t _y_type; + _y_type y; + + typedef uint16_t _polar_data_valid_type; + _polar_data_valid_type polar_data_valid; + + typedef uint32_t _dist_type; + _dist_type dist; + + typedef uint32_t _phi_type; + _phi_type phi; + + typedef uint16_t _opt_reflector_data_valid_type; + _opt_reflector_data_valid_type opt_reflector_data_valid; + + typedef uint16_t _local_id_type; + _local_id_type local_id; + + typedef uint16_t _global_id_type; + _global_id_type global_id; + + typedef uint8_t _type_type; + _type_type type; + + typedef uint16_t _sub_type_type; + _sub_type_type sub_type; + + typedef uint16_t _quality_type; + _quality_type quality; + + typedef uint32_t _timestamp_type; + _timestamp_type timestamp; + + typedef uint16_t _size_type; + _size_type size; + + typedef uint16_t _hit_count_type; + _hit_count_type hit_count; + + typedef uint16_t _mean_echo_type; + _mean_echo_type mean_echo; + + typedef uint16_t _start_index_type; + _start_index_type start_index; + + typedef uint16_t _end_index_type; + _end_index_type end_index; + + typedef int8_t _pos_valid_type; + _pos_valid_type pos_valid; + + typedef float _pos_x_type; + _pos_x_type pos_x; + + typedef float _pos_y_type; + _pos_y_type pos_y; + + + + + + typedef std::shared_ptr< ::sick_scan::NAVReflectorData_ > Ptr; + typedef std::shared_ptr< ::sick_scan::NAVReflectorData_ const> ConstPtr; + +}; // struct NAVReflectorData_ + +typedef ::sick_scan::NAVReflectorData_ > NAVReflectorData; + +typedef std::shared_ptr< ::sick_scan::NAVReflectorData > NAVReflectorDataPtr; +typedef std::shared_ptr< ::sick_scan::NAVReflectorData const> NAVReflectorDataConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sick_scan::NAVReflectorData_ & v) +{ +ros::message_operations::Printer< ::sick_scan::NAVReflectorData_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sick_scan::NAVReflectorData_ & lhs, const ::sick_scan::NAVReflectorData_ & rhs) +{ + return lhs.cartesian_data_valid == rhs.cartesian_data_valid && + lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.polar_data_valid == rhs.polar_data_valid && + lhs.dist == rhs.dist && + lhs.phi == rhs.phi && + lhs.opt_reflector_data_valid == rhs.opt_reflector_data_valid && + lhs.local_id == rhs.local_id && + lhs.global_id == rhs.global_id && + lhs.type == rhs.type && + lhs.sub_type == rhs.sub_type && + lhs.quality == rhs.quality && + lhs.timestamp == rhs.timestamp && + lhs.size == rhs.size && + lhs.hit_count == rhs.hit_count && + lhs.mean_echo == rhs.mean_echo && + lhs.start_index == rhs.start_index && + lhs.end_index == rhs.end_index && + lhs.pos_valid == rhs.pos_valid && + lhs.pos_x == rhs.pos_x && + lhs.pos_y == rhs.pos_y; +} + +template +bool operator!=(const ::sick_scan::NAVReflectorData_ & lhs, const ::sick_scan::NAVReflectorData_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sick_scan + +namespace roswrap +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sick_scan::NAVReflectorData_ > + : TrueType + { }; + +template +struct IsMessage< ::sick_scan::NAVReflectorData_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sick_scan::NAVReflectorData_ > + : TrueType + { }; + +template +struct IsFixedSize< ::sick_scan::NAVReflectorData_ const> + : TrueType + { }; + +template +struct HasHeader< ::sick_scan::NAVReflectorData_ > + : FalseType + { }; + +template +struct HasHeader< ::sick_scan::NAVReflectorData_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::sick_scan::NAVReflectorData_ > +{ + static const char* value() + { + return "c834b1997d0f1a7b5b282c13153f6e67"; + } + + static const char* value(const ::sick_scan::NAVReflectorData_&) { return value(); } + static const uint64_t static_value1 = 0xc834b1997d0f1a7bULL; + static const uint64_t static_value2 = 0x5b282c13153f6e67ULL; +}; + +template +struct DataType< ::sick_scan::NAVReflectorData_ > +{ + static const char* value() + { + return "sick_scan/NAVReflectorData"; + } + + static const char* value(const ::sick_scan::NAVReflectorData_&) { return value(); } +}; + +template +struct Definition< ::sick_scan::NAVReflectorData_ > +{ + static const char* value() + { + return "# ROS message of NAV350ReflectorData, see sick_nav_scandata_parser.h and NAV-350 telegram listing for details.\n" +"\n" +"# cartesian data in lidar coordinates in mm\n" +"uint16 cartesian_data_valid\n" +"int32 x\n" +"int32 y\n" +"\n" +"# polar data in lidar coordinates in mm and mdeg\n" +"uint16 polar_data_valid\n" +"uint32 dist\n" +"uint32 phi\n" +"\n" +"# optional reflector data\n" +"uint16 opt_reflector_data_valid\n" +"uint16 local_id\n" +"uint16 global_id\n" +"uint8 type \n" +"uint16 sub_type\n" +"uint16 quality\n" +"uint32 timestamp\n" +"uint16 size\n" +"uint16 hit_count\n" +"uint16 mean_echo\n" +"uint16 start_index\n" +"uint16 end_index\n" +"\n" +"# reflector position in ros coordinates\n" +"int8 pos_valid # pose_x, pose_y and pose_yaw are valid if pose_valid > 0\n" +"float32 pos_x # x position in ros coordinates in m\n" +"float32 pos_y # y position in ros coordinates in m\n" +; + } + + static const char* value(const ::sick_scan::NAVReflectorData_&) { return value(); } +}; + +} // namespace message_traits +} // namespace roswrap + +namespace roswrap +{ +namespace serialization +{ + + template struct Serializer< ::sick_scan::NAVReflectorData_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.cartesian_data_valid); + stream.next(m.x); + stream.next(m.y); + stream.next(m.polar_data_valid); + stream.next(m.dist); + stream.next(m.phi); + stream.next(m.opt_reflector_data_valid); + stream.next(m.local_id); + stream.next(m.global_id); + stream.next(m.type); + stream.next(m.sub_type); + stream.next(m.quality); + stream.next(m.timestamp); + stream.next(m.size); + stream.next(m.hit_count); + stream.next(m.mean_echo); + stream.next(m.start_index); + stream.next(m.end_index); + stream.next(m.pos_valid); + stream.next(m.pos_x); + stream.next(m.pos_y); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct NAVReflectorData_ + +} // namespace serialization +} // namespace roswrap + +namespace roswrap +{ +namespace message_operations +{ + +template +struct Printer< ::sick_scan::NAVReflectorData_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sick_scan::NAVReflectorData_& v) + { + s << indent << "cartesian_data_valid: "; + Printer::stream(s, indent + " ", v.cartesian_data_valid); + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "polar_data_valid: "; + Printer::stream(s, indent + " ", v.polar_data_valid); + s << indent << "dist: "; + Printer::stream(s, indent + " ", v.dist); + s << indent << "phi: "; + Printer::stream(s, indent + " ", v.phi); + s << indent << "opt_reflector_data_valid: "; + Printer::stream(s, indent + " ", v.opt_reflector_data_valid); + s << indent << "local_id: "; + Printer::stream(s, indent + " ", v.local_id); + s << indent << "global_id: "; + Printer::stream(s, indent + " ", v.global_id); + s << indent << "type: "; + Printer::stream(s, indent + " ", v.type); + s << indent << "sub_type: "; + Printer::stream(s, indent + " ", v.sub_type); + s << indent << "quality: "; + Printer::stream(s, indent + " ", v.quality); + s << indent << "timestamp: "; + Printer::stream(s, indent + " ", v.timestamp); + s << indent << "size: "; + Printer::stream(s, indent + " ", v.size); + s << indent << "hit_count: "; + Printer::stream(s, indent + " ", v.hit_count); + s << indent << "mean_echo: "; + Printer::stream(s, indent + " ", v.mean_echo); + s << indent << "start_index: "; + Printer::stream(s, indent + " ", v.start_index); + s << indent << "end_index: "; + Printer::stream(s, indent + " ", v.end_index); + s << indent << "pos_valid: "; + Printer::stream(s, indent + " ", v.pos_valid); + s << indent << "pos_x: "; + Printer::stream(s, indent + " ", v.pos_x); + s << indent << "pos_y: "; + Printer::stream(s, indent + " ", v.pos_y); + } +}; + +} // namespace message_operations +} // namespace roswrap + +#endif // SICK_SCAN_MESSAGE_NAVREFLECTORDATA_H diff --git a/roswrap/src/rossimu/kinetic/src/rossimu.cpp b/roswrap/src/rossimu/kinetic/src/rossimu.cpp index e2e73444..9a8575e0 100644 --- a/roswrap/src/rossimu/kinetic/src/rossimu.cpp +++ b/roswrap/src/rossimu/kinetic/src/rossimu.cpp @@ -440,7 +440,7 @@ int rossimu_settings(ros::NodeHandle& nhPriv) { // set all parameters, which are necessary for debugging without using roslaunch. Just start roscore at the beginning of your debug session int tmpactive_echos = 1; - int tmpEcho_filter = 1; + int tmpEcho_filter = 2; bool tmpauto_reboot = true; std::string tmpframe_id = "cloud"; std::string scannerName; @@ -456,11 +456,6 @@ int rossimu_settings(ros::NodeHandle& nhPriv) tmpintensity = true; tmphostname = "192.168.0.24"; } - if (scannerName.compare("sick_rms_3xx") == 0) - { - tmpintensity = true; - tmphostname = "192.168.0.232"; - } double tmpminang = -60 / 180.0 * M_PI; // MRS6124-TEST with +/- 30� double tmpmaxang = +60 / 180.0 * M_PI; diff --git a/test/emulator/config/rviz2_cfg_mrs100_emu.rviz b/test/emulator/config/rviz2_cfg_multiscan_emu.rviz similarity index 100% rename from test/emulator/config/rviz2_cfg_mrs100_emu.rviz rename to test/emulator/config/rviz2_cfg_multiscan_emu.rviz diff --git a/test/emulator/config/rviz2_cfg_mrs100_emu_360.rviz b/test/emulator/config/rviz2_cfg_multiscan_emu_360.rviz similarity index 97% rename from test/emulator/config/rviz2_cfg_mrs100_emu_360.rviz rename to test/emulator/config/rviz2_cfg_multiscan_emu_360.rviz index 379aa47e..2f40d499 100644 --- a/test/emulator/config/rviz2_cfg_mrs100_emu_360.rviz +++ b/test/emulator/config/rviz2_cfg_multiscan_emu_360.rviz @@ -57,9 +57,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 2.2205970287323 + Max Intensity: 2.2196974754333496 Min Color: 0; 0; 0 - Min Intensity: -1.7436585426330566 + Min Intensity: -1.7437893152236938 Name: PointCloud2 Position Transformer: XYZ Selectable: true @@ -71,7 +71,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /cloud_360 + Value: /cloud_fullframe Use Fixed Frame: true Use rainbow: true Value: true diff --git a/test/emulator/config/rviz2_cfg_mrs100_emu_laserscan.rviz b/test/emulator/config/rviz2_cfg_multiscan_emu_laserscan.rviz similarity index 99% rename from test/emulator/config/rviz2_cfg_mrs100_emu_laserscan.rviz rename to test/emulator/config/rviz2_cfg_multiscan_emu_laserscan.rviz index a6a0b26c..a66e9cd2 100644 --- a/test/emulator/config/rviz2_cfg_mrs100_emu_laserscan.rviz +++ b/test/emulator/config/rviz2_cfg_multiscan_emu_laserscan.rviz @@ -151,7 +151,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world_5 + Fixed Frame: world_0 Frame Rate: 30 Name: root Tools: diff --git a/test/emulator/config/rviz2_cfg_mrs100_emu_pcap.rviz b/test/emulator/config/rviz2_cfg_multiscan_emu_pcap.rviz similarity index 100% rename from test/emulator/config/rviz2_cfg_mrs100_emu_pcap.rviz rename to test/emulator/config/rviz2_cfg_multiscan_emu_pcap.rviz diff --git a/test/emulator/config/rviz2_cfg_mrs100_windows.rviz b/test/emulator/config/rviz2_cfg_multiscan_windows.rviz similarity index 100% rename from test/emulator/config/rviz2_cfg_mrs100_windows.rviz rename to test/emulator/config/rviz2_cfg_multiscan_windows.rviz diff --git a/test/emulator/config/rviz2_cfg_mrs100_windows_360.rviz b/test/emulator/config/rviz2_cfg_multiscan_windows_360.rviz similarity index 97% rename from test/emulator/config/rviz2_cfg_mrs100_windows_360.rviz rename to test/emulator/config/rviz2_cfg_multiscan_windows_360.rviz index 201d7fca..a9c61893 100644 --- a/test/emulator/config/rviz2_cfg_mrs100_windows_360.rviz +++ b/test/emulator/config/rviz2_cfg_multiscan_windows_360.rviz @@ -58,9 +58,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 2.2988598346710205 + Max Intensity: 2.2205970287323 Min Color: 0; 0; 0 - Min Intensity: -1.6527236700057983 + Min Intensity: -1.7456190586090088 Name: PointCloud2 Position Transformer: XYZ Selectable: true @@ -72,7 +72,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /cloud_360 + Value: /cloud_fullframe Use Fixed Frame: true Use rainbow: true Value: true diff --git a/test/emulator/config/rviz2_cfg_mrs100_windows_laserscan.rviz b/test/emulator/config/rviz2_cfg_multiscan_windows_laserscan.rviz similarity index 100% rename from test/emulator/config/rviz2_cfg_mrs100_windows_laserscan.rviz rename to test/emulator/config/rviz2_cfg_multiscan_windows_laserscan.rviz diff --git a/test/emulator/config/rviz2_cfg_nav350_windows.rviz b/test/emulator/config/rviz2_cfg_nav350_windows.rviz new file mode 100644 index 00000000..4145ee36 --- /dev/null +++ b/test/emulator/config/rviz2_cfg_nav350_windows.rviz @@ -0,0 +1,196 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 759 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 755 + Min Color: 0; 0; 0 + Min Intensity: 32 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + sick_scan: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sick_nav_350/nav_reflectors + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + cloud: + Value: true + nav: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + cloud: + nav: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: nav + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 5.277319431304932 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.135401487350464 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 978 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000014200000381fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003700000381000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002fdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000037000002fd000000a100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004250000038100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1387 + X: 998 + Y: 9 diff --git a/test/emulator/config/rviz2_cfg_rmsxxxx_windows.rviz b/test/emulator/config/rviz2_cfg_rmsxxxx_windows.rviz new file mode 100644 index 00000000..e0607aee --- /dev/null +++ b/test/emulator/config/rviz2_cfg_rmsxxxx_windows.rviz @@ -0,0 +1,223 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /PointCloud21 + - /PointCloud22 + Splitter Ratio: 0.5562499761581421 + Tree Height: 582 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sick_nav_350/nav_reflectors + Value: false + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: x + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 12.825773239135742 + Min Color: 0; 0; 0 + Min Intensity: 2.146681308746338 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 10 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sick_rms_xxxx/cloud_radar_rawtarget + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: objLen + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 10 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sick_rms_xxxx/cloud_radar_track + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: radar + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 19.97243881225586 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 4.059158802032471 + Y: -0.9834800362586975 + Z: 0.004065176006406546 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.135401487350464 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 801 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000142000002d0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000037000002d0000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002fdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000037000002fd000000a100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002d9000002d000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1055 + X: 998 + Y: 9 diff --git a/test/emulator/config/rviz2_emulator_cfg_nav350.rviz b/test/emulator/config/rviz2_emulator_cfg_nav350.rviz new file mode 100644 index 00000000..815f34d5 --- /dev/null +++ b/test/emulator/config/rviz2_emulator_cfg_nav350.rviz @@ -0,0 +1,222 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 721 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 713 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 713 + Min Color: 0; 0; 0 + Min Intensity: 25 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sick_nav_350/scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + sick_scan: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sick_nav_350/nav_reflectors + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + cloud: + Value: true + nav: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + cloud: + nav: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: cloud + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 4.811324119567871 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5597608089447021 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 3.1404106616973877 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 950 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001560000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000441fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000441000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003810000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1245 + X: 1055 + Y: 35 diff --git a/test/emulator/config/rviz_cfg_mrs100_emu.rviz b/test/emulator/config/rviz_cfg_multiscan_emu.rviz similarity index 100% rename from test/emulator/config/rviz_cfg_mrs100_emu.rviz rename to test/emulator/config/rviz_cfg_multiscan_emu.rviz diff --git a/test/emulator/config/rviz_cfg_mrs100_emu_360.rviz b/test/emulator/config/rviz_cfg_multiscan_emu_360.rviz similarity index 98% rename from test/emulator/config/rviz_cfg_mrs100_emu_360.rviz rename to test/emulator/config/rviz_cfg_multiscan_emu_360.rviz index 8ded1872..d6b9a492 100644 --- a/test/emulator/config/rviz_cfg_mrs100_emu_360.rviz +++ b/test/emulator/config/rviz_cfg_multiscan_emu_360.rviz @@ -25,7 +25,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Preferences: PromptSaveOnExit: true Toolbars: @@ -74,7 +74,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points - Topic: /cloud_360 + Topic: /cloud_fullframe Unreliable: false Use Fixed Frame: true Use rainbow: true diff --git a/test/emulator/config/rviz_cfg_mrs100_emu_360_2nd.rviz b/test/emulator/config/rviz_cfg_multiscan_emu_360_2nd.rviz similarity index 100% rename from test/emulator/config/rviz_cfg_mrs100_emu_360_2nd.rviz rename to test/emulator/config/rviz_cfg_multiscan_emu_360_2nd.rviz diff --git a/test/emulator/config/rviz_cfg_mrs100_emu_laserscan.rviz b/test/emulator/config/rviz_cfg_multiscan_emu_laserscan.rviz similarity index 97% rename from test/emulator/config/rviz_cfg_mrs100_emu_laserscan.rviz rename to test/emulator/config/rviz_cfg_multiscan_emu_laserscan.rviz index 6b1418de..7fc16ae4 100644 --- a/test/emulator/config/rviz_cfg_mrs100_emu_laserscan.rviz +++ b/test/emulator/config/rviz_cfg_multiscan_emu_laserscan.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /PointCloud22 + - /LaserScan1/Status1 Splitter Ratio: 0.5 Tree Height: 373 - Class: rviz/Selection @@ -25,7 +25,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: LaserScan Preferences: PromptSaveOnExit: true Toolbars: @@ -90,8 +90,8 @@ Visualization Manager: Class: rviz/LaserScan Color: 255; 255; 255 Color Transformer: Intensity - Decay Time: 10 - Enabled: false + Decay Time: 1 + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -102,11 +102,11 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.009999999776482582 Style: Points - Topic: /sick_scansegment_xd/scan_segment + Topic: /multiScan/scan_segment Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Class: rviz/Axes Enabled: true @@ -128,7 +128,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -143,12 +143,12 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: world + Fixed Frame: world_0 Frame Rate: 30 Name: root Tools: @@ -209,4 +209,4 @@ Window Geometry: collapsed: true Width: 1048 X: 1303 - Y: 556 + Y: 519 diff --git a/test/emulator/config/rviz_emulator_api_mrs100.rviz b/test/emulator/config/rviz_emulator_api_multiscan.rviz similarity index 97% rename from test/emulator/config/rviz_emulator_api_mrs100.rviz rename to test/emulator/config/rviz_emulator_api_multiscan.rviz index 3128c724..0889bf6c 100644 --- a/test/emulator/config/rviz_emulator_api_mrs100.rviz +++ b/test/emulator/config/rviz_emulator_api_multiscan.rviz @@ -24,7 +24,6 @@ Panels: Name: Views Splitter Ratio: 0.535315990447998 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: PointCloud2 @@ -109,7 +108,7 @@ Visualization Manager: Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity - Decay Time: 0.10000000149011612 + Decay Time: 1 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -180,7 +179,7 @@ Window Geometry: Height: 870 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002c8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003aa000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000397000002c800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000002c8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003aa000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000397000002c800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/test/emulator/config/rviz_emulator_api_mrs100_polar.rviz b/test/emulator/config/rviz_emulator_api_multiscan_polar.rviz similarity index 100% rename from test/emulator/config/rviz_emulator_api_mrs100_polar.rviz rename to test/emulator/config/rviz_emulator_api_multiscan_polar.rviz diff --git a/test/emulator/config/rviz_emulator_cfg_nav350.rviz b/test/emulator/config/rviz_emulator_cfg_nav350.rviz index e16abefa..4224e46d 100644 --- a/test/emulator/config/rviz_emulator_cfg_nav350.rviz +++ b/test/emulator/config/rviz_emulator_cfg_nav350.rviz @@ -5,9 +5,10 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /PointCloud21 - Splitter Ratio: 0.5 - Tree Height: 799 + - /Status1 + - /TF1 + Splitter Ratio: 0.529411792755127 + Tree Height: 660 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -24,7 +25,6 @@ Panels: Name: Views Splitter Ratio: 0.535315990447998 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: PointCloud2 @@ -64,7 +64,7 @@ Visualization Manager: Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity - Decay Time: 2 + Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 @@ -90,6 +90,85 @@ Visualization Manager: Reference Frame: cloud Show Trail: false Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /sick_nav_350/scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /sick_scan_xd_api_test/api_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sick_nav_350/nav_reflectors + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -118,7 +197,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 5.548028469085693 + Distance: 4.505097389221191 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -126,25 +205,25 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 0.07465445250272751 + Y: -0.014377681538462639 + Z: 0.0011957025853917003 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.489796757698059 + Pitch: 1.5547966957092285 Target Frame: - Yaw: 3.1164960861206055 + Yaw: 3.1364963054656982 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1096 + Height: 957 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000003aafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003aa000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003aa000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054f0000003efc0100000002fb0000000800540069006d006501000000000000054f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003f3000003aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001560000031ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000031f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003aa000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e50000003efc0100000002fb0000000800540069006d00650100000000000004e5000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003890000031f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -153,6 +232,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1359 - X: 717 + Width: 1253 + X: 1275 Y: 27 diff --git a/test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz b/test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz index ee1203f7..7f50648c 100644 --- a/test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz +++ b/test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz @@ -27,7 +27,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: @@ -88,7 +88,7 @@ Visualization Manager: Min Value: -10 Value: true Axis: Z - Channel Name: objLen + Channel Name: objId Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity @@ -183,4 +183,4 @@ Window Geometry: collapsed: true Width: 1359 X: 718 - Y: 189 + Y: 78 diff --git a/test/emulator/config/rviz_emulator_cfg_ros2_rms.rviz b/test/emulator/config/rviz_emulator_cfg_ros2_rms.rviz index 3ee7b8ab..f21264f3 100644 --- a/test/emulator/config/rviz_emulator_cfg_ros2_rms.rviz +++ b/test/emulator/config/rviz_emulator_cfg_ros2_rms.rviz @@ -138,7 +138,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /sick_rms_3xx/cloud_radar_rawtarget + Value: /sick_rms_xxxx/cloud_radar_rawtarget Use Fixed Frame: true Use rainbow: true Value: true @@ -171,7 +171,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /sick_rms_3xx/cloud_radar_track + Value: /sick_rms_xxxx/cloud_radar_track Use Fixed Frame: true Use rainbow: true Value: true diff --git a/test/emulator/config/rviz_emulator_cfg_ros2_rms2xxx.rviz b/test/emulator/config/rviz_emulator_cfg_ros2_rms2xxx.rviz index fb193a4a..101847e2 100644 --- a/test/emulator/config/rviz_emulator_cfg_ros2_rms2xxx.rviz +++ b/test/emulator/config/rviz_emulator_cfg_ros2_rms2xxx.rviz @@ -7,7 +7,7 @@ Panels: - /Global Options1 - /PointCloud21 - /PointCloud22 - Splitter Ratio: 0.5 + Splitter Ratio: 0.37974682450294495 Tree Height: 721 - Class: rviz_common/Selection Name: Selection @@ -53,7 +53,7 @@ Visualization Manager: Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 - Color Transformer: "" + Color Transformer: Intensity Decay Time: 0 Enabled: true Invert Rainbow: false @@ -62,17 +62,17 @@ Visualization Manager: Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 - Position Transformer: "" + Position Transformer: XYZ Selectable: true - Size (Pixels): 3 + Size (Pixels): 7 Size (m): 0.05000000074505806 - Style: Flat Squares + Style: Points Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /sick_rms_2xxx/cloud_radar_rawtarget + Value: /sick_rms_xxxx/cloud_radar_rawtarget Use Fixed Frame: true Use rainbow: true Value: true @@ -91,13 +91,13 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0 + Max Intensity: 1.7999999523162842 Min Color: 0; 0; 0 - Min Intensity: 0 + Min Intensity: 1.7999999523162842 Name: PointCloud2 Position Transformer: XYZ Selectable: true - Size (Pixels): 5 + Size (Pixels): 10 Size (m): 0.009999999776482582 Style: Points Topic: @@ -105,10 +105,17 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /sick_rms_2xxx/cloud_radar_track + Value: /sick_rms_xxxx/cloud_radar_track Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -152,16 +159,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 41.06394577026367 + Distance: 24.625856399536133 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 11.868752479553223 - Y: -1.7466347217559814 - Z: 0.6080088019371033 + X: 6.792377948760986 + Y: -1.4866989850997925 + Z: 0.39943429827690125 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -178,7 +185,7 @@ Window Geometry: Height: 950 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000441fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000441000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003810000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001dc0000035cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000441fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000441000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002fb0000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: diff --git a/test/emulator/launch/emulator_rms3xx.launch b/test/emulator/launch/emulator_rms3xx.launch deleted file mode 100644 index 46dc22b9..00000000 --- a/test/emulator/launch/emulator_rms3xx.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/test/emulator/scandata/20221018_rms_1xxx_ascii_rms2_objects.pcapng.json b/test/emulator/scandata/20221018_rms_1xxx_ascii_rms2_objects.pcapng.json index 1e31a49b..b2d386e3 100644 --- a/test/emulator/scandata/20221018_rms_1xxx_ascii_rms2_objects.pcapng.json +++ b/test/emulator/scandata/20221018_rms_1xxx_ascii_rms2_objects.pcapng.json @@ -317,8 +317,8 @@ }, "tcp.comment": "All entries created by pcap_json_converter, not by Wireshark.", "tcp.producer": "pcap_json_converter.py --pcap_filename=20221018_rms_1xxx_ascii_rawtarget_object.pcapng", - "tcp.description": ".sRA DItype F RMS1731C.636111.", - "tcp.payload": "02:73:52:41:20:44:49:74:79:70:65:20:46:20:52:4d:53:31:37:33:31:43:2d:36:33:36:31:31:31:03" + "tcp.description": ".sRA DItype F RMS2731C.636111.", + "tcp.payload": "02:73:52:41:20:44:49:74:79:70:65:20:46:20:52:4d:53:32:37:33:31:43:2d:36:33:36:31:31:31:03" } } } diff --git a/test/pcap_json_converter/pcap_json_converter.cmd b/test/pcap_json_converter/pcap_json_converter.cmd index ef5fbc17..abf7d07b 100644 --- a/test/pcap_json_converter/pcap_json_converter.cmd +++ b/test/pcap_json_converter/pcap_json_converter.cmd @@ -38,13 +38,10 @@ python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220803_lms5 python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220802_lms111.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220505_lms511_wireshark_issue49.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220323_nav350_binary.pcapng -python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220317-rms3xx-ascii.pcapng -python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220317-rms3xx-binary.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220316-rms1000-ascii.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220316-rms1000-binary.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20211201_MRS_1xxx_IMU_with_movement.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20211201_RMS_1xxx_start_up.pcapng -python pcap_json_converter.py --pcap_filename=../emulator/scandata/20211201_RMS_3xx_start_up.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20210722_102600_tim_781_sick_scan_xd.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20210722_103100_tim_781_sick_scan.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20210722_112100_ldmrs_sick_scan_xd.pcapng diff --git a/test/python/multiscan_laserscan_msg_to_pointcloud.py b/test/python/multiscan_laserscan_msg_to_pointcloud.py new file mode 100644 index 00000000..e9848bc0 --- /dev/null +++ b/test/python/multiscan_laserscan_msg_to_pointcloud.py @@ -0,0 +1,117 @@ +# Receive LaserScan messages and convert to pointcloud + +import math +import numpy as np +import os +import sys +import threading +import time + +# set __ROS_VERSION to 1 (ROS-1), or 2 (ROS-2) +__ROS_VERSION = os.getenv("ROS_VERSION") +if __ROS_VERSION is not None: + __ROS_VERSION = int(__ROS_VERSION) +if __ROS_VERSION == 2: + import rclpy + from rclpy.node import Node + from rclpy.qos import QoSProfile + from rclpy.qos import QoSReliabilityPolicy + from sensor_msgs.msg import LaserScan + from sensor_msgs.msg import PointCloud2, PointField +else: + import rospy + from sensor_msgs.msg import LaserScan + from sensor_msgs.msg import PointCloud2, PointField + class Node: + pass + +class LaserScanSubscriber(Node): + + def __init__(self, __ROS_VERSION, num_messages_decay): + self.__ROS_VERSION = __ROS_VERSION + if self.__ROS_VERSION == 2: # ROS-2 + super().__init__("multiscan_laserscan_msg_to_pointcloud") + self.num_messages_decay = num_messages_decay + self.scan_msg_array = [] + if self.__ROS_VERSION == 2: # ROS-2 + self.scan_subscription = self.create_subscription(LaserScan, "/sick_scansegment_xd/scan_segment", self.listener_callback, 10) # QoSProfile(depth=16*12*3,reliability=QoSReliabilityPolicy.RELIABLE)) + self.cloud_publisher = self.create_publisher(PointCloud2, "/laserscan_msg_cloud", 10) + else: # ROS-1 + self.scan_subscription = rospy.Subscriber("/sick_scansegment_xd/scan_segment", LaserScan, self.listener_callback, queue_size=16*12*3) # multiScan: 16 layer, 12 segments, 3 echos + self.cloud_publisher = rospy.Publisher("/laserscan_msg_cloud", PointCloud2, queue_size=10) + self.frame_id_elevation_table = { # Elevation table for Multiscan 136 (16 layer): + "world_0": -22.71 * math.pi / 180, # layer:0 elevation: -22.71 deg + "world_1": -17.56 * math.pi / 180, # layer:1 elevation: -17.56 deg + "world_2": -12.48 * math.pi / 180, # layer:2 elevation: -12.48 deg + "world_3": -7.51 * math.pi / 180, # layer:3 elevation: -7.51 deg + "world_4": -2.49 * math.pi / 180, # layer:4 elevation: -2.49 deg + "world_5": -0.07 * math.pi / 180, # layer:5 elevation: -0.07 deg + "world_6": +2.43 * math.pi / 180, # layer:6 elevation: +2.43 deg + "world_7": +7.29 * math.pi / 180, # layer:7 elevation: +7.29 deg + "world_8": +12.79 * math.pi / 180, # layer:8 elevation: +12.79 deg + "world_9": +17.28 * math.pi / 180, # layer:9 elevation: +17.28 deg + "world_10": +21.94 * math.pi / 180, # layer:10 elevation: +21.94 deg + "world_11": +26.73 * math.pi / 180, # layer:11 elevation: +26.73 deg + "world_12": +31.86 * math.pi / 180, # layer:12 elevation: +31.86 deg + "world_13": +33.42 * math.pi / 180, # layer:13 elevation: +33.42 deg + "world_14": +37.18 * math.pi / 180, # layer:14 elevation: +37.18 deg + "world_15": +42.79 * math.pi / 180 # layer:15 elevation: +42.79 deg + } + + def listener_callback(self, msg): + # print("subscriber: {}".format(msg)) + # print("subscriber: {}".format(msg.header.frame_id)) + self.scan_msg_array.append(msg) + if len(self.scan_msg_array) >= self.num_messages_decay: + # Create pointcloud header and field description + ros_pointcloud = PointCloud2() + ros_pointcloud.header.seq = msg.header.seq + ros_pointcloud.header.stamp = msg.header.stamp + ros_pointcloud.header.frame_id = "world" + ros_pointcloud.width = 0 + for scan_msg in self.scan_msg_array: + ros_pointcloud.width = ros_pointcloud.width + len(scan_msg.ranges) + ros_pointcloud.height = 1 + ros_pointcloud.is_bigendian = False + ros_pointcloud.is_dense = True + ros_pointcloud.fields = [ PointField("x", 0, PointField.FLOAT32, 1), PointField("y", 4, PointField.FLOAT32, 1), PointField("z", 8, PointField.FLOAT32, 1), PointField("intensity", 12, PointField.FLOAT32, 1) ] + ros_pointcloud.point_step = 16 + ros_pointcloud.row_step = ros_pointcloud.point_step * ros_pointcloud.width + # Convert laserscan message to pointcloud + cloud_data = np.zeros(4 * ros_pointcloud.width * ros_pointcloud.height, dtype=np.float32) + cloud_point_idx = 0 + for scan_msg in self.scan_msg_array: + azimuth = scan_msg.angle_min + azimuth_inc = scan_msg.angle_increment + elevation = self.frame_id_elevation_table[scan_msg.header.frame_id] + elevation_cos = math.cos(elevation) + elevation_sin = math.sin(elevation) + for point_idx, range in enumerate(scan_msg.ranges): + cloud_data[4 * cloud_point_idx + 0] = range * elevation_cos * math.cos(azimuth) # x + cloud_data[4 * cloud_point_idx + 1] = range * elevation_cos * math.sin(azimuth) # y + cloud_data[4 * cloud_point_idx + 2] = range * elevation_sin # z + cloud_data[4 * cloud_point_idx + 3] = scan_msg.intensities[point_idx] # intensity + cloud_point_idx = cloud_point_idx + 1 + azimuth = azimuth + azimuth_inc + ros_pointcloud.data = cloud_data.tostring() + self.cloud_publisher.publish(ros_pointcloud) + self.scan_msg_array = [] + +def main(args=None): + global __ROS_VERSION + num_messages_decay = 3 * 12 * 16 # decay over one complete fullscan, multiScan: 16 layer, 12 segments, 3 echos + if __ROS_VERSION == 2: # ROS-2 + rclpy.init() + subscriber = LaserScanSubscriber(__ROS_VERSION, num_messages_decay) + rclpy.spin(subscriber) + rclpy.shutdown() + + else: # ROS-1 + node = rospy.init_node("multiscan_laserscan_msg_to_pointcloud") + subscriber = LaserScanSubscriber(__ROS_VERSION, num_messages_decay) + # rospy.spin() + while not rospy.is_shutdown(): + rospy.sleep(0.1) + +if __name__ == '__main__': + main() diff --git a/test/python/mrs100_pcap_player.py b/test/python/multiscan_pcap_player.py similarity index 81% rename from test/python/mrs100_pcap_player.py rename to test/python/multiscan_pcap_player.py index c244722e..c1a8ba98 100644 --- a/test/python/mrs100_pcap_player.py +++ b/test/python/multiscan_pcap_player.py @@ -1,6 +1,6 @@ """ - MRS100 test emulator to parse pcapng files recorded from MRS100 and replay the UDP packages - to emulate a local MRS100 lidar. + multiScan test emulator to parse pcapng files recorded from multiScan and replay the UDP packages + to emulate a local multiScan lidar. The UDP Sender sends packets via UDP over localhost, Port:2115 @@ -45,7 +45,7 @@ def readPcapngFile(pcap_filename, verbose): if isinstance(block, EnhancedPacket): # Decode a single pcap block if block.captured_len != block.packet_len: - print("## mrs100_pcap_player block {}: {} byte block truncated to {} bytes".format(block_cnt, block.packet_len, block.captured_len)) + print("## multiscan_pcap_player block {}: {} byte block truncated to {} bytes".format(block_cnt, block.packet_len, block.captured_len)) block_data = Ether(block.packet_data) block_decoded = block_data for n in range(0,10): @@ -56,9 +56,9 @@ def readPcapngFile(pcap_filename, verbose): else: break # if len(block_decoded.payload) <= 0: - # print("## mrs100_pcap_player block {}: empty payload ignored in data {}".format(block_cnt, block_data)) + # print("## multiscan_pcap_player block {}: empty payload ignored in data {}".format(block_cnt, block_data)) # if not isinstance(block_decoded.payload, scapy.packet.Raw): - # print("## mrs100_pcap_player block {}: block_decoded.payload = {} is no instance of scapy.packet.Raw".format(block_cnt, block_decoded.payload)) + # print("## multiscan_pcap_player block {}: block_decoded.payload = {} is no instance of scapy.packet.Raw".format(block_cnt, block_decoded.payload)) # print("block {}: timestamp = {}".format(block_cnt, block.timestamp)) # print("block {}: packet_data = {}".format(block_cnt, block.packet_data)) # print("block {}: payload = {}".format(block_cnt, block_decoded.payload)) @@ -76,9 +76,9 @@ def readPcapngFile(pcap_filename, verbose): if __name__ == "__main__": - pcap_filename = "mrs100.pcapng" # "../../../../30_LieferantenDokumente/40_Realdaten/20201103_Realdaten/mrs100.pcapng" + pcap_filename = "multiscan.pcapng" # "../../../../30_LieferantenDokumente/40_Realdaten/20201103_Realdaten/multiscan.pcapng" udp_port = 2115 # UDP port to send msgpack datagrams - udp_send_rate = 0 # send rate in msgpacks per second, 240 for MRS100, or 0 to send corresponding to pcap-timestamps, or udp_send_rate > 1000 for max. rate + udp_send_rate = 0 # send rate in msgpacks per second, 240 for multiScan, or 0 to send corresponding to pcap-timestamps, or udp_send_rate > 1000 for max. rate udp_dst_ip = "" num_repetitions = 1 verbose = 0 @@ -86,7 +86,7 @@ def readPcapngFile(pcap_filename, verbose): arg_parser = argparse.ArgumentParser() arg_parser.add_argument("--pcap_filename", help="pcapng filepath", default=pcap_filename, type=str) arg_parser.add_argument("--udp_port", help="udp port", default=udp_port, type=int) - arg_parser.add_argument("--send_rate", help="udp send rate in msgpacks per second, 240 for MRS100, or 0 to send by pcap-timestamps, or > 10000 for max. rate", default=udp_send_rate, type=int) + arg_parser.add_argument("--send_rate", help="udp send rate in msgpacks per second, 240 for multiScan, or 0 to send by pcap-timestamps, or > 10000 for max. rate", default=udp_send_rate, type=int) arg_parser.add_argument("--dst_ip", help="udp destination ip, e.g. 127.0.0.1 or ", default=udp_dst_ip, type=str) arg_parser.add_argument("--repeat", help="number of repetitions", default=num_repetitions, type=int) arg_parser.add_argument("--verbose", help="print verbose messages", default=verbose, type=int) @@ -99,14 +99,14 @@ def readPcapngFile(pcap_filename, verbose): verbose = cli_args.verbose # Read and parse pcap file, extract udp raw data - print("mrs100_pcap_player: reading pcapfile \"{}\" ...".format(pcap_filename)) + print("multiscan_pcap_player: reading pcapfile \"{}\" ...".format(pcap_filename)) blocks_payload, blocks_timestamp = readPcapngFile(pcap_filename, verbose) - print("mrs100_pcap_player: sending {} udp packets ...".format(len(blocks_payload))) + print("multiscan_pcap_player: sending {} udp packets ...".format(len(blocks_payload))) # Init upd sender udp_sender_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) # UDP socket udp_sender_socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) # Enable broadcasting mode - print("mrs100_pcap_player: sending on udp port {}, send_rate={}".format(udp_port, udp_send_rate)) + print("multiscan_pcap_player: sending on udp port {}, send_rate={}".format(udp_port, udp_send_rate)) # Send udp raw data for repeat_cnt in range(num_repetitions): @@ -129,4 +129,4 @@ def readPcapngFile(pcap_filename, verbose): # else: # brute force delay, for performance tests on 2. PC only # forced_delay(2.0e-4) send_timestamp = msg_timestamp - print("mrs100_pcap_player finished.") + print("multiscan_pcap_player finished.") diff --git a/test/python/mrs100_receiver.py b/test/python/multiscan_receiver.py similarity index 97% rename from test/python/mrs100_receiver.py rename to test/python/multiscan_receiver.py index f40a5350..11be1c0f 100644 --- a/test/python/mrs100_receiver.py +++ b/test/python/multiscan_receiver.py @@ -93,7 +93,7 @@ def msgpackKeyToken(s): return msgpack_tokenized_keys_str2int[s] -class MRS100Receiver: +class MultiScanReceiver: def __init__(self, scannerIP = "192.168.0.1", hostIP = "192.168.0.102", port=2115, nbScans = 100, writeMsgpackFile = ""): self.scannerIP = scannerIP @@ -231,7 +231,7 @@ def parseData(self, payload): # print("phi =", phi0) # end debugging except Exception: - print("mrs100_receiver: parseData failed") + print("multiscan_receiver: parseData failed") return segmentData def convertToPointCloud(self, scanVector): @@ -295,7 +295,7 @@ def showScan(self, pointCloud): else: self.ax.clear() - self.ax.set_title('MRS100 Emulation Segment {}'.format(self.showCount)) + self.ax.set_title('multiScan Emulation Segment {}'.format(self.showCount)) self.ax.set_xlim(-1, 1) self.ax.set_ylim(-1, 1) self.ax.set_zlim(-1, 1) @@ -343,7 +343,7 @@ def readFromMSGPACKFile(self, fileName): if __name__ == "__main__": - receiver = MRS100Receiver(scannerIP = "127.0.0.1", hostIP = "127.0.0.1", port = 2115, nbScans = 1000, writeMsgpackFile = "./mrs100_dump") + receiver = MultiScanReceiver(scannerIP = "127.0.0.1", hostIP = "127.0.0.1", port = 2115, nbScans = 1000, writeMsgpackFile = "./multiscan_dump") mode = 'live' diff --git a/test/python/mrs100_sopas_test_server.py b/test/python/multiscan_sopas_test_server.py similarity index 92% rename from test/python/mrs100_sopas_test_server.py rename to test/python/multiscan_sopas_test_server.py index f64b571a..6813e48b 100644 --- a/test/python/mrs100_sopas_test_server.py +++ b/test/python/multiscan_sopas_test_server.py @@ -38,7 +38,7 @@ def hexStringToByteArray(message): class ColaResponseMap: # Constructor - def __init__(self, cola_binary = 0): + def __init__(self, cola_binary = 0, val_FREchoFilter = 0): if cola_binary > 0: self.mapped_response = { "sMN SetAccessMode": "02:02:02:02:00:00:00:13:73:41:4e:20:53:65:74:41:63:63:65:73:73:4d:6f:64:65:20:01:38" , @@ -58,7 +58,7 @@ def __init__(self, cola_binary = 0): "sWN ScanDataFormat": "\x02sWA ScanDataFormat\x03", # "sWN ScanDataFormat 1" -> "sWA ScanDataFormat" "sWN ScanDataPreformatting": "\x02sWA ScanDataPreformatting\x03", # "sWN ScanDataPreformatting 1" -> "sWA ScanDataPreformatting" "sWN ScanDataEthSettings": "\x02sWA ScanDataEthSettings\x03", # "sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115" -> "sWA ScanDataEthSettings" - "sRN FREchoFilter": "\x02sRA FREchoFilter 0\x03", # "sRN FREchoFilter" -> "sRA FREchoFilter 0" (default: 0, i.e. first echo only, echo_count = 1) + "sRN FREchoFilter": "\x02sRA FREchoFilter {}\x03".format(val_FREchoFilter), # "sRN FREchoFilter" -> "sRA FREchoFilter 0" (default: 0, i.e. first echo only, echo_count = 1) "sRN LFPangleRangeFilter": "\x02sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1\x03", # "sRN LFPangleRangeFilter" -> "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1" "sRN LFPlayerFilter": "\x02sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1\x03", # "sRN LFPlayerFilter" -> "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" "sWN FREchoFilter": "\x02sWA FREchoFilter\x03", # "sWN FREchoFilter 1" -> "sWA FREchoFilter" @@ -79,9 +79,10 @@ def findResponse(self, request): class SopasTestServer: # Constructor - def __init__(self, tcp_port = 2111, cola_binary = 0): + def __init__(self, tcp_port = 2111, cola_binary = 0, val_FREchoFilter = 0): self.tcp_port = tcp_port self.cola_binary = cola_binary + self.val_FREchoFilter = val_FREchoFilter # Waits for an incoming tcp connection and connects to the tcp client def connect(self): @@ -136,7 +137,7 @@ def sendTelegram(self, telegram, verbosity): # Runs the message loop, i.e. receives binary cola telegrams and sends a response to the client def run(self): - response_map = ColaResponseMap() + response_map = ColaResponseMap(self.cola_binary, self.val_FREchoFilter) print("SopasTestServer: running main loop...") while True: # Receive a cola telegram @@ -161,17 +162,20 @@ def run(self): # Configuration tcp_port = 2111 # tcp port to listen for tcp connections cola_binary = 0 # cola ascii (0) or binary (1) + val_FREchoFilter = 0 # default configuration: FREchoFilter=0 # Overwrite with command line arguments arg_parser = argparse.ArgumentParser() arg_parser.add_argument("--tcp_port", help="tcp port to listen for tcp connections", default=tcp_port, type=int) arg_parser.add_argument("--cola_binary", help="cola ascii (0) or binary (1)", default=cola_binary, type=int) + arg_parser.add_argument("--FREchoFilter", help="FREchoFilter, 0 (first echo, default), 1 (all echos) or 2 (last echo)", default=val_FREchoFilter, type=int) cli_args = arg_parser.parse_args() tcp_port = cli_args.tcp_port cola_binary = cli_args.cola_binary + val_FREchoFilter = cli_args.FREchoFilter # Run test server - server = SopasTestServer(tcp_port, cola_binary) + server = SopasTestServer(tcp_port, cola_binary, val_FREchoFilter) server.connect() server.run() \ No newline at end of file diff --git a/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py b/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py index 0a9bcc60..31523d85 100644 --- a/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py +++ b/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py @@ -95,8 +95,8 @@ def pySickScanCartesianPointCloudMsgToXYZ(pointcloud_msg): # Callback for cartesian pointcloud messages def pySickScanCartesianPointCloudMsgCallback(api_handle, pointcloud_msg): pointcloud_msg = pointcloud_msg.contents # dereference msg pointer (pointcloud_msg = pointcloud_msg[0]) - print("pySickScanCartesianPointCloudMsgCallback: api_handle={}, {}x{} pointcloud, {} echo(s), segment {}".format( - api_handle, pointcloud_msg.width, pointcloud_msg.height, pointcloud_msg.num_echos , pointcloud_msg.segment_idx)) + print("pySickScanCartesianPointCloudMsgCallback (ROS-{}): api_handle={}, {}x{} pointcloud, {} echo(s), segment {}".format( + __ROS_VERSION, api_handle, pointcloud_msg.width, pointcloud_msg.height, pointcloud_msg.num_echos , pointcloud_msg.segment_idx)) global api_test_settings # Note: Pointcloud conversion and visualization consumes cpu time, therefore we convert and publish the cartesian pointcloud with low frequency. cur_timestamp = datetime.datetime.now() @@ -112,8 +112,8 @@ def pySickScanCartesianPointCloudMsgCallback(api_handle, pointcloud_msg): # Callback for polar pointcloud messages def pySickScanPolarPointCloudMsgCallback(api_handle, pointcloud_msg): pointcloud_msg = pointcloud_msg.contents # dereference msg pointer (pointcloud_msg = pointcloud_msg[0]) - print("pySickScanPolarPointCloudMsgCallback: api_handle={}, {}x{} pointcloud, {} echo(s), segment {}".format( - api_handle, pointcloud_msg.width, pointcloud_msg.height, pointcloud_msg.num_echos , pointcloud_msg.segment_idx)) + print("pySickScanPolarPointCloudMsgCallback (ROS-{}): api_handle={}, {}x{} pointcloud, {} echo(s), segment {}".format( + __ROS_VERSION, api_handle, pointcloud_msg.width, pointcloud_msg.height, pointcloud_msg.num_echos , pointcloud_msg.segment_idx)) if __ROS_VERSION == 1: # Convert polar pointcloud_msg to cartesian ros pointcloud and publish. # Note: Pointcloud conversion from polar to cartesian is too cpu-intensive to process all segments from a Multiscan136. @@ -198,6 +198,11 @@ def pySickScanVisualizationMarkerCallback(api_handle, visualizationmarker_msg): ros_markers = SickScanApiConvertMarkerArrayToROS(visualizationmarker_msg.markers) api_test_settings.ros_visualizationmarker_publisher.publish(ros_markers) +# Callback for SickScanNavPoseLandmarkMsg messages +def pySickScanNavPoseLandmarkCallback(api_handle, navposelandmark_msg): + navposelandmark_msg = navposelandmark_msg.contents # dereference msg pointer + print("pySickScanNavPoseLandmarkCallback: api_handle={}, NavPoseLandmark message: x={:.3}, y={:.3}, yaw={:.4}, {} reflectors".format(api_handle, navposelandmark_msg.pose_x, navposelandmark_msg.pose_y, navposelandmark_msg.pose_yaw, navposelandmark_msg.reflectors.size)) + # # Python examples for SickScanApiWaitNext-functions ("message polling") # @@ -212,6 +217,19 @@ def pyrunSickScanApiTestWaitNext(sick_scan_library, api_handle): radarscan_msg = SickScanRadarScan() ldmrsobjectarray_msg = SickScanLdmrsObjectArray() visualizationmarker_msg = SickScanVisualizationMarkerMsg() + navposelandmark_msg = SickScanNavPoseLandmarkMsg() + odom_msg = SickScanOdomVelocityMsg() + odom_msg.vel_x = +1.0 + odom_msg.vel_y = -1.0 + odom_msg.omega = 0.5 + odom_msg.timestamp_sec = 12345 + odom_msg.timestamp_nsec = 6789 + navodom_msg = SickScanNavOdomVelocityMsg() + navodom_msg.vel_x = +1.0 + navodom_msg.vel_y = -1.0 + navodom_msg.omega = 0.5 + navodom_msg.timestamp = 123456789 + navodom_msg.coordbase = 0 global api_test_settings while api_test_settings.polling: # Get/poll the next cartesian PointCloud message @@ -270,6 +288,16 @@ def pyrunSickScanApiTestWaitNext(sick_scan_library, api_handle): elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextVisualizationMarkerMsg failed, error code {} ({})".format(ret, int(ret))) SickScanApiFreeVisualizationMarkerMsg(sick_scan_library, api_handle, ctypes.pointer(visualizationmarker_msg)) + # Get/poll the next SickScanNavPoseLandmarkMsg message + ret = SickScanApiWaitNextNavPoseLandmarkMsg(sick_scan_library, api_handle, ctypes.pointer(navposelandmark_msg), wait_next_message_timeout) + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): + pySickScanNavPoseLandmarkCallback(api_handle, ctypes.pointer(navposelandmark_msg)) + elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): + print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextNavPoseLandmarkMsg failed, error code {} ({})".format(ret, int(ret))) + SickScanApiFreeNavPoseLandmarkMsg(sick_scan_library, api_handle, ctypes.pointer(navposelandmark_msg)) + # Send NAV350 odom message example + # ret = SickScanApiNavOdomVelocityMsg(sick_scan_library, api_handle, ctypes.pointer(navodom_msg)) + # ret = SickScanApiOdomVelocityMsg(sick_scan_library, api_handle, ctypes.pointer(odom_msg)) # # Python usage example for sick_scan_api @@ -349,6 +377,10 @@ def pyrunSickScanApiTestWaitNext(sick_scan_library, api_handle): visualizationmarker_callback = SickScanVisualizationMarkerCallback(pySickScanVisualizationMarkerCallback) SickScanApiRegisterVisualizationMarkerMsg(sick_scan_library, api_handle, visualizationmarker_callback) + # Register a callback for SickScanNavPoseLandmarkMsg messages + navposelandmark_callback = SickScanNavPoseLandmarkCallback(pySickScanNavPoseLandmarkCallback) + SickScanApiRegisterNavPoseLandmarkMsg(sick_scan_library, api_handle, navposelandmark_callback) + # Run main loop if __ROS_VERSION == 0: while True: @@ -398,6 +430,7 @@ def pyrunSickScanApiTestWaitNext(sick_scan_library, api_handle): SickScanApiDeregisterRadarScanMsg(sick_scan_library, api_handle, radarscan_callback) SickScanApiDeregisterLdmrsObjectArrayMsg(sick_scan_library, api_handle, ldmrsobjectarray_callback) SickScanApiDeregisterVisualizationMarkerMsg(sick_scan_library, api_handle, visualizationmarker_callback) + SickScanApiDeregisterNavPoseLandmarkMsg(sick_scan_library, api_handle, navposelandmark_callback) SickScanApiClose(sick_scan_library, api_handle) SickScanApiRelease(sick_scan_library, api_handle) SickScanApiUnloadLibrary(sick_scan_library) diff --git a/test/python/sopas_json_test_server.py b/test/python/sopas_json_test_server.py index 5203129d..f577bd95 100644 --- a/test/python/sopas_json_test_server.py +++ b/test/python/sopas_json_test_server.py @@ -26,9 +26,10 @@ class SopasTestServer: # Constructor - def __init__(self, tcp_port = 2112, json_tcp_payloads = []): + def __init__(self, tcp_port = 2112, json_tcp_payloads = [], verbosity = 0): self.tcp_port = tcp_port self.json_tcp_payloads = json_tcp_payloads + self.verbosity = verbosity self.run_message_loop = False # Waits for an incoming tcp connection and connects to the tcp client @@ -51,24 +52,30 @@ def receiveTelegram(self, recv_timeout_sec): payload_idx = -1 ready_to_recv = select.select([self.clientsocket], [], [], recv_timeout_sec) if ready_to_recv[0]: - byte_recv = b"\x00" - while byte_recv != b"\x02": - byte_recv = self.clientsocket.recv(1) - payload = payload + byte_recv - while True: - byte_recv = self.clientsocket.recv(1) + try: + byte_recv = b"\x00" + while byte_recv != b"\x02": + byte_recv = self.clientsocket.recv(1) payload = payload + byte_recv - if payload in self.json_tcp_payloads: - payload_idx = self.json_tcp_payloads.index(payload) - break - print("SopasTestServer.receiveTelegram(): received {} byte telegram {}".format(len(payload), payload)) + while True: + byte_recv = self.clientsocket.recv(1) + payload = payload + byte_recv + if payload in self.json_tcp_payloads: + payload_idx = self.json_tcp_payloads.index(payload) + break + except Exception as exc: + print("## ERROR receiveTelegram(): exception {}".format(exc)) + if self.verbosity > 1: + print("SopasTestServer.receiveTelegram(): received {} byte telegram {}".format(len(payload), payload)) + elif self.verbosity > 0: + print("SopasTestServer.receiveTelegram(): received {} byte telegram".format(len(payload))) return payload, payload_idx # Sends a cola telegram "as is" - def sendTelegram(self, telegram, verbosity): - if verbosity > 1: + def sendTelegram(self, telegram): + if self.verbosity > 1: print("SopasTestServer.sendTelegram(): sending {} byte telegram {}".format((len(telegram)), telegram)) - elif verbosity > 0: + elif self.verbosity > 0: print("SopasTestServer.sendTelegram(): sending {} byte telegram".format(len(telegram))) self.clientsocket.send(telegram) @@ -83,10 +90,25 @@ def run(self): # Lookup sopas response to sopas request if json_tcp_payload_idx >= 0 and json_tcp_payload_idx + 1 < len(self.json_tcp_payloads): response_payload = self.json_tcp_payloads[json_tcp_payload_idx + 1] - print("SopasTestServer: request={}, response={}".format(received_telegram, response_payload)) - self.sendTelegram(response_payload, 2) + if self.verbosity > 0: + print("SopasTestServer: request={}, response={}".format(received_telegram, response_payload)) + self.sendTelegram(response_payload) + # Some NAV-350 sopas requests have 2 responses (1. response: acknowledge, 2. response: data), send next telegram in this case + if received_telegram[8:28] == b"sMN mNEVAChangeState" or received_telegram[8:26] == b"sMN mNMAPDoMapping": + if json_tcp_payload_idx + 2 < len(self.json_tcp_payloads): + response_payload = self.json_tcp_payloads[json_tcp_payload_idx + 2] + if self.verbosity > 0: + print("SopasTestServer: request={}, response={}".format(received_telegram, response_payload)) + self.sendTelegram(response_payload) else: print("## ERROR SopasTestServer: request={} not found in json file".format(received_telegram)) + #response_payload = received_telegram + #if response_payload[8:11] == b"sWN": + # response_payload[8:11] = b"sWA" + #if response_payload[8:11] == b"sMN": + # response_payload[8:11] = b"sMA" + #print("## ERROR SopasTestServer: dummy_response={}".format(response_payload)) + #self.sendTelegram(response_payload) time.sleep(0.01) # Starts the message loop in a background thread @@ -110,6 +132,7 @@ def stop(self): json_file = "../emulator/scandata/20221018_rms_1xxx_ascii_rawtarget_object.pcapng.json" # input jsonfile with sopas requests, responses and telegrams verbosity = 2 # print all telegrams send_rate = 10 # send 10 scandata telegrams per second + send_scandata_after = 5 # start to send scandata 5 seconds after server start # Overwrite with command line arguments arg_parser = argparse.ArgumentParser() @@ -118,12 +141,14 @@ def stop(self): arg_parser.add_argument("--scandata_id", help="sopas id of scandata telegrams, e.g. \"sSN LMDradardata\"", default=scandata_id, type=str) arg_parser.add_argument("--send_rate", help="send rate in telegrams per second", default=send_rate, type=float) arg_parser.add_argument("--verbosity", help="verbosity (0, 1 or 2)", default=verbosity, type=int) + arg_parser.add_argument("--scandata_after", help="start to send scandata after some seconds", default=send_scandata_after, type=float) cli_args = arg_parser.parse_args() tcp_port = cli_args.tcp_port json_file = cli_args.json_file scandata_id = cli_args.scandata_id verbosity = cli_args.verbosity send_rate = cli_args.send_rate + send_scandata_after = cli_args.scandata_after # Parse json file print("sopas_json_test_server: parsing json file \"{}\":".format(json_file)) @@ -140,24 +165,25 @@ def stop(self): # print("tcp_description: \"{}\", tcp_payload: \"{}\", payload_bytes: {}".format(tcp_description, tcp_payload, payload)) except Exception as exc: print("## ERROR parsing file {}: \"{}\", exception {}".format(json_file, json_entry, exc)) - for json_tcp_payload in json_tcp_payloads: - print("{}".format(json_tcp_payload)) + if verbosity > 1: + for json_tcp_payload in json_tcp_payloads: + print("{}".format(json_tcp_payload)) # Run sopas test server print("sopas_json_test_server: running event loop ...") - server = SopasTestServer(tcp_port, json_tcp_payloads) + server = SopasTestServer(tcp_port, json_tcp_payloads, verbosity) server.connect() server.start() # Send sopas telegrams, e.g. "sSN LMDradardata ..." - time.sleep(5) + time.sleep(send_scandata_after) print("sopas_json_test_server: start sending scandata \"{}\" ...".format(scandata_id)) scandata_id = bytearray(scandata_id.encode()) while True: for payload in json_tcp_payloads: if payload.find(scandata_id,0) >= 0: # print("sopas_json_test_server: sending scandata \"{}\" ...".format(payload)) - server.sendTelegram(payload, 2) + server.sendTelegram(payload) time.sleep(1.0 / send_rate) server.stop() diff --git a/test/scripts/run_linux_ros1_api_test.bash b/test/scripts/run_linux_ros1_api_test.bash index d8c1124f..1ff023df 100644 --- a/test/scripts/run_linux_ros1_api_test.bash +++ b/test/scripts/run_linux_ros1_api_test.bash @@ -37,19 +37,29 @@ function start_mrs100_emulator() { echo -e "\n----------------------------------------------------------" echo -e "run_api_test: starting mrs100 (multiscan136) emulation ...\n" - python3 ./src/sick_scan_xd/test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & - sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_api_mrs100.rviz --opengl 210 & - sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_api_mrs100_polar.rviz --opengl 210 & + python3 ./src/sick_scan_xd/test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & + + sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_api_multiscan.rviz --opengl 210 & + sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_api_multiscan_polar.rviz --opengl 210 & sleep 1 } -# Start rms3xx radar emulator and rviz -function start_rms3xx_emulator() +# Start rms2xxx radar emulator and rviz +function start_rms2xxx_emulator() { echo -e "\n-------------------------------------------" - echo -e "run_api_test: starting rms3xx radar emulation ...\n" - roslaunch sick_scan emulator_rms3xx.launch & - sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_api_rms1xxx.rviz --opengl 210 & + echo -e "run_api_test: starting rms2xxx radar emulation ...\n" + python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20221018_rms_1xxx_ascii_rms2_objects.pcapng.json --scandata_id="sSN LMDradardata" --send_rate=10 --verbosity=1 & + sleep 1 +} + +# Start nav350 emulator and rviz +function start_nav350_emulator() +{ + echo -e "\n----------------------------------------------------------" + echo -e "run_api_test: starting nav350 emulation ...\n" + python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20230126_nav350_4reflectors_moving.pcapng.json --scandata_id="sAN mNPOSGetData" --send_rate=8 --verbosity=0 & + sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_nav350.rviz --opengl 210 & sleep 1 } @@ -77,13 +87,15 @@ function kill_simu() rosnode kill -a ; sleep 1 killall sick_scan_xd_api_test killall sick_scan_emulator - pkill -f mrs100_sopas_test_server.py + pkill -f multiscan_sopas_test_server.py pkill -f sick_scan_xd_api_test.py + pkill -f sopas_json_test_server.py sleep 1 killall -9 sick_scan_xd_api_test killall -9 sick_scan_emulator - pkill -9 -f mrs100_sopas_test_server.py + pkill -9 -f multiscan_sopas_test_server.py pkill -9 -f sick_scan_xd_api_test.py + pkill -9 -f sopas_json_test_server.py } kill_simu @@ -93,11 +105,11 @@ printf "\033c" # Build and run minimalistic api usage examples (Python, C, C++) # pushd ../../examples/scripts -./build_run_api_examples_linux.bash +# ./build_run_api_examples_linux.bash popd # -# API test against simulated TiM7xx, MRS100, LDMRS, MRS1xxx and RMS3xx +# API test against simulated TiM7xx, MRS100, LDMRS, MRS1xxx and RMSxxxx # pushd ../../../.. @@ -118,6 +130,7 @@ if [ $roscore_running -lt 1 ] ; then sleep 3 fi + # # Run python examples # @@ -130,10 +143,10 @@ kill_simu # Start mrs100 (multiscan136) emulator and run sick_scan_xd_api_test (python example) start_mrs100_emulator -python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./src/sick_scan_xd/launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_frame_id:=cloud & +python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./src/sick_scan_xd/launch/sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_frame_id:=cloud & # Play pcapng-files to emulate MRS100 output -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2 -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2 +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 waitUntilRvizClosed 1 kill_simu @@ -149,10 +162,16 @@ python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py waitUntilRvizClosed 10 kill_simu -# Start rms3xx radar emulator and run sick_scan_xd_api_test (python example) -start_rms3xx_emulator -python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./src/sick_scan_xd/launch/sick_rms_3xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & -waitUntilRvizClosed 10 +# Start rms2xxx radar emulator and run sick_scan_xd_api_test (python example) +start_rms2xxx_emulator +python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./src/sick_scan_xd/launch/sick_rms_xxxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & +sleep 10 +kill_simu + +# Start nav350 emulator and run sick_scan_xd_api_test (python example) +start_nav350_emulator +python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./src/sick_scan_xd/launch/sick_nav_350.launch hostname:=127.0.0.1 nav_do_initial_mapping:=True sw_pll_only_publish:=False & +waitUntilRvizClosed 15 kill_simu # @@ -169,10 +188,10 @@ kill_simu # Start mrs100 (multiscan136) emulator and run sick_scan_xd_api_test (cpp example) start_mrs100_emulator -rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_frame_id:=cloud" & +rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_frame_id:=cloud" & # Play pcapng-files to emulate MRS100 output -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2 -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2 +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 waitUntilRvizClosed 1 kill_simu @@ -188,85 +207,103 @@ rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/laun waitUntilRvizClosed 10 kill_simu -# Start rms3xx radar emulator and run sick_scan_xd_api_test (cpp example) -start_rms3xx_emulator -rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_rms_3xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False" & -waitUntilRvizClosed 10 +# Start rms2xxx radar emulator and run sick_scan_xd_api_test (cpp example) +start_rms2xxx_emulator +rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_rms_xxxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False" & +sleep 10 +kill_simu + +# Start nav350 emulator and run sick_scan_xd_api_test (cpp example) +start_nav350_emulator +rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_nav_350.launch hostname:=127.0.0.1 nav_do_initial_mapping:=True sw_pll_only_publish:=False" & +waitUntilRvizClosed 15 kill_simu # # Run python examples with polling # -# Start tim7xx emulator and run sick_scan_xd_api_test (python example) +# Start tim7xx emulator and run sick_scan_xd_api_test (python example with polling) start_tim7xx_emulator python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py _polling:=1 ./src/sick_scan_xd/launch/sick_tim_7xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & waitUntilRvizClosed 40 kill_simu -# Start mrs100 (multiscan136) emulator and run sick_scan_xd_api_test (python example) +# Start mrs100 (multiscan136) emulator and run sick_scan_xd_api_test (python example with polling) start_mrs100_emulator -python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py _polling:=1 ./src/sick_scan_xd/launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_frame_id:=cloud & +python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py _polling:=1 ./src/sick_scan_xd/launch/sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_frame_id:=cloud & # Play pcapng-files to emulate MRS100 output -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2 -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2 +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 waitUntilRvizClosed 1 kill_simu -# Start ldmrs emulator and run sick_scan_xd_api_test (python example) +# Start ldmrs emulator and run sick_scan_xd_api_test (python example with polling) start_ldmrs_emulator python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py _polling:=1 ./src/sick_scan_xd/launch/sick_ldmrs.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & waitUntilRvizClosed 15 kill_simu -# Start mrs1xxx emulator with imu messages and run sick_scan_xd_api_test (python example) +# Start mrs1xxx emulator with imu messages and run sick_scan_xd_api_test (python example with polling) start_mrs1xxx_emulator python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py _polling:=1 ./src/sick_scan_xd/launch/sick_mrs_1xxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & waitUntilRvizClosed 10 kill_simu -# Start rms3xx radar emulator and run sick_scan_xd_api_test (python example) -start_rms3xx_emulator -python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py _polling:=1 ./src/sick_scan_xd/launch/sick_rms_3xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & -waitUntilRvizClosed 10 +# Start rms2xxx radar emulator and run sick_scan_xd_api_test (python example with polling) +start_rms2xxx_emulator +python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py _polling:=1 ./src/sick_scan_xd/launch/sick_rms_xxxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & +sleep 10 +kill_simu + +# Start nav350 emulator and run sick_scan_xd_api_test (python example with polling) +start_nav350_emulator +python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py _polling:=1 ./src/sick_scan_xd/launch/sick_nav_350.launch hostname:=127.0.0.1 nav_do_initial_mapping:=True sw_pll_only_publish:=False & +waitUntilRvizClosed 15 kill_simu # # Run cpp examples with polling # -# Start tim7xx emulator and run sick_scan_xd_api_test (cpp example) +# Start tim7xx emulator and run sick_scan_xd_api_test (cpp example with polling) start_tim7xx_emulator # rosrun --prefix 'gdb -ex run --args' sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_tim_7xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False" _polling:=1 rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_tim_7xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False" _polling:=1 & waitUntilRvizClosed 40 kill_simu -# Start mrs100 (multiscan136) emulator and run sick_scan_xd_api_test (cpp example) +# Start mrs100 (multiscan136) emulator and run sick_scan_xd_api_test (cpp example with polling) start_mrs100_emulator -rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_frame_id:=cloud" _polling:=1 & +rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_frame_id:=cloud" _polling:=1 & # Play pcapng-files to emulate MRS100 output -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2 -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2 +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 waitUntilRvizClosed 1 kill_simu -# Start ldmrs emulator and run sick_scan_xd_api_test (cpp example) +# Start ldmrs emulator and run sick_scan_xd_api_test (cpp example with polling) start_ldmrs_emulator rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_ldmrs.launch hostname:=127.0.0.1 sw_pll_only_publish:=False" _polling:=1 & waitUntilRvizClosed 15 kill_simu -# Start mrs1xxx emulator with imu messages and run sick_scan_xd_api_test (cpp example) +# Start mrs1xxx emulator with imu messages and run sick_scan_xd_api_test (cpp example with polling) start_mrs1xxx_emulator rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_mrs_1xxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False" _polling:=1 & waitUntilRvizClosed 10 kill_simu -# Start rms3xx radar emulator and run sick_scan_xd_api_test (cpp example) -start_rms3xx_emulator -rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_rms_3xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False" _polling:=1 & -waitUntilRvizClosed 10 +# Start rms2xxx radar emulator and run sick_scan_xd_api_test (cpp example with polling) +start_rms2xxx_emulator +rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_rms_xxxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False" _polling:=1 & +sleep 10 +kill_simu + +# Start nav350 emulator and run sick_scan_xd_api_test (cpp example with polling) +start_nav350_emulator +rosrun sick_scan sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_nav_350.launch hostname:=127.0.0.1 nav_do_initial_mapping:=True sw_pll_only_publish:=False" _polling:=1 & +waitUntilRvizClosed 15 kill_simu echo -e "run_api_test finished\n" diff --git a/test/scripts/run_linux_ros1_simu_add_transform_dynamically.bash b/test/scripts/run_linux_ros1_simu_add_transform_dynamically.bash index c29ffa9d..aa0de6eb 100644 --- a/test/scripts/run_linux_ros1_simu_add_transform_dynamically.bash +++ b/test/scripts/run_linux_ros1_simu_add_transform_dynamically.bash @@ -18,20 +18,11 @@ function start_ldmrs_emulator() sleep 1 } -# Start rms3xx radar emulator and rviz -function start_rms3xx_emulator() -{ - echo -e "\nrun_linux_ros1_simu_add_transform: starting rms3xx radar emulation ...\n" - roslaunch sick_scan emulator_rms3xx.launch & - sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_rms3xx_add_transform.rviz --opengl 210 & - sleep 1 -} - # Start mrs100 (multiscan136) emulator and rviz function start_mrs100_emulator() { echo -e "\nrun_linux_ros1_simu_add_transform: starting mrs100 (multiscan136) emulation ...\n" - python3 ./src/sick_scan_xd/test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & + python3 ./src/sick_scan_xd/test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_mrs100_add_transform.rviz --opengl 210 & sleep 1 } @@ -56,8 +47,8 @@ function kill_simu() rosnode kill -a ; sleep 1 killall sick_generic_caller ; sleep 1 killall sick_scan_emulator ; sleep 1 - pkill -f mrs100_sopas_test_server.py - pkill -f mrs100_pcap_player.py + pkill -f multiscan_sopas_test_server.py + pkill -f multiscan_pcap_player.py } # Sets parameter add_transform_xyz_rpy dynamically, moves and rotates the pointcloud @@ -110,10 +101,10 @@ function run_simu_tim7xx() function run_simu_mrs100() { start_mrs100_emulator - echo -e "\nrun_linux_ros1_simu_add_transform.bash: starting sick_scan sick_scansegment_xd.launch, no transform\n" - roslaunch sick_scan sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 add_transform_xyz_rpy:=0,0,0,0,0,0 add_transform_check_dynamic_updates:=true & + echo -e "\nrun_linux_ros1_simu_add_transform.bash: starting sick_scan sick_multiscan.launch, no transform\n" + roslaunch sick_scan sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 add_transform_xyz_rpy:=0,0,0,0,0,0 add_transform_check_dynamic_updates:=true & sleep 5 - python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=10 & + python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=10 & run_simu_transforms sick_scansegment_xd/add_transform_xyz_rpy 2.0 kill_simu } @@ -129,18 +120,6 @@ function run_simu_ldmrs() kill_simu } -# Run sick_generic_caller with rms3xx radar and additional transform -function run_simu_rms3xx() -{ - start_rms3xx_emulator - echo -e "\nrun_linux_ros1_simu_add_transform.bash: starting sick_scan sick_rms_3xx.launch, no transform\n" - roslaunch sick_scan sick_rms_3xx.launch hostname:=127.0.0.1 add_transform_xyz_rpy:=0,0,0,0,0,0 add_transform_check_dynamic_updates:=true & - sleep 5 - run_simu_transforms sick_rms_3xx/add_transform_xyz_rpy 3.0 - kill_simu -} - - printf "\033c" pushd ../../../.. if [ -f /opt/ros/melodic/setup.bash ] ; then source /opt/ros/melodic/setup.bash ; fi @@ -159,7 +138,6 @@ fi run_simu_tim7xx run_simu_mrs100 run_simu_ldmrs -run_simu_rms3xx echo -e "\nrun_linux_ros1_simu_add_transform.bash finished.\n" popd diff --git a/test/scripts/run_linux_ros1_simu_add_transform_static.bash b/test/scripts/run_linux_ros1_simu_add_transform_static.bash index 6bac3744..5b0548f5 100644 --- a/test/scripts/run_linux_ros1_simu_add_transform_static.bash +++ b/test/scripts/run_linux_ros1_simu_add_transform_static.bash @@ -20,21 +20,11 @@ function start_ldmrs_emulator() sleep 1 } -# Start rms3xx radar emulator and rviz -function start_rms3xx_emulator() -{ - echo -e "\nrun_linux_ros1_simu_add_transform: starting rms3xx radar emulation ...\n" - roslaunch sick_scan emulator_rms3xx.launch & - sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_rms3xx_add_transform.rviz --opengl 210 & - sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_rms3xx_add_transform_origin.rviz --opengl 210 & - sleep 1 -} - # Start mrs100 (multiscan136) emulator and rviz function start_mrs100_emulator() { echo -e "\nrun_linux_ros1_simu_add_transform: starting mrs100 (multiscan136) emulation ...\n" - python3 ./src/sick_scan_xd/test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & + python3 ./src/sick_scan_xd/test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_mrs100_add_transform.rviz --opengl 210 & sleep 1 ; rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_mrs100_add_transform_origin.rviz --opengl 210 & sleep 1 @@ -60,8 +50,8 @@ function kill_simu() rosnode kill -a ; sleep 1 killall sick_generic_caller ; sleep 1 killall sick_scan_emulator ; sleep 1 - pkill -f mrs100_sopas_test_server.py - pkill -f mrs100_pcap_player.py + pkill -f multiscan_sopas_test_server.py + pkill -f multiscan_pcap_player.py } # Run sick_generic_caller with tim7xx and additional transform @@ -80,11 +70,11 @@ function run_simu_tim7xx() function run_simu_mrs100() { tx=$1 ; ty=$2 ; tz=$3 ; roll=$4 ; pitch=$5 ; yaw=$6 ; duration_sec=$7 - echo -e "\nrun_linux_ros1_simu_add_transform.bash: starting sick_scan sick_scansegment_xd.launch (MRS100/Multiscan136) with additional transform ($tx, $ty, $tz, $roll, $pitch, $yaw)\n" + echo -e "\nrun_linux_ros1_simu_add_transform.bash: starting sick_scan sick_multiscan.launch (MRS100/Multiscan136) with additional transform ($tx, $ty, $tz, $roll, $pitch, $yaw)\n" start_mrs100_emulator - roslaunch sick_scan sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 add_transform_xyz_rpy:=$tx,$ty,$tz,$roll,$pitch,$yaw & + roslaunch sick_scan sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 add_transform_xyz_rpy:=$tx,$ty,$tz,$roll,$pitch,$yaw & rosrun tf static_transform_publisher $tx $ty $tz $yaw $pitch $roll world origin 100 & - python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=1 + python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=1 kill_simu } @@ -100,18 +90,6 @@ function run_simu_ldmrs() kill_simu } -# Run sick_generic_caller with rms3xx radar and additional transform -function run_simu_rms3xx() -{ - tx=$1 ; ty=$2 ; tz=$3 ; roll=$4 ; pitch=$5 ; yaw=$6 ; duration_sec=$7 - echo -e "\nrun_linux_ros1_simu_add_transform.bash: starting sick_scan sick_rms_3xx.launch with additional transform ($tx, $ty, $tz, $roll, $pitch, $yaw)\n" - start_rms3xx_emulator - roslaunch sick_scan sick_rms_3xx.launch hostname:=127.0.0.1 add_transform_xyz_rpy:=$tx,$ty,$tz,$roll,$pitch,$yaw & - rosrun tf static_transform_publisher $tx $ty $tz $yaw $pitch $roll radar origin 100 & - waitUntilRvizClosed $duration_sec - kill_simu -} - printf "\033c" pushd ../../../.. if [ -f /opt/ros/melodic/setup.bash ] ; then source /opt/ros/melodic/setup.bash ; fi @@ -151,11 +129,6 @@ run_simu_ldmrs 0 0 0 0.0000000 0.0000000 0.0000000 10 # ldmrs, x=0, y run_simu_ldmrs 0 0 0 0.0000000 0.0000000 0.7853982 15 # ldmrs, x=0, y=0, z=0, roll=0, pitch=0, yaw=45 deg, duration_sec=15 run_simu_ldmrs 0.5 -0.5 -0.5 -0.7853982 -0.7853982 -0.7853982 15 # ldmrs, x=+0.5, y=-0.5, z=-0.5, roll=-45, pitch=-45, yaw=-45 deg, duration_sec=15 -# Run sick_generic_caller with rms3xx radar and additional transforms -run_simu_rms3xx 0 0 0 0.0000000 0.0000000 0.0000000 10 # rms3xx radar, x=0, y=0, z=0, roll=0, pitch=0, yaw=0 deg, duration_sec=15 -run_simu_rms3xx 0 0 0 0.0000000 0.0000000 0.7853982 15 # rms3xx radar, x=0, y=0, z=0, roll=0, pitch=0, yaw=45 deg, duration_sec=15 -run_simu_rms3xx 0.5 -0.5 -0.5 -0.7853982 -0.7853982 -0.7853982 15 # rms3xx radar, x=+0.5, y=-0.5, z=-0.5, roll=-45, pitch=-45, yaw=-45 deg, duration_sec=15 - echo -e "\nrun_linux_ros1_simu_add_transform.bash finished.\n" popd diff --git a/test/scripts/run_linux_ros1_simu_mrs100.bash b/test/scripts/run_linux_ros1_simu_mrs100.bash deleted file mode 100644 index e4977d20..00000000 --- a/test/scripts/run_linux_ros1_simu_mrs100.bash +++ /dev/null @@ -1,91 +0,0 @@ -#!/bin/bash - -# killall and cleanup after exit -function killall_cleanup() -{ - rosnode kill -a - killall sick_generic_caller - pkill -f mrs100_sopas_test_server.py - pkill -f mrs100_laserscan_msg_to_pointcloud.py -} - -# Run example ros service calls -function call_service_examples() -{ - sleep 0.1 ; rosservice list - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN IsSystemReady'}" # response: "sAN IsSystemReady 1" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115'}" # response: "sWA ScanDataEthSettings" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataFormat 1'}" # response: "sWA ScanDataFormat" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataPreformatting 1'}" # response: "sWA ScanDataPreformatting" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataEnable 1'}" # response: "sWA ScanDataEnable" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN LMCstartmeas'}" # response: "sAN LMCstartmeas" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN Run'}" # response: "sAN Run 1" -} - -# Run example ros service calls for filter settings -function call_service_filter_examples() -{ - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPlayerFilter'}" # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN FREchoFilter 1'}" # response: "sWA FREchoFilter" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1'}" # response: "sWA LFPangleRangeFilter" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1'}" # response: "sWA LFPlayerFilter" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN Run'}" # response: "sAN Run 1" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1" - sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPlayerFilter'}" # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" -} - -# -# Run sick_scansegment_xd on ROS1-Linux -# - -pushd ../../../.. -printf "\033c" -source /opt/ros/noetic/setup.bash -# source ./install_isolated/setup.bash -source ./devel_isolated/setup.bash -killall_cleanup -sleep 1 -rm -rf ~/.ros/log -sleep 1 - -# Run mrs100 emulator (sopas test server) -python3 ./src/sick_scan_xd/test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_cfg_mrs100_emu_laserscan.rviz & -sleep 1 -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_cfg_mrs100_emu.rviz & -sleep 1 -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_cfg_mrs100_emu_360.rviz & -sleep 1 - -# Start sick_generic_caller with sick_scansegment_xd -# Note: To verify laserscan messages, we configure laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", i.e. a laserscan message is published for each segment, each layer and each echo. -# By default, laserscan messages are only activated for layer 5 (elevation -0.07 degree, max number of scan points) -# All laserscan messages are converted to pointcloud by mrs100_laserscan_msg_to_pointcloud.py using a hardcoded elevation table. -# Note: Option laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" should not be used for performance tests. -echo -e "run_lidar3d.bash: sick_scan sick_scansegment_xd.launch ..." -roslaunch sick_scan sick_scansegment_xd.launch hostname:="127.0.0.1" udp_receiver_ip:="127.0.0.1" laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" & -sleep 3 -python3 ./src/sick_scan_xd/test/python/mrs100_laserscan_msg_to_pointcloud.py & -sleep 1 - -# Run example ros service calls -call_service_examples -call_service_filter_examples -sleep 3 - -# Play pcapng-files to emulate MRS100 output -echo -e "\nPlaying pcapng-files to emulate MRS100. Note: Start of UDP msgpacks in 20220915_mrs100_msgpack_output.pcapng takes a while...\n" -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 --repeat=1 -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=1 -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 --repeat=1 -sleep 3 - -# Shutdown -echo -e "run_lidar3d.bash finished, killing all processes ..." -killall_cleanup -popd diff --git a/test/scripts/run_linux_ros1_simu_multiscan.bash b/test/scripts/run_linux_ros1_simu_multiscan.bash new file mode 100644 index 00000000..6b1e42e4 --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_multiscan.bash @@ -0,0 +1,89 @@ +#!/bin/bash + +# killall and cleanup after exit +function killall_cleanup() +{ + rosnode kill -a + killall sick_generic_caller + pkill -f multiscan_sopas_test_server.py + pkill -f multiscan_laserscan_msg_to_pointcloud.py +} + +# Run example ros service calls +function call_service_examples() +{ + sleep 0.1 ; rosservice list + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sMN IsSystemReady'}" # response: "sAN IsSystemReady 1" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115'}" # response: "sWA ScanDataEthSettings" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sWN ScanDataFormat 1'}" # response: "sWA ScanDataFormat" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sWN ScanDataPreformatting 1'}" # response: "sWA ScanDataPreformatting" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sWN ScanDataEnable 1'}" # response: "sWA ScanDataEnable" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sMN LMCstartmeas'}" # response: "sAN LMCstartmeas" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sMN Run'}" # response: "sAN Run 1" +} + +# Run example ros service calls for filter settings +function call_service_filter_examples() +{ + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sRN LFPlayerFilter'}" # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sWN FREchoFilter 1'}" # response: "sWA FREchoFilter" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sWN LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1'}" # response: "sWA LFPangleRangeFilter" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sWN LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1'}" # response: "sWA LFPlayerFilter" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sMN Run'}" # response: "sAN Run 1" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1" + sleep 0.1 ; rosservice call /multiScan/ColaMsg "{request: 'sRN LFPlayerFilter'}" # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" +} + +# +# Run sick_scansegment_xd on ROS1-Linux +# + +pushd ../../../.. +printf "\033c" +source /opt/ros/noetic/setup.bash +# source ./install_isolated/setup.bash +source ./devel_isolated/setup.bash +killall_cleanup +sleep 1 +rm -rf ~/.ros/log +sleep 1 + +# Run Multiscan136 emulator (sopas test server) +python3 ./src/sick_scan_xd/test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_cfg_multiscan_emu_laserscan.rviz & +sleep 1 +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_cfg_multiscan_emu.rviz & +sleep 1 +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_cfg_multiscan_emu_360.rviz & +sleep 1 + +# Start sick_generic_caller with sick_scansegment_xd +# Note: To verify laserscan messages, we configure laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", i.e. a laserscan message is published for each segment, each layer and each echo. +# By default, laserscan messages are only activated for layer 5 (elevation -0.07 degree, max number of scan points) +# All laserscan messages are converted to pointcloud by mrs100_laserscan_msg_to_pointcloud.py using a hardcoded elevation table. +# Note: Option laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" should not be used for performance tests. +echo -e "run_lidar3d.bash: sick_scan sick_multiscan.launch ..." +roslaunch sick_scan sick_multiscan.launch hostname:="127.0.0.1" udp_receiver_ip:="127.0.0.1" laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" & +sleep 3 +python3 ./src/sick_scan_xd/test/python/multiscan_laserscan_msg_to_pointcloud.py & +sleep 1 + +# Run example ros service calls +call_service_examples +call_service_filter_examples +sleep 3 + +# Play pcapng-files to emulate MRS100 output +echo -e "\nPlaying pcapng-files to emulate MRS100. Note: Start of UDP msgpacks in 20220915_mrs100_msgpack_output.pcapng takes a while...\n" +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 --repeat=1 +sleep 3 + +# Shutdown +echo -e "run_lidar3d.bash finished, killing all processes ..." +killall_cleanup +popd diff --git a/test/scripts/run_linux_ros1_simu_nav350.bash b/test/scripts/run_linux_ros1_simu_nav350.bash new file mode 100644 index 00000000..b3ae8be3 --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_nav350.bash @@ -0,0 +1,57 @@ +#!/bin/bash +printf "\033c" +pushd ../../../.. +if [ -f /opt/ros/melodic/setup.bash ] ; then source /opt/ros/melodic/setup.bash ; fi +if [ -f /opt/ros/noetic/setup.bash ] ; then source /opt/ros/noetic/setup.bash ; fi +if [ -f ./devel_isolated/setup.bash ] ; then source ./devel_isolated/setup.bash ; fi + +echo -e "run_simu_nav350.bash: starting NAV-350 emulation\n" + +# Start roscore if not yet running +roscore_running=`(ps -elf | grep roscore | grep -v grep | wc -l)` +if [ $roscore_running -lt 1 ] ; then + roscore & + sleep 3 +fi + +# Start sick_scan emulator +python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20230126_nav350_4reflectors_moving.pcapng.json --scandata_id="sAN mNPOSGetData" --send_rate=8 --verbosity=0 & +sleep 1 + +# Start rviz +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_nav350.rviz --opengl 210 & +sleep 1 + +# Start sick_scan driver for rms +echo -e "Launching sick_scan sick_nav_350.launch\n" +# nav_landmark_imk_file=`(pwd)`/src/sick_scan_xd/test/emulator/scandata/20230126_nav350_4reflectors_moving.imk +# roslaunch sick_scan sick_nav_350.launch hostname:=127.0.0.1 nav_do_initial_mapping:=False nav_set_landmark_layout_by_imk_file:=$nav_landmark_imk_file sw_pll_only_publish:=False & +# roslaunch sick_scan sick_nav_350.launch hostname:=127.0.0.1 nav_do_initial_mapping:=False sw_pll_only_publish:=False & +roslaunch sick_scan sick_nav_350.launch hostname:=127.0.0.1 nav_do_initial_mapping:=True sw_pll_only_publish:=False & +sleep 1 + +# Wait for 'q' or 'Q' to exit or until rviz is closed +while true ; do + echo -e "NAV-350 emulation running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key + if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi + rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)` + if [ $rviz_running -lt 1 ] ; then break ; fi +done + +# Service examples: +# rosservice call /sick_nav_350/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" +# rosservice call /sick_nav_350/ColaMsg "{request: 'sMN mNPOSGetData 1 2'}" + +# Odometry examples (set velocity) +# rostopic pub --once /sick_nav_350/nav_odom_velocity sick_scan/NAVOdomVelocity '{vel_x: 0.0, vel_y: 0.0, omega: 0.0, timestamp: 0, coordbase: 0}' +# rostopic pub --once /sick_nav_350/odom nav_msgs/Odometry '{twist: { twist: { linear: {x: 1.0, y: -1.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.5}}}}' +# rostopic pub --rate 10 /sick_nav_350/nav_odom_velocity sick_scan/NAVOdomVelocity '{vel_x: 0.0, vel_y: 0.0, omega: 0.0, timestamp: 0, coordbase: 0}' +# rostopic pub --rate 10 /sick_nav_350/nav_odom_velocity sick_scan/NAVOdomVelocity '{vel_x: 1.0, vel_y: -1.0, omega: 0.5, timestamp: 123456789, coordbase: 0}' +# rostopic pub --rate 10 /sick_nav_350/odom nav_msgs/Odometry '{twist: { twist: { linear: {x: 1.0, y: -1.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.5}}}}' + +# Shutdown +echo -e "Finishing NAV-350 emulation, shutdown ros nodes\n" +rosnode kill -a ; sleep 1 +killall sick_generic_caller ; sleep 1 +popd + diff --git a/test/scripts/run_linux_ros1_simu_rms1xxx.bash b/test/scripts/run_linux_ros1_simu_rms1xxx.bash deleted file mode 100644 index 7e7d1124..00000000 --- a/test/scripts/run_linux_ros1_simu_rms1xxx.bash +++ /dev/null @@ -1,52 +0,0 @@ -#!/bin/bash -printf "\033c" -pushd ../../../.. -if [ -f /opt/ros/melodic/setup.bash ] ; then source /opt/ros/melodic/setup.bash ; fi -if [ -f /opt/ros/noetic/setup.bash ] ; then source /opt/ros/noetic/setup.bash ; fi -if [ -f ./devel_isolated/setup.bash ] ; then source ./devel_isolated/setup.bash ; fi -# if [ -f ./install_isolated/setup.bash ] ; then source ./install_isolated/setup.bash ; fi -# if [ -f ./install/setup.bash ] ; then source ./install/setup.bash ; fi - -echo -e "run_simu_rms.bash: starting rms emulation\n" - -# Start roscore if not yet running -roscore_running=`(ps -elf | grep roscore | grep -v grep | wc -l)` -if [ $roscore_running -lt 1 ] ; then - roscore & - sleep 3 -fi - -# Start sick_scan emulator -roslaunch sick_scan emulator_rms1xxx.launch & -sleep 1 - -# Start rviz -# Note: Due to a bug in opengl 3 in combination with rviz and VMware, opengl 2 should be used by rviz option --opengl 210 -# See https://github.com/ros-visualization/rviz/issues/1444 and https://github.com/ros-visualization/rviz/issues/1508 for further details - -#rosrun rviz rviz -d ./src/sick_scan_xd/launch/rviz/radar_rawtarget_vrad.rviz --opengl 210 & -#rosrun rviz rviz -d ./src/sick_scan_xd/launch/rviz/radar_rawtarget_amplitude.rviz --opengl 210 & -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_rms1xxx.rviz --opengl 210 & -sleep 1 - -# Start sick_scan driver for rms -echo -e "Launching sick_scan sick_rms_1xxx.launch\n" -# roslaunch sick_scan sick_rms_1xxx.launch hostname:=192.168.0.151 & -roslaunch sick_scan sick_rms_1xxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & -sleep 1 - -# Wait for 'q' or 'Q' to exit or until rviz is closed -while true ; do - echo -e "rms emulation running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key - if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi - rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)` - if [ $rviz_running -lt 1 ] ; then break ; fi -done - -# Shutdown -echo -e "Finishing rms emulation, shutdown ros nodes\n" -rosnode kill -a ; sleep 1 -killall sick_generic_caller ; sleep 1 -killall sick_scan_emulator ; sleep 1 -popd - diff --git a/test/scripts/run_linux_ros1_simu_rms1xxx_ascii.bash b/test/scripts/run_linux_ros1_simu_rms1xxx_ascii.bash deleted file mode 100644 index ed636b54..00000000 --- a/test/scripts/run_linux_ros1_simu_rms1xxx_ascii.bash +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -printf "\033c" -pushd ../../../.. -if [ -f /opt/ros/melodic/setup.bash ] ; then source /opt/ros/melodic/setup.bash ; fi -if [ -f /opt/ros/noetic/setup.bash ] ; then source /opt/ros/noetic/setup.bash ; fi -if [ -f ./devel_isolated/setup.bash ] ; then source ./devel_isolated/setup.bash ; fi -# if [ -f ./install_isolated/setup.bash ] ; then source ./install_isolated/setup.bash ; fi -# if [ -f ./install/setup.bash ] ; then source ./install/setup.bash ; fi - -echo -e "run_simu_rms.bash: starting rms emulation\n" - -# Start roscore if not yet running -roscore_running=`(ps -elf | grep roscore | grep -v grep | wc -l)` -if [ $roscore_running -lt 1 ] ; then - roscore & - sleep 3 -fi - -# Start sick_scan emulator -# python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20221018_rms_1xxx_ascii_rawtarget_object.pcapng.json --scandata_id="sSN LMDradardata" --send_rate=10 --verbosity=2 & -python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20221018_rms_1xxx_ascii_rms2_objects.pcapng.json --scandata_id="sSN LMDradardata" --send_rate=10 --verbosity=2 & -sleep 1 - -# Start rviz -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_rms1xxx.rviz --opengl 210 & -sleep 1 - -# Start sick_scan driver for rms -echo -e "Launching sick_scan sick_rms_1xxx.launch\n" -roslaunch sick_scan sick_rms_1xxx_ascii.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & -sleep 1 - -# Wait for 'q' or 'Q' to exit or until rviz is closed -while true ; do - echo -e "rms emulation running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key - if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi - rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)` - if [ $rviz_running -lt 1 ] ; then break ; fi -done - -# Shutdown -echo -e "Finishing rms emulation, shutdown ros nodes\n" -rosnode kill -a ; sleep 1 -killall sick_generic_caller ; sleep 1 -killall sick_scan_emulator ; sleep 1 -popd - diff --git a/test/scripts/run_linux_ros1_simu_rms2xxx_ascii.bash b/test/scripts/run_linux_ros1_simu_rms2xxx_ascii.bash deleted file mode 100644 index bff37f49..00000000 --- a/test/scripts/run_linux_ros1_simu_rms2xxx_ascii.bash +++ /dev/null @@ -1,46 +0,0 @@ -#!/bin/bash -printf "\033c" -pushd ../../../.. -if [ -f /opt/ros/melodic/setup.bash ] ; then source /opt/ros/melodic/setup.bash ; fi -if [ -f /opt/ros/noetic/setup.bash ] ; then source /opt/ros/noetic/setup.bash ; fi -if [ -f ./devel_isolated/setup.bash ] ; then source ./devel_isolated/setup.bash ; fi -# if [ -f ./install_isolated/setup.bash ] ; then source ./install_isolated/setup.bash ; fi -# if [ -f ./install/setup.bash ] ; then source ./install/setup.bash ; fi - -echo -e "run_simu_rms.bash: starting rms emulation\n" - -# Start roscore if not yet running -roscore_running=`(ps -elf | grep roscore | grep -v grep | wc -l)` -if [ $roscore_running -lt 1 ] ; then - roscore & - sleep 3 -fi - -# Start sick_scan emulator -python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20221018_rms_1xxx_ascii_rms2_objects.pcapng.json --scandata_id="sSN LMDradardata" --send_rate=10 --verbosity=2 & -sleep 1 - -# Start rviz -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz --opengl 210 & -sleep 1 - -# Start sick_scan driver for rms -echo -e "Launching sick_scan sick_rms_2xxx.launch\n" -roslaunch sick_scan sick_rms_2xxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & -sleep 1 - -# Wait for 'q' or 'Q' to exit or until rviz is closed -while true ; do - echo -e "rms emulation running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key - if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi - rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)` - if [ $rviz_running -lt 1 ] ; then break ; fi -done - -# Shutdown -echo -e "Finishing rms emulation, shutdown ros nodes\n" -rosnode kill -a ; sleep 1 -killall sick_generic_caller ; sleep 1 -killall sick_scan_emulator ; sleep 1 -popd - diff --git a/test/scripts/run_linux_ros1_simu_rms3xx.bash b/test/scripts/run_linux_ros1_simu_rms3xx.bash deleted file mode 100644 index ccddc444..00000000 --- a/test/scripts/run_linux_ros1_simu_rms3xx.bash +++ /dev/null @@ -1,52 +0,0 @@ -#!/bin/bash -printf "\033c" -pushd ../../../.. -if [ -f /opt/ros/melodic/setup.bash ] ; then source /opt/ros/melodic/setup.bash ; fi -if [ -f /opt/ros/noetic/setup.bash ] ; then source /opt/ros/noetic/setup.bash ; fi -if [ -f ./devel_isolated/setup.bash ] ; then source ./devel_isolated/setup.bash ; fi -# if [ -f ./install_isolated/setup.bash ] ; then source ./install_isolated/setup.bash ; fi -# if [ -f ./install/setup.bash ] ; then source ./install/setup.bash ; fi - -echo -e "run_simu_rms.bash: starting rms emulation\n" - -# Start roscore if not yet running -roscore_running=`(ps -elf | grep roscore | grep -v grep | wc -l)` -if [ $roscore_running -lt 1 ] ; then - roscore & - sleep 3 -fi - -# Start sick_scan emulator -roslaunch sick_scan emulator_rms3xx.launch & -sleep 1 - -# Start rviz -# Note: Due to a bug in opengl 3 in combination with rviz and VMware, opengl 2 should be used by rviz option --opengl 210 -# See https://github.com/ros-visualization/rviz/issues/1444 and https://github.com/ros-visualization/rviz/issues/1508 for further details - -# rosrun rviz rviz -d ./src/sick_scan_xd/launch/rviz/radar_rawtarget_vrad.rviz --opengl 210 & -# rosrun rviz rviz -d ./src/sick_scan_xd/launch/rviz/radar_rawtarget_amplitude.rviz --opengl 210 & -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_rms1xxx.rviz --opengl 210 & -sleep 1 - -# Start sick_scan driver for rms -echo -e "Launching sick_scan sick_rms_3xx.launch\n" -# roslaunch sick_scan sick_rms_3xx.launch hostname:=192.168.0.151 & -roslaunch sick_scan sick_rms_3xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & -sleep 1 - -# Wait for 'q' or 'Q' to exit or until rviz is closed -while true ; do - echo -e "rms emulation running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key - if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi - rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)` - if [ $rviz_running -lt 1 ] ; then break ; fi -done - -# Shutdown -echo -e "Finishing rms emulation, shutdown ros nodes\n" -rosnode kill -a ; sleep 1 -killall sick_generic_caller ; sleep 1 -killall sick_scan_emulator ; sleep 1 -popd - diff --git a/test/scripts/run_linux_ros1_simu_rmsxxxx.bash b/test/scripts/run_linux_ros1_simu_rmsxxxx.bash new file mode 100644 index 00000000..4c624c67 --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_rmsxxxx.bash @@ -0,0 +1,55 @@ +#!/bin/bash + +# Wait for max 30 seconds, or until 'q' or 'Q' pressed, or until rviz is closed +function waitUntilRvizClosed() +{ + duration_sec=$1 + sleep 1 + for ((cnt=1;cnt<=$duration_sec;cnt++)) ; do + echo -e "sick_scan_xd running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key + if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi + rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)` + if [ $rviz_running -lt 1 ] ; then break ; fi + done +} + +# Shutdown simulation, kill all nodes and processes +function kill_simu() +{ + echo -e "Finishing rms emulation, shutdown ros nodes\n" + pkill -f sopas_json_test_server.py + rosnode kill -a ; sleep 1 + killall sick_generic_caller ; sleep 1 + killall sick_scan_emulator ; sleep 1 +} + +printf "\033c" +pushd ../../../.. +if [ -f /opt/ros/melodic/setup.bash ] ; then source /opt/ros/melodic/setup.bash ; fi +if [ -f /opt/ros/noetic/setup.bash ] ; then source /opt/ros/noetic/setup.bash ; fi +if [ -f ./devel_isolated/setup.bash ] ; then source ./devel_isolated/setup.bash ; fi + +echo -e "run_simu_rmsxxxx.bash: starting RMSxxxx emulation\n" + +# Start roscore if not yet running +roscore_running=`(ps -elf | grep roscore | grep -v grep | wc -l)` +if [ $roscore_running -lt 1 ] ; then + roscore & + sleep 3 +fi + +# Run rms1xxx emulator, rviz and launch sick_scan sick_rms_xxxx.launch (ascii) +python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20220316-rms1000-ascii.pcapng.json --scandata_id="sSN LMDradardata" --send_rate=10 --verbosity=1 & +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_rms1xxx.rviz --opengl 210 & +roslaunch sick_scan sick_rms_xxxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & +waitUntilRvizClosed 15 +kill_simu + +# Run rms2xxx emulator, rviz and launch sick_scan sick_rms_xxxx.launch (ascii) +python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20221018_rms_1xxx_ascii_rms2_objects.pcapng.json --scandata_id="sSN LMDradardata" --send_rate=10 --verbosity=1 & +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz --opengl 210 & +roslaunch sick_scan sick_rms_xxxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & +waitUntilRvizClosed 15 +kill_simu + +popd diff --git a/test/scripts/run_linux_ros2_simu_mrs100.bash b/test/scripts/run_linux_ros2_simu_multiscan.bash similarity index 75% rename from test/scripts/run_linux_ros2_simu_mrs100.bash rename to test/scripts/run_linux_ros2_simu_multiscan.bash index fba94b48..c9773174 100644 --- a/test/scripts/run_linux_ros2_simu_mrs100.bash +++ b/test/scripts/run_linux_ros2_simu_multiscan.bash @@ -5,8 +5,8 @@ function killall_cleanup() { sleep 1 ; killall -SIGINT rviz2 sleep 1 ; killall -SIGINT sick_generic_caller - sleep 1 ; pkill -f mrs100_sopas_test_server.py - sleep 1 ; pkill -f mrs100_laserscan_msg_to_pointcloud.py + sleep 1 ; pkill -f multiscan_sopas_test_server.py + sleep 1 ; pkill -f multiscan_laserscan_msg_to_pointcloud.py sleep 1 ; killall -9 rviz2 sleep 1 ; killall -9 sick_generic_caller } @@ -55,25 +55,25 @@ sleep 1 rm -rf ~/.ros/log sleep 1 -# Run mrs100 emulator (sopas test server) -python3 ./src/sick_scan_xd/test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & -ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_emu_laserscan.rviz & +# Run multiscan emulator (sopas test server) +python3 ./src/sick_scan_xd/test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & +ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_multiscan_emu_laserscan.rviz & sleep 1 -ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_emu.rviz & +ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_multiscan_emu.rviz & sleep 1 -ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_emu_360.rviz & +ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_multiscan_emu_360.rviz & sleep 1 # Start sick_generic_caller with sick_scansegment_xd # Note: To verify laserscan messages, we configure laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", i.e. a laserscan message is published for each segment, each layer and each echo. # By default, laserscan messages are only activated for layer 5 (elevation -0.07 degree, max number of scan points) -# All laserscan messages are converted to pointcloud by mrs100_laserscan_msg_to_pointcloud.py using a hardcoded elevation table. +# All laserscan messages are converted to pointcloud by multiscan_laserscan_msg_to_pointcloud.py using a hardcoded elevation table. # Note: Option laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" should not be used for performance tests. -# ros2 launch sick_scan sick_scansegment_xd.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" & -echo -e "run_lidar3d.bash: sick_scan sick_scansegment_xd.launch.py ..." -ros2 launch sick_scan sick_scansegment_xd.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" & +# ros2 launch sick_scan sick_multiscan.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" & +echo -e "run_lidar3d.bash: sick_scan sick_multiscan.launch.py ..." +ros2 launch sick_scan sick_multiscan.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" & sleep 3 -# python3 ./src/sick_scan_xd/test/python/mrs100_laserscan_msg_to_pointcloud.py & +# python3 ./src/sick_scan_xd/test/python/multiscan_laserscan_msg_to_pointcloud.py & # sleep 1 # Run example ros service calls @@ -81,11 +81,9 @@ sleep 3 #call_service_filter_examples #sleep 3 -# Play pcapng-files to emulate MRS100 output -echo -e "\nPlaying pcapng-files to emulate MRS100. Note: Start of UDP msgpacks in 20220915_mrs100_msgpack_output.pcapng takes a while...\n" -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 --repeat=1 -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=1 -python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 --repeat=1 +# Play pcapng-files to emulate multiScan output +echo -e "\nPlaying pcapng-files to emulate multiScan. Note: Start of UDP msgpacks in 20220915_multiscan_msgpack_output.pcapng takes a while...\n" +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 --repeat=1 sleep 3 # Shutdown diff --git a/test/scripts/run_linux_ros2_simu_nav350.bash b/test/scripts/run_linux_ros2_simu_nav350.bash new file mode 100644 index 00000000..861dd7be --- /dev/null +++ b/test/scripts/run_linux_ros2_simu_nav350.bash @@ -0,0 +1,37 @@ +#!/bin/bash +printf "\033c" +pushd ../../../.. +if [ -f /opt/ros/eloquent/setup.bash ] ; then source /opt/ros/eloquent/setup.bash ; fi +if [ -f /opt/ros/foxy/setup.bash ] ; then source /opt/ros/foxy/setup.bash ; fi +source ./install/setup.bash + +echo -e "run_simu_nav350.bash: starting NAV-350 emulation\n" + +# Start sick_scan emulator +python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20230126_nav350_4reflectors_moving.pcapng.json --scandata_id="sAN mNPOSGetData" --send_rate=8 --verbosity=0 & +sleep 1 + +# Start rviz +ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_emulator_cfg_nav350.rviz & +sleep 1 + +# Start sick_scan driver for rms +echo -e "Launching sick_scan sick_nav_350.launch\n" +ros2 launch sick_scan sick_nav_350.launch.py hostname:=127.0.0.1 & +sleep 1 + +# Wait for 'q' or 'Q' to exit or until rviz is closed +while true ; do + echo -e "NAV-350 emulation running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key + if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi + rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)` + if [ $rviz_running -lt 1 ] ; then break ; fi +done + +# Shutdown +echo -e "Finishing NAV-350 emulation, shutdown ros nodes\n" +killall sick_generic_caller ; sleep 1 +pkill -f sopas_json_test_server.py ; sleep 1 +killall rviz2 +popd + diff --git a/test/scripts/run_linux_ros2_simu_rms1xxx.bash b/test/scripts/run_linux_ros2_simu_rms1xxx.bash deleted file mode 100644 index f3df87a7..00000000 --- a/test/scripts/run_linux_ros2_simu_rms1xxx.bash +++ /dev/null @@ -1,53 +0,0 @@ -#!/bin/bash - -# -# Set environment -# - -function simu_killall() -{ - sleep 1 ; killall -SIGINT rviz2 - sleep 1 ; killall -SIGINT sick_generic_caller - sleep 1 ; killall -SIGINT sick_scan_emulator - sleep 1 ; killall -9 rviz2 - sleep 1 ; killall -9 sick_generic_caller - sleep 1 ; killall -9 sick_scan_emulator - sleep 1 -} - -simu_killall -printf "\033c" -pushd ../../../.. -if [ -f /opt/ros/eloquent/setup.bash ] ; then source /opt/ros/eloquent/setup.bash ; fi -if [ -f /opt/ros/foxy/setup.bash ] ; then source /opt/ros/foxy/setup.bash ; fi -source ./install/setup.bash - -# -# Run simulation: -# 1. Start sick_scan_emulator -# 2. Start sick_scan driver sick_generic_caller -# 3. Run rviz -# - -echo -e "run_linux_ros2_simu_rms.bash: starting rms emulation\n" -cp -f ./src/sick_scan_xd/test/emulator/scandata/20211201_RMS_1xxx_start_up.pcapng.json /tmp/lmd_scandata.pcapng.json -sleep 1 ; ros2 run sick_scan sick_scan_emulator ./src/sick_scan_xd/test/emulator/launch/emulator_rms1xxx.launch scanner_type:=sick_rms_1xxx & -# sleep 1 ; ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_1xxx.launch hostname:=127.0.0.1 port:=2111 & -# sleep 1 ; ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_1xxx.launch hostname:=127.0.0.1 port:=2111 sw_pll_only_publish:=False & -sleep 1 ; ros2 launch sick_scan sick_rms_1xxx.launch.py hostname:=127.0.0.1 port:=2111 sw_pll_only_publish:=False & -sleep 1 ; ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2_rms.rviz & - -# Wait for 'q' or 'Q' to exit or until rviz is closed ... -while true ; do - echo -e "rms emulation running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key - if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi - rviz_running=`(ps -elf | grep rviz2 | grep -v grep | wc -l)` - if [ $rviz_running -lt 1 ] ; then break ; fi -done - -# ... or stop simulation after 30 seconds -# sleep 30 - -simu_killall -popd - diff --git a/test/scripts/run_linux_ros2_simu_rms2xxx.bash b/test/scripts/run_linux_ros2_simu_rms2xxx.bash deleted file mode 100644 index 321b2a84..00000000 --- a/test/scripts/run_linux_ros2_simu_rms2xxx.bash +++ /dev/null @@ -1,50 +0,0 @@ -#!/bin/bash - -# -# Set environment -# - -function simu_killall() -{ - sleep 1 ; killall -SIGINT rviz2 - sleep 1 ; killall -SIGINT sick_generic_caller - sleep 1 ; killall -9 rviz2 - sleep 1 ; killall -9 sick_generic_caller - sleep 1 -} - -simu_killall -printf "\033c" -pushd ../../../.. -if [ -f /opt/ros/eloquent/setup.bash ] ; then source /opt/ros/eloquent/setup.bash ; fi -if [ -f /opt/ros/foxy/setup.bash ] ; then source /opt/ros/foxy/setup.bash ; fi -source ./install/setup.bash - -# -# Run simulation: -# 1. Start sick_scan_emulator -# 2. Start sick_scan driver sick_generic_caller -# 3. Run rviz -# - -echo -e "run_linux_ros2_simu_rms.bash: starting rms emulation\n" - -# Start sick_scan emulator -python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20221018_rms_1xxx_ascii_rms2_objects.pcapng.json --scandata_id="sSN LMDradardata" --send_rate=10 --verbosity=2 & -sleep 1 ; ros2 launch sick_scan sick_rms_2xxx.launch.py hostname:=127.0.0.1 sw_pll_only_publish:=False & -sleep 1 ; ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2_rms2xxx.rviz & - -# Wait for 'q' or 'Q' to exit or until rviz is closed ... -while true ; do - echo -e "rms emulation running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key - if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi - rviz_running=`(ps -elf | grep rviz2 | grep -v grep | wc -l)` - if [ $rviz_running -lt 1 ] ; then break ; fi -done - -# ... or stop simulation after 30 seconds -# sleep 30 - -simu_killall -popd - diff --git a/test/scripts/run_linux_ros2_simu_rms3xx.bash b/test/scripts/run_linux_ros2_simu_rms3xx.bash deleted file mode 100644 index bf66fa09..00000000 --- a/test/scripts/run_linux_ros2_simu_rms3xx.bash +++ /dev/null @@ -1,53 +0,0 @@ -#!/bin/bash - -# -# Set environment -# - -function simu_killall() -{ - sleep 1 ; killall -SIGINT rviz2 - sleep 1 ; killall -SIGINT sick_generic_caller - sleep 1 ; killall -SIGINT sick_scan_emulator - sleep 1 ; killall -9 rviz2 - sleep 1 ; killall -9 sick_generic_caller - sleep 1 ; killall -9 sick_scan_emulator - sleep 1 -} - -simu_killall -printf "\033c" -pushd ../../../.. -if [ -f /opt/ros/eloquent/setup.bash ] ; then source /opt/ros/eloquent/setup.bash ; fi -if [ -f /opt/ros/foxy/setup.bash ] ; then source /opt/ros/foxy/setup.bash ; fi -source ./install/setup.bash - -# -# Run simulation: -# 1. Start sick_scan_emulator -# 2. Start sick_scan driver sick_generic_caller -# 3. Run rviz -# - -echo -e "run_linux_ros2_simu_rms.bash: starting rms emulation\n" -cp -f ./src/sick_scan_xd/test/emulator/scandata/20211201_RMS_3xx_start_up.pcapng.json /tmp/lmd_scandata.pcapng.json -sleep 1 ; ros2 run sick_scan sick_scan_emulator ./src/sick_scan_xd/test/emulator/launch/emulator_rms3xx.launch scanner_type:=sick_rms_3xx & -# sleep 1 ; ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_3xx.launch hostname:=127.0.0.1 port:=2111 & -# sleep 1 ; ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_3xx.launch hostname:=127.0.0.1 port:=2111 sw_pll_only_publish:=False & -sleep 1 ; ros2 launch sick_scan sick_rms_3xx.launch.py hostname:=127.0.0.1 port:=2111 sw_pll_only_publish:=False & -sleep 1 ; ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2_rms.rviz & - -# Wait for 'q' or 'Q' to exit or until rviz is closed ... -while true ; do - echo -e "rms emulation running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key - if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi - rviz_running=`(ps -elf | grep rviz2 | grep -v grep | wc -l)` - if [ $rviz_running -lt 1 ] ; then break ; fi -done - -# ... or stop simulation after 30 seconds -# sleep 30 - -simu_killall -popd - diff --git a/test/scripts/run_linux_ros2_simu_rmsxxxx.bash b/test/scripts/run_linux_ros2_simu_rmsxxxx.bash new file mode 100644 index 00000000..4f67b908 --- /dev/null +++ b/test/scripts/run_linux_ros2_simu_rmsxxxx.bash @@ -0,0 +1,48 @@ +#!/bin/bash + +# Wait for max 30 seconds, or until 'q' or 'Q' pressed, or until rviz is closed +function waitUntilRvizClosed() +{ + duration_sec=$1 + sleep 1 + for ((cnt=1;cnt<=$duration_sec;cnt++)) ; do + echo -e "sick_scan_xd running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key + if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi + rviz_running=`(ps -elf | grep rviz2 | grep -v grep | wc -l)` + if [ $rviz_running -lt 1 ] ; then break ; fi + done +} + +# Shutdown simulation, kill all nodes and processes +function kill_simu() +{ + echo -e "Finishing rms emulation, shutdown ros nodes\n" + pkill -f sopas_json_test_server.py + killall -SIGINT sick_generic_caller ; sleep 1 + killall -SIGINT rviz2 ; sleep 1 + killall -9 sick_generic_caller ; sleep 1 + killall -9 rviz2 ; sleep 1 +} + +printf "\033c" +pushd ../../../.. +if [ -f /opt/ros/eloquent/setup.bash ] ; then source /opt/ros/eloquent/setup.bash ; fi +if [ -f /opt/ros/foxy/setup.bash ] ; then source /opt/ros/foxy/setup.bash ; fi +source ./install/setup.bash +echo -e "run_simu_rmsxxxx.bash: starting RMSxxxx emulation\n" + +# Run rms1xxx emulator, rviz and launch sick_scan sick_rms_xxxx.launch (ascii) +python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20220316-rms1000-ascii.pcapng.json --scandata_id="sSN LMDradardata" --send_rate=10 --verbosity=1 & +ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2_rms2xxx.rviz & +ros2 launch sick_scan sick_rms_xxxx.launch.py hostname:=127.0.0.1 sw_pll_only_publish:=False & +waitUntilRvizClosed 15 +kill_simu + +# Run rms2xxx emulator, rviz and launch sick_scan sick_rms_xxxx.launch (ascii) +python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20221018_rms_1xxx_ascii_rms2_objects.pcapng.json --scandata_id="sSN LMDradardata" --send_rate=10 --verbosity=1 & +ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2_rms2xxx.rviz & +ros2 launch sick_scan sick_rms_xxxx.launch.py hostname:=127.0.0.1 sw_pll_only_publish:=False & +waitUntilRvizClosed 15 +kill_simu + +popd diff --git a/test/scripts/run_linux_simu_mrs100.bash b/test/scripts/run_linux_simu_mrs100.bash deleted file mode 100644 index 6b84f29f..00000000 --- a/test/scripts/run_linux_simu_mrs100.bash +++ /dev/null @@ -1,36 +0,0 @@ -#!/bin/bash - -# killall and cleanup after exit -function killall_cleanup() -{ - killall sick_generic_caller - pkill -f mrs100_sopas_test_server.py -} - -# -# Run sick_generic_caller with sick_scansegment_xd.launch (Test MRS100 on linux native with test server) -# - -pushd ../.. -printf "\033c" -killall_cleanup - -# Run mrs100 emulator (sopas test server) -python3 ./test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & -sleep 1 - -# Start sick_generic_caller with sick_scansegment_xd -./build_linux/sick_generic_caller ./launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 & -sleep 3 - -# Play pcapng-files to emulate MRS100 output -echo -e "\nPlaying pcapng-files to emulate MRS100. Note: Start of UDP msgpacks in 20220915_mrs100_msgpack_output.pcapng takes a while...\n" -python3 ./test/python/mrs100_pcap_player.py --pcap_filename=./test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 -python3 ./test/python/mrs100_pcap_player.py --pcap_filename=./test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2 -python3 ./test/python/mrs100_pcap_player.py --pcap_filename=./test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 -sleep 3 - -pkill -f ./test/emulator/test_server.py -killall sick_generic_caller -popd - diff --git a/test/scripts/run_linux_simu_multiscan.bash b/test/scripts/run_linux_simu_multiscan.bash new file mode 100644 index 00000000..3cbd1249 --- /dev/null +++ b/test/scripts/run_linux_simu_multiscan.bash @@ -0,0 +1,36 @@ +#!/bin/bash + +# killall and cleanup after exit +function killall_cleanup() +{ + killall sick_generic_caller + pkill -f multiscan_sopas_test_server.py + pkill -f multiscan_pcap_player.py +} + +# +# Run sick_generic_caller with sick_multiscan.launch (Test MRS100 on linux native with test server) +# + +pushd ../.. +printf "\033c" +killall_cleanup + +# Run mrs100 emulator (sopas test server) +python3 ./test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & +sleep 1 + +# Start sick_generic_caller with sick_scansegment_xd +./build_linux/sick_generic_caller ./launch/sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 & +sleep 3 + +# Play pcapng-files to emulate MRS100 output +echo -e "\nPlaying pcapng-files to emulate MRS100. Note: Start of UDP msgpacks in 20220915_mrs100_msgpack_output.pcapng takes a while...\n" +python3 ./test/python/multiscan_pcap_player.py --pcap_filename=./test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 +python3 ./test/python/multiscan_pcap_player.py --pcap_filename=./test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2 +python3 ./test/python/multiscan_pcap_player.py --pcap_filename=./test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 +sleep 3 + +killall_cleanup +popd + diff --git a/test/scripts/run_win64_mrs100_emulator.cmd b/test/scripts/run_win64_multiscan_emulator.cmd similarity index 84% rename from test/scripts/run_win64_mrs100_emulator.cmd rename to test/scripts/run_win64_multiscan_emulator.cmd index b2a9769c..8ff9187e 100644 --- a/test/scripts/run_win64_mrs100_emulator.cmd +++ b/test/scripts/run_win64_multiscan_emulator.cmd @@ -2,19 +2,19 @@ REM Emulator version vom 06.12.2021: REM Start subsysApp.Win64.Release.exe only REM Login as Authorized Client REM For playback: load file 30_Datenemulator\navlayer_prototype.sdr.msgpack -REM pushd ..\..\..\..\..\30_LieferantenDokumente\30_Datenemulator\20211206_MRS100_Emulator +REM pushd ..\..\..\..\..\30_LieferantenDokumente\30_Datenemulator\20211206_multiScan_Emulator REM start subsysApp.Win64.Release.exe REM @timeout /t 3 REM start http:\\127.0.0.1:80 REM popd -REM Emulator version vom 21.09.2022 (mrs100_0.10.2.18pre.rar): -REM Unpack mrs100_0.10.2.18pre.rar +REM Emulator version vom 21.09.2022 (multiscan_0.10.2.18pre.rar): +REM Unpack multiscan_0.10.2.18pre.rar REM Start subsysBase and then subsyApp REM Login as Developer REM For playback: load file 30_Datenemulator\realData_20220817_155932.sdr.msgpack -pushd ..\..\..\..\..\30_LieferantenDokumente\30_Datenemulator\20220921_mrs100_0.10.2.18pre +pushd ..\..\..\..\..\30_LieferantenDokumente\30_Datenemulator\20220921_multiscan_0.10.2.18pre cd subsysBase start "subsysBase" subsysBase.Release.exe @timeout /t 3 diff --git a/test/scripts/run_win64_ros2_simu_mrs100_with_emulator.cmd b/test/scripts/run_win64_ros2_simu_mrs100_with_emulator.cmd deleted file mode 100644 index 39a8452d..00000000 --- a/test/scripts/run_win64_ros2_simu_mrs100_with_emulator.cmd +++ /dev/null @@ -1,31 +0,0 @@ -REM -REM Run sick_scan on ROS-2 Windows with mrs100 emulator -REM - -if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat ) -if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat ) -rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH% - -pushd ..\..\..\.. -call .\install\setup.bat -rem start "ros2 echo cloud" ros2 topic echo /cloud -start "rviz2 mrs100" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_windows.rviz -start "rviz2 mrs100 360" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_windows_360.rviz -@timeout /t 3 - -REM -REM Start mrs100 emulator -REM - -rem call run_win64_mrs100_emulator.cmd - -REM -REM Start sick_scan on ROS-2 Windows -REM - -rem ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_topic:=/cloud publish_topic_all_segments:=/cloud_360 publish_frame_id:=world add_transform_xyz_rpy:=0,0,0,0,0,0 -ros2 launch sick_scan sick_scansegment_xd.launch.py hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 - -popd -@pause - diff --git a/test/scripts/run_win64_ros2_simu_mrs100_with_player.cmd b/test/scripts/run_win64_ros2_simu_mrs100_with_player.cmd deleted file mode 100644 index 0c71cadd..00000000 --- a/test/scripts/run_win64_ros2_simu_mrs100_with_player.cmd +++ /dev/null @@ -1,63 +0,0 @@ -REM -REM Run sick_scan on ROS-2 Windows with mrs100 pcapng player -REM - -if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat ) -if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat ) -rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64 -rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64 -rem set PATH=%PYTHON_DIR%;%PYTHON_DIR%\DLLs;%PYTHON_DIR%\Lib;%PYTHON_DIR%\Scripts;%PATH% -rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH% - -pushd ..\..\..\.. -call .\install\setup.bat -rem start "ros2 echo cloud" ros2 topic echo /cloud -start "rviz2 mrs100" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_windows_laserscan.rviz -start "rviz2 mrs100" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_windows.rviz -start "rviz2 mrs100 360" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_windows_360.rviz -@timeout /t 3 - -REM -REM Start sopas test server -REM - -set PATH=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python39_64;%PATH% -python --version -where python -start "python mrs100_sopas_test_server.py" cmd /k python ./src/sick_scan_xd/test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0 -@timeout /t 3 - -REM Note: To verify laserscan messages, we configure laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", i.e. a laserscan message is published for each segment, each layer and each echo. -REM By default, laserscan messages are only activated for layer 5 (elevation -0.07 degree, max number of scan points) -REM All laserscan messages are converted to pointcloud by mrs100_laserscan_msg_to_pointcloud.py using a hardcoded elevation table. -REM Note: Option laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" should not be used for performance tests. -REM ros2 launch sick_scan sick_scansegment_xd.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" -REM start "python mrs100_laserscan_msg_to_pointcloud.py" cmd /k python ./src/sick_scan_xd/test/python/mrs100_laserscan_msg_to_pointcloud.py -REM @timeout /t 1 - -REM -REM Start sick_scan on ROS-2 Windows -REM - -rem start "ros2 sick_generic_caller" ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_topic:=/cloud publish_topic_all_segments:=/cloud_360 publish_frame_id:=world add_transform_xyz_rpy:=0,0,0,0,0,0 -start "ros2 sick_scan" ros2 launch sick_scan sick_scansegment_xd.launch.py hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 - -@timeout /t 3 - -REM -REM Run pcapng player -REM - -rem set PATH=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python39_64;%PATH% -@echo. -@echo Playing pcapng-files to emulate MRS100. Note: Start of UDP msgpacks in 20220915_mrs100_msgpack_output.pcapng takes a while... -@echo. -rem python ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 --verbose=1 -python ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 -python ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 -python ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 -@timeout /t 3 -popd - -@pause - diff --git a/test/scripts/run_win64_ros2_simu_multiscan_with_emulator.cmd b/test/scripts/run_win64_ros2_simu_multiscan_with_emulator.cmd new file mode 100644 index 00000000..7a9ce271 --- /dev/null +++ b/test/scripts/run_win64_ros2_simu_multiscan_with_emulator.cmd @@ -0,0 +1,31 @@ +REM +REM Run sick_scan on ROS-2 Windows with multiScan emulator +REM + +if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat ) +if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat ) +rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH% + +pushd ..\..\..\.. +call .\install\setup.bat +rem start "ros2 echo cloud" ros2 topic echo /cloud +start "rviz2 multiscan" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_multiscan_windows.rviz +start "rviz2 multiscan 360" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_multiscan_windows_360.rviz +@timeout /t 3 + +REM +REM Start multiscan emulator +REM + +rem call run_win64_multiscan_emulator.cmd + +REM +REM Start sick_scan on ROS-2 Windows +REM + +rem ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_topic:=/cloud publish_topic_all_segments:=/cloud_fullframe publish_frame_id:=world add_transform_xyz_rpy:=0,0,0,0,0,0 +ros2 launch sick_scan sick_multiscan.launch.py hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 + +popd +@pause + diff --git a/test/scripts/run_win64_ros2_simu_multiscan_with_player.cmd b/test/scripts/run_win64_ros2_simu_multiscan_with_player.cmd new file mode 100644 index 00000000..dc02e093 --- /dev/null +++ b/test/scripts/run_win64_ros2_simu_multiscan_with_player.cmd @@ -0,0 +1,60 @@ +REM +REM Run sick_scan on ROS-2 Windows with multiScan pcapng player +REM + +if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat ) +if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat ) +rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64 +rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64 +rem set PATH=%PYTHON_DIR%;%PYTHON_DIR%\DLLs;%PYTHON_DIR%\Lib;%PYTHON_DIR%\Scripts;%PATH% +rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH% + +pushd ..\..\..\.. +call .\install\setup.bat +rem start "ros2 echo cloud" ros2 topic echo /cloud +start "rviz2 multiscan" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_multiscan_windows_laserscan.rviz +start "rviz2 multiscan" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_multiscan_windows.rviz +start "rviz2 multiscan 360" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_multiscan_windows_360.rviz +@timeout /t 3 + +REM +REM Start sopas test server +REM + +set PATH=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python39_64;%PATH% +python --version +where python +start "python multiscan_sopas_test_server.py" cmd /k python ./src/sick_scan_xd/test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 +@timeout /t 3 + +REM Note: To verify laserscan messages, we configure laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", i.e. a laserscan message is published for each segment, each layer and each echo. +REM By default, laserscan messages are only activated for layer 5 (elevation -0.07 degree, max number of scan points) +REM All laserscan messages are converted to pointcloud by multiscan_laserscan_msg_to_pointcloud.py using a hardcoded elevation table. +REM Note: Option laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" should not be used for performance tests. +REM ros2 launch sick_scan sick_multiscan.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" laserscan_layer_filter:="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" +REM start "python multiscan_laserscan_msg_to_pointcloud.py" cmd /k python ./src/sick_scan_xd/test/python/multiscan_laserscan_msg_to_pointcloud.py +REM @timeout /t 1 + +REM +REM Start sick_scan on ROS-2 Windows +REM + +rem start "ros2 sick_generic_caller" ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_topic:=/cloud publish_topic_all_segments:=/cloud_fullframe publish_frame_id:=world add_transform_xyz_rpy:=0,0,0,0,0,0 +start "ros2 sick_scan" ros2 launch sick_scan sick_multiscan.launch.py hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 + +@timeout /t 3 + +REM +REM Run pcapng player +REM + +rem set PATH=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python39_64;%PATH% +@echo. +@echo Playing pcapng-files to emulate multiScan. Note: Start of UDP msgpacks in 20220915_multiscan_msgpack_output.pcapng takes a while... +@echo. +python ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 +@timeout /t 3 +popd + +@pause + diff --git a/test/scripts/run_win64_ros2_simu_nav350.cmd b/test/scripts/run_win64_ros2_simu_nav350.cmd new file mode 100644 index 00000000..58c5f8f1 --- /dev/null +++ b/test/scripts/run_win64_ros2_simu_nav350.cmd @@ -0,0 +1,35 @@ +REM +REM Run sick_scan on ROS-2 Windows with nav350 pcapng player +REM + +if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat ) +if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat ) +rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64 +rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64 +rem set PATH=%PYTHON_DIR%;%PYTHON_DIR%\DLLs;%PYTHON_DIR%\Lib;%PYTHON_DIR%\Scripts;%PATH% +rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH% + +pushd ..\..\..\.. +call .\install\setup.bat +rem start "ros2 echo cloud" ros2 topic echo /cloud +start "rviz2 nav350" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_nav350_windows.rviz +@timeout /t 3 + +REM +REM Start sopas test server +REM + +set PATH=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python39_64;%PATH% +python --version +where python +start "python sopas_json_test_server.py" cmd /k python ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20230126_nav350_4reflectors_moving.pcapng.json --scandata_id="sAN mNPOSGetData" --send_rate=8 --verbosity=0 +@timeout /t 3 + +REM +REM Run sick_scan on ROS-2 Windows +REM + +ros2 launch sick_scan sick_nav_350.launch.py hostname:=127.0.0.1 +popd +@pause + diff --git a/test/scripts/run_win64_ros2_simu_rmsxxxx.cmd b/test/scripts/run_win64_ros2_simu_rmsxxxx.cmd new file mode 100644 index 00000000..66fb5201 --- /dev/null +++ b/test/scripts/run_win64_ros2_simu_rmsxxxx.cmd @@ -0,0 +1,36 @@ +REM +REM Run sick_scan on ROS-2 Windows with rms xxxx emulator +REM + +if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat ) +if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat ) +rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64 +rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64 +rem set PATH=%PYTHON_DIR%;%PYTHON_DIR%\DLLs;%PYTHON_DIR%\Lib;%PYTHON_DIR%\Scripts;%PATH% +rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH% + +pushd ..\..\..\.. +call .\install\setup.bat +set PATH=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python39_64;%PATH% +python --version +where python + +REM +REM Run rms1, rms2 emulator and launch sick_scan sick_rms_xxxx.launch (ascii) +REM + +for %%f in ( 20220316-rms1000-ascii.pcapng.json 20221018_rms_1xxx_ascii_rms2_objects.pcapng.json ) do ( + + start "rviz2 rmsxxxx" /min ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_rmsxxxx_windows.rviz + @echo Running ros2 launch sick_scan sick_rms_xxxx.launch.py with emulator, scandata file ./src/sick_scan_xd/test/emulator/scandata/%%f + start "python sopas_json_test_server.py" /min cmd /c python ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/%%f --scandata_id="sSN LMDradardata" --send_rate=10 --verbosity=1 + start "python ros2 launch sick_rms_xxxx.launch.py" /min cmd /c ros2 launch sick_scan sick_rms_xxxx.launch.py hostname:=127.0.0.1 sw_pll_only_publish:=False + @timeout /t 15 + taskkill /im sick_generic_caller.exe /t /f + taskkill /im python.exe /t /f + taskkill /im rviz2.exe /t /f +) + +@pause +popd + diff --git a/test/scripts/run_win64_simu_mrs100.cmd b/test/scripts/run_win64_simu_mrs100.cmd deleted file mode 100644 index b1a7635d..00000000 --- a/test/scripts/run_win64_simu_mrs100.cmd +++ /dev/null @@ -1,71 +0,0 @@ -REM -REM Run a basic sick_generic_caller unittest on Windows 64 (standalone, no ROS required) with a test server emulating a basic MRS100 device -REM - -rem set PATH=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64;%PATH% -rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH% - -REM -REM Convert pcapng-files to msgpack and json -REM pip install msgpack -REM -REM pushd ..\python -REM python --version -REM REM -REM REM Convert 20220915_mrs100_msgpack_output.pcapng to msgpack/json (16-bit RSSI record) -REM REM -REM del /f/q mrs100_dump*.msgpack -REM del /f/q mrs100_dump*.msgpack.hex -REM start python mrs100_receiver.py -REM python mrs100_pcap_player.py --pcap_filename=../emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 --verbose=1 -REM move /y .\mrs100_dump_23644.msgpack 20220915_mrs100_msgpack_output.msgpack -REM move /y .\mrs100_dump_23644.msgpack.hex 20220915_mrs100_msgpack_output.msgpack.hex -REM REM -REM REM Convert 20210929_mrs100_token_udp.pcapng to msgpack/json (8-bit RSSI record) -REM REM -REM del /f/q mrs100_dump*.msgpack -REM del /f/q mrs100_dump*.msgpack.hex -REM start python mrs100_receiver.py -REM python mrs100_pcap_player.py --pcap_filename=../emulator/scandata/20210929_mrs100_token_udp.pcapng --verbose=0 -REM move /y .\mrs100_dump_12472.msgpack 20210929_mrs100_token_udp.msgpack -REM move /y .\mrs100_dump_12472.msgpack.hex 20210929_mrs100_token_udp.msgpack.hex -REM del /f/q mrs100_dump*.msgpack -REM del /f/q mrs100_dump*.msgpack.hex -REM popd - -REM -REM Start sopas test server -REM - -pushd ..\..\build_win64 -python --version -start "python mrs100_sopas_test_server.py" cmd /k python ../test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0 -@timeout /t 3 - -REM -REM Start sick_generic_caller -REM - -start "sick_generic_caller" cmd /k .\Debug\sick_generic_caller.exe ../launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 -@timeout /t 3 - -REM -REM Run pcapng player: -REM 20220915_mrs100_msgpack_output.pcapng: 16-bit RSSI -REM 20210929_mrs100_token_udp.pcapng and 20210929_mrs100_cola-a-start-stop-scandata-output.pcapng: 8-bit RSSI -REM - -@echo. -@echo Playing pcapng-files to emulate MRS100. Note: Start of UDP msgpacks in 20220915_mrs100_msgpack_output.pcapng takes a while... -@echo. -rem python ../test/python/mrs100_pcap_player.py --pcap_filename=../test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 --verbose=1 -rem python ../test/python/mrs100_pcap_player.py --pcap_filename=../test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 --send_rate=240 -python ../test/python/mrs100_pcap_player.py --pcap_filename=../test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 -@timeout /t 3 -python ../test/python/mrs100_pcap_player.py --pcap_filename=../test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 -@timeout /t 3 -python ../test/python/mrs100_pcap_player.py --pcap_filename=../test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115 -@timeout /t 3 - -popd -@pause diff --git a/test/scripts/run_win64_simu_multiscan.cmd b/test/scripts/run_win64_simu_multiscan.cmd new file mode 100644 index 00000000..0ecabced --- /dev/null +++ b/test/scripts/run_win64_simu_multiscan.cmd @@ -0,0 +1,63 @@ +REM +REM Run a basic sick_generic_caller unittest on Windows 64 (standalone, no ROS required) with a test server emulating a basic multiScan device +REM + +rem set PATH=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64;%PATH% +rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH% + +REM +REM Convert pcapng-files to msgpack and json +REM pip install msgpack +REM +REM pushd ..\python +REM python --version +REM REM +REM REM Convert 20220915_multiscan_msgpack_output.pcapng to msgpack/json (16-bit RSSI record) +REM REM +REM del /f/q multiscan_dump*.msgpack +REM del /f/q multiscan_dump*.msgpack.hex +REM start python multiscan_receiver.py +REM python multiscan_pcap_player.py --pcap_filename=../emulator/scandata/20220915_multiscan_msgpack_output.pcapng --udp_port=2115 --verbose=1 +REM move /y .\multiscan_dump_23644.msgpack 20220915_multiscan_msgpack_output.msgpack +REM move /y .\multiscan_dump_23644.msgpack.hex 20220915_multiscan_msgpack_output.msgpack.hex +REM REM +REM REM Convert 20210929_multiscan_token_udp.pcapng to msgpack/json (8-bit RSSI record) +REM REM +REM del /f/q multiscan_dump*.msgpack +REM del /f/q multiscan_dump*.msgpack.hex +REM start python multiscan_receiver.py +REM python multiscan_pcap_player.py --pcap_filename=../emulator/scandata/20210929_multiscan_token_udp.pcapng --verbose=0 +REM move /y .\multiscan_dump_12472.msgpack 20210929_multiscan_token_udp.msgpack +REM move /y .\multiscan_dump_12472.msgpack.hex 20210929_multiscan_token_udp.msgpack.hex +REM del /f/q multiscan_dump*.msgpack +REM del /f/q multiscan_dump*.msgpack.hex +REM popd + +REM +REM Start sopas test server +REM + +pushd ..\..\build_win64 +python --version +start "python multiscan_sopas_test_server.py" cmd /k python ../test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 +@timeout /t 3 + +REM +REM Start sick_generic_caller +REM + +start "sick_generic_caller" cmd /k .\Debug\sick_generic_caller.exe ../launch/sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 +@timeout /t 3 + +REM +REM Run pcapng player: +REM + +@echo. +@echo Playing pcapng-files to emulate multiScan. Note: Start of UDP msgpacks in 20220915_mrs100_msgpack_output.pcapng takes a while... +@echo. +python ../test/python/multiscan_pcap_player.py --pcap_filename=../test/emulator/scandata/20220915_mrs100_msgpack_output.pcapng --udp_port=2115 +@timeout /t 3 + +popd +@pause diff --git a/test/sick_scan_test_mrs_6xxx_all_echos.xml b/test/sick_scan_test_mrs_6xxx_all_echos.xml index 0598971f..9f3f8b2e 100644 --- a/test/sick_scan_test_mrs_6xxx_all_echos.xml +++ b/test/sick_scan_test_mrs_6xxx_all_echos.xml @@ -4,7 +4,7 @@ - + diff --git a/test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp b/test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp index eea983c0..a904d0a8 100644 --- a/test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp +++ b/test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp @@ -203,6 +203,12 @@ static void apiTestVisualizationMarkerMsgCallback(SickScanApiHandle apiHandle, c #endif } +// Example callback for NAV350 Pose- and Landmark messages +static void apiTestNavPoseLandmarkMsgCallback(SickScanApiHandle apiHandle, const SickScanNavPoseLandmarkMsg* msg) +{ + printf("[Info]: apiTestNavPoseLandmarkMsgCallback(apiHandle:%p): pose_x=%f, pose_y=%f, yaw=%f, %d reflectors\n", apiHandle, msg->pose_x, msg->pose_y, msg->pose_yaw, (int)msg->reflectors.size); +} + // Receive lidar message by SickScanApiWaitNext-functions ("message polling") static void runSickScanApiTestWaitNext(SickScanApiHandle* apiHandle, bool* run_flag) { @@ -214,6 +220,19 @@ static void runSickScanApiTestWaitNext(SickScanApiHandle* apiHandle, bool* run_f SickScanRadarScan radarscan_msg; SickScanLdmrsObjectArray ldmrsobjectarray_msg; SickScanVisualizationMarkerMsg visualizationmarker_msg; + SickScanNavPoseLandmarkMsg navposelandmark_msg; + SickScanOdomVelocityMsg odom_msg; + odom_msg.vel_x = +1.0f; + odom_msg.vel_y = -1.0f; + odom_msg.omega = 0.5f; + odom_msg.timestamp_sec = 12345; + odom_msg.timestamp_nsec = 6789; + SickScanNavOdomVelocityMsg navodom_msg; + navodom_msg.vel_x = +1.0f; + navodom_msg.vel_y = -1.0f; + navodom_msg.omega = 0.5f; + navodom_msg.timestamp = 123456789; + navodom_msg.coordbase = 0; while(run_flag && *run_flag) { // Get/poll the next cartesian PointCloud message @@ -279,6 +298,18 @@ static void runSickScanApiTestWaitNext(SickScanApiHandle* apiHandle, bool* run_f else if (ret != SICK_SCAN_API_SUCCESS && ret != SICK_SCAN_API_TIMEOUT) printf("## ERROR sick_scan_xd_api_test: SickScanApiWaitNextVisualizationMarkerMsg failed\n"); SickScanApiFreeVisualizationMarkerMsg(*apiHandle, &visualizationmarker_msg); + + // Get/poll the next NAV350 Pose- and Landmark message + ret = SickScanApiWaitNextNavPoseLandmarkMsg(*apiHandle, &navposelandmark_msg, wait_next_message_timeout); + if (ret == SICK_SCAN_API_SUCCESS) + apiTestNavPoseLandmarkMsgCallback(*apiHandle, &navposelandmark_msg); + else if (ret != SICK_SCAN_API_SUCCESS && ret != SICK_SCAN_API_TIMEOUT) + printf("## ERROR sick_scan_xd_api_test: SickScanApiWaitNextNavPoseLandmarkMsg failed\n"); + SickScanApiFreeNavPoseLandmarkMsg(*apiHandle, &navposelandmark_msg); + + // Send NAV350 odom message example + // ret = SickScanApiNavOdomVelocityMsg(*apiHandle, &navodom_msg); + // ret = SickScanApiOdomVelocityMsg(*apiHandle, &odom_msg); } } @@ -365,6 +396,10 @@ int main(int argc, char** argv) // Register a callback for VisualizationMarker messages if((ret = SickScanApiRegisterVisualizationMarkerMsg(apiHandle, apiTestVisualizationMarkerMsgCallback)) != SICK_SCAN_API_SUCCESS) exitOnError("SickScanApiRegisterVisualizationMarkerMsg failed", ret); + + // Register a callback for NAV350 Pose- and Landmark messages messages + if((ret = SickScanApiRegisterNavPoseLandmarkMsg(apiHandle, apiTestNavPoseLandmarkMsgCallback)) != SICK_SCAN_API_SUCCESS) + exitOnError("SickScanApiRegisterVisualizationSickScanApiRegisterNavPoseLandmarkMsgMarkerMsg failed", ret); } // Run main loop @@ -393,6 +428,7 @@ int main(int argc, char** argv) SickScanApiDeregisterRadarScanMsg(apiHandle, apiTestRadarScanMsgCallback); SickScanApiDeregisterLdmrsObjectArrayMsg(apiHandle, apiTestLdmrsObjectArrayCallback); SickScanApiDeregisterVisualizationMarkerMsg(apiHandle, apiTestVisualizationMarkerMsgCallback); + SickScanApiDeregisterNavPoseLandmarkMsg(apiHandle, apiTestNavPoseLandmarkMsgCallback); } if((ret = SickScanApiClose(apiHandle)) != SICK_SCAN_API_SUCCESS) exitOnError("SickScanApiClose failed", ret); diff --git a/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c b/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c index 41016048..f2274a9b 100644 --- a/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c +++ b/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c @@ -149,6 +149,24 @@ static SickScanApiWaitNextVisualizationMarkerMsg_PROCTYPE ptSickScanApiWaitNextV typedef int32_t(*SickScanApiFreeVisualizationMarkerMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanVisualizationMarkerMsg* msg); static SickScanApiFreeVisualizationMarkerMsg_PROCTYPE ptSickScanApiFreeVisualizationMarkersg = 0; +typedef int32_t(*SickScanApiRegisterNavPoseLandmarkMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkCallback callback); +static SickScanApiRegisterNavPoseLandmarkMsg_PROCTYPE ptSickScanApiRegisterNavPoseLandmarkMsg = 0; + +typedef int32_t(*SickScanApiDeregisterNavPoseLandmarkMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkCallback callback); +static SickScanApiDeregisterNavPoseLandmarkMsg_PROCTYPE ptSickScanApiDeregisterNavPoseLandmarkMsg = 0; + +typedef int32_t(*SickScanApiWaitNextNavPoseLandmarkMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkMsg* msg, double timeout_sec); +static SickScanApiWaitNextNavPoseLandmarkMsg_PROCTYPE ptSickScanApiWaitNextNavPoseLandmarkMsg = 0; + +typedef int32_t(*SickScanApiFreeNavPoseLandmarkMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkMsg* msg); +static SickScanApiFreeNavPoseLandmarkMsg_PROCTYPE ptSickScanApiFreeNavPoseLandmarkMsg = 0; + +typedef int32_t(*SickScanApiNavOdomVelocityMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanNavOdomVelocityMsg* msg); +static SickScanApiNavOdomVelocityMsg_PROCTYPE ptSickScanApiNavOdomVelocityMsg = 0; + +typedef int32_t(*SickScanApiOdomVelocityMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanOdomVelocityMsg* msg); +static SickScanApiOdomVelocityMsg_PROCTYPE ptSickScanApiOdomVelocityMsg = 0; + /* * Functions and macros to initialize and close the API and a lidar */ @@ -237,6 +255,12 @@ int32_t SickScanApiUnloadLibrary() ptSickScanApiFreeLdmrsObjectArrayMsg = 0; ptSickScanApiWaitNextVisualizationMarkerMsg = 0; ptSickScanApiFreeVisualizationMarkersg = 0; + ptSickScanApiRegisterNavPoseLandmarkMsg = 0; + ptSickScanApiDeregisterNavPoseLandmarkMsg = 0; + ptSickScanApiWaitNextNavPoseLandmarkMsg = 0; + ptSickScanApiFreeNavPoseLandmarkMsg = 0; + ptSickScanApiNavOdomVelocityMsg = 0; + ptSickScanApiOdomVelocityMsg = 0; return ret; } @@ -594,3 +618,56 @@ int32_t SickScanApiFreeVisualizationMarkerMsg(SickScanApiHandle apiHandle, SickS printf("## ERROR SickScanApiFreeVisualizationMarkerMsg: library call SickScanApiFreeVisualizationMarkerMsg() failed, error code %d\n", ret); return ret; } + +/* +* NAV350 support +*/ + +int32_t SickScanApiRegisterNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkCallback callback) +{ + CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiRegisterNavPoseLandmarkMsg, "SickScanApiRegisterNavPoseLandmarkMsg", SickScanApiRegisterNavPoseLandmarkMsg_PROCTYPE); + int32_t ret = (ptSickScanApiRegisterNavPoseLandmarkMsg ? (ptSickScanApiRegisterNavPoseLandmarkMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED); + if (ret != SICK_SCAN_API_SUCCESS) + printf("## ERROR SickScanApiRegisterNavPoseLandmarkMsg: library call SickScanApiRegisterNavPoseLandmarkMsg() failed, error code %d\n", ret); + return ret; +} +int32_t SickScanApiDeregisterNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkCallback callback) +{ + CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiDeregisterNavPoseLandmarkMsg, "SickScanApiDeregisterNavPoseLandmarkMsg", SickScanApiDeregisterNavPoseLandmarkMsg_PROCTYPE); + int32_t ret = (ptSickScanApiDeregisterNavPoseLandmarkMsg ? (ptSickScanApiDeregisterNavPoseLandmarkMsg(apiHandle, callback)) : SICK_SCAN_API_NOT_INITIALIZED); + if (ret != SICK_SCAN_API_SUCCESS) + printf("## ERROR SickScanApiDeregisterNavPoseLandmarkMsg: library call SickScanApiDeregisterNavPoseLandmarkMsg() failed, error code %d\n", ret); + return ret; +} +int32_t SickScanApiWaitNextNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkMsg* msg, double timeout_sec) +{ + CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiWaitNextNavPoseLandmarkMsg, "SickScanApiWaitNextNavPoseLandmarkMsg", SickScanApiWaitNextNavPoseLandmarkMsg_PROCTYPE); + int32_t ret = (ptSickScanApiWaitNextNavPoseLandmarkMsg ? (ptSickScanApiWaitNextNavPoseLandmarkMsg(apiHandle, msg, timeout_sec)) : SICK_SCAN_API_NOT_INITIALIZED); + if (ret != SICK_SCAN_API_SUCCESS && ret != SICK_SCAN_API_TIMEOUT) + printf("## ERROR SickScanApiWaitNextNavPoseLandmarkMsg: library call SickScanApiWaitNextNavPoseLandmarkMsg() failed, error code %d\n", ret); + return ret; +} +int32_t SickScanApiFreeNavPoseLandmarkMsg(SickScanApiHandle apiHandle, SickScanNavPoseLandmarkMsg* msg) +{ + CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiFreeNavPoseLandmarkMsg, "SickScanApiFreeNavPoseLandmarkMsg", SickScanApiFreeNavPoseLandmarkMsg_PROCTYPE); + int32_t ret = (ptSickScanApiFreeNavPoseLandmarkMsg ? (ptSickScanApiFreeNavPoseLandmarkMsg(apiHandle, msg)) : SICK_SCAN_API_NOT_INITIALIZED); + if (ret != SICK_SCAN_API_SUCCESS) + printf("## ERROR SickScanApiFreeNavPoseLandmarkMsg: library call SickScanApiFreeNavPoseLandmarkMsg() failed, error code %d\n", ret); + return ret; +} +int32_t SickScanApiNavOdomVelocityMsg(SickScanApiHandle apiHandle, SickScanNavOdomVelocityMsg* msg) +{ + CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiNavOdomVelocityMsg, "SickScanApiNavOdomVelocityMsg", SickScanApiNavOdomVelocityMsg_PROCTYPE); + int32_t ret = (ptSickScanApiNavOdomVelocityMsg ? (ptSickScanApiNavOdomVelocityMsg(apiHandle, msg)) : SICK_SCAN_API_NOT_INITIALIZED); + if (ret != SICK_SCAN_API_SUCCESS) + printf("## ERROR SickScanApiNavOdomVelocityMsg: library call SickScanApiNavOdomVelocityMsg() failed, error code %d\n", ret); + return ret; +} +int32_t SickScanApiOdomVelocityMsg(SickScanApiHandle apiHandle, SickScanOdomVelocityMsg* msg) +{ + CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiOdomVelocityMsg, "SickScanApiOdomVelocityMsg", SickScanApiOdomVelocityMsg_PROCTYPE); + int32_t ret = (ptSickScanApiOdomVelocityMsg ? (ptSickScanApiOdomVelocityMsg(apiHandle, msg)) : SICK_SCAN_API_NOT_INITIALIZED); + if (ret != SICK_SCAN_API_SUCCESS) + printf("## ERROR SickScanApiOdomVelocityMsg: library call SickScanApiOdomVelocityMsg() failed, error code %d\n", ret); + return ret; +} diff --git a/test/src/sick_scansegment_xd/lidar3d_mrs100_recv.cpp b/test/src/sick_scansegment_xd/lidar3d_multiscan_recv.cpp similarity index 81% rename from test/src/sick_scansegment_xd/lidar3d_mrs100_recv.cpp rename to test/src/sick_scansegment_xd/lidar3d_multiscan_recv.cpp index e7be5a5b..4885e8c1 100644 --- a/test/src/sick_scansegment_xd/lidar3d_mrs100_recv.cpp +++ b/test/src/sick_scansegment_xd/lidar3d_multiscan_recv.cpp @@ -1,5 +1,5 @@ /* - * @brief lidar3d_mrs100_recv implements a ROS node to receive and publish data from the new sick 3D lidar multiScan136. + * @brief lidar3d_multiscan_recv implements a ROS node to receive and publish data from the new sick 3D lidar multiScan136. * * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim * Copyright (C) 2020 SICK AG, Waldkirch @@ -56,7 +56,7 @@ #include "sick_scansegment_xd/scansegement_threads.h" /* - * main runs lidar3d_mrs100_recv: + * main runs lidar3d_multiscan_recv: * - Initialize udp receiver, msgpack converter and ros publisher, * - Run threads to receive, convert, export and publish msgpack data, * - Optionally save to csv-file, @@ -68,22 +68,22 @@ int main(int argc, char** argv) // Configuration sick_scansegment_xd::Config config; if (!config.Init(argc, argv)) - ROS_ERROR_STREAM("## ERROR lidar3d_mrs100_recv: Config::Init() failed, using default values."); - ROS_INFO_STREAM("lidar3d_mrs100_recv started."); + ROS_ERROR_STREAM("## ERROR lidar3d_multiscan_recv: Config::Init() failed, using default values."); + ROS_INFO_STREAM("lidar3d_multiscan_recv started."); sick_scansegment_xd::MsgPackThreads msgpack_threads; if(!msgpack_threads.start(config)) { - ROS_ERROR_STREAM("## ERROR lidar3d_mrs100_recv: sick_scansegment_xd::MsgPackThreads::start() failed"); + ROS_ERROR_STREAM("## ERROR lidar3d_multiscan_recv: sick_scansegment_xd::MsgPackThreads::start() failed"); } // Run event loop #if defined __ROS_VERSION && __ROS_VERSION > 1 rclcpp::spin(config.node); - ROS_INFO_STREAM("lidar3d_mrs100_recv finishing, ros shutdown."); + ROS_INFO_STREAM("lidar3d_multiscan_recv finishing, ros shutdown."); #elif defined __ROS_VERSION && __ROS_VERSION > 0 ros::spin(); - ROS_INFO_STREAM("lidar3d_mrs100_recv finishing, ros shutdown."); + ROS_INFO_STREAM("lidar3d_multiscan_recv finishing, ros shutdown."); #else // Run background task until ENTER key pressed while(true) { @@ -91,7 +91,7 @@ int main(int argc, char** argv) int c; if (KBHIT() && ((c = GETCH()) == 27 || c == 'q' || c == 'Q')) { - ROS_INFO_STREAM("lidar3d_mrs100_recv: key " << c << " pressed, aborting..."); + ROS_INFO_STREAM("lidar3d_multiscan_recv: key " << c << " pressed, aborting..."); break; } } @@ -100,8 +100,8 @@ int main(int argc, char** argv) if(!msgpack_threads.stop()) { - ROS_ERROR_STREAM("## ERROR lidar3d_mrs100_recv: sick_scansegment_xd::MsgPackThreads::stop() failed"); + ROS_ERROR_STREAM("## ERROR lidar3d_multiscan_recv: sick_scansegment_xd::MsgPackThreads::stop() failed"); } - ROS_INFO_STREAM("lidar3d_mrs100_recv finished."); + ROS_INFO_STREAM("lidar3d_multiscan_recv finished."); return 0; } diff --git a/tools/test_server/src/test_server_cola_msg.cpp b/tools/test_server/src/test_server_cola_msg.cpp index ca5aa8f0..a2306abe 100644 --- a/tools/test_server/src/test_server_cola_msg.cpp +++ b/tools/test_server/src/test_server_cola_msg.cpp @@ -252,7 +252,7 @@ sick_scan::test::TestServerColaMsg::TestServerColaMsg(rosNodePtr nh, double send {"sWN TransmitObjects", encodeColaTelegram("sWA TransmitObjects", {}, {}, is_binary_idx > 0)}, {"sWN TCTrackingMode", encodeColaTelegram("sWA TCTrackingMode", {}, {}, is_binary_idx > 0)}, {"sRN SCdevicestate", encodeColaTelegram("sRA SCdevicestate", {1}, {1}, is_binary_idx > 0)}, - {"sRN DItype", encodeColaTelegram("sRA DItype D RMS3xx-xxxxxx", {}, {}, is_binary_idx > 0)}, + {"sRN DItype", encodeColaTelegram("sRA DItype F RMS2731C.636111", {}, {}, is_binary_idx > 0)}, {"sRN ODoprh", encodeColaTelegram("sRA ODoprh", {451}, {2}, is_binary_idx > 0)}, {"sMN mSCloadappdef", encodeColaTelegram("sAN mSCloadappdef", {}, {}, is_binary_idx > 0)}, {"sRN SerialNumber", encodeColaTelegram("sRA SerialNumber", {"18340008"}, is_binary_idx > 0)},