From 57be1b5dddde3c59a701ed45e19075920f6b4a3b Mon Sep 17 00:00:00 2001 From: rostest Date: Thu, 10 Nov 2022 18:28:03 +0100 Subject: [PATCH] LMS 1xxx support LMS 1xxx support with scan configuration (scan frequency and angular resolution for firmware 2.x) --- CHANGELOG.md | 4 + REQUIREMENTS.md | 41 ++-- USAGE.md | 26 ++- cfg/SickScan.cfg | 2 +- driver/src/sick_generic_laser.cpp | 2 +- driver/src/sick_generic_parser.cpp | 6 +- driver/src/sick_generic_radar.cpp | 8 +- driver/src/sick_scan_common.cpp | 47 ++++- include/sick_scan/sick_generic_parser.h | 3 +- launch/sick_lms_1xxx.launch | 4 + launch/sick_lms_1xxx_v2.launch | 83 ++++++++ launch/sick_lms_1xxx_v2.launch.py | 39 ++++ launch/sick_rms_2xxx.launch | 71 +++++++ launch/sick_rms_2xxx.launch.py | 39 ++++ .../config/rviz_emulator_cfg_rms2xxx.rviz | 186 +++++++++++++++++ .../rviz_emulator_cfg_ros2_rms2xxx.rviz | 190 +++++++++++++++++ test/emulator/config/rviz_lms1xxx.rviz | 194 ++++++++++++++++++ .../emulator_lms1xxx-150hz-0.75deg.launch | 15 ++ .../emulator_lms1xxx-37.5hz-0.1875deg.launch | 15 ++ .../emulator_lms1xxx-75hz-0.375deg.launch | 15 ++ .../pcap_json_converter.cmd | 3 + test/scripts/run_linux_ros1_simu_lms1xxx.bash | 68 ++++++ .../run_linux_ros1_simu_lms1xxx_v2x.bash | 68 ++++++ ...=> run_linux_ros1_simu_rms2xxx_ascii.bash} | 7 +- test/scripts/run_linux_ros2_simu_rms2xxx.bash | 50 +++++ 25 files changed, 1139 insertions(+), 47 deletions(-) create mode 100644 launch/sick_lms_1xxx_v2.launch create mode 100644 launch/sick_lms_1xxx_v2.launch.py create mode 100644 launch/sick_rms_2xxx.launch create mode 100644 launch/sick_rms_2xxx.launch.py create mode 100644 test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz create mode 100644 test/emulator/config/rviz_emulator_cfg_ros2_rms2xxx.rviz create mode 100644 test/emulator/config/rviz_lms1xxx.rviz create mode 100644 test/emulator/launch/emulator_lms1xxx-150hz-0.75deg.launch create mode 100644 test/emulator/launch/emulator_lms1xxx-37.5hz-0.1875deg.launch create mode 100644 test/emulator/launch/emulator_lms1xxx-75hz-0.375deg.launch create mode 100644 test/scripts/run_linux_ros1_simu_lms1xxx.bash create mode 100644 test/scripts/run_linux_ros1_simu_lms1xxx_v2x.bash rename test/scripts/{run_linux_ros1_simu_rms3xx_ascii.bash => run_linux_ros1_simu_rms2xxx_ascii.bash} (87%) create mode 100644 test/scripts/run_linux_ros2_simu_rms2xxx.bash diff --git a/CHANGELOG.md b/CHANGELOG.md index ed77f653..14c26f1e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,8 +8,12 @@ features that will be removed in future versions **Removed** for deprecated feat ## Released ## +### v2.8.11 - LMS 1xxx support + - **Update** LMS 1xxx support with scan configuration (scan frequency and angular resolution for firmware 2.x) + ### v2.8.10 - RMS ascii emulator and tests - **Update** RMS ascii emulator and tests + - **Update** RMS2xxx support ### v2.8.9 - MRS-1000 layer angle conversion, improved MRS 1xxx support - **Fixed** MRS-1000 layer angle conversion for slam support diff --git a/REQUIREMENTS.md b/REQUIREMENTS.md index f5ceed83..648b401a 100644 --- a/REQUIREMENTS.md +++ b/REQUIREMENTS.md @@ -6,44 +6,45 @@ Driver for SICK lidar and radar sensors - supported scanner types: | **device name** | **part no.** | **description** | **tested?** | |--------------------|------------------------------------------------------------------------------------------------------------------------------|------------------------------------------------|:---------------:| -| MRS6124 | [6065086](https://www.sick.com/de/en/detection-and-ranging-solutions/3d-lidar-sensors/mrs6000/c/g448151) | 24 layer max. range: 200 m, ang. resol. 0.13 [deg] hor., 0.0625 [deg] ver. The SICK MRS6124 is a multi-layer, multi-echo 3D laser scanner that is geared towards rough outdoor environments. | ? [stable]| +| MRS6124 | [6065086](https://www.sick.com/de/en/detection-and-ranging-solutions/3d-lidar-sensors/mrs6000/c/g448151) | 24 layer max. range: 200 m, ang. resol. 0.13 [deg] hor., 0.0625 [deg] ver. The SICK MRS6124 is a multi-layer, multi-echo 3D laser scanner that is geared towards rough outdoor environments. | [stable]| | | Scan-Rate: 10 Hz | | -| MRS1104 | [1081208](https://www.sick.com/sg/en/detection-and-ranging-solutions/3d-lidar-sensors/mrs1000/mrs1104c-111011/p/p495044) | 4 layer max. range: 64 m, ang. resol. 0.25 [deg] hor., 2.50 [deg] ver. | ? [stable]| +| MRS1104 | [1081208](https://www.sick.com/sg/en/detection-and-ranging-solutions/3d-lidar-sensors/mrs1000/mrs1104c-111011/p/p495044) | 4 layer max. range: 64 m, ang. resol. 0.25 [deg] hor., 2.50 [deg] ver. | [stable]| | | | Scan-Rate: 50 Hz, 4x12.5 Hz | | | | | Scan-Rate: 150 Hz, 4x37.5 Hz | | -| TiM240 | [1104981](https://www.sick.com/ag/en/detection-and-ranging-solutions/2d-lidar-sensors/tim2xx/tim240-2050300/p/p654443) | 1 layer max. range: 10 m, ang. resol. 1.00 [deg], 240 [deg]| ? [stable]| +| TiM240 | [1104981](https://www.sick.com/ag/en/detection-and-ranging-solutions/2d-lidar-sensors/tim2xx/tim240-2050300/p/p654443) | 1 layer max. range: 10 m, ang. resol. 1.00 [deg], 240 [deg]| [stable]| | | | Scan-Rate: 14.5 Hz | | -| TiM551 | [1060445](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/tim5xx/tim551-2050001/p/p343045) | 1 layer max. range: 10 m, ang. resol. 1.00[deg] | ? [stable]| +| TiM551 | [1060445](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/tim5xx/tim551-2050001/p/p343045) | 1 layer max. range: 10 m, ang. resol. 1.00[deg] | [stable]| | | | Scan-Rate: 15 Hz | | -| TiM561 | [1071419](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/tim5xx/tim561-2050101/p/p369446) | 1 layer max. range: 10 m, ang. resol. 0.33 [deg]| ? [stable]| +| TiM561 | [1071419](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/tim5xx/tim561-2050101/p/p369446) | 1 layer max. range: 10 m, ang. resol. 0.33 [deg]| [stable]| | | | Scan-Rate: 15 Hz | | -| TiM571 | [1079742](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/tim5xx/tim571-2050101/p/p412444) | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| ? [stable]| +| TiM571 | [1079742](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/tim5xx/tim571-2050101/p/p412444) | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| [stable]| | | | Scan-Rate: 15 Hz | | -| TiM771S | [1105052](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim771s-2174104/p/p660929) | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| ? [stable]| +| TiM771S | [1105052](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim771s-2174104/p/p660929) | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| [stable]| | | | Scan-Rate: 15 Hz | | -| TiM781 | [1096807](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim781-2174101/p/p594148) | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| ? [stable]| +| TiM781 | [1096807](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim781-2174101/p/p594148) | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| [stable]| | | | Scan-Rate: 15 Hz | | -| TiM781S | [1096363](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim781s-2174104/p/p594149) | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| ? [stable]| +| TiM781S | [1096363](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim781s-2174104/p/p594149) | 1 layer max. range: 25 m, ang. resol. 0.33 [deg]| [stable]| | | | Scan-Rate: 15 Hz | | -| LMS511-10100 PRO | [e.g. 1046135](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/lms5xx/c/g179651) | 1 layer max. range: 80 m, ang. resol. 0.167 [deg]| ? [stable]| +| LMS511-10100 PRO | [e.g. 1046135](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/lms5xx/c/g179651) | 1 layer max. range: 80 m, ang. resol. 0.167 [deg]| [stable]| | | | Scan-Rate: 100 Hz | | -| LMS1104 | [1092445](https://www.sick.com/ag/en/detection-and-ranging-solutions/2d-lidar-sensors/lms1000/c/g387151) | 1 layer max. range: 64 m, ang. resol. 0.25 [deg] | ? [stable]| +| LMS1104 | [1092445](https://www.sick.com/ag/en/detection-and-ranging-solutions/2d-lidar-sensors/lms1000/c/g387151) | 1 layer max. range: 64 m, ang. resol. 0.25 [deg] | [stable]| | | | Scan-Rate: 150 Hz, 4x37.5 Hz | -| LMS1xx-Family | [e.g. 1041114](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/lms1xx/c/g91901) | 1 layer max. range: 28 m, ang. resol. 0.25 [deg]| ? [stable]| +| LMS1xx-Family | [e.g. 1041114](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/lms1xx/c/g91901) | 1 layer max. range: 28 m, ang. resol. 0.25 [deg]| [stable]| | | | Scan-Rate: 15 Hz | | -| LMS4xxx-Family | [e.g. 1091423](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/lms4000/lms4111r-13000/p/p578044?ff_data) | 1 layer max. range: 3 m, ang. resol. 0,0833 [deg], opening angle: +/- 50 [deg] | ? [stable]| +| LMS4xxx-Family | [e.g. 1091423](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/lms4000/lms4111r-13000/p/p578044?ff_data) | 1 layer max. range: 3 m, ang. resol. 0,0833 [deg], opening angle: +/- 50 [deg] | [stable]| | | | Scan-Rate: 600 Hz | | -| LDMRS | | 4 or 8 layer, max. range: 50/320 m, ang. resol. 0.025°/.../0.25 [deg] | ? [stable]| +| LDMRS | | 4 or 8 layer, max. range: 50/320 m, ang. resol. 0.025°/.../0.25 [deg] | [stable]| | | | Scan-Rate: 12.5-50 Hz | | -| LRS4000 | [1098855](https://www.sick.com/no/en/detection-and-ranging-solutions/2d-lidar-sensors/lrs4000/c/g555594) | 1 layer, max. range: 130 m, ang. resol. 0.125/0.25/0.5 [deg] | ? [stable]| +| LRS4000 | [1098855](https://www.sick.com/no/en/detection-and-ranging-solutions/2d-lidar-sensors/lrs4000/c/g555594) | 1 layer, max. range: 130 m, ang. resol. 0.125/0.25/0.5 [deg] | [stable]| | | | Scan-Rate: 12.5-25 Hz | | -| NAV310 | [e.g. 1060834](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/nav3xx/nav310-3211/p/p349345) | 1 layer max. range: 250 m, ang. resol. 0.125 - 1.0 [deg] | ? [stable]| +| NAV310 | [e.g. 1060834](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/nav3xx/nav310-3211/p/p349345) | 1 layer max. range: 250 m, ang. resol. 0.125 - 1.0 [deg] | [stable]| | | | Scan-Rate: 8 Hz | | -| NAV210+NAV245 | [e.g. 1074308](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/nav2xx/c/g356151) | 1 layer max. range: 100 m, ang. resol. 0.25 [deg]| ? [stable]| +| NAV210+NAV245 | [e.g. 1074308](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/nav2xx/c/g356151) | 1 layer max. range: 100 m, ang. resol. 0.25 [deg]| [stable]| | | | Scan-Rate: 25 Hz | | -| RMS3xx | [8021530](https://cdn.sick.com/media/docs/4/04/504/Operating_instructions_RMS3xx_en_IM0075504.PDF)| Radar Sensor | ? [stable]| -| RMS1xxx | [1107598](https://www.sick.com/de/en/detection-and-ranging-solutions/radar-sensors/rms1000/rms1731c-636111/p/p660833)| 1D Radar Sensor | ? [stable]| -| Multiscan136 | prototype | The MultiScan136 Beta is a new lidar with 16 lidar units rotating around a vertical axis | ? [prototype]| +| RMS1xxx | [1107598](https://www.sick.com/de/en/detection-and-ranging-solutions/radar-sensors/rms1000/rms1731c-636111/p/p660833)| 1D Radar Sensor | [stable]| +| RMS2xxx | | Radar Sensor | [prototype]| +| RMS3xx | [8021530](https://cdn.sick.com/media/docs/4/04/504/Operating_instructions_RMS3xx_en_IM0075504.PDF)| Radar Sensor | [stable]| +| Multiscan136 | prototype | The MultiScan136 Beta is a new lidar with 16 lidar units rotating around a vertical axis | [prototype]| Note: * LDMRS family is currently not supported on Windows. diff --git a/USAGE.md b/USAGE.md index 30407b99..a648e1b1 100644 --- a/USAGE.md +++ b/USAGE.md @@ -36,12 +36,18 @@ Use the following commands to run the sick_scan_xd driver for a specific scanner * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_mrs_1xxx.launch` * Windows native: `sick_generic_caller sick_mrs_1xxx.launch` * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_mrs_1xxx.launch` -- For LMS1104: +- For LMS1104 with firmware 1.x: * Linux native: `sick_generic_caller sick_lms_1xxx.launch` * Linux ROS-1: `roslaunch sick_scan sick_lms_1xxx.launch` * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_1xxx.launch` * Windows native: `sick_generic_caller sick_lms_1xxx.launch` * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_1xxx.launch` +- For LMS1104 with firmware 2.x: + * Linux native: `sick_generic_caller sick_lms_1xxx_v2.launch` + * Linux ROS-1: `roslaunch sick_scan sick_lms_1xxx_v2.launch` + * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_1xxx_v2.launch` + * Windows native: `sick_generic_caller sick_lms_1xxx_v2.launch` + * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_1xxx_v2.launch` - For TiM240-prototype: * Linux native: `sick_generic_caller sick_tim_240.launch` * Linux ROS-1: `roslaunch sick_scan sick_tim_240.launch` @@ -107,18 +113,24 @@ Use the following commands to run the sick_scan_xd driver for a specific scanner * 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 RMS3xx-family: - * Linux native: `sick_generic_caller sick_rms_3xx.launch` - * Linux ROS-1: `roslaunch sick_scan sick_rms_3xx.launch` - * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_3xx.launch` - * Windows native: `sick_generic_caller sick_rms_3xx.launch` - * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_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 RMS3xx-family: + * Linux native: `sick_generic_caller sick_rms_3xx.launch` + * Linux ROS-1: `roslaunch sick_scan sick_rms_3xx.launch` + * Linux ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_3xx.launch` + * Windows native: `sick_generic_caller sick_rms_3xx.launch` + * Windows ROS-2: `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_3xx.launch` - For Multiscan136 (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:=` diff --git a/cfg/SickScan.cfg b/cfg/SickScan.cfg index d026bbcb..930ddbc4 100755 --- a/cfg/SickScan.cfg +++ b/cfg/SickScan.cfg @@ -93,7 +93,7 @@ gen.add("locationName", str_t,0, "Device Location Name",""), gen.add("timelimit", double_t, 0, "Network time limit for datagram request [sec].", 5.0, 0.1,100.0) gen.add("cloud_output_mode", int_t, 0, "[0] Pointcloud is dense all layers in one cloud,[1] Each layer in its own cloud message to improve timestamp accuracy,[2] layers are split to achieve approx. 1 KHz data rate", 0,0,2) gen.add("ang_res", double_t, 0, "Angular resolution in deg/scan set to 0 to use scanner default", 0 ,0,10) -gen.add("scan_freq", double_t, 0, "Scan frequency set to 0 to use scanner default",0 ,0,100) +gen.add("scan_freq", double_t, 0, "Scan frequency set to 0 to use scanner default", 0, 0, 150) gen.add("encoder_mode", int_t, 0, "-1:No Encoder, 0:Off, 1:Single increment, 2:Direction Phase, 3:Direction Level",-1 ,-1,3) gen.add("scan_cfg_list_entry", int_t, 0, "scan config for | LRS 3601 3611 |OEM 1501|NAV 310 |LRS 3600 3610 4000 |OEM 1500|",-1 ,1,71) # LD Series switching table: 1-46, LRS4000 switching table: 1-71 # gen.add("mean_filter", int_t, 0, "Number of averages for mean filter 0 means filter is disabled", 0, 0, 100) diff --git a/driver/src/sick_generic_laser.cpp b/driver/src/sick_generic_laser.cpp index ebac824b..b3597e24 100644 --- a/driver/src/sick_generic_laser.cpp +++ b/driver/src/sick_generic_laser.cpp @@ -93,7 +93,7 @@ #define SICK_GENERIC_MAJOR_VER "2" #define SICK_GENERIC_MINOR_VER "8" -#define SICK_GENERIC_PATCH_LEVEL "10" +#define SICK_GENERIC_PATCH_LEVEL "11" #define DELETE_PTR(p) if(p){delete(p);p=0;} diff --git a/driver/src/sick_generic_parser.cpp b/driver/src/sick_generic_parser.cpp index 5cfebf93..4c853441 100644 --- a/driver/src/sick_generic_parser.cpp +++ b/driver/src/sick_generic_parser.cpp @@ -501,8 +501,9 @@ 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_3XX_NAME); // Radar scanner 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_NAV_350_NAME); allowedScannerNames.push_back(SICK_SCANNER_NAV_2XX_NAME); @@ -825,7 +826,8 @@ 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_3XX_NAME) == 0) // Radar + if (basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_2XXX_NAME) == 0 + || basicParams[i].getScannerName().compare(SICK_SCANNER_RMS_3XX_NAME) == 0) // Radar { basicParams[i].setNumberOfMaximumEchos(1); basicParams[i].setNumberOfLayers(0); // for radar scanner diff --git a/driver/src/sick_generic_radar.cpp b/driver/src/sick_generic_radar.cpp index 0a8fe50e..699f29f0 100644 --- a/driver/src/sick_generic_radar.cpp +++ b/driver/src/sick_generic_radar.cpp @@ -494,10 +494,10 @@ 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 - { - return std::vector(); // unrecoverable parse error - } + // if(num_16_bit_channels > 4) // max 4 16-bit channel in case of RMS1xxx and RMS3xx + // { + // return std::vector(); // unrecoverable parse error + // } for(int channel_idx = 0; channel_idx < num_16_bit_channels; channel_idx++) { success = success && appendRadarDatagramField(datagram, datagram_length, 5, fields); // 5 byte Content string (identifier) diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp index 9c8bb3db..9590dbec 100644 --- a/driver/src/sick_scan_common.cpp +++ b/driver/src/sick_scan_common.cpp @@ -2563,7 +2563,7 @@ namespace sick_scan // special for LMS1000 TODO unify this if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0) { - ROS_INFO("Angular settings for LMS 1000 not reliable.\n"); + ROS_INFO("Angular start/stop settings for LMS 1000 not reliable.\n"); double askAngleStart = -137.0; double askAngleEnd = +137.0; @@ -3070,10 +3070,10 @@ namespace sick_scan // set scanning angle for lms1xx and lms5xx double scan_freq = 0; double ang_res = 0; - rosDeclareParam(nh, "scan_freq", scan_freq); // filter_echos - rosGetParam(nh, "scan_freq", scan_freq); // filter_echos - rosDeclareParam(nh, "ang_res", ang_res); // filter_echos - rosGetParam(nh, "ang_res", ang_res); // filter_echos + rosDeclareParam(nh, "scan_freq", scan_freq); + rosGetParam(nh, "scan_freq", scan_freq); + rosDeclareParam(nh, "ang_res", ang_res); + rosGetParam(nh, "ang_res", ang_res); if (scan_freq != 0 || ang_res != 0) { if (scan_freq != 0 && ang_res != 0) @@ -3085,6 +3085,7 @@ namespace sick_scan else { if(this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_1XX_NAME) == 0 + || this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0 || this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0) { // "sMN mLMPsetscancfg" for lms1xx and lms5xx: @@ -3103,6 +3104,11 @@ namespace sick_scan lmp_scancfg_sector.start_angle = -450000; lmp_scancfg_sector.stop_angle = +2250000; } + else if(this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0) + { + lmp_scancfg_sector.start_angle = -480000; + lmp_scancfg_sector.stop_angle = +2280000; + } else if(this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0) { double angle_offset_rad = this->parser_->getCurrentParamPtr()->getScanAngleShift(); @@ -3143,6 +3149,7 @@ namespace sick_scan std::string lmp_scancfg_sopas; if (sick_scan::SickScanParseUtil::LMPscancfgToSopas(lmp_scancfg, lmp_scancfg_sopas)) { + ROS_INFO_STREAM("Sending mLMPsetscancfg request: { " << lmp_scancfg.print() << " }"); std::vector reqBinary, lmp_scancfg_reply; if (useBinaryCmd) { @@ -3155,6 +3162,14 @@ namespace sick_scan result = sendSopasAndCheckAnswer(lmp_scancfg_sopas, &lmp_scancfg_reply); RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, lmp_scancfg_reply); // No response, non-recoverable connection error (return error and do not try other commands) } + std::string sopasReplyString = replyToString(lmp_scancfg_reply); + if (strncmp(sopasReplyString.c_str(), "sAN mLMPsetscancfg ", 19) == 0) + { + sick_scan::SickScanParseUtil::LMPscancfg scancfg_response; + sick_scan::SickScanParseUtil::SopasToLMPscancfg(sopasReplyString, scancfg_response); + ROS_INFO_STREAM("sAN mLMPsetscancfg: scan frequency = " << (scancfg_response.scan_frequency/100.0) << " Hz, angular resolution = " + << (scancfg_response.sector_cfg.size() > 0 ? (scancfg_response.sector_cfg[0].angular_resolution / 10000.0) : -1.0) << " deg."); + } } else { @@ -3199,6 +3214,14 @@ namespace sick_scan this->convertAscii2BinaryCmd(requestLMDscancfgRead, &reqBinary); result = sendSopasAndCheckAnswer(reqBinary, &sopasReplyBinVec[CMD_GET_PARTIAL_SCAN_CFG]); RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, sopasReplyBinVec[CMD_GET_PARTIAL_SCAN_CFG]); // No response, non-recoverable connection error (return error and do not try other commands) + std::string sopasReplyString = replyToString(sopasReplyBinVec[CMD_GET_PARTIAL_SCAN_CFG]); + if (strncmp(sopasReplyString.c_str(), "sRA LMPscancfg ", 15) == 0) + { + sick_scan::SickScanParseUtil::LMPscancfg scancfg_response; + sick_scan::SickScanParseUtil::SopasToLMPscancfg(sopasReplyString, scancfg_response); + ROS_INFO_STREAM("sRA LMPscancfg: scan frequency = " << (scancfg_response.scan_frequency/100.0) << " Hz, angular resolution = " + << (scancfg_response.sector_cfg.size() > 0 ? (scancfg_response.sector_cfg[0].angular_resolution / 10000.0) : -1.0) << " deg."); + } } else { @@ -3983,6 +4006,7 @@ namespace sick_scan EncoderMsg.header.frame_id = "Encoder"; ROS_HEADER_SEQ(EncoderMsg.header, numPacketsProcessed); msg.header.stamp = recvTimeStamp + rosDurationFromSec(config_.time_offset); // default: ros-timestamp at message received, will be updated by software-pll + msg.header.frame_id = config_.frame_id; // Use configured frame_id for both laser scan and pointcloud messages double elevationAngleInRad = 0.0; short elevAngleX200 = 0; // signed short (F5 B2 -> Layer 24 // F5B2h -> -2638/200= -13.19° @@ -4257,7 +4281,7 @@ namespace sick_scan { // numEchos char szTmp[255] = {0}; - if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() > 1) + if (false) // if (this->parser_->getCurrentParamPtr()->getNumberOfLayers() > 1) // Use configured frame_id for both laser scan and pointcloud messages { const char *cpFrameId = config_.frame_id.c_str(); #if 0 @@ -4374,6 +4398,12 @@ namespace sick_scan const int numChannels = 4; // x y z i (for intensity) int numTmpLayer = numOfLayers; + if(this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0) + { + numTmpLayer = 1; // LMS_1XXX has 4 interlaced layer, each layer published in one pointcloud message + baseLayer = 0; + layer = 0; + } cloud_.header.stamp = recvTimeStamp + rosDurationFromSec(config_.time_offset); @@ -4440,7 +4470,10 @@ namespace sick_scan { float angle = (float)config_.min_ang; - + if(this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0) // Can we use msg.angle_min this for all lidars? + { + angle = msg.angle_min - angleShift; // LMS-1xxx has 4 interlaced layer with different start angle in each layer, start angle parsed from LMDscandata and set in msg.angle_min + } float *cosAlphaTablePtr = &cosAlphaTable[0]; float *sinAlphaTablePtr = &sinAlphaTable[0]; diff --git a/include/sick_scan/sick_generic_parser.h b/include/sick_scan/sick_generic_parser.h index dccfbc16..988803dc 100644 --- a/include/sick_scan/sick_generic_parser.h +++ b/include/sick_scan/sick_generic_parser.h @@ -70,8 +70,9 @@ #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_3XX_NAME "sick_rms_3xx" #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_NAV_350_NAME "sick_nav_350" #define SICK_SCANNER_NAV_2XX_NAME "sick_nav_2xx" diff --git a/launch/sick_lms_1xxx.launch b/launch/sick_lms_1xxx.launch index b8fda6f8..353e319f 100644 --- a/launch/sick_lms_1xxx.launch +++ b/launch/sick_lms_1xxx.launch @@ -51,6 +51,10 @@ + + diff --git a/launch/sick_lms_1xxx_v2.launch b/launch/sick_lms_1xxx_v2.launch new file mode 100644 index 00000000..933dedaa --- /dev/null +++ b/launch/sick_lms_1xxx_v2.launch @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/sick_lms_1xxx_v2.launch.py b/launch/sick_lms_1xxx_v2.launch.py new file mode 100644 index 00000000..aa8dda3e --- /dev/null +++ b/launch/sick_lms_1xxx_v2.launch.py @@ -0,0 +1,39 @@ +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_lms_1xxx_v2.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_2xxx.launch b/launch/sick_rms_2xxx.launch new file mode 100644 index 00000000..48cc39c2 --- /dev/null +++ b/launch/sick_rms_2xxx.launch @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/sick_rms_2xxx.launch.py b/launch/sick_rms_2xxx.launch.py new file mode 100644 index 00000000..8063e39d --- /dev/null +++ b/launch/sick_rms_2xxx.launch.py @@ -0,0 +1,39 @@ +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/test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz b/test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz new file mode 100644 index 00000000..ee1203f7 --- /dev/null +++ b/test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz @@ -0,0 +1,186 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /PointCloud22 + - /Axes1 + Splitter Ratio: 0.5 + Tree Height: 799 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.535315990447998 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/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: vrad + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + 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.10000000149011612 + Style: Flat Squares + Topic: /cloud_radar_rawtarget + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: objLen + 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): 10 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud_radar_track + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: + Show Trail: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: radar + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 51.23906707763672 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 17.75350570678711 + Y: -0.8858721852302551 + Z: 1.3521872758865356 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.4947969913482666 + Target Frame: + Yaw: 3.1364974975585938 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1096 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000003aafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003aa000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003aa000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054f0000003efc0100000002fb0000000800540069006d006501000000000000054f000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003f3000003aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1359 + X: 718 + Y: 189 diff --git a/test/emulator/config/rviz_emulator_cfg_ros2_rms2xxx.rviz b/test/emulator/config/rviz_emulator_cfg_ros2_rms2xxx.rviz new file mode 100644 index 00000000..fb193a4a --- /dev/null +++ b/test/emulator/config/rviz_emulator_cfg_ros2_rms2xxx.rviz @@ -0,0 +1,190 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /PointCloud21 + - /PointCloud22 + 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: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sick_rms_2xxx/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: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sick_rms_2xxx/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: 41.06394577026367 + 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 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.529760718345642 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 3.1254122257232666 + 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_lms1xxx.rviz b/test/emulator/config/rviz_lms1xxx.rviz new file mode 100644 index 00000000..8dc3db6c --- /dev/null +++ b/test/emulator/config/rviz_lms1xxx.rviz @@ -0,0 +1,194 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /PointCloud21 + - /LaserScan1 + Splitter Ratio: 0.5 + Tree Height: 863 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.535315990447998 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 0.10000000149011612 + Class: rviz/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: 100 + 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/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 1 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: false + Size (Pixels): 5 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.029999999329447746 + Reference Frame: cloud + Show Trail: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /sick_lms_1xx/marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + 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: 1 + 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): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /sick_lms_1xxx/scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: cloud + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 0.5704911351203918 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0.2102484107017517 + Y: -0.9448534846305847 + Z: 0.00026011772570200264 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.4997963905334473 + Target Frame: + Yaw: 3.1214962005615234 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1160 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000003eafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003ea000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003aa000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007cf0000003efc0100000002fb0000000800540069006d00650100000000000007cf000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000673000003ea00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1999 + X: 327 + Y: 27 diff --git a/test/emulator/launch/emulator_lms1xxx-150hz-0.75deg.launch b/test/emulator/launch/emulator_lms1xxx-150hz-0.75deg.launch new file mode 100644 index 00000000..cdbd2af9 --- /dev/null +++ b/test/emulator/launch/emulator_lms1xxx-150hz-0.75deg.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/test/emulator/launch/emulator_lms1xxx-37.5hz-0.1875deg.launch b/test/emulator/launch/emulator_lms1xxx-37.5hz-0.1875deg.launch new file mode 100644 index 00000000..acdd7f43 --- /dev/null +++ b/test/emulator/launch/emulator_lms1xxx-37.5hz-0.1875deg.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/test/emulator/launch/emulator_lms1xxx-75hz-0.375deg.launch b/test/emulator/launch/emulator_lms1xxx-75hz-0.375deg.launch new file mode 100644 index 00000000..e19087ac --- /dev/null +++ b/test/emulator/launch/emulator_lms1xxx-75hz-0.375deg.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/test/pcap_json_converter/pcap_json_converter.cmd b/test/pcap_json_converter/pcap_json_converter.cmd index 8ef7a31c..09721df2 100644 --- a/test/pcap_json_converter/pcap_json_converter.cmd +++ b/test/pcap_json_converter/pcap_json_converter.cmd @@ -13,6 +13,9 @@ echo PYTHON_DIR=%PYTHON_DIR% python --version @echo. +python pcap_json_converter.py --pcap_filename=../emulator/scandata/20221110-LMS1xxx-150hz-0.75deg.pcapng +python pcap_json_converter.py --pcap_filename=../emulator/scandata/20221110-LMS1xxx-75hz-0.375deg.pcapng +python pcap_json_converter.py --pcap_filename=../emulator/scandata/20221110-LMS1xxx-37.5hz-0.1875deg.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20221018_rms_1xxx_ascii_rawtarget_object.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220803_lms511.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220802_lms111.pcapng diff --git a/test/scripts/run_linux_ros1_simu_lms1xxx.bash b/test/scripts/run_linux_ros1_simu_lms1xxx.bash new file mode 100644 index 00000000..151d746e --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_lms1xxx.bash @@ -0,0 +1,68 @@ +#!/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() +{ + rosnode kill -a ; sleep 1 + killall sick_generic_caller ; sleep 1 + killall sick_scan_emulator ; sleep 1 +} + +# Start emulator and run sick_scan +function run_simu() +{ + rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_lms1xxx.rviz --opengl 210 & + emulator_launchfile=$1 + echo -e "Starting emulator $emulator_launchfile\n" + sleep 1 ; roslaunch sick_scan $emulator_launchfile scanner_type:=sick_lms_1xxx & + sleep 1 ; echo -e "Launching sick_scan sick_lms_1xxx.launch\n" + sleep 1 ; roslaunch sick_scan sick_lms_1xxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & + sleep 1 ; waitUntilRvizClosed 30 + kill_simu +} + +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_lms1xxx.bash: starting lms1xxx 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 lms1xxx simulation (firmware version <= 1.x without scan_freq and ang_res configuration, see https://github.com/SICKAG/sick_scan_xd/issues/122): +# 1. Start rviz +# 2. Start sick_scan emulator with field settings for sick_lms_1xxx and play +# 20221110-LMS1xxx-150hz-0.75deg.pcapng.json, 20221110-LMS1xxx-75hz-0.375deg.pcapng.json and 20221110-LMS1xxx-37.5hz-0.1875deg.pcapng.json +# 3. Start sick_scan driver for lms1xxx +# 4. Wait for 'q' or 'Q' to exit, until rviz is closed, or max. 60 seconds +# 5. Shutdown +run_simu emulator_lms1xxx-150hz-0.75deg.launch +run_simu emulator_lms1xxx-75hz-0.375deg.launch +run_simu emulator_lms1xxx-37.5hz-0.1875deg.launch + +# Shutdown +echo -e "Finishing lms1xxx emulation, shutdown ros nodes\n" +kill_simu + +popd + diff --git a/test/scripts/run_linux_ros1_simu_lms1xxx_v2x.bash b/test/scripts/run_linux_ros1_simu_lms1xxx_v2x.bash new file mode 100644 index 00000000..1e7e1c52 --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_lms1xxx_v2x.bash @@ -0,0 +1,68 @@ +#!/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() +{ + rosnode kill -a ; sleep 1 + killall sick_generic_caller ; sleep 1 + killall sick_scan_emulator ; sleep 1 +} + +# Start emulator and run sick_scan +function run_simu() +{ + rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_lms1xxx.rviz --opengl 210 & + emulator_launchfile=$1 + echo -e "Starting emulator $emulator_launchfile\n" + sleep 1 ; roslaunch sick_scan $emulator_launchfile scanner_type:=sick_lms_1xxx & + sleep 1 ; echo -e "Launching sick_scan sick_lms_1xxx.launch\n" + sleep 1 ; roslaunch sick_scan sick_lms_1xxx_v2.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & + sleep 1 ; waitUntilRvizClosed 30 + kill_simu +} + +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_lms1xxx.bash: starting lms1xxx 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 lms1xxx simulation (firmware version >= 2.0 with scan_freq and ang_res configuration, see https://github.com/SICKAG/sick_scan_xd/issues/122): +# 1. Start rviz +# 2. Start sick_scan emulator with field settings for sick_lms_1xxx and play +# 20221110-LMS1xxx-150hz-0.75deg.pcapng.json, 20221110-LMS1xxx-75hz-0.375deg.pcapng.json and 20221110-LMS1xxx-37.5hz-0.1875deg.pcapng.json +# 3. Start sick_scan driver for lms1xxx +# 4. Wait for 'q' or 'Q' to exit, until rviz is closed, or max. 60 seconds +# 5. Shutdown +run_simu emulator_lms1xxx-150hz-0.75deg.launch +run_simu emulator_lms1xxx-75hz-0.375deg.launch +run_simu emulator_lms1xxx-37.5hz-0.1875deg.launch + +# Shutdown +echo -e "Finishing lms1xxx emulation, shutdown ros nodes\n" +kill_simu + +popd + diff --git a/test/scripts/run_linux_ros1_simu_rms3xx_ascii.bash b/test/scripts/run_linux_ros1_simu_rms2xxx_ascii.bash similarity index 87% rename from test/scripts/run_linux_ros1_simu_rms3xx_ascii.bash rename to test/scripts/run_linux_ros1_simu_rms2xxx_ascii.bash index 154efba3..bff37f49 100644 --- a/test/scripts/run_linux_ros1_simu_rms3xx_ascii.bash +++ b/test/scripts/run_linux_ros1_simu_rms2xxx_ascii.bash @@ -17,17 +17,16 @@ if [ $roscore_running -lt 1 ] ; then fi # Start sick_scan emulator -# roslaunch sick_scan emulator_rms3xx.launch & 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_rms3xx.rviz --opengl 210 & +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_1xxx.launch\n" -roslaunch sick_scan sick_rms_3xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & +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 diff --git a/test/scripts/run_linux_ros2_simu_rms2xxx.bash b/test/scripts/run_linux_ros2_simu_rms2xxx.bash new file mode 100644 index 00000000..321b2a84 --- /dev/null +++ b/test/scripts/run_linux_ros2_simu_rms2xxx.bash @@ -0,0 +1,50 @@ +#!/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 +