diff --git a/CHANGELOG.md b/CHANGELOG.md index 81fba7a1..8a45b28f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,26 @@ features that will be removed in future versions **Removed** for deprecated feat ## Unreleased ## +### Release v3.6.0 + - **add** Launchfiles and configuration for picoScan120 + - **add** Optional AngleRangeFilter and IntervalFilter for picoScan + - **fix** Obsolete topic "/sick_multiscan/scan" removed + - **add** IMU automatically deactivated after receiving an error code for IMU activation from picoScan w/o addons + - **fix** Customization of hash values for authorization #366 + - **fix** Replaced builtin_addressof expressions #370 + - **add** Different UDP timeouts for state initial and running, improved UDP timeout handling + - **fix** Picoscan range_min value in laserScan message #382 + - **add** Support for RMS2xxx LIDoutputstate telegrams + - **fix** sick_generic_caller debug assertion #385 + - **add** Check of udp receiver ip at startup + - **add** cmake-option to overwrite optimization level + - **change** Documentation restructured + - **add** Improved field evaluation TiM7xx, Tim7xxS (publish LIDinputstate messages, configuration and services for options FieldSetSelectionMethod and ActiveFieldSet) + - **add** PicoScan parameter add_transform_xyz_rpy #399 + - **fix** LMS4000 encoder settings #403 + - **fix** CMake-flag for target sick_scan_xd_api_dockertest #404 + - **change** Merge PR #405 (typo) and PR #406 (sick_scan_xd_api_test) + ## Released ## ### Release v3.5.0 diff --git a/CHANGELOG.rst b/CHANGELOG.rst index aee20243..076efbe1 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -4,6 +4,25 @@ Changelog for package sick_scan_xd Forthcoming ----------- +* Release v3.6.0 + * add: Launchfiles and configuration for picoScan120 + * add: Optional AngleRangeFilter and IntervalFilter for picoScan + * fix: Obsolete topic "/sick_multiscan/scan" removed + * add: IMU automatically deactivated after receiving an error code for IMU activation from picoScan w/o addons + * fix: Customization of hash values for authorization #366 + * fix: Replaced builtin_addressof expressions #370 + * add: Different UDP timeouts for state initial and running, improved UDP timeout handling + * fix: Picoscan range_min value in laserScan message #382 + * add: Support for RMS2xxx LIDoutputstate telegrams + * fix: sick_generic_caller debug assertion #385 + * add: Check of udp receiver ip at startup + * add: cmake-option to overwrite optimization level + * change: Documentation restructured + * add: Improved field evaluation TiM7xx, Tim7xxS (publish LIDinputstate messages, configuration and services for options FieldSetSelectionMethod and ActiveFieldSet) + * fix: PicoScan parameter add_transform_xyz_rpy #399 + * fix: LMS4000 encoder settings #403 + * fix: CMake-flag for target sick_scan_xd_api_dockertest #404 + * change: Merge PR #405 (typo) and PR #406 (sick_scan_xd_api_test) 3.5.0 (2024-xx-xx) ------------------ diff --git a/CMakeLists.txt b/CMakeLists.txt index 073acb8d..0d516c35 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,7 +51,19 @@ option(BUILD_DEBUG_TARGET "Build debug target (ON) or release (OFF)" ON) # OFF ( # endif() if(NOT WIN32) - add_compile_options(-O3) + if(O EQUAL 0) + add_compile_options(-g -O0) + message(STATUS "Building sick_scan_xd with -g -O0") + elseif(O EQUAL 1) + add_compile_options(-g -O1) + message(STATUS "Building sick_scan_xd with -O1") + elseif(O EQUAL 2) + add_compile_options(-g -O2) + message(STATUS "Building sick_scan_xd with -O2") + else() + add_compile_options(-O3) + message(STATUS "Building sick_scan_xd with -O3") + endif() endif() # Added CMP0022 and CMP0048 to avoid cmake warnings if (POLICY CMP0022) @@ -238,6 +250,7 @@ if(ROS_VERSION EQUAL 1) Encoder.msg LFErecFieldMsg.msg LFErecMsg.msg + LIDinputstateMsg.msg LIDoutputstateMsg.msg RadarObject.msg RadarPreHeader.msg @@ -265,6 +278,8 @@ if(ROS_VERSION EQUAL 1) FILES ColaMsgSrv.srv ECRChangeArrSrv.srv + FieldSetReadSrv.srv + FieldSetWriteSrv.srv GetContaminationResultSrv.srv LIDoutputstateSrv.srv SCdevicestateSrv.srv @@ -370,6 +385,7 @@ if(ROS_VERSION EQUAL 2) "msg/ImuExtended.msg" "msg/LFErecFieldMsg.msg" "msg/LFErecMsg.msg" + "msg/LIDinputstateMsg.msg" "msg/LIDoutputstateMsg.msg" "msg/NAVLandmarkData.msg" "msg/NAVOdomVelocity.msg" @@ -388,6 +404,8 @@ if(ROS_VERSION EQUAL 2) # service files "srv/ColaMsgSrv.srv" "srv/ECRChangeArrSrv.srv" + "srv/FieldSetReadSrv.srv" + "srv/FieldSetWriteSrv.srv" "srv/GetContaminationResultSrv.srv" "srv/LIDoutputstateSrv.srv" "srv/SCdevicestateSrv.srv" @@ -813,23 +831,25 @@ if(BUILD_SICK_SCAN_XD_API_TEST EQUAL 1) elseif(ROS_VERSION EQUAL 2) target_link_libraries(sick_scan_xd_api_test "pthread") # pthread required for std::thread endif() - endif() - add_executable(sick_scan_xd_api_dockertest - driver/src/sick_scan_xd_api/sick_scan_api_converter.cpp - test/src/sick_scan_xd_api/sick_scan_xd_api_dockertest.cpp - test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c - ) - target_link_libraries(sick_scan_xd_api_dockertest ${LIB_JSONCPP}) - if(NOT WIN32) - target_link_libraries(sick_scan_xd_api_dockertest "dl") # link with dl for dynamic library loading - if(ROS_VERSION EQUAL 0) - target_link_libraries(sick_scan_xd_api_dockertest "pthread") # pthread required for std::thread - target_link_options(sick_scan_xd_api_dockertest PUBLIC "LINKER:--no-as-needed") # fixes exception "Enable multithreading to use std::thread: Operation not permitted" - elseif(ROS_VERSION EQUAL 1) - target_link_libraries(sick_scan_xd_api_dockertest ${catkin_LIBRARIES}) - add_dependencies(sick_scan_xd_api_dockertest ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) - elseif(ROS_VERSION EQUAL 2) - target_link_libraries(sick_scan_xd_api_dockertest "pthread") # pthread required for std::thread + endif() + if(${ENABLE_EMULATOR} OR ENABLE_EMULATOR EQUAL ON OR CMAKE_ENABLE_DOCKERTESTS EQUAL 1) # i.e. development or dockertests + add_executable(sick_scan_xd_api_dockertest + driver/src/sick_scan_xd_api/sick_scan_api_converter.cpp + test/src/sick_scan_xd_api/sick_scan_xd_api_dockertest.cpp + test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c + ) + target_link_libraries(sick_scan_xd_api_dockertest ${LIB_JSONCPP}) + if(NOT WIN32) + target_link_libraries(sick_scan_xd_api_dockertest "dl") # link with dl for dynamic library loading + if(ROS_VERSION EQUAL 0) + target_link_libraries(sick_scan_xd_api_dockertest "pthread") # pthread required for std::thread + target_link_options(sick_scan_xd_api_dockertest PUBLIC "LINKER:--no-as-needed") # fixes exception "Enable multithreading to use std::thread: Operation not permitted" + elseif(ROS_VERSION EQUAL 1) + target_link_libraries(sick_scan_xd_api_dockertest ${catkin_LIBRARIES}) + add_dependencies(sick_scan_xd_api_dockertest ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) + elseif(ROS_VERSION EQUAL 2) + target_link_libraries(sick_scan_xd_api_dockertest "pthread") # pthread required for std::thread + endif() endif() endif() endif() diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..ade1891e --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,602 @@ + + +Executive Summary +--- + +This documentation is intended to provide background information on the maintenance and extension of the repository. + +Table of Contents +--- + +- [Adding a new device to the driver](#adding-a-new-device-to-the-driver) + - [Naming Convention](#naming-convention) + - [Launch Files](#launch-files) + - [Code Modification](#code-modification) +- [Bloom release](#bloom-release) + - [First time installation of toolchain](#first-time-installation-of-toolchain) + - [Release build for ROS 1](#release-build-for-ros-1) + - [Release build for ROS 2](#release-build-for-ros-2) + - [Check status](#check-status) + - [Useful links and information](#useful-links-and-information) +- [Testing](#testing) + - [Unit tests](#unit-tests) + - [Examples](#examples) +- [Simulation](#simulation) + - [Windows](#windows) + - [Linux](#linux) +- [Profiling](#profiling) + - [Installation](#installation) + - [Usage](#usage) + +# Adding a new device to the driver + +This driver is designed to support several different scanner types (including radar) from Sick. This documentation describes how to add additional devices to the driver. + +## Naming Convention + +For each device type a name pattern is assigned as follows: +`` +sick__ +`` + +The name type is used in the code to decide which scanner-specific parameters are set. +The name type is passed as a parameter as follows: +``` + +``` + +## Launch Files + +A launch file is created for each device type, +which usually has the same naming convention as the scanner type. +To create a new device, it is recommended to copy, rename and edit an existing launch file. + +## Code Modification + +1. Hint: Construction of parser: + ``` + sick_scan_xd::SickGenericParser *parser = new sick_scan_xd::SickGenericParser(scannerName); + ``` +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_XXXX_NAME); + in the file sick_generic_parser.cpp + +4. Add new parameter block like + ``` + if (basicParams[i].getScannerName().compare(SICK_SCANNER_MRS_1XXX_NAME) == 0) + {... + } in the file sick_generic_parser.cpp + ``` + +5. Copy the file sick_generic_radar.cpp and add a new class following the structure +of this file. + +# Bloom release + +## First time installation of toolchain + +1. Install on Linux: + * Install bloom: + ``` + sudo apt-get update + sudo apt-get install python3-bloom python3-catkin-pkg + ``` + * Install docker: + ``` + pushd /tmp + curl -fsSL https://get.docker.com -o get-docker.sh + sudo sh get-docker.sh + sudo usermod -aG docker $USER + popd + shutdown -r now # reboot + # short quicktest + docker --version + docker info + docker run hello-world + ``` + * Install ros-buildfarm: + ``` + # sudo apt-get install python3-ros-buildfarm # not successfully, unable to locate + pip3 install ros-buildfarm # installs ros-buildfarm 3.0 successfully + ``` + +2. Build the prerelease: + * Short version to build a prerelase: + * Run the following commands: + ``` + mkdir -p ./ws_sick_scan_xd_bloom/src + cd ./ws_sick_scan_xd_bloom/src + git clone -b master https://github.com/SICKAG/sick_scan_xd.git + cd ./sick_scan_xd/test/scripts + ./run_linux_ros1_bloom.bash + ``` + * Fix any errors during the prerelease build and check in + * Repeat `./run_linux_ros1_bloom.bash` until the the prerelease build finishes without errors + * Alternative version: + * Open http://prerelease.ros.org/noetic in the brower + * Add a custom repository: `sick_scan_xd` , `https://github.com/SICKAG/sick_scan_xd` , `master` (or `feature/bloom_pretest` or any other branch to test) + * Add a custom repository: `msgpack11` , `https://github.com/SICKAG/msgpack11` , `master` + * Add a custom repository: `libsick_ldmrs` , `https://github.com/SICKAG/libsick_ldmrs` , `master` + * Confirm next steps (i.e. URL of build farm: https://raw.githubusercontent.com/ros-infrastructure/ros_buildfarm_config/production/index.yaml, Ubuntu focal) + * Click on `Generate command` + * Run the generated command, i.e.: + ``` + source /opt/ros/noetic/setup.bash + mkdir -p /tmp/prerelease_job + cd /tmp/prerelease_job + generate_prerelease_script.py \ + https://raw.githubusercontent.com/ros-infrastructure/ros_buildfarm_config/production/index.yaml \ + noetic default ubuntu focal amd64 \ + --custom-repo \ + sick_scan_xd:git:https://github.com/SICKAG/sick_scan_xd:master \ + msgpack11:git:https://github.com/SICKAG/msgpack11:master \ + libsick_ldmrs:git:https://github.com/SICKAG/libsick_ldmrs:master \ + --level 1 \ + --output-dir ./ + ``` + * Run `printf "\033c" ; rm -rf ~/.ccache ; mkdir -p ~/.ccache ; ./prerelease.sh` in folder `/tmp/prerelease_job` + * In case of error message `/usr/lib/ccache/cc is not able to compile a simple test program`: + * Remove folder `~/.ccache` before running `./prerelease.sh` + * See https://answers.ros.org/question/347063/error-pre-release-melodic/ + * Fix any errors during the prerelease build and check in + * Remove the temporary build folder by `rm -rf /tmp/prerelease_job` + * Repeat until `prerelease.sh` finishes without errors. + +3. Submit package sick_scan_xd for indexing (noetic) + * Fork `https://github.com/ros/rosdistro` -> `https://github.com//rosdistro.git` + * `git clone https://github.com//rosdistro.git` + * Edit file `rosdistro/noetic/distribution.yaml` and add after `sick_scan`: + ``` + sick_scan_xd: + doc: + type: git + url: https://github.com/SICKAG/sick_scan_xd.git + version: master + ``` + * `cd rosdistro ; source /opt/ros/noetic/setup.bash ; rosdistro_reformat file://"$(pwd)"/index.yaml` + * git commit: `git commit -m "Adding sick_scan_xd to documentation index for distro noetic" distribution.yaml` + * git push: `git push origin master` + * Submit a pull request on `https://github.com//rosdistro` + +4. For ROS 2 humble: Follow instructions on https://docs.ros.org/en/humble/How-To-Guides/Releasing/Releasing-a-Package.html + +**_NOTE:_** Bloom releases for ROS 2 foxy are not longer supported (Pull request failed, "This pull request changes files for a ROS distribution that is no longer supported (End Of Life)") + + * Submit package sick_scan_xd for indexing (ROS 2 humble) + * Reset fork `https://github.com//rosdistro.git` to origin/master or delete the fork and create a new one -> `https://github.com//rosdistro.git` + * `git clone https://github.com//rosdistro.git` + * Edit file `rosdistro/humble/distribution.yaml` and add after `sick_safevisionary_ros2`: + ``` + sick_scan_xd: + doc: + type: git + url: https://github.com/SICKAG/sick_scan_xd.git + version: develop + status: developed + ``` + * git commit and push ("Adding sick_scan_xd to documentation index for distro humble") + * Submit a pull request on `https://github.com//rosdistro` + * Do the same for any new ROS 2 version, e.g. iron and jazzy (`rosdistro/iron/distribution.yaml`, `rosdistro/jazzy/distribution.yaml`) + * [Start a new release team](https://github.com/ros2-gbp/ros2-gbp-github-org/issues/new?assignees=&labels=&template=new_release_team.md&title=Add+release+team) + * ROS 2 sick_scan_xd team: https://github.com/orgs/ros2-gbp/teams/sick_scan_xd + * ROS 2 sick_scan_xd release repository: https://github.com/ros2-gbp/sick_scan_xd-release + +## Release build for ROS 1 + +* Build a prerelease (dry run in a docker container): + * Run the following commands: + ``` + git clone -b master https://github.com/SICKAG/sick_scan_xd.git + cd ./sick_scan_xd/test/scripts + sudo dos2unix ./*.bash ; sudo chmod a+x ./*.bash + ./run_linux_ros1_bloom.bash + ``` + * Fix any errors during the prerelease build and check in + * Repeat `./run_linux_ros1_bloom.bash` until the the prerelease build finishes without errors + +* Build a binary release: follow https://wiki.ros.org/bloom/Tutorials/FirstTimeRelease + * Update version number in package.xml, minor version number should be incremented at least + * Create resp. update CHANGELOG.rst: + ``` + source /opt/ros/noetic/setup.bash + cd ./src/sick_scan_xd + rm ./CHANGELOG.rst + catkin_generate_changelog --all # create CHANGELOG.rst + ``` + * Commit and pull all changes incl. CHANGELOG.rst and package.xml: + ``` + git add CHANGELOG.rst package.xml + git commit -m "Update CHANGELOG.rst and package version" + git push + ``` + * Run `catkin_prepare_release` and `bloom-release` in folder `src/sick_scan_xd`: + ``` + source /opt/ros/noetic/setup.bash + catkin_prepare_release -y + bloom-release --rosdistro noetic --track noetic sick_scan_xd # at first time: call with option --edit for configuration + ``` + * For the initial release (first time): Run `bloom-release` in folder `src/sick_scan_xd` with option `--edit`: + ``` + source /opt/ros/noetic/setup.bash + catkin_prepare_release -y + bloom-release --rosdistro noetic --track noetic sick_scan_xd --edit + Release repository url: https://github.com/SICKAG/sick_scan_xd-release.git + upstream: + Upstream Repository URI: https://github.com/SICKAG/sick_scan_xd.git + Upstream VCS Type: + Version: + Release Tag: + Upstream Devel Branch: feature/bloom_pretest + ROS Distro: noetic + Patches Directory: + Release Repository Push URL: + ``` + * Check status: https://index.ros.org/p/sick_scan_xd/#noetic + * Install binary release: `sudo apt update ; sudo apt-get install ros-noetic-sick-scan-xd`. Note from https://wiki.ros.org/bloom/Tutorials/FirstTimeRelease : Packages built are periodically synchronized over to the shadow-fixed and public repositories, so it might take as long as a month before your package is available on the public ROS debian repositories (i.e. available via apt-get). + +## Release build for ROS 2 + +For ROS 2 follow the instructions on https://docs.ros.org/en/humble/How-To-Guides/Releasing/Releasing-a-Package.html : +* Checkout the sick_scan_xd version to be released and run: + ``` + git clone -b master https://github.com/SICKAG/sick_scan_xd.git + cd ./sick_scan_xd + rm ./CHANGELOG.rst + catkin_generate_changelog --all # create CHANGELOG.rst + ``` +* Commit CHANGELOG.rst and optional modifications: + ``` + git add CHANGELOG.rst + git commit -m "Update CHANGELOG.rst" + git push + ``` +* Run `catkin_prepare_release` and `bloom-release`: + ``` + bloom-release --rosdistro humble --track humble sick_scan_xd # at first time: call with option --new-track + ``` + For the initial release (i.e. at the first time): Run bloom-relase configuration with option --new-track: + `bloom-release --new-track --rosdistro humble --track humble sick_scan_xd` + * Release repository url: https://github.com/ros2-gbp/sick_scan_xd-release.git + * Upstream: + * Upstream Repository URI: https://github.com/SICKAG/sick_scan_xd.git + * Upstream Devel Branch: develop + * ROS Distro: humble + After the initial release has been approved: Run + ``` + sudo rosdep init + rosdep update + ``` + +## Check status +Jenkins build status: +* ROS 1 noetic jenkins build status: https://build.ros.org/job/Ndev__sick_scan_xd__ubuntu_focal_amd64/lastBuild/ +* ROS 2 humble jenkins build status: https://build.ros2.org/job/Hdev__sick_scan_xd__ubuntu_jammy_amd64/lastBuild/ +* ROS 2 iron jenkins build status: https://build.ros2.org/job/Idev__sick_scan_xd__ubuntu_jammy_amd64/lastBuild/ +* ROS 2 jazzy jenkins build status: https://build.ros2.org/job/Jdev__sick_scan_xd__ubuntu_noble_amd64/lastBuild/ +* ROS 1 jenkins: https://build.ros.org/search/?q=sick_scan_xd +* ROS 2 jenkins: https://build.ros2.org/search/?q=sick_scan_xd + +Release repositories: +* ROS 1 release repository: https://github.com/SICKAG/sick_scan_xd-release +* ROS 2 release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git + + +Show version and list information about prebuilt binaries: +``` +sudo apt update +sudo apt show ros-noetic-sick-scan-xd +sudo apt show ros-humble-sick-scan-xd +sudo apt show ros-iron-sick-scan-xd +sudo apt show ros-jazzy-sick-scan-xd +``` + +Installation of prebuilt binaries: +``` +sudo apt update +sudo apt-get install ros-noetic-sick-scan-xd +sudo apt-get install ros-humble-sick-scan-xd +sudo apt-get install ros-iron-sick-scan-xd +sudo apt-get install ros-jazzy-sick-scan-xd +sudo apt-get remove ros-noetic-sick-scan-xd +sudo apt-get remove ros-humble-sick-scan-xd +sudo apt-get remove ros-iron-sick-scan-xd +sudo apt-get remove ros-jazzy-sick-scan-xd +``` + + +## Useful links and information + +* http://wiki.ros.org/bloom +* https://wiki.ros.org/bloom/Tutorials/FirstTimeRelease +* https://docs.ros.org/en/humble/How-To-Guides/Releasing/Releasing-a-Package.html + + +**Bloom builds an old sick_scan_xd version (ROS 1)** + +Check `devel_branch` in https://github.com/SICKAG/sick_scan_xd-release/blob/master/tracks.yaml . If devel_branch is an old branch, replace it with e.g. `develop` or `master`, or update the `` to a new version. + +**Bloom builds an old sick_scan_xd version (ROS 2)** +Check `devel_branch` in https://github.com/ros2-gbp/sick_scan_xd-release/blob/master/tracks.yaml . If devel_branch is an old branch, replace it with e.g. `develop` or `master`, or update the `` to a new version. + +**Bloom builds a new sick_scan_xd version, but apt still installs an old version** + + * Check the sick_scan_xd version in the release repositories https://github.com/SICKAG/sick_scan_xd-release.git (ROS 1) and https://github.com/ros2-gbp/sick_scan_xd-release.git (ROS 2) + * Install bloom (if not yet done) using `sudo apt-get install python-bloom` on Linux or `pip install -U bloom` on Windows + * Run + ``` + bloom-release --rosdistro noetic -d sick_scan_xd # release repository: https://github.com/SICKAG/sick_scan_xd-release.git, argument -d enables debug infos + bloom-release --rosdistro humble -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos + bloom-release --rosdistro iron -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos + bloom-release --rosdistro jazzy -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos + ``` + * In case of github 2FA errors: Follow http://wiki.ros.org/bloom/Tutorials/GithubManualAuthorization to create a 2FA token and configure the token in file `~/.config/bloom`. + * Note: Updates of release repository https://github.com/SICKAG/sick_scan_xd-release.git require github authentification via ssh. See https://docs.github.com/en/authentication/connecting-to-github-with-ssh and https://wiki.ros.org/bloom/Tutorials/GithubManualAuthorization for details. + +# Testing + +## Unit tests + +For a quick unit test after installation without the sensor hardware, a test server is provided to simulate a scanner. It implements a simple tcp server, which responds to binary cola messages and sends predefined LMDscandata to a tcp-client. The sick_scan_xd driver can connect to the local test server instead of the lidar device for offline-tests. Please note, that this test server does not emulate a Lidar sensor. It just sends some simple scan data and response messages to a tcp client. It can be used for a quick unit test after build and install. + +To build the test server, activate cmake option `ENABLE_EMULATOR` in CMakeLists.txt and rebuild sick_scan_xd. By default, option `ENABLE_EMULATOR` is switched off. + +For a unit test of LMS1xx, run the following commands in different terminals: + +``` +cd sick_scan_xd +source ./install/setup.bash + +# Start sick_scan_xd emulator +roslaunch sick_scan_xd emulator_lms1xx.launch & +sleep 1 + +# Start rviz +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_lms1xx.rviz & +sleep 1 + +# Start sick_scan_xd driver +roslaunch sick_scan_xd sick_lms_1xx.launch hostname:=127.0.0.1 +``` + +For a unit test of LMS5xx, run the following commands in different terminals: + +``` +cd sick_scan_xd +source ./install/setup.bash + +# Start sick_scan_xd emulator +roslaunch sick_scan_xd emulator_lms5xx.launch & +sleep 1 + +# Start rviz +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_lms5xx.rviz & +sleep 1 + +# Start sick_scan_xd driver +roslaunch sick_scan_xd sick_lms_5xx.launch hostname:=127.0.0.1 +``` + +For a unit test of LMS7xx, run the following commands in different terminals: + +``` +cd sick_scan_xd +source ./install/setup.bash + +# Start sick_scan_xd emulator +roslaunch sick_scan_xd emulator_01_default.launch & +sleep 1 + +# Start rviz +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg.rviz & +sleep 1 + +# Start sick_scan_xd driver +roslaunch sick_scan_xd sick_tim_7xx.launch hostname:=127.0.0.1 +``` + +For a unit test of LMS7xxS, run the following commands in different terminals: + +``` +cd sick_scan_xd +source ./install/setup.bash + +# Start sick_scan_xd emulator +roslaunch sick_scan_xd emulator_01_default.launch & +sleep 1 + +# Start rviz +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg.rviz & +sleep 1 + +# Start sick_scan_xd driver +roslaunch sick_scan_xd sick_tim_7xxS.launch hostname:=127.0.0.1 +``` + +Alternatively, you can just run the test scripts provided in folder `sick_scan_xd/test/scripts`: + +``` +cd sick_scan_xd/test/scripts +./makeall.bash +./run_simu_lms1xx.bash +./run_simu_lms5xx.bash +./run_simu_tim7xx_tim7xxS.bash +``` + +Make sure to finish all sick_scan_xd nodes after a test. All nodes can be killed by +``` +rosnode kill -a ; sleep 1 +killall sick_generic_caller ; sleep 1 +killall sick_scan_emulator ; sleep 1 +``` + +## Examples + +rviz example screenshots using sick_scan_xd with LMS1xx and LMS5xx test server: + +![emulator_lms1xx_screenshot.png](doc/emulator_lms1xx_screenshot.png) + +rviz example screenshots using sick_scan_xd with LMS7xx and LMS7xxS test server: + +![emulator_lms1xx_screenshot.png](doc/emulator_lms7xx_screenshot.png) + +Further examples are provided in folder `test/scripts`. + +# Simulation + +For unittests without sensor hardware, a simple test server is provided. To build the test server, call either cmake with option `-DCMAKE_ENABLE_EMULATOR=1`, or activate cmake option `ENABLE_EMULATOR` in CMakeLists.txt. Then rebuild sick_scan_xd. By default, option `ENABLE_EMULATOR` is switched off. + +Please note that this just builds a simple test server for basic unittests of sick_scan_xd drivers. Its purpose is to run basic tests and to help with diagnosis in case of issues. It does not emulate a real scanner! + +Simulation requires jsoncpp. Install with `sudo apt-get install libjsoncpp-dev` on Linux and with `vcpkg install jsoncpp:x64-windows` on Windows. + +You can find examples to test and run sick_scan_xd in offline mode in folder `test/scripts`. Their purpose is to demonstrate the usage of the sick_scan_xd driver. Please feel free to customize the scripts or use them as a starting point for own projects. + +## Windows + +Run script `run_simu_lms_5xx.cmd` in folder `test/scripts` or execute the following commands: + +1. Start the test server: + ``` + cd .\build + start "testserver" cmd /k python ../test/emulator/test_server.py --scandata_file=../test/emulator/scandata/20210302_lms511.pcapng.scandata.txt --scandata_frequency=20.0 --tcp_port=2112 + @timeout /t 1 + ``` + +2. Run sick_generic_caller. On native Windows: + ``` + .\Debug\sick_generic_caller.exe ../launch/sick_lms_5xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False + ``` + On Windows with ROS 2: + ``` + ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_5xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False + ``` + +3. Open file `image_viewer.html` in folder `demo` in your browser to view a jpg-image of the current scan. + +Note, that python version 3 incl. runtime dlls must be accessable, f.e. by extending the PATH environment variable: +``` +set PYTHON_DIR=%ProgramFiles(x86)%/Microsoft Visual Studio/Shared/Python37_64 +set PATH=%PYTHON_DIR%;%PYTHON_DIR%/Scripts;c:\vcpkg\installed\x64-windows\bin;%PATH% +``` + +Further examples are provided in folder `test/scripts`. + +## Linux + +Run script `run_simu_lms_5xx.bash` in folder `test/scripts` or execute the following commands: + +1. Start the test server: + ``` + python3 ./test/emulator/test_server.py --scandata_file=./test/emulator/scandata/20210302_lms511.pcapng.scandata.txt --scandata_frequency=20.0 --tcp_port=2112 & + sleep 1 + ``` + +2. Run sick_generic_caller. + - On native Linux: + ``` + ./build/sick_generic_caller ./launch/sick_lms_5xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & + ``` + - On Linux with ROS 1: + ``` + roslaunch sick_scan_xd sick_lms_5xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & + ``` + - On Linux with ROS 2: + ``` + ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_5xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & + ``` + +3. View the point cloud. + - On native Linux:
+ Open file `image_viewer.html` in folder `demo` in a browser (f.e. firefox) to view a jpg-image of the current scan. + - On Linux with ROS 1: + ``` + rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_lms5xx.rviz & + ``` + - On Linux with ROS 2: + ``` + rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_lms5xx.rviz & + ``` + +Further examples are provided in folder `test/scripts`. + +# Profiling + +Since the existing node can basically be used on different platforms, bottlenecks can occur with weak hardware. To better analyze these bottlenecks, software profiling can be performed. +The following example shows how to perform profiling. +For further details on profiling, please refer to https://baptiste-wicht.com/posts/2011/09/profile-c-application-with-callgrind-kcachegrind.html, for example. + +## Installation + +First of all, you need to install Callgrind and KCachegrind. +You also need to install graphviz in order to view the call graph in KCachegrind. The applications are already packaged for the most important Linux distributions. You can just use apt-get to install them: +``` +sudo apt-get install valgrind kcachegrind graphviz +``` +## Usage +We have to start by profiling the application with Callgrind. To profile an application with Callgrind, you just have to prepend the Callgrind invocation in front of your normal program invocation: +``` +valgrind --tool=callgrind program [program_options] +``` +In order to establish a reference to the source code during profiling, the program must be compiled with debug symbols, this can be done with catkin_make +``` +catkin_make install -DCMAKE_BUILD_TYPE=Debug +``` +It is necessary to create a rosmaster so that the sick_scan_xd node can connect to it because we can't use roslaunch during profiling. +``` +roscore +``` +To set the parameters we start a node as usual with roslaunch. +``` +roslaunch sick_scan_xd sick_lms_5xx.launch hostname:=192.168.0.151 +``` +While this node is running we can use ```ps -aef| grep sick_scan_xd``` to determine the program path and the call parameters. +``` +rosuser@ROS-NB:~$ ps -aef|grep sick_scan_xd +rosuser 4839 2443 0 14:43 pts/1 00:00:00 /usr/bin/python /opt/ros/melodic/bin/roslaunch sick_scan_xd sick_lms_5xx.launch hostname:=192.168.0.151 +rosuser 4854 4839 1 14:43 ? 00:00:03 /home/rosuser/ros_catkin_ws/devel/lib/sick_scan_xd/sick_generic_caller __name:=sick_lms_5xx __log:=/home/rosuser/.ros/log/f9861670-304c-11e9-9839-54e1ad2921b6/sick_lms_5xx-1.log +rosuser 4910 4875 0 14:46 pts/6 00:00:00 grep --color=auto sick_scan_xd +``` +now we can close the node and restart with callgrid +``` +valgrind --tool=callgrind program /home/rosuser/ros_catkin_ws/devel/lib/sick_scan_xd/sick_generic_caller __name:=sick_lms_5xx +``` +The result will be stored in a callgrind.out.XXX file where XXX will be the process identifier. +You can read this file using a text editor, but it won't be very useful because it's very cryptic. +That's here that KCacheGrind will be useful. You can launch KCacheGrind using command line +or in the program menu if your system installed it here. Then, you have to open your profile file. + +The first view present a list of all the profiled functions. You can see the inclusive +and the self cost of each function and the location of each one. + +![src_view.png](doc/src_view.png) + +Once you click on a function, the other views are filled with information. The view in uppper right part of the window gives some information about the selected function. + +![profile_002](doc/profile_002.png) + +The view have several tabs presenting different information: + +* Types : Present the types of events that have been recorded. In our case, it's not really interesting, it's just the number of instructions fetch +* Callers : List of the direct callers. +* All Callers : List of all the callers, it seems the callers and the callers of the callers. +* Callee Map : A map of the callee, personally, I do not really understand this view, but it's a kind of call graph representing the cost of the functions. +* Source code : The source code of the function if the application has been compiled with the debug symbol. + +And finally, you have another view with data about the selected function. + +![profile_003](doc/profile_003.png) + +Again, several tabs: + +* Callees : The direct callees of the function +* Call Graph : The call graph from the function to the end +* All Callees : All the callees and the callees of the callees +* Caller Map : All functions are represented as blocks the size corresponds to their CPU time. Callees are stacked on the callers. +* Machine Code : The machine code of the function if the application has been profiled with --dump-instr=yes option + +You have also several display options and filter features to find exactly what you want and display it the way you want. + +The information provided by KCacheGrind can be very useful to find which functions takes too much time or which functions are called too much. +This text is an adopted version of https://baptiste-wicht.com/posts/2011/09/profile-c-application-with-callgrind-kcachegrind.html . Thanks to Baptiste Wicht. \ No newline at end of file diff --git a/CREDITS.md b/CREDITS.md deleted file mode 100644 index 023c2576..00000000 --- a/CREDITS.md +++ /dev/null @@ -1,15 +0,0 @@ -## Creators - -**Michael Lehning** - -- - -on behalf of SICK AG - -- - ------------------------------------------------------------------------- - - - -![Lehning Logo](http://www.lehning.de/style/banner.jpg "LEHNING Logo") diff --git a/FAQ.md b/FAQ.md deleted file mode 100644 index 5e671d83..00000000 --- a/FAQ.md +++ /dev/null @@ -1,311 +0,0 @@ -# sick_scan_xd FAQ - -## How to run multiple sensors concurrently? - -:question: How can I run multiple sensors concurrently with sick_scan_xd ? - -:white_check_mark: To support multiple sensors, sick_scan_xd has to be started multiple times, with one sick_scan_xd-node for each sensor. See [multiple_lidars.md](doc/multiple_lidars.md) for details. - -## Driver restarts again and again after "sFA" message - -:question: The sick_scan_xd driver restarts again and again after an error message "sFA". - -:white_check_mark: The behaviour is intentional. The error message "sFA" can be caused by faulty configuration or errors in the lidar. Correct operation after this error message is not guaranteed. In this case, the driver restarts itself. It is recommended to identify and correct the error using its error number ("`sFA`"). The SOPAS error codes are listed in the manual. - -## Driver restarts after timeout error - -:question: The sick_scan_xd driver changes the communication protocol and restarts after a timeout error. - -:white_check_mark: The use of binary communication (Cola-B) is highly recommended due to better compatibility, lower network traffic and general support. -Recommendation: -1. Set parameter "use_binary_protocol" to "true" in the launch file, and -2. Set the lidar communication mode with the SOPAS ET software to binary and save this setting in the scanner's EEPROM. - -## Changes in launchfiles are ignored - -:question: roslaunch still uses an old version after modifying the launch-file. - -:white_check_mark: After modifying a launch-file, it has to be installed by running `catkin_make_isolated --install --cmake-args -DROS_VERSION=1` -to be located and used by `roslaunch`. - -## ROS-2 launchfile support - -:question: How can I create a ROS-2 node in python to run sick_generic_caller from a launch.py-file in ROS-2? - -:white_check_mark: Example to launch a TiM-7xx node in ROS-2: -``` - sick_scan_pkg_prefix = get_package_share_directory('sick_scan_xd') - tim_launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/sick_tim_7xx.launch') - tim_top_node = Node( - package='sick_scan_xd', - executable='sick_generic_caller', - output='screen', - arguments=[ - tim_launch_file_path, - 'nodename:=/lidars/tim_top', - 'hostname:=192.168.0.110', - 'cloud_topic:=/lidars/tim_top/cloud', - 'frame_id:=tim_top' - ] - ) -``` -Thanks to user JWhitleyWork. - -## Timestamps - -:question: What timestamp is provided in the pointcloud and laserscan messages? - -:white_check_mark: Details about timestamps are given in [timing.md](doc/timing.md) and [software_pll.md](doc/software_pll.md). - -In a nutshell: -The lidars do not work with absolute time stamps but with "ticks". There are two types of tick timestamps: -* Tick timestamps for the generation -* Tick timestamps for sending the message -To match the tick timestamps against the system time, a straight line equation is estimated via the so-called software PLL, which can be used to calculate from the system time to the ticks in the lidar (and vice versa). The assumption is that the tick timestamp for sending the message corresponds to the system time when receiving the message. In reality there will be a tiny delay here. This delay can be ignored. - -With the help of this straight line equation, one can now calculate the system time at which the data is generated in the lidar. - -Summary: -1. lidar: stamp with ticks the generation (first shot in the scan) (TICK_GEN) -2. lidar: stamp with ticks the sending of the scan message (TICK_TRANSMIT) -3. PC: Stamp with system time the receiving of the message -4. PC: Calculate back to system time of generation: -System time of generation = System time of receiving - (TICK_TRANSMIT - TICK_GEN)/TICK_FREQUENCY - -## Laserscan messages with multiple frame ids - -:question: sick_scan_xd publishes laserscan messages for multiScan and picoScan with multiple frame ids and possibly inconsistent data. Which frame id is correct? - -:white_check_mark: By default, an echo filter is activated in the multiScan and picoScan launchfile. This echo filter suppresses multiple echos, e.g. echos from an object and a protective glass pane. The default configuration is "last echo only". In this case (i.e. one echo only), the fullframe laserscan messages on topic scan_fullframe all have identical frame ids for each layer, i.e. "world_\". For the multiScan lidars with 16 layers, sick_scan_xd publishes laserscan messages with frame ids "world_1", "world_2" up to "world_16". For picoScan lidars with 1 layer, there is just one frame id "world_1". - -In case of multiple echos (i.e. echo filter is deactivated), each echo is published by a laserscan message with different frame ids "world_\_\". For picoScan lidars with 3 echos, there are 3 frame ids "world_1_0", "world_1_1", "world_1_2" published. For multiScan lidars with 16 layers and 3 echos, there are 48 different frame ids published "world_1_0", "world_1_1", "world_1_2", "world_2_0", "world_2_1", "world_2_2", ... , "world_16_0", "world_16_1", "world_16_2". - -This behaviour is intended, since a laserscan message can not contain multiple ranges for a single scan point at one azimuth angle. Therefore, there have to be different laserscan messages for each layer and each echo. Layer and echo of a laserscan message are identified by the frame id. - -## Compilation errors - -:question: Compiler reports errors in file `/opt/ros//include/sick_scan` - -:white_check_mark: If sick_scan was previously installed using `apt-get install ros--sick-scan`, you have to remove previous versions using `apt-get purge ros--sick-scan`. Run the following steps for a complete rebuild: -1. Run `sudo apt-get purge ros--sick-scan` (e.g. `sudo apt-get purge ros-noetic-sick-scan`) to remove previously installed sick_scan-packages -2. Remove the folders sick_scan_xd/build, sick_scan_xd/build_isolated, sick_scan_xd/devel, sick_scan_xd/devel_isolated, sick_scan_xd/install and sick_scan_xd/install_isolated -3. Rebuild - -:question: cmake cannot find diagnostic_updater - -:white_check_mark: On ROS-2 foxy, package diagnostic_updater needs to be installed by -``` -sudo apt-get update -sudo apt-get install ros-$ROS_DISTRO-diagnostic-updater # install diagnostic_updater -# E.g. to install diagnostic_updater on foxy, run -# sudo apt-get install ros-foxy-diagnostic-updater -``` - -:question: catkin gives me the following error message: -`By not providing "FindSICKLDMRS.cmake" in CMAKE_MODULE_PATH this project ..., but CMake did not find one."` - -:white_check_mark: One problem with ROS is that it doesn't automatically rebuild everything if you just append "-DLMRRS=0". -If you accidentally did the following call before - -``` -catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -``` -you must remove the build/devel/install-directories created by the ROS build process. -For this please run the following commands to remove the directories, which holds the previous build results: -``` -cd ~/ros_catkin_ws -rm -rf build_isolated -rm -rf devel_isolated -rm -rf install_isolated -rm -rf devel -``` -It is possible that not all directories are present in this list. But that does not matter. -The only subdirectory left should be "src". -You can check this with the following command: - -``` -ls */ -d -``` -The output should be: -``` -src/ -``` -After doing this please rerun the command -catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 - -## rviz or rviz2 do not work as expected - -:question: rviz shows a grey point cloud. The size of points can be adjusted. - -:white_check_mark: Check in the launch file that the intensity flag is set to True. - -:question: rviz shows a grey point cloud and the size of points can not be adjusted. - -:white_check_mark: Probably in this case you are running Linux in a virtual machine. In this case, OpenGL may not work correctly in the VM. rviz then chooses a kind of "fallback solution" and deactivates the colors. -Also, changing the "Size" and "Style" display in rviz has no effect on the display of the pointcloud data. - -The problem can be avoided by starting rviz with the following sequence: - -``` -export LIBGL_ALWAYS_SOFTWARE=1 -rosrun rviz rviz -``` - -:question: rviz2 on Ubuntu 24 with ROS-2 jazzy crashes immediately after start - -:white_check_mark: This can be a wayland vs. X11 problem. Try `export QT_QPA_PLATFORM=xcb` before starting rviz2. See https://github.com/ros-visualization/rviz/issues/1442#issuecomment-553900795 and https://blog.martin-graesslin.com/blog/2015/07/porting-qt-applications-to-wayland/ for further information. - -## Angular resolution and scan frequency - -:question: The angular resolution/ number of shots is too small - -:white_check_mark: Possibly Mean or Median filters are activated. Use Sopas ET to deactivate them and store this settings permanent on the device, see picture. -![Sopas_filter](doc/tim5xxx_filter.PNG) -Further information can be found at http://wiki.ros.org/rviz/Troubleshooting. - -:question: The scanner and message frequency is lower than expected - -:white_check_mark: Mean or median filters decrease the scanner frequency. Check and configure filter settings with SOPAS ET or deactivate them in the launchfile (MRS1xxx, LMS1xxx, LMS4xxx, LRS4xxx only): -``` - - -``` -By default, filter settings are not overwritten, i.e. the filter settings stored in the lidars EEPROM apply (factory defaults resp. SOPAS ET filter settings). - -## LMS1xxx angular resolution - -:question: Independent of the configuration, the LMS1xxx pointcloud always displays 0.75 [deg] angular resolution - -:white_check_mark: Using higher resolutions, the LMS1xxx sends scan data interlaced. See [interlacing.md](doc/interlacing.md) for details. - -## "ERROR: Tcp::open: Failed to open TCP connection to 192.168.0.1, aborting." - -:question: Question: -sick_generic_caller gives you an answer like: -```bash -"ERROR: Tcp::open: Failed to open TCP connection to 192.168.0.1, aborting." -``` - -:white_check_mark: Answer: See [network.md](doc/network.md) for network diagnosis and recommended configuration. - -## IP Address of Lidar - -:question: Question: -My scanner does not use the default ip address. What shall I do? - - - -:white_check_mark: Answer: -There are two options doing this: - -* Permanently: -Replace the following entry with your ip address. -```bash - -``` -* Temporarily -Use a command line argument in addition to the launch file argument: -```bash - hostname:=192.168.0.2 -``` - -## Timeout Warning - -:question: Question: -During start phase the are warning/error message like - -```bash -no answer received after 5000 ms. Maybe sopas mode is wrong. -``` -and some more warning/error messages: - -:white_check_mark: Answer: -In this case the driver tries to start the scanner in binary mode. If this is not possible, warnings and error messages are generated. -The driver switches the scanner from ASCII mode to binary mode and then restarts communication. The messages can therefore be ignored. -For a long-term solution, we recommend switching from ASCII to binary communication with SOPAS ET under Windows. - -## Own Data Handling - -:question: Question: -I would like to process data with my own methods. - -:white_check_mark: Answer: -Search for keyword "PUBLISH_DATA:" in the code and replace the code for writing -jpeg-files and CSV-files with your own source code. - -## Occasionally missing scan data - -:question: Question: -Occasionally, no scan data appear, but the scanner is still reachable using `ping ` or `nc -z -v ` - -:white_check_mark: Answer: -This is likely to be a data capture issue. In any case it's recommend (if not already set) to use SOPAS-Binary instead of SOPAS-ASCII, because here the data rate is lower. - -In addition, the following measures can help to determine the source of the problems: -a) Exchange the cables to the lidar(s) incl. the network cables -b) Exchange the power supply to the lidar(s) -c) avoidance of interconnection of other components (like switch etc.) -d) upgrade hardware and firmware of devices (if new versions available) - -If it is not a hardware problem (e.g. cable), check if there are other software components using the network interface. - -If there are still problems, use Wireshark to see if there are any communication problems reported. Wireshark can export the recorded network traffic as text (see screenshot). Search for entries like "LMDscandata" in the generated text file. This marks the beginning of a new scan message. You can then compare the order of the timestamps of these packets to see if there were any failures. - -![Wireshark screenshot](https://user-images.githubusercontent.com/33296049/124088216-01aa2280-da53-11eb-91ae-2b88b37e08eb.png) - -## python_d.exe not found - -:question: Question: -On Windows with ROS, cmake complains "python_d.exe not found" when running rosidl generator - -:white_check_mark: Answer: -Workaround: Copy python.exe to python_d.exe in the python folder. - -## Debugging - -:question: Question: -How can I debug sick_generic_caller on ROS-1? - -:white_check_mark: Answer: -Build with compiler option `-g` and run sick_generic_caller as described using a launchfile. Stop sick_generic_caller (Ctrl-C or kill) and save the current ros parameter using `rosparam dump .yaml`. Load these parameter with `rosparam load .yaml` and restart sick_generic_caller in gdb or in your IDE. - -## Curved lines on a straight wall - -:question: Question: -The X,Y points of the lidar show a curved line even though the lidar is scanning a straight wall. How can this be? - -:white_check_mark: Answer: -This effect occurs when the lidar has multiple planes that are tilted up or down. In this case, the laser beams of this plane do not lie on a flat plane. Rather, the beams lie on a cone. If the laser beams then hit a wall, the result is a curved course of the lidar points. If the lidar is horizontal and the wall is vertical, this is a hyperbola (see following figure): - -![cone_section](doc/cone_section.png) - - - -This image is generated using the website https://www.intmath.com/plane-analytic-geometry/conic-sections-summary-interactive.php - -Thus, the mathematical laws for a conic section apply, as they are explained e.g. at Wikipedia at https://en.wikipedia.org/wiki/Conic_section. - -## Interlacing - -:question: Question: -How should I interpret the scan rate and lidar resolution from the manual? What is the relationship between ROS point cloud publishing rate and scan frequency here? - -:white_check_mark: Answer: - -The angular resolution and scan frequency is configurable in many lidars such as the LRS-4xxx or MRS-1xxx. Depending on the lidar type, angular resolution and scan frequency can be set in the launch file either via the parameter "scan_cfg_list_entry" or the parameters "ang_res" and "scan_freq". Angular resolution and scan frequency are not independent of each other. If no default settings are used, the values must be selected according to the manual for the respective lidar and set in launch file. - -An increase in resolution is achieved by interlacing by a factor of N. This means that N consecutive scans are rotated by a constant angular offset. Each scan in itself still has the physically given angular resolution and frequency. By concatenating N interlaced scans, the angular resolution is increased by the factor N. - -Example: The default setting of an MRS-1xxx is 0.25 degrees horizontal angular resolution at 50 Hz scan frequency without interlacing and an angular range of 275 degrees in total. I.e. each scan measures the distance at the hor. angles [ ..., 0.000, 0.250, 0.500, 0.750, ... ]. - -If 0.125 degrees horizontal angular resolution is configured at 25 Hz scan frequency, the scans are performed with 2 times interlacing (N=2). Every 2nd scan is horizontally shifted by 0.125 degrees. I.e. each scan measures alternately at the hor. angles [ ..., 0.000, 0.250, 0.500, 0.750, ... ] and [ ... , 0.125, 0.375, 0.625, 0.875 ... ]. 50 single scans per second resp. 25 interlaced scans per second are sent. - -If 0.0625 degrees horizontal angular resolution at 12.5 Hz scan frequency is configured, the scans are performed with 4 times interlacing (N=4). Successive scans are shifted horizontally by 0.0625 degrees each. That is, each scan measures alternately at the hor. angles [ ..., 0.000, 0.250, 0.500, 0.750, ... ], [... , 0.0625, 0.3125, 0.5625, 0.8125 ... ], [... , 0.125, 0.375, 0.625, 0.875 ... ] and [... , 0.1875, 0.4375, 0.6875, 0.9375 ... ]. 50 single scans per second resp. 12.5 interlaced scans per second are sent. - -In interlacing mode, laser scan and point cloud messages are published interlaced, too. In rviz, the higher angular resolution is clearly visible when the decay time is increased. - -With a scan frequency of 50 Hz and 4 active layers, the lidar will send a new scan line each 0.02 seconds. Each layer is scanned after 0.08 seconds resp. with 12.5 Hz (4 layers multiplied by 12.5 Hz = 50 Hz scan frequency). The point cloud accumulates all active layers and is therefore published with 12.5 Hz (i.e. scan frequency divided by number of layers). - -If you check the publishing rate of the point cloud messages of a MRS-1104, you will measure 12.4 Hz, since the scans of 4 layers are accumulated in 1 point cloud message (50 hz scan frequency divided by 4 layers = 12.5 Hz point cloud publishing frequency). The resolution of each single point cloud message is 0.125 [deg]. Only by interleaving 4 consecutive messages you get the high resolution of 0.0625 [deg]. - -See [interlacing.md](doc/interlacing.md) for further informations. diff --git a/GETTINGSTARTED.md b/GETTINGSTARTED.md deleted file mode 100644 index 0a4a9cae..00000000 --- a/GETTINGSTARTED.md +++ /dev/null @@ -1,56 +0,0 @@ -# Getting started - -Run the following steps for a quick start: - -1. Create a workspace (e.g. folder `sick_scan_ws`), clone the sick_scan_xd repository and build sick_generic_caller and shared library: - - * For **Linux without ROS**: Follow the [build instructions for Linux generic without ROS](INSTALL-GENERIC.md#build-on-linux-generic-without-ros) - - * For **Linux with ROS-1**: Follow the [build instructions for Linux ROS1](INSTALL-ROS1.md#build-on-linux-ros1) - - * For **Linux with ROS-2**: Follow the [build instructions for Linux ROS2](INSTALL-ROS2.md#build-on-linux-ros2) - - * For **Windows without ROS**: Follow the [build instructions for Windows without ROS](INSTALL-GENERIC.md#build-on-windows) - - * For **Windows with ROS-2**: Follow the [build instructions for Windows with ROS2](INSTALL-ROS2.md#build-on-windows-ros2) - -2. Connect your lidar. Check the network connection by `ping `. - -3. Run the sick_scan_xd driver: - - * For **Linux without ROS**: Use the sick_scan_xd API and run `sick_scan_xd_api_test hostname:=`, e.g.: - ``` - cd ./sick_scan_ws - export LD_LIBRARY_PATH=.:`pwd`/build:$LD_LIBRARY_PATH # append absolute path to the build folder - ./build/sick_scan_xd_api_test ./sick_scan_xd/launch/sick_tim_7xx.launch hostname:=192.168.0.1 - ``` - - * For **Linux with ROS-1**: Launch sick_scan_xd: `roslaunch sick_scan_xd hostname:=`, e.g.: - ``` - cd ./sick_scan_ws - source ./devel_isolated/setup.bash - roslaunch sick_scan_xd sick_tim_7xx.launch hostname:=192.168.0.1 - ``` - - * For **Linux with ROS-2**: Run `ros2 launch sick_scan_xd hostname:=`, e.g.: - ``` - cd ./sick_scan_ws - source ./install/setup.bash - ros2 launch sick_scan_xd sick_tim_7xx.launch.py hostname:=192.168.0.1 - ``` - - * For **Windows without ROS**: Use the sick_scan_xd API and run `sick_scan_xd_api_test hostname:=`, e.g.: - ``` - cd .\sick_scan_ws\sick_scan_xd - set PATH=.;.\build;..\build\Debug;%PATH% - .\build\Debug\sick_scan_xd_api_test.exe launch/sick_tim_7xx.launch hostname:=192.168.0.1 - ``` - - * For **Windows with ROS-2**: Run `ros2 launch sick_scan_xd hostname:=`, e.g.: - ``` - cd .\sick_scan_ws - call .\install\setup.bat - ros2 launch sick_scan_xd sick_tim_7xx.launch.py hostname:=192.168.0.1 - ``` - - See [Run sick_scan_xd driver](USAGE.md) and [sick_scan_xd API](doc/sick_scan_api/sick_scan_api.md) for configuration and further details. diff --git a/INSTALL-GENERIC.md b/INSTALL-GENERIC.md deleted file mode 100644 index d5ccd5df..00000000 --- a/INSTALL-GENERIC.md +++ /dev/null @@ -1,107 +0,0 @@ -## Build on Linux generic without ROS - -Run the following steps to build sick_scan_xd on Linux (no ROS required): - -1. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): - ``` - mkdir -p ./sick_scan_ws - cd ./sick_scan_ws - ``` - -2. Clone repositories https://github.com/SICKAG/libsick_ldmrs and https://github.com/SICKAG/sick_scan_xd: - ``` - git clone https://github.com/SICKAG/libsick_ldmrs.git - git clone -b master https://github.com/SICKAG/sick_scan_xd.git - ``` - -3. Build libsick_ldmrs (required only once for LDMRS sensors): - ``` - pushd libsick_ldmrs - mkdir -p ./build - cd ./build - cmake -G "Unix Makefiles" .. - make -j4 - sudo make -j4 install - popd - ``` - -4. Build sick_generic_caller and libsick_scan_xd_shared_lib.so: - ``` - mkdir -p ./build - pushd ./build - rm -rf ./* - export ROS_VERSION=0 - cmake -DROS_VERSION=0 -G "Unix Makefiles" ../sick_scan_xd - make -j4 - sudo make -j4 install - popd - ``` - -Note: LDMRS sensors are currently not supported on Raspberry. Build with cmake flag `-DLDMRS=0 -DRASPBERRY=1` on Raspberry: - ``` - cmake -DROS_VERSION=0 -DLDMRS=0 -DRASPBERRY=1 -G "Unix Makefiles" ../sick_scan_xd - ``` - -Note: libsick_ldmrs is only required to support LDMRS sensors. If you do not need or want to support LDMRS, you can skip building libsick_ldmrs. To build sick_generic_caller without LDMRS support, switch off option `BUILD_WITH_LDMRS_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DLDMRS=0`: - ``` - cmake -DROS_VERSION=0 -DLDMRS=0 -G "Unix Makefiles" ../sick_scan_xd - ``` - -Note: To build sick_generic_caller without multiScan100/picoScan100 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 - ``` - -cmake flags can be combined. Use flags `-DLDMRS=0 -DSCANSEGMENT_XD=0` to build without LDMRS and scansegment_xd support: - ``` - cmake -DROS_VERSION=0 -DLDMRS=0 -DSCANSEGMENT_XD=0 -G "Unix Makefiles" ../sick_scan_xd - ``` - -Note: To create source code documentation by doxygen, run -``` -cd ./doxygen -doxygen ./docs/Doxyfile -``` - -## Build on Windows - -To install sick_scan_xd on Windows, follow the steps below: - -1. If not yet done, install Visual Studio. Visual Studio 2019 Community or Professional Edition is recommended. - -2. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): - ``` - mkdir sick_scan_ws - cd sick_scan_ws - ``` - -3. Clone repository https://github.com/SICKAG/sick_scan_xd: - ``` - git clone -b master https://github.com/SICKAG/sick_scan_xd.git - ``` - -4. Build sick_generic_caller and sick_scan_xd_shared_lib.dll with cmake and Visual Studio 2019: - ``` - cd sick_scan_xd - set _os=x64 - set _cmake_string=Visual Studio 16 2019 - set _msvc=Visual Studio 2019 - set _cmake_build_dir=build - if not exist %_cmake_build_dir% mkdir %_cmake_build_dir% - pushd %_cmake_build_dir% - cmake -DROS_VERSION=0 -G "%_cmake_string%" .. - cmake --build . --clean-first --config Debug - cmake --build . --clean-first --config Release - REM open sick_scan_xd.sln in Visual Studio 2019 for development and debugging - popd - ``` - For development or debugging, open file `sick_scan_xd\build\sick_scan_xd.sln` in Visual Studio. To install the library and header in the system folder, run `cmake --build . --target install` with admin priviledges. - -After successful build, binary files `sick_generic_caller.exe` and `sick_scan_xd_shared_lib.dll` are created in folders `sick_scan_xd\build\Debug` and `sick_scan_xd\build\Release`. - -Note: LDMRS sensors are currently not supported on Windows. - -Note: To build sick_generic_caller without multiScan100/picoScan100 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 deleted file mode 100644 index 013a2b55..00000000 --- a/INSTALL-ROS1.md +++ /dev/null @@ -1,88 +0,0 @@ -# Build on Linux ROS1 - -To build resp. install sick_scan_xd on Linux with ROS-1, you can build sick_scan_xd from sources or install prebuilt binaries. - -## Install prebuilt binaries - -Run the following steps to install sick_scan_xd on Linux with ROS 1 noetic: - -``` -sudo apt update -sudo apt-get install ros-noetic-sick-scan-xd -``` - -After successful installation, you can run sick_scan_xd using `roslaunch sick_scan_xd `, e.g. `roslaunch sick_scan_xd sick_picoscan.launch` for picoscan. sick_scan_xd can be removed by `sudo apt-get remove ros-noetic-sick-scan-xd`. - -## Build from sources - -Run the following steps to build sick_scan_xd on Linux with ROS 1: - -1. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): - ``` - mkdir -p ./sick_scan_ws - cd ./sick_scan_ws - ``` - -2. Clone repositories https://github.com/SICKAG/libsick_ldmrs and https://github.com/SICKAG/sick_scan_xd: - ``` - mkdir ./src - pushd ./src - git clone https://github.com/SICKAG/libsick_ldmrs.git - git clone -b master 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: - ``` - source /opt/ros/noetic/setup.bash # replace noetic by your ros distro - catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -Wno-dev - source ./devel_isolated/setup.bash - # source ./install_isolated/setup.bash - ``` - For ROS versions other than noetic, please replace `source /opt/ros/noetic/setup.bash` with your ros distribution. - -Note: LDMRS sensors are currently not supported on Raspberry. Build with cmake flag `-DLDMRS=0 -DRASPBERRY=1` on Raspberry: - ``` - catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -DRASPBERRY=1 -Wno-dev - ``` - -Note: libsick_ldmrs is only required to support LDMRS sensors. If you do not need or want to support LDMRS, you can skip building libsick_ldmrs. To build sick_generic_caller without LDMRS support, switch off option `BUILD_WITH_LDMRS_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call catkin_make_isolated with option `-DLDMRS=0`: - ``` - catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -Wno-dev - ``` - -Note: To build sick_generic_caller without multiScan136/sick_scansegment_xd/picoScan150 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 **without multiScan100/picoScan100 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 multiScan100/picoScan100 support** -``` - catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -DSCANSEGMENT_XD=0 -Wno-dev -``` -* **Without LDMRS-support** and **with multiScan100/picoScan100 support** -``` - catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -Wno-dev -``` -* **with LDMRS-support** and **without multiScan100/picoScan100 support** -``` - catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DSCANSEGMENT_XD=0 -Wno-dev -``` -* **with LDMRS-support** and **with multiScan100/picoScan100 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 deleted file mode 100644 index dc67f89f..00000000 --- a/INSTALL-ROS2.md +++ /dev/null @@ -1,150 +0,0 @@ -## Build on Linux ROS2 - -To build resp. install sick_scan_xd on Linux with ROS-2, you can build sick_scan_xd from sources or install prebuilt binaries. - -## Install prebuilt binaries - -Run the following steps to install sick_scan_xd on Linux with ROS 2 humble: - -``` -sudo apt update -sudo apt-get install ros-humble-sick-scan-xd -``` - -After successful installation, you can run sick_scan_xd using `ros2 launch sick_scan_xd .py`, e.g. `ros2 launch sick_scan_xd sick_multiscan.launch.py` for multiScan. sick_scan_xd can be removed by `sudo apt-get remove ros-humble-sick-scan-xd`. - -Note: ROS-2 humble debian packages require Ubuntu 22 or newer, see https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html for system requirements. - -## Build from sources - -Run the following steps to build sick_scan_xd on Linux with ROS 2: - -1. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): - ``` - mkdir -p ./sick_scan_ws - cd ./sick_scan_ws - ``` - -2. Clone repositories https://github.com/SICKAG/libsick_ldmrs and https://github.com/SICKAG/sick_scan_xd: - ``` - mkdir ./src - pushd ./src - git clone https://github.com/SICKAG/libsick_ldmrs.git - git clone -b master 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: - ``` - source /opt/ros/foxy/setup.bash # replace foxy by your ros distro - colcon build --packages-select libsick_ldmrs --event-handlers console_direct+ - source ./install/setup.bash - colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" --event-handlers console_direct+ - source ./install/setup.bash - ``` - For ROS versions other than foxy, please replace `source /opt/ros/foxy/setup.bash` with your ros distribution. - -Note: LDMRS sensors are currently not supported on Raspberry. Build with cmake flag `-DLDMRS=0 -DRASPBERRY=1` on Raspberry: - ``` - colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DRASPBERRY=0" --event-handlers console_direct+ - ``` - -Note: libsick_ldmrs is only required to support LDMRS sensors. If you do not need or want to support LDMRS, you can skip building libsick_ldmrs. To build sick_generic_caller without LDMRS support, switch off option `BUILD_WITH_LDMRS_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call colcon with option `-DLDMRS=0`: - ``` - colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" --event-handlers console_direct+ - ``` -Note: To build sick_generic_caller without multiScan136/sick_scansegment_xd/picoScan150 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_xd --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 **without multiScan100/picoScan100 support**: - ``` - colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ - ``` - -Note: Depending on the ROS-2 distribution, package diagnostic_updater might not be found (compiler error: `diagnostic_updater.hpp not found`). In this case package diagnostic_updater has to be installed by -``` -sudo apt install ros-${ROS_DISTRO}-diagnostic-updater -sudo apt install ros-${ROS_DISTRO}-diagnostic-msgs -# E.g. to install diagnostic_updater on foxy, run -# sudo apt-get install ros-foxy-diagnostic-updater -# sudo apt install ros-foxy-diagnostic-msgs -``` - -### Summary for the different build options: - -* **Without LDMRS-support** and **without multiScan100/picoScan100 support** - -``` -colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ -``` -* **Without LDMRS-support** and **with multiScan100/picoScan100 support** -``` -colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" --event-handlers console_direct+ -``` -* **with LDMRS-support** and **without multiScan100/picoScan100 support** -``` -colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ -``` -* **with LDMRS-support** and **with multiScan100/picoScan100 support** -``` - colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " --event-handlers console_direct+ -``` - - -Note: To create source code documentation by doxygen, run -``` -cd ./doxygen -doxygen ./docs/Doxyfile -``` - -## Build on Windows ROS2 - -To install sick_scan_xd on Windows with ROS-2, follow the steps below: - -1. If not yet done, install Visual Studio. Visual Studio 2019 Community or Professional Edition is recommended. - -2. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): - ``` - mkdir sick_scan_ws - cd sick_scan_ws - ``` - -3. Clone repository https://github.com/SICKAG/sick_scan_xd: - ``` - mkdir .\src - pushd .\src - git clone -b master https://github.com/SICKAG/sick_scan_xd.git - popd - ``` - -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 sick_scan_xd --cmake-args " -DROS_VERSION=2" --event-handlers console_direct+ - call .\install\setup.bat - ``` - -Note: LDMRS sensors are currently not supported on Windows. - -Note: 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_xd --cmake-args " -DROS_VERSION=2" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ - ``` diff --git a/README.md b/README.md index 52218f45..2062caf0 100644 --- a/README.md +++ b/README.md @@ -1,56 +1,135 @@ -# sick_scan_xd - -This project provides a driver for the SICK LiDARs and Radar sensors mentioned [here](REQUIREMENTS.md). The driver supports both Linux (native, ROS1, ROS2) and Windows (native and ROS2). See the [CHANGELOG.md](CHANGELOG.md) for the latest updates. - -## Table of Contents - -- [Executive Summary](#executive-summary) -- [Supported SICK LIDAR and Radar sensors](REQUIREMENTS.md) -- [Getting started](GETTINGSTARTED.md) -- [Build targets](#build-targets) -- [Build on Linux generic without ROS](INSTALL-GENERIC.md#build-on-linux-generic-without-ros) -- [Build on Linux ROS1](INSTALL-ROS1.md#build-on-linux-ros1) -- [Build on Linux ROS2](INSTALL-ROS2.md#build-on-linux-ros2) -- [Build on Windows](INSTALL-GENERIC.md#build-on-windows) -- [Build on Windows ROS2](INSTALL-ROS2.md#build-on-windows-ros2) -- [USAGE](USAGE.md) -- [Driver API](doc/sick_scan_api/sick_scan_api.md) -- [IMU-Support](#imu-Support) -- [Radar](doc/radar.md) -- [picoScan100/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) -- [Software Overview](#software-overview) -- [Raspberry support](doc/raspberry.md) -- [Interlacing](doc/interlacing.md) -- [FAQ](FAQ.md) -- [Further support](SUPPORT.md) -- [CREDITS](CREDITS.md) - -## Executive Summary - -* sick_scan_xd supports - * ROS1 (Linux) - * ROS2 (Linux and Windows) - * a Driver for generic use (Linux and Windows native) - * a API for C/C++ or python applications - * x64 and ARM-64 architecture -* sick_scan_xd provides a driver for the SICK LiDARs and Radar sensors mentioned [here](REQUIREMENTS.md). -* sick_scan_xd is designed to easily integrate new devices, features and improvements on all targets. -* sick_scan_xd has no dependencies to 3rd party libraries like boost or pthread. -* sick_scan_xd offers all features on all targets if the devices support the features. - -## Repository organization - -The repository supports two main branches. - -The **"master"** branch is the branch that contains official releases that are tagged and versioned and also included in the ROS distribution. - -If you want to work with this official branch, you must explicitly specify this branch in the 'git clone' command by adding "-b master". - -The "develop" branch is the default branch and contains the latest development status. + + +Executive Summary +--- + +This project provides a driver for the SICK LiDARs and Radar sensors mentioned [here](#supported-sick-devices). The driver supports both Linux (native, ROS 1, ROS 2) and Windows (native and ROS 2). See the [CHANGELOG.md](CHANGELOG.md) for the latest updates. + +**Main features and characteristics** +* Support of ROS 1 (Linux), ROS 2 (Linux and Windows) +* Generic C/C++ and python API for usage without ROS (Linux and Windows) +* SLAM support +* Compatible with x64 and ARM64 architecture (incl. Raspberry Pi) +* No dependencies to 3rd party libraries like boost + +Table of Contents +--- + +
+ Expand to full table of contents + +- [Repository organization](#repository-organization) +- [Supported SICK Devices](#supported-sick-devices) +- [Getting started](#getting-started) + - [Detecting SICK devices in the network](#detecting-sick-devices-in-the-network) + - [Change IP address](#change-ip-address) + - [Starting with a new lidar](#starting-with-a-new-lidar) + - [Test connection (Linux)](#test-connection-linux) +- [Building the driver](#building-the-driver) + - [Build on Linux ROS 1](#build-on-linux-ros-1) + - [Install prebuilt binaries](#install-prebuilt-binaries) + - [Build from sources](#build-from-sources) + - [Build on Linux ROS 2](#build-on-linux-ros-2) + - [Install prebuilt binaries](#install-prebuilt-binaries-1) + - [Build from sources](#build-from-sources-1) + - [Build on Windows ROS 2](#build-on-windows-ros-2) + - [Build on Linux generic without ROS](#build-on-linux-generic-without-ros) + - [Build on Windows generic without ROS](#build-on-windows-generic-without-ros) +- [Running the driver](#running-the-driver) + - [Starting Scanner with Specific IP Address](#starting-scanner-with-specific-ip-address) + - [Start Multiple Nodes](#start-multiple-nodes) + - [Common parameters](#common-parameters) + - [Further useful parameters and features](#further-useful-parameters-and-features) + - [ROS services](#ros-services) + - [Driver states, timeouts](#driver-states-timeouts) + - [SOPAS Mode](#sopas-mode) + - [Example Startup Sequence](#example-startup-sequence) +- [Driver features and additional information](#driver-features-and-additional-information) + - [Software Overview](#software-overview) + - [Software structure](#software-structure) + - [Message receiving and message handling](#message-receiving-and-message-handling) + - [sick\_scansegment\_xd](#sick_scansegment_xd) + - [Files and folders](#files-and-folders) + - [Generic Driver API](#generic-driver-api) + - [Overview](#overview) + - [Build and test shared library](#build-and-test-shared-library) + - [Usage example](#usage-example) + - [C-API](#c-api) + - [Useful links](#useful-links) + - [Timestamps and synchronization (Software PLL)](#timestamps-and-synchronization-software-pll) + - [Coordinate transforms](#coordinate-transforms) + - [IMU Support](#imu-support) + - [Encoders](#encoders) + - [Connecting encoders](#connecting-encoders) + - [Example circuit to trigger encoder counts](#example-circuit-to-trigger-encoder-counts) + - [Activation of encoder information](#activation-of-encoder-information) + - [Field Evaluation Information](#field-evaluation-information) + - [Field monitoring messages](#field-monitoring-messages) + - [Visualization with rviz](#visualization-with-rviz) + - [Cola commands](#cola-commands) + - [Tools, emulation and unittests](#tools-emulation-and-unittests) + - [SLAM Support](#slam-support) + - [Introduction](#introduction) + - [Measuring Principle](#measuring-principle) + - [NAV350 ROS 1 SLAM example](#nav350-ros-1-slam-example) + - [NAV350 ROS 2 SLAM example](#nav350-ros-2-slam-example) + - [picoScan100 ROS 1 SLAM example](#picoscan100-ros-1-slam-example) + - [MRS1104 SLAM support](#mrs1104-slam-support) + - [Google Cartographer](#google-cartographer) + - [OctoMap](#octomap) + - [RTAB-Map](#rtab-map) + - [Raspberry Pi Support](#raspberry-pi-support) + - [multiScan100 example](#multiscan100-example) + - [Performance](#performance) + - [Troubleshooting](#troubleshooting) + - [More Tools](#more-tools) +- [Device specific information](#device-specific-information) + - [picoScan100/multiScan100](#picoscan100multiscan100) + - [Configuration](#configuration) + - [IMU support](#imu-support-1) + - [SOPAS support](#sopas-support) + - [Run multiScan100 and picoScan100 simultaneously](#run-multiscan100-and-picoscan100-simultaneously) + - [Visualization](#visualization) + - [PointCloud memory layout](#pointcloud-memory-layout) + - [Customized point clouds](#customized-point-clouds) + - [Customized point clouds on a Raspberry Pi](#customized-point-clouds-on-a-raspberry-pi) + - [MSGPACK validation](#msgpack-validation) + - [Firewall configuration](#firewall-configuration) + - [SOPAS support for sick\_scan\_segment\_xd](#sopas-support-for-sick_scan_segment_xd) + - [TiM781 and TiM781S](#tim781-and-tim781s) + - [TiM240](#tim240) + - [NAV350](#nav350) + - [Process loop](#process-loop) + - [Initialization and setup](#initialization-and-setup) + - [Messages](#messages) + - [Odometry messages](#odometry-messages) + - [Angle compensation](#angle-compensation) + - [MRS6124](#mrs6124) + - [Timing between Layers](#timing-between-layers) + - [MRS600 with RMS1000](#mrs600-with-rms1000) + - [RMS1000](#rms1000) + - [Raw Targets](#raw-targets) + - [Tracking Objects](#tracking-objects) + - [ROS message for Radar](#ros-message-for-radar) + - [Launch Files](#launch-files) + - [Parameter for Radar Usage](#parameter-for-radar-usage) + - [Radar Datagram](#radar-datagram) + - [RMS1000 and LMS1000 combination](#rms1000-and-lms1000-combination) + - [LMS1000 and MRS1000 interlacing](#lms1000-and-mrs1000-interlacing) +- [FAQ](#faq) + - [General](#general) + - [Networking](#networking) + - [multiScan100 / picoScan100](#multiscan100--picoscan100) +- [Troubleshooting and technical support](#troubleshooting-and-technical-support) +- [Creators and contribution](#creators-and-contribution) +- [Keywords](#keywords) + + +
+ +# Repository organization + +The repository supports two main branches. The **"master"** branch is the branch that contains official releases that are tagged and versioned and also included in the ROS distribution. If you want to work with this official branch, you must explicitly specify this branch in the 'git clone' command by adding "-b master". The "develop" branch is the default branch and contains the latest development status. Example: @@ -64,111 +143,4145 @@ Checking out the latest development status: git clone https://github.com/SICKAG/sick_scan_xd.git ``` -## Build targets +# Supported SICK Devices + +The driver supports Ethernet-IPv4-based communication with all of the following SICK products. + +| 2D LiDAR sensors | Part no. | 3D LiDAR sensors | Part no.| RADAR sensors | Part no.| +| ----------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------- | ------------ | -------------------------------------------------------------- | ------- | -------------------------------------------------------------- | +| picoScan100 [(supports native ROS 2 as well)](https://support.sick.com/sick-knowledgebase/article/?code=KA-09438) | [e.g. 1134610](https://www.sick.com/de/en/search?text=1134610) | multiScan100 | [e.g. 1131164](https://www.sick.com/de/en/search?text=1131164) | RMS1000 | [e.g. 1107598](https://www.sick.com/de/en/search?text=1107598) | +| LRS4000 | [e.g. 1098855](https://www.sick.com/de/en/search?text=1098855) | MRS1000 | [e.g. 1081208](https://www.sick.com/de/en/search?text=1081208) | RMS2000 | [e.g. 1129088](https://www.sick.com/de/en/search?text=1129088) | +| TiM2xx | [1104981](https://www.sick.com/de/en/search?text=1104981) | MRS6124 | [6065086](https://www.sick.com/de/en/search?text=6065086) | | | +| TiM5xx | [e.g. 1060445](https://www.sick.com/de/en/search?text=1060445) | LD-MRS | [e.g. 1115128](https://www.sick.com/de/de/p/p662073) | | | +| TiM7xxS | [e.g. 1105052](https://www.sick.com/de/en/search?text=1105052) | | | | | +| TiM7xx | [e.g 1096807](https://www.sick.com/de/en/search?text=1096807) | | | | | +| LMS5xx | [e.g. 1046135](https://www.sick.com/de/en/search?text=1046135) | | | | | +| LMS1000 | [1092445](https://www.sick.com/de/en/search?text=1092445) | | | | | +| LMS1xx | [e.g. 1041114](https://www.sick.com/de/en/search?text=1041114) | | | | | +| LMS4000 | [e.g. 1091423](https://www.sick.com/de/en/search?text=1091423) | | | | | +| LD-LRS | [e.g. 1060831](https://www.sick.com/de/en/search?text=1060831) | | | | | +| LD-OEM | [e.g. 1060828](https://www.sick.com/de/en/search?text=1060828) | | | | | +| NAV3xx | [e.g. 1060834](https://www.sick.com/de/en/search?text=1060834) | | | | | +| NAV2xx | [e.g. 1074308](https://www.sick.com/de/en/search?text=1074308) | | | | | + + +> **_NOTE:_** +> * LD-MRS family is currently not supported on Windows. +> * ROS services require installation of ROS 1 or ROS 2. +> * ROS services are currently not available for LD-MRS. +> * Publishing point cloud data requires ROS 1 or ROS 2. On native Linux and native Windows, point cloud data are exported via API +> * The driver is not tested on MAC + +# Getting started + +Run the following steps for a quick start: + +1. Create a workspace (e.g. folder `sick_scan_ws`), clone the sick_scan_xd repository and build sick_generic_caller and shared library: + + * For **Linux without ROS**: Follow the [build instructions for Linux generic without ROS](#build-on-linux-generic-without-ros) + * For **Linux with ROS 1**: Follow the [build instructions for Linux ROS 1](#build-on-linux-ros-1) + * For **Linux with ROS 2**: Follow the [build instructions for Linux ROS 2](#build-on-linux-ros-2) + * For **Windows without ROS**: Follow the [build instructions for Windows without ROS](#build-on-windows-generic-without-ros) + * For **Windows with ROS 2**: Follow the [build instructions for Windows with ROS 2](#build-on-windows-ros-2) + +2. Connect your lidar. Check the network connection by `ping `. + +3. Run the sick_scan_xd driver: + + For **Linux without ROS**: Use the sick_scan_xd API and run `sick_scan_xd_api_test hostname:=`, e.g.: + ``` + cd ./sick_scan_ws + export LD_LIBRARY_PATH=.:`pwd`/build:$LD_LIBRARY_PATH # append absolute path to the build folder + ./build/sick_scan_xd_api_test ./sick_scan_xd/launch/sick_tim_7xx.launch hostname:=192.168.0.1 + ``` + + For **Linux with ROS 1**: Launch sick_scan_xd: `roslaunch sick_scan_xd hostname:=`, e.g.: + ``` + cd ./sick_scan_ws + source ./devel_isolated/setup.bash + roslaunch sick_scan_xd sick_tim_7xx.launch hostname:=192.168.0.1 + ``` + + For **Linux with ROS 2**: Run `ros2 launch sick_scan_xd hostname:=`, e.g.: + ``` + cd ./sick_scan_ws + source ./install/setup.bash + ros2 launch sick_scan_xd sick_tim_7xx.launch.py hostname:=192.168.0.1 + ``` + + For **Windows without ROS**: Use the sick_scan_xd API and run `sick_scan_xd_api_test hostname:=`, e.g.: + ``` + cd .\sick_scan_ws\sick_scan_xd + set PATH=.;.\build;..\build\Debug;%PATH% + .\build\Debug\sick_scan_xd_api_test.exe launch/sick_tim_7xx.launch hostname:=192.168.0.1 + ``` + + For **Windows with ROS 2**: Run `ros2 launch sick_scan_xd hostname:=`, e.g.: + ``` + cd .\sick_scan_ws + call .\install\setup.bat + ros2 launch sick_scan_xd sick_tim_7xx.launch.py hostname:=192.168.0.1 + ``` + +## Detecting SICK devices in the network + +The Python script +```sick_scan_xd/tools/sick_generic_device_finder/sick_generic_device_finder.py``` +sends a UDP broadcast to which all available scanners respond with a device description. +The variable ```UDP_IP = "192.168.0.255"``` defines the broadcast address used by the script. +If you are using a different IP address configuration on your host pc you have to change this variable according to the broadcast address of your network card. +```ifconfig``` shows the broadcast address for every network adapter. + +## Change IP address + +The IP address of the device can be changed with a customized launch file. The following launch sequence is an example: +```roslaunch sick_scan_xd sick_new_ip.launch hostname:=192.168.0.1 new_IP:=192.168.0.100``` + +The launch file restarts the lidar after the address change and stops the sick_scan_xd node. After a few seconds of booting time the scanner is reachable under the new IP address. The Python script is experimental. It is known that some ethernet adapter are not fully supported. As a fallback solution you can always use the SOPAS ET software under windows. + +## Starting with a new lidar + +The lidar is delivered with a standard IP address, to read or change it the [SICK SOPAS ET](https://www.sick.com/de/de/sopas-engineering-tool-2018/p/p367244) for windows can be used. +When the tool is started, a search is performed which lists all scanners available in the network. -sick_scan_xd can be build on 64-bit Linux and Windows, with and without ROS, with and without LDMRS. The following table shows the allowed combinations and how to build. +![SOPAS start](doc/ipconfig/scanner_found.PNG "SOPAS start") -| **target** | **cmake settings** | **build script** | -|------------|--------------------|------------------| -| Linux, native, LDMRS | BUILD_WITH_LDMRS_SUPPORT ON | cd test/scripts && chmod a+x ./*.bash && ./makeall_linux.bash | -| Linux, native, no LDMRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test/scripts && chmod a+x ./*.bash && ./makeall_linux_no_ldmrs.bash | -| Linux, ROS-1, LDMRS | BUILD_WITH_LDMRS_SUPPORT ON | cd test/scripts && chmod a+x ./*.bash && ./makeall_ros1.bash | -| Linux, ROS-1, no LDMRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test/scripts && chmod a+x ./*.bash && ./makeall_ros1_no_ldmrs.bash | -| Linux, ROS-2, LDMRS | BUILD_WITH_LDMRS_SUPPORT ON | cd test/scripts && chmod a+x ./*.bash && ./makeall_ros2.bash | -| Linux, ROS-2, no LDMRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test/scripts && chmod a+x ./*.bash && ./makeall_ros2_no_ldmrs.bash | -| Windows, native, no LDMRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test\\scripts && make_win64.cmd | -| Windows, ROS-2, no LDMRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test\\scripts && make_ros2.cmd | +Double-click to select the scanner for the project + +![SOPAS select](doc/ipconfig/scanner_added.PNG "SOPAS select") + +Double click on the lidar icon to open the configuration menu of the scanner. Select here the network configuration menu and set the parameters by clicking on the save icon (red arrow) + +![SOPAS save network](doc/ipconfig/set_config.PNG "SOPAS save network") + +To ensure that the settings are stored even after a power cycle, they must be stored in the eeprom. To do this, click on the eeprom icon and confirm the save. + +![SOPAS save eeprom](doc/ipconfig/save_permanent.PNG "SOPAS save eeprom") + +To test the settings under Windows use the commands ```ipconfig``` and ```ping``` in the ```cmd.exe```. Make sure that the lidar and host pc have different IP addresses e.g. 192.168.0.1 for the pc and 192.168.0.2 for the scanner. + +## Test connection (Linux) + +to test the settings under the Linux target system you can use netcat to check if a TCP connection to the scanner can be established +```nc -z -v -w5 $SCANNERIPADDRESS 2112``` +the connection can be successfully established + +![Linux netcat scanner](doc/ipconfig/nc_scanner.PNG "linux netcat scanner") + +unlike a ping, the connection attempt to the host PC is not successful + +![Linux netcat host](doc/ipconfig/nc_win_host.PNG "linux netcat host") + +# Building the driver + +sick_scan_xd can be build on 64-bit Linux and Windows, with and without ROS, with and without LDMRS. The following table shows the allowed combinations and how to build. sick_scan_xd supports 64 bit Linux and Windows, 32 bit systems are not supported. + + +| **target** | **cmake settings** | **build script** | +| -------------------------- | ---------------------------- | ---------------------------------------------------------------------- | +| Linux, native, LD-MRS | BUILD_WITH_LDMRS_SUPPORT ON | cd test/scripts && chmod a+x ./*.bash && ./makeall_linux.bash | +| Linux, native, no LD-MRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test/scripts && chmod a+x ./*.bash && ./makeall_linux_no_ldmrs.bash | +| Linux, ROS 1, LD-MRS | BUILD_WITH_LDMRS_SUPPORT ON | cd test/scripts && chmod a+x ./*.bash && ./makeall_ros1.bash | +| Linux, ROS 1, no LD-MRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test/scripts && chmod a+x ./*.bash && ./makeall_ros1_no_ldmrs.bash | +| Linux, ROS 2, LD-MRS | BUILD_WITH_LDMRS_SUPPORT ON | cd test/scripts && chmod a+x ./*.bash && ./makeall_ros2.bash | +| Linux, ROS 2, no LD-MRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test/scripts && chmod a+x ./*.bash && ./makeall_ros2_no_ldmrs.bash | +| Windows, native, no LD-MRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test\\scripts && make_win64.cmd | +| Windows, ROS 2, no LD-MRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test\\scripts && make_ros2.cmd | If you're using ROS, set your ROS-environment before running one of these scripts, f.e. -* `source /opt/ros/noetic/setup.bash` for ROS-1 noetic, or -* `source /opt/ros/foxy/setup.bash` for ROS-2 foxy, or -* `source /opt/ros/humble/setup.bash` for ROS-2 humble. +`source /opt/ros/noetic/setup.bash` for ROS 1 noetic, or +`source /opt/ros/foxy/setup.bash` for ROS 2 foxy, or +`source /opt/ros/humble/setup.bash` for ROS 2 humble. +See the build descriptions for more details. -See the build descriptions for more details: -* [Build on Linux generic without ROS](INSTALL-GENERIC.md#build-on-linux-generic-without-ros) -* [Build on Linux ROS1](INSTALL-GENERIC.md#build-on-linux-ros1) -* [Build on Linux ROS2](INSTALL-ROS2.md#build-on-linux-ros2) -* [Build on Windows](INSTALL-GENERIC.md#build-on-windows) -* [Build on Windows ROS2](INSTALL-ROS2.md#build-on-windows-ros2) +## Build on Linux ROS 1 -sick_scan_xd supports 64 bit Linux and Windows, 32 bit systems are not supported. +To build resp. install sick_scan_xd on Linux with ROS 1, you can build sick_scan_xd from sources or install prebuilt binaries. -## Driver API +### Install prebuilt binaries -sick_scan_xd provides a C API, which can be used by any programming language with C-bindings, e.g. in C/C++ or python applications. See [sick_scan_api.md](doc/sick_scan_api/sick_scan_api.md) for further details. +Run the following steps to install sick_scan_xd on Linux with ROS 1 noetic: -## IMU Support +``` +sudo apt update +sudo apt-get install ros-noetic-sick-scan-xd +``` -Devices of the MRS6xxx and MRS1xxx series are available with an optionally built-in IMU. -Further information on the implementation and use of the experimental Imu support can be found on the [Imu page](doc/IMU.md). +After successful installation, you can run sick_scan_xd using `roslaunch sick_scan_xd `, e.g. `roslaunch sick_scan_xd sick_picoscan.launch` for picoScan. sick_scan_xd can be removed by `sudo apt-get remove ros-noetic-sick-scan-xd`. -## Radar support +### Build from sources -See [radar documentation](doc/radar.md) for RMSxxxx support. +Run the following steps to build sick_scan_xd on Linux with ROS 1: -## multiScan100 support +1. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): + ``` + mkdir -p ./sick_scan_ws + cd ./sick_scan_ws + ``` -See [sick_scan_segment_xd](doc/sick_scan_segment_xd.md) for multiScan100 support. +2. Clone repositories https://github.com/SICKAG/libsick_ldmrs and https://github.com/SICKAG/sick_scan_xd : + ``` + mkdir ./src + pushd ./src + git clone https://github.com/SICKAG/libsick_ldmrs.git + git clone -b master 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: + ``` + source /opt/ros/noetic/setup.bash # replace noetic by your ros distro + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -Wno-dev + source ./devel_isolated/setup.bash + # source ./install_isolated/setup.bash + ``` + For ROS versions other than noetic, please replace `source /opt/ros/noetic/setup.bash` with your ros distribution. -## Software PLL +LD-MRS sensors are currently not supported on Raspberry. Build with cmake flag `-DLDMRS=0 -DRASPBERRY=1` on Raspberry: + ``` + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -DRASPBERRY=1 -Wno-dev + ``` -A software pll is used to convert LiDAR timestamps in ticks to the ros system time. See [software_pll](doc/software_pll.md) for further details. +libsick_ldmrs is only required to support LD-MRS sensors. If you do not need or want to support LDMRS, you can skip building libsick_ldmrs. To build sick_generic_caller without LD-MRS support, switch off option `BUILD_WITH_LDMRS_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call catkin_make_isolated with option `-DLDMRS=0`: + ``` + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -Wno-dev + ``` -## Field Evaluation Information +To build sick_generic_caller without multiScan136/sick_scansegment_xd/picoScan150 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 + ``` -The LMS1xx, LMS5xx, TiM7xx and TiM7xxS families support extensions for field monitoring. See [field_monitoring_extensions](doc/field_monitoring_extensions.md) for further details. +cmake flags can be combined. Use flags `-DLDMRS=0 -DSCANSEGMENT_XD=0` to build **without LDMRS** and **without multiScan100/picoScan100 support**: + ``` + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -DSCANSEGMENT_XD=0 -Wno-dev + ``` -## Run sick_scan_xd driver +By default, sick_scan_xd builds with compiler flag `-O3` (optimization level for max speed). The optimization level can be overwritten (e.g. for debugging) by cmake flag `-DO=0` (compiler flags `-g -O0`), `-DO=1` (for compiler flags `-O1`) or `-DO=2` (for compiler flags `-O2`). -See [USAGE](USAGE.md) how to run and configure the sick_scan_xd driver. +**Build options** -## Software Overview +**Without LDMRS-support** and **without multiScan100/picoScan100 support** +``` + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -DSCANSEGMENT_XD=0 -Wno-dev +``` +**Without LDMRS-support** and **with multiScan100/picoScan100 support** +``` + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -Wno-dev +``` +**with LDMRS-support** and **without multiScan100/picoScan100 support** +``` + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DSCANSEGMENT_XD=0 -Wno-dev +``` +**with LDMRS-support** and **with multiScan100/picoScan100 support** +``` + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -Wno-dev +``` + +To create source code documentation by doxygen, run + +``` +cd ./doxygen +doxygen ./docs/Doxyfile +``` + +## Build on Linux ROS 2 + +To build resp. install sick_scan_xd on Linux with ROS 2, you can build sick_scan_xd from sources or install prebuilt binaries. + +### Install prebuilt binaries + +Run the following steps to install sick_scan_xd on Linux with ROS 2 humble: + +``` +sudo apt update +sudo apt-get install ros-humble-sick-scan-xd +``` + +After successful installation, you can run sick_scan_xd using `ros2 launch sick_scan_xd .py`, e.g. `ros2 launch sick_scan_xd sick_multiscan.launch.py` for multiScan. sick_scan_xd can be removed by `sudo apt-get remove ros-humble-sick-scan-xd`. + +> **_NOTE:_** ROS 2 humble Debian packages require Ubuntu 22 or newer, see https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html for system requirements. + +### Build from sources + +Run the following steps to build sick_scan_xd on Linux with ROS 2: + +1. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): + ``` + mkdir -p ./sick_scan_ws + cd ./sick_scan_ws + ``` + +2. Clone repositories https://github.com/SICKAG/libsick_ldmrs and https://github.com/SICKAG/sick_scan_xd : + ``` + mkdir ./src + pushd ./src + git clone https://github.com/SICKAG/libsick_ldmrs.git + git clone -b master 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: + ``` + source /opt/ros/foxy/setup.bash # replace foxy by your ros distro + colcon build --packages-select libsick_ldmrs --event-handlers console_direct+ + source ./install/setup.bash + colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" --event-handlers console_direct+ + source ./install/setup.bash + ``` + For ROS versions other than foxy, please replace `source /opt/ros/foxy/setup.bash` with your ros distribution. + +LD-MRS sensors are currently not supported on Raspberry. Build with cmake flag `-DLDMRS=0 -DRASPBERRY=1` on Raspberry: + ``` + colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DRASPBERRY=0" --event-handlers console_direct+ + ``` + +libsick_ldmrs is only required to support LD-MRS sensors. If you do not need or want to support LDMRS, you can skip building libsick_ldmrs. To build sick_generic_caller without LD-MRS support, switch off option `BUILD_WITH_LDMRS_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call colcon with option `-DLDMRS=0`: + ``` + colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" --event-handlers console_direct+ + ``` +To build sick_generic_caller without multiScan136/sick_scansegment_xd/picoScan150 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_xd --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 **without multiScan100/picoScan100 support**: + ``` + colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ + ``` + +Depending on the ROS 2 distribution, package diagnostic_updater might not be found (compiler error: `diagnostic_updater.hpp not found`). In this case package diagnostic_updater has to be installed by +``` +sudo apt install ros-${ROS_DISTRO}-diagnostic-updater +sudo apt install ros-${ROS_DISTRO}-diagnostic-msgs +# E.g. to install diagnostic_updater on foxy, run +# sudo apt-get install ros-foxy-diagnostic-updater +# sudo apt install ros-foxy-diagnostic-msgs +``` + +By default, sick_scan_xd builds with compiler flag `-O3` (optimization level for max speed). The optimization level can be overwritten (e.g. for debugging) by cmake flag `-DO=0` (compiler flags `-g -O0`), `-DO=1` (for compiler flags `-O1`) or `-DO=2` (for compiler flags `-O2`). + +**Build options** + +**Without LDMRS-support** and **without multiScan100/picoScan100 support** + +``` +colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ +``` + +**Without LDMRS-support** and **with multiScan100/picoScan100 support** +``` +colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" --event-handlers console_direct+ +``` + +**with LDMRS-support** and **without multiScan100/picoScan100 support** +``` +colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ +``` + +**with LDMRS-support** and **with multiScan100/picoScan100 support** +``` + colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " --event-handlers console_direct+ +``` + +> **_NOTE:_** To create source code documentation by doxygen, run +``` +cd ./doxygen +doxygen ./docs/Doxyfile +``` + +## Build on Windows ROS 2 + +To install sick_scan_xd on Windows with ROS 2, follow the steps below: + +1. If not yet done, install Visual Studio. Visual Studio 2019 Community or Professional Edition is recommended. + +2. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): + ``` + mkdir sick_scan_ws + cd sick_scan_ws + ``` + +3. Clone repository https://github.com/SICKAG/sick_scan_xd : + ``` + mkdir .\src + pushd .\src + git clone -b master https://github.com/SICKAG/sick_scan_xd.git + popd + ``` + +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 + ``` + 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 + ``` + 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 sick_scan_xd --cmake-args " -DROS_VERSION=2" --event-handlers console_direct+ + call .\install\setup.bat + ``` + +> **_NOTE:_** +> * LD-MRS sensors are currently not supported on Windows. +> * 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_xd --cmake-args " -DROS_VERSION=2" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+ + ``` + +## Build on Linux generic without ROS + +Run the following steps to build sick_scan_xd on Linux (no ROS required): + +1. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): + ``` + mkdir -p ./sick_scan_ws + cd ./sick_scan_ws + ``` + +2. Clone repositories https://github.com/SICKAG/libsick_ldmrs and https://github.com/SICKAG/sick_scan_xd : + ``` + git clone https://github.com/SICKAG/libsick_ldmrs.git + git clone -b master https://github.com/SICKAG/sick_scan_xd.git + ``` + +3. Build libsick_ldmrs (required only once for LD-MRS sensors): + ``` + pushd libsick_ldmrs + mkdir -p ./build + cd ./build + cmake -G "Unix Makefiles" .. + make -j4 + sudo make -j4 install + popd + ``` + +4. Build sick_generic_caller and libsick_scan_xd_shared_lib.so: + ``` + mkdir -p ./build + pushd ./build + rm -rf ./* + export ROS_VERSION=0 + cmake -DROS_VERSION=0 -G "Unix Makefiles" ../sick_scan_xd + make -j4 + sudo make -j4 install + popd + ``` + +LD-MRS sensors are currently not supported on Raspberry. Build with cmake flag `-DLDMRS=0 -DRASPBERRY=1` on Raspberry: + ``` + cmake -DROS_VERSION=0 -DLDMRS=0 -DRASPBERRY=1 -G "Unix Makefiles" ../sick_scan_xd + ``` + +libsick_ldmrs is only required to support LD-MRS sensors. If you do not need or want to support LDMRS, you can skip building libsick_ldmrs. To build sick_generic_caller without LD-MRS support, switch off option `BUILD_WITH_LDMRS_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DLDMRS=0`: + ``` + cmake -DROS_VERSION=0 -DLDMRS=0 -G "Unix Makefiles" ../sick_scan_xd + ``` + +To build sick_generic_caller without multiScan100/picoScan100 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 + ``` + +cmake flags can be combined. Use flags `-DLDMRS=0 -DSCANSEGMENT_XD=0` to build without LD-MRS and scansegment_xd support: + ``` + cmake -DROS_VERSION=0 -DLDMRS=0 -DSCANSEGMENT_XD=0 -G "Unix Makefiles" ../sick_scan_xd + ``` + +By default, sick_scan_xd builds with compiler flag `-O3` (optimization level for max speed). The optimization level can be overwritten (e.g. for debugging) by cmake flag `-DO=0` (compiler flags `-g -O0`), `-DO=1` (for compiler flags `-O1`) or `-DO=2` (for compiler flags `-O2`). + +> **_NOTE:_** To create source code documentation by doxygen, run +``` +cd ./doxygen +doxygen ./docs/Doxyfile +``` + +## Build on Windows generic without ROS + +To install sick_scan_xd on Windows, follow the steps below: + +1. If not yet done, install Visual Studio. Visual Studio 2019 Community or Professional Edition is recommended. + +2. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): + ``` + mkdir sick_scan_ws + cd sick_scan_ws + ``` + +3. Clone repository https://github.com/SICKAG/sick_scan_xd : + ``` + git clone -b master https://github.com/SICKAG/sick_scan_xd.git + ``` + +4. Build sick_generic_caller and sick_scan_xd_shared_lib.dll with cmake and Visual Studio 2019: + ``` + cd sick_scan_xd + set _os=x64 + set _cmake_string=Visual Studio 16 2019 + set _msvc=Visual Studio 2019 + set _cmake_build_dir=build + if not exist %_cmake_build_dir% mkdir %_cmake_build_dir% + pushd %_cmake_build_dir% + cmake -DROS_VERSION=0 -G "%_cmake_string%" .. + cmake --build . --clean-first --config Debug + cmake --build . --clean-first --config Release + REM open sick_scan_xd.sln in Visual Studio 2019 for development and debugging + popd + ``` + For development or debugging, open file `sick_scan_xd\build\sick_scan_xd.sln` in Visual Studio. To install the library and header in the system folder, run `cmake --build . --target install` with admin privileges. + +After successful build, binary files `sick_generic_caller.exe` and `sick_scan_xd_shared_lib.dll` are created in folders `sick_scan_xd\build\Debug` and `sick_scan_xd\build\Release`. + +> **_NOTE:_** +> * LD-MRS sensors are currently not supported on Windows. +> * To build sick_generic_caller without multiScan100/picoScan100 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%" .. + ``` + +# Running the driver + +The sick_scan_xd driver can be started on the command line by `sick_generic_caller [hostname:=]`. The start process varies slightly depending on the target OS: + +On native Linux without ROS, call + + ```sick_generic_caller ``` + +On Linux with ROS 1, call + + ```./devel_isolated/setup.bash``` + + ```roslaunch sick_scan_xd ``` + +On Linux with ROS 2, call + + ```source ./install/setup.bash``` + + ```ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/``` + +On native Windows without ROS, call + + ```sick_generic_caller ``` + +On Windows with ROS 2, call + + ```call .\install\setup.bat``` + + ```ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/``` + +Use the following commands to run the sick_scan_xd driver for a specific scanner type: + +- For MRS6124: + * Linux native: `sick_generic_caller sick_mrs_6xxx.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_mrs_6xxx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_mrs_6xxx.launch` + * Windows native: `sick_generic_caller sick_mrs_6xxx.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_mrs_6xxx.launch` +- For MRS1104: + * Linux native: `sick_generic_caller sick_mrs_1xxx.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_mrs_1xxx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd 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_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_mrs_1xxx.launch` +- For LMS1104 with firmware 1.x: + * Linux native: `sick_generic_caller sick_lms_1xxx.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_lms_1xxx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd 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_xd 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_xd sick_lms_1xxx_v2.launch` + * Linux ROS 2: `ros2 run sick_scan_xd 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_xd 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_xd sick_tim_240.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_240.launch` + * Windows native: `sick_generic_caller sick_tim_240.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_240.launch` +- For TiM5xx-family: + * Linux native: `sick_generic_caller sick_tim_5xx.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_tim_5xx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_5xx.launch` + * Windows native: `sick_generic_caller sick_tim_5xx.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_5xx.launch` +- For TiM7xx-family (no safety scanner): + * Linux native: `sick_generic_caller sick_tim_7xx.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_tim_7xx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch` + * Windows native: `sick_generic_caller sick_tim_7xx.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch` +- For TiM7xxS-family (safety scanner): + * Linux native: `sick_generic_caller sick_tim_7xxS.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_tim_7xxS.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xxS.launch` + * Windows native: `sick_generic_caller sick_tim_7xxS.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xxS.launch` +- For LMS1xx-family: + * Linux native: `sick_generic_caller sick_lms_1xx.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_lms_1xx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_1xx.launch` + * Windows native: `sick_generic_caller sick_lms_1xx.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_1xx.launch` +- For LMS5xx-family: + * Linux native: `sick_generic_caller sick_lms_5xx.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_lms_5xx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_5xx.launch` + * Windows native: `sick_generic_caller sick_lms_5xx.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_5xx.launch` +- For LMS4xxx-family: + * Linux native: `sick_generic_caller sick_lms_4xxx.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_lms_4xxx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_4xxx.launch` + * Windows native: `sick_generic_caller sick_lms_4xxx.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_4xxx.launch` +- For LRS4000: + * Linux native: `sick_generic_caller sick_lrs_4xxx.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_lrs_4xxx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_4xxx.launch` + * Windows native: `sick_generic_caller sick_lrs_4xxx.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_4xxx.launch` +- For LDMRS-family: + * Linux native: `sick_generic_caller sick_ldmrs.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_ldmrs.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_ldmrs.launch` + * Note that LD-MRS are currently not supported on Windows +- For LRS36x0: + * Linux native: `sick_generic_caller sick_lrs_36x0.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_lrs_36x0.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x0.launch` + * Windows native: `sick_generic_caller sick_lrs_36x0.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x0.launch` +- For LRS36x0 mounted upside down: + * Linux native: `sick_generic_caller sick_lrs_36x0_upside_down.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_lrs_36x0_upside_down.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x0_upside_down.launch` + * Windows native: `sick_generic_caller sick_lrs_36x0_upside_down.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x0_upside_down.launch` +
For upside down mounted devices, the point cloud 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 LRS36x1: + * Linux native: `sick_generic_caller sick_lrs_36x1.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_lrs_36x1.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x1.launch` + * Windows native: `sick_generic_caller sick_lrs_36x1.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x1.launch` +- For LRS36x1 mounted upside down: + * Linux native: `sick_generic_caller sick_lrs_36x1_upside_down.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_lrs_36x1_upside_down.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x1_upside_down.launch` + * Windows native: `sick_generic_caller sick_lrs_36x1_upside_down.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x1_upside_down.launch` + +
For upside down mounted devices, the point cloud 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_xd sick_oem_15xx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd 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_xd 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_xd sick_nav_2xx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_2xx.launch` + * Windows native: `sick_generic_caller sick_nav_2xx.launch` + * Windows ROS 2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_2xx.launch` +- For NAV310: + * Linux native: `sick_generic_caller sick_nav_31x.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_nav_31x.launch` + * Linux ROS 2: `ros2 run sick_scan_xd 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_xd 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_xd sick_nav_350.launch` + * Linux ROS 2: `ros2 run sick_scan_xd 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_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_350.launch` +- For RMS-family (RMS1xxx, RMS2xxx): + * Linux native: `sick_generic_caller sick_rms_xxxx.launch` + * Linux ROS 1: `roslaunch sick_scan_xd sick_rms_xxxx.launch` + * Linux ROS 2: `ros2 run sick_scan_xd 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_xd 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_xd sick_multiscan.launch hostname:= udp_receiver_ip:=` + * Linux ROS 2: `ros2 launch sick_scan_xd 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_xd 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). +- For picoScan150: + * Linux native: `sick_generic_caller sick_picoscan.launch hostname:= udp_receiver_ip:=` + * Linux ROS 1: `roslaunch sick_scan_xd sick_picoscan.launch hostname:= udp_receiver_ip:=` + * Linux ROS 2: `ros2 launch sick_scan_xd sick_picoscan.launch.py hostname:= udp_receiver_ip:=` + * Windows native: `sick_generic_caller sick_picoscan.launch hostname:= udp_receiver_ip:=` + * Windows ROS 2: `ros2 launch sick_scan_xd sick_picoscan.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 command line options are + +- `hostname:=` to connect to a sensor with a given IP address. Default value is always the factory default IP address of the scanner. + +Further (common and scanner specific) options can be set via launch file, see [Common parameters](#common-parameters) and configure the settings in the launch file corresponding to the scanner type. + +> **_NOTE:_** After modifying a launch-file, it has to be installed by running `catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -Wno-dev` +to be located and used by `roslaunch`. + +On ROS 2 you can launch sick_generic_caller by python launch files, too. Use +``` +ros2 launch sick_scan_xd .launch.py := +``` +E.g. for LMS-5xx: `ros2 launch sick_scan_xd sick_lms_5xx.launch.py hostname:=192.168.0.1` + +The launch.py-files on ROS 2 passes the corresponding launch-file to the driver: [sick_lms_5xx.launch.py](launch/sick_lms_5xx.launch.py) gives an example for LMS-5xx. Parameter can be overwritten +* either by command line, e.g.
`ros2 launch sick_scan_xd sick_lms_5xx.launch.py hostname:=192.168.0.1`, +* or by passing additional arguments in the launch.py-file, e.g.
`node = Node(package='sick_scan_xd', executable='sick_generic_caller', arguments=[launch_file_path, 'hostname:=192.168.0.1'])` + +## Starting Scanner with Specific IP Address + +To start the scanner with a specific IP address, option `hostname:=` can be used. +The hostname is the ip-address of the scanner, e.g. +``` +sick_generic_caller sick_tim_5xx.launch hostname:=192.168.0.71 # Linux native +roslaunch sick_scan_xd sick_tim_5xx.launch hostname:=192.168.0.71 # Linux ROS 1 +ros2 run sick_scan_xd sick_generic_caller sick_tim_5xx.launch hostname:=192.168.0.71 # Linux ROS 2 +sick_generic_caller sick_tim_5xx.launch hostname:=192.168.0.71 # Windows native +ros2 run sick_scan_xd sick_generic_caller sick_tim_5xx.launch hostname:=192.168.0.71 # Windows ROS 2 +``` + +## Start Multiple Nodes + +Multiple nodes can be started to support multiple sensors. In this case, multiple instances of sick_scan_xd have to be started, each node with different name and topic. ROS 1 example to run two TiM 7xx devices with ip address `192.168.0.1` and `192.168.0.2`: + +``` +roslaunch sick_scan_xd sick_tim_7xx.launch nodename:=sick_tim_7xx_1 hostname:=192.168.0.1 cloud_topic:=cloud_1 & +roslaunch sick_scan_xd sick_tim_7xx.launch nodename:=sick_tim_7xx_2 hostname:=192.168.0.2 cloud_topic:=cloud_2 & +``` + +On Linux with ROS 1, multiple nodes to support multiple sensors can be started by one launch file, too. +Take the launch file [sick_tim_5xx_twin.launch](launch/sick_tim_5xx_twin.launch) as an example. +Remapping the scan and cloud topics is essential to distinguish the scandata and provide TF information. + +ROS 2 example to run two TiM 7xx devices with IP address `192.168.0.1` and `192.168.0.2`: + +``` +ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch nodename:=sick_tim_7xx_1 hostname:=192.168.0.1 cloud_topic:=cloud_1 & +ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch nodename:=sick_tim_7xx_2 hostname:=192.168.0.2 cloud_topic:=cloud_2 & +``` + +To support multiple sensors, sick_scan_xd has to be started multiple times, with one sick_scan_xd-node for each sensor. By default, each sick_scan_xd-node connects to "192.168.0.1" and publishes its point cloud on topic "cloud". Therefore both the node name, the ip-address of the sensor and the point cloud topic have to be configured differently for each node. + +Node name, ip-address and point cloud topic can be configured in the launch-file or by command line argument: + +Topic, nodename and ip configuration in a launch-file (example for TiM7xx): + + ``` + + + + + + + + + + ``` + +Topic, node name and ip configuration by command line (ROS 1 example for TiM7xx): + + ``` + roslaunch sick_scan_xd sick_tim_7xx.launch nodename:=sick_tim_7xx_1 hostname:=192.168.0.1 cloud_topic:=cloud_1 + roslaunch sick_scan_xd sick_tim_7xx.launch nodename:=sick_tim_7xx_2 hostname:=192.168.0.2 cloud_topic:=cloud_2 + ``` + +Topic, node name and IP configuration by command line (ROS 2 example for TiM7xx): + + ``` + ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch nodename:=sick_tim_7xx_1 hostname:=192.168.0.1 cloud_topic:=cloud_1 + ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch nodename:=sick_tim_7xx_2 hostname:=192.168.0.2 cloud_topic:=cloud_2 + ``` + +Scripts [run_linux_ros1_simu_tim7xx_twin.bash](test/scripts/run_linux_ros1_simu_tim7xx_twin.bash) and [run_linux_ros2_simu_tim7xx_twin.bash](test/scripts/run_linux_ros2_simu_tim7xx_twin.bash) show a complete example with emulation of two TiM7xx sensors and two sick_scan_xd nodes running concurrently using different nodenames and topics. + +To run two multiScan100 or picoScan100 devices simultanously, each sick_scan_xd node must be configured with different lidar ip addresses and udp ports, different node names, different ros topics and frame ids for each point cloud. Therefore the following launch file parameter should be overwritten by individual settings for each lidar: +* "hostname": e.g. "192.168.0.190" and "192.168.0.98" +* "nodename": e.g. sick_picoscan0" and "sick_picoscan1" +* "publish_frame_id": e.g. "world0" and "world1" +* "publish_laserscan_segment_topic": e.g. "scan0_segment" and "scan1_segment" +* "publish_laserscan_fullframe_topic": e.g. "scan0_fullframe" and "scan1_fullframe" +* "imu_topic": e.g. "imu0" and "imu1" +* "udp_port": e.g. "56661" and "56662" +* "imu_udp_port": e.g. "7503" and "7504" +* individual topics and frame ids for each customized point cloud, e.g. + * replace all "topic=/cloud_" by "topic=/cloud0_" resp. "topic=/cloud1_" + * replace all "frameid=world" by "frameid=world0" resp. "frameid=world1" +It is recommend to first verify the launch file configurations separately for each picoScan100 before running them simultanously. + +For picoScan100 and multiScan, parameter udp_receiver_ip must be set to the ip address of the PC running sick_scan_xd. It is recommend to use ip addresses in the same subnet. + +> **_NOTE:_** The sick_scan_xd API does not support running multiple lidars simultaneously in a single process.** Currently the sick_scan_xd API does not support the single or multi-threaded use of 2 or more lidars in one process, since the sick_scan_xd library is not guaranteed to be thread-safe. To run multiple lidars simultaneously, we recommend using ROS or running sick_scan_xd in multiple and separate processes, so that each process serves one sensor. + +## Common parameters + +For the launch-file settings and the tag/values pairs the following keywords are supported: + +| Keyword | Meaning | Default value | Hint | +| -------------------------- | ------------------------- | ------------- | ---------------------------------------------------------------- | +| scanner_type | Scanner family | ??? | see list above | +| min_ang | Start scan angle in [rad] | -2.3998277 | | +| max_ang | End scan angle in [rad] | +2.3998277 | | +| intensity_resolution_16bit | Switch between 8Bit/16Bit | "false" | do not change | +| hostname | Ip address of scanner | 192.168.0.1 | change to scanner ip address in your network (see faq) | +| port | port number | 2112 | do not change, check firewall rules if there is blocking traffic | +| timelimit | Timelimit in [sec] | 5 | do not change | + +- `scanner_type` + Name of the used scanner. Usually this is also the name of the launch file. This entry is used to differentiate + between the various scanner properties within the software code. + +- `hostname` + IP-address of the scanner (default: 192.168.0.1) + +- `port` + IP-port of the scanner (default: 2112) + +- `min_ang` + Start angle in [rad] + +- `max_ang` + End angle in [rad] + +- `use_binary_protocol` + Switch between SOPAS Binary and SOPAS ASCII protocol + +- `intensity` + Enable or disable transport of intensity values + +- `intensity_resolution_16bit` + If true, the intensity values is transferred as 16 bit value. If false, as 8 bit value. + +- `min_intensity` + If min_intensity > 0, all range values in a LaserScan message are set to infinity, if their intensity value is below min_intensity + +- `cloud_topic` + Topic name of the published pointcloud2 data + +- `frame_id` + Frame id used for the published data + +Tag/value pairs of the command line overwrite settings in the launch file. +The use of the parameters can be looked up in the launch files. This is also recommended as a starting point. + +## Further useful parameters and features + +- **Timestamps**: If parameter`sw_pll_only_publish` is true (default), an internal Software PLL is used to sync the scan generation timestamps to system timestamps. See [Timestamps and synchronization (Software PLL)](#timestamps-and-synchronization-software-pll) for details. + +- **Angle compensation**: For highest angle accuracy the NAV-Lidar series supports an [angle compensation mechanism](#angle-compensation). + +- **Angle correction**: MRS1000 lidars transmit accurate azimuth angles for each scan point. Therefore, the stride (angle increment) of the MRS1000 azimuth angles in polar and cartesian point clouds is not exactly constant. Since laserscan messages assume a constant angle increment, scan points in point cloud and laserscan messages have slightly different azimuth angles. + +- **Field monitoring**: The **LMS1xx**, **LMS5xx**, **TiM7xx** and **TiM7xxS** families have [extended settings for field monitoring](#field-evaluation-information). + +- **Radar devices**: For radar devices (RMS1000/RMS2000), 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 launch file: + ``` + + + ``` + By default, radar objects are tracked. + +- **Coordinate transform**: An optional coordinate transform can be applied to the point cloud. See [coordinate transforms](#coordinate-transforms)) for details. + +## ROS services + +On ROS 1 and ROS 2, services can be used to send COLA commands to the sensor. This can be very helpful for diagnosis, e.g. by querying the device status or its id. + +Use the following examples to run a cola command on ROS 1: +``` +rosservice call /sick_lms_5xx/ColaMsg "{request: 'sMN IsSystemReady'}" +rosservice call /sick_lms_5xx/ColaMsg "{request: 'sRN SCdevicestate'}" +rosservice call /sick_lms_5xx/ColaMsg "{request: 'sEN LIDinputstate 1'}" +rosservice call /sick_lms_5xx/ColaMsg "{request: 'sEN LIDoutputstate 1'}" +rosservice call /sick_lms_5xx/ColaMsg "{request: 'sMN LMCstartmeas'}" +rosservice call /sick_lms_5xx/SCdevicestate "{}" # query device state +rosservice call /sick_lms_5xx/SCreboot "{}" # execute a software reset on the device +rosservice call /sick_lms_5xx/SCsoftreset "{}" # save current parameter and shut down device +``` + +Use the following examples to run a cola command on ROS 2: +``` +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN IsSystemReady'}" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN SCdevicestate'}" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sEN LIDinputstate 1'}" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sEN LIDoutputstate 1'}" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN LMCstartmeas'}" +ros2 service call /SCdevicestate sick_scan_xd/srv/SCdevicestateSrv "{}" # query device state +ros2 service call /SCreboot sick_scan_xd/srv/SCrebootSrv "{}" # execute a software reset on the device +ros2 service call /SCsoftreset sick_scan_xd/srv/SCsoftresetSrv "{}" # save current parameter and shut down device +``` + +Use ros service `SickScanExit` to stop the scanner and driver: +``` +rosservice call /sick_nav_31x/SickScanExit "{}" # stop scanner and driver on ROS 1 +ros2 service call /SickScanExit sick_scan_xd/srv/SickScanExitSrv "{}" # stop scanner and driver on ROS 2 +``` + +> **_NOTE:_** +> * The COLA commands are sensor specific. See the user manual and telegram listing for further details. +> * ROS services require installation of ROS 1 or ROS 2, i.e. services for Cola commands are currently not supported on native Linux or native Windows. +> * ROS services are currently not available for the LDMRS. +> * ROS service "ColaMsg" should only be used for diagnosis. It is not recommended to change the lidar settings while the driver is running. Otherwise the driver settings can become different or inconsistant to the lidar settings. Restart the driver after changing lidar settings by SOAPS ET or SOPAS commands. +> * Some SOPAS commands like `sMN SetAccessMode 3 F4724744` stop the current measurement. In this case, the driver restarts after a timeout (5 seconds by default). To process those SOPAS commands without restart, you can +> * send `sMN LMCstartmeas` and `sMN Run` to switch again into measurement mode within the timeout, or +> * increase the driver timeout `read_timeout_millisec_default` in the launch-file. + +Additional services can be available for specific lidars. Service "GetContaminationResult" is e.g. available for MRS1xxx, LMS1000 and multiScan: +``` +# ROS 1 example for service GetContaminationResult (LMS 1xxx) +rosservice call /sick_lms_1xxx/GetContaminationResult "{}" +# ROS 2 example for service GetContaminationResult (LMS 1xxx) +ros2 service call /GetContaminationResult sick_scan_xd/srv/GetContaminationResultSrv "{}" +``` + +Example sequence with stop and start measurement to set a particle filter (TiM-7xxx on ROS 1): +``` +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LFPparticle'}" # response: "sRA LFPparticle \\x00\\x01\\xf4" +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sWN LFPparticle 0101F4'}" # response: "sWA LFPparticle" +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN LMCstartmeas'}" +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN Run'}" +``` + +## Driver states, timeouts + +The driver runs in two different states: + +1. Initialization: The scanner is initialized and configured by a list of sopas commands + +2. Measurement: The scanner is operational, scandata are transmitted and the point cloud is published. +After start, the driver enters initialization mode. After successful initialization, the driver switches automatically into measurement mode. + +The communication between driver and scanner is monitored. In case of communication timeouts, e.g. due to network problems, the TCP connection is reset and the scanner is re-initialized. The driver uses 3 different timeouts (i.e time since last message received from lidar): + +1. In measurement mode: If no messages arrive for 5 seconds [timeout 0], the TCP/IP connection is closed. After a short delay, the tcp connection is reopened and the driver switches to initialization mode and reinitializes the Lidar. + +2. In initialization mode: If no messages received after 120 sec [Timeout 1] the TCP/IP connection is closed. After a short delay, the tcp connection is reopened and the driver switches to initialization mode and reinitializes the Lidar. + +3. In any mode: If no messages received after 150 sec [Timeout 2] the driver terminates. + +> **_NOTE:_** The internal timer is reset on successful communication. i.e. the timeout refers to the time of the last message from the Lidar. If there was no message yet, then the time of programme start is used. + +All timeouts can be configured in the launchfile: +``` + + + + +``` + +The following diagram shows the transition between the driver states: + +![driverStatesDiagram](./doc/driverStatesDiagram1.png) + +> **_NOTE:_** Timeout 2 (i.e. no lidar message after 150 seconds) terminates the driver. By default, the driver does not restart automatically. It is therefor recommended to run the driver within an endless loop, e.g. in bash: + +``` +while(true) ; do roslaunch sick_scan_xd [] ; done +``` + +The following table summarizes the timeout parameter: + +![timeout_parameter](./doc/timeout_parameter.png) + +Details of timeout settings: + +* message_monitoring_enabled: Enable or disable timeouts and monitoring. Disabling deactivates any error handling in case of network problems. Recommended default value: True + +* read_timeout_millisec_default: Read timeout in milliseconds in operational (measurement) mode. If no datagrams are received from lidar within 5 seconds (default), the TCP socket is closed and the lidar is reinitialized. + +* read_timeout_millisec_startup: Read timeout in milliseconds during initialization after startup. If SOPAS commands are not responded within 120 seconds (default), the TCP socket is closed and lidar is reinitialized. + +* read_timeout_millisec_kill_node: Pointcloud timeout in milliseconds in operational (measurement) mode. If the sick_scan_xd does not publish a point cloud within the last 150 seconds, the sick_scan_xd process is killed. Should never happen, but is the �last resort� to exit after any kind of error (e.g. socket hangs up and blocks after network trouble). + +* All timeouts configured in milliseconds + +* To disable timeouts (not recommended): + * Set message_monitoring_enabled = false, or + * Set timeouts to "infinite" values, i.e MAX_INT = 2147483647 milliseconds (24.9 days) + +* To disable point cloud monitoring (not recommended): + * read_timeout_millisec_kill_node <= 0 deactivates pointcloud monitoring + +* Parameter read_timeout_millisec_default and read_timeout_millisec_startup: value 0 and negative values are currently NOT mapped to other values, i.e. will cause an immediately timeout error. Use value 2147483647 or message_monitoring_enabled = false to deactivate read timeouts (not recommended) + +## SOPAS Mode + +This driver supports both COLA-B (binary) and COLA-A (ASCII) communication with the laser scanner. Binary mode is activated by default, since this mode generates less network traffic and enables more compatibility to all scanners. +If the communication mode set in the scanner memory is different from that used by the driver, the scanner's communication mode is changed. This requires a restart of the TCP-IP connection, which can extend the start time by up to 30 seconds. +There are two ways to prevent this: +1. Recommended: + * Set the communication mode with the SOPAS ET software to binary and save this setting in the scanner's EEPROM. + * Set "use_binary_protocol" to default value "true". +2. Use the parameter "use_binary_protocol" to overwrite the default settings of the driver. + +## Example Startup Sequence + +The following ROS boot protocol shows the typical start sequence when starting a SICK laser scanner. The MRS6124 is shown here as an example. However, the startup sequence is generally similar for all scanners. + +After a firmware update, the following Quickcheck is performed: +1. Is the device accessible via ping? +2. Can the device be started with the corresponding generic launch file? +3. At the end of the launch process, the device switches to receive mode + for scan data? Typically the last command sent is ```sEA LMDscandata \x01```. +4. Check with rviz: Is it possible to see the Pointcloud2 data or similar? Is the shown data reasonable? +5. Check the scan rate with the command +``` +rostopic hz /cloud +``` +6. Further inspection, if any, by dumping Pointcloud2 data. +The header is of particular interest here. A typical call can therefore look as follows: +``` +rostopic echo /cloud|grep frame -B 7 -A 26 +``` +**Example Sequence** +``` +roslaunch sick_scan_xd sick_mrs_6xxx.launch hostname:=192.168.0.25 +... logging to /home/rosuser/.ros/log/75631922-6109-11e9-b76f-54e1ad2921b6/roslaunch-ROS-NB-10680.log +Checking log directory for disk usage. This may take awhile. +Press Ctrl-C to interrupt +Done checking log file disk usage. Usage is <1GB. +started roslaunch server http://ROS-NB:40757/ +SUMMARY +======== +PARAMETERS + * /rosdistro: melodic + * /rosversion: 1.14.3 + * /sick_mrs_6xxx/filter_echos: 0 + * /sick_mrs_6xxx/hostname: 192.168.0.25 + * /sick_mrs_6xxx/max_ang: 1.047197333 + * /sick_mrs_6xxx/min_ang: -1.040216 + * /sick_mrs_6xxx/port: 2112 + * /sick_mrs_6xxx/range_max: 250.0 + * /sick_mrs_6xxx/range_min: 0.1 + * /sick_mrs_6xxx/scanner_type: sick_mrs_6xxx + * /sick_mrs_6xxx/timelimit: 5 + * /sick_mrs_6xxx/use_binary_protocol: True +NODES + / + sick_mrs_6xxx (sick_scan_xd/sick_generic_caller) +auto-starting new master +process[master]: started with pid [10690] +ROS_MASTER_URI=http://localhost:11311 +setting /run_id to 75631922-6109-11e9-b76f-54e1ad2921b6 +process[rosout-1]: started with pid [10701] +started core service [/rosout] +process[sick_mrs_6xxx-2]: started with pid [10708] +[ INFO] [1555502887.036684738]: sick_generic_caller V. 001.003.016 +[ INFO] [1555502887.036717573]: Program arguments: /home/rosuser/ros_catkin_ws/devel/lib/sick_scan_xd/sick_generic_caller +[ INFO] [1555502887.036725741]: Program arguments: __name:=sick_mrs_6xxx +[ INFO] [1555502887.036731933]: Program arguments: __log:=/home/rosuser/.ros/log/75631922-6109-11e9-b76f-54e1ad2921b6/sick_mrs_6xxx-2.log +[ INFO] [1555502887.048425000]: Found sopas_protocol_type param overwriting default protocol: +[ INFO] [1555502887.048956468]: Binary protocol activated +[ INFO] [1555502887.048984179]: Start initialising scanner [Ip: 192.168.0.25] [Port: 2112] +[ INFO] [1555502887.067528995]: Publishing laserscan-pointcloud2 to cloud +[ INFO] [1555502887.071035827]: Parameter setting for +[ INFO] [1555502887.271739084]: Sending : sMN SetAccessMode 0x03 0xf4 0x72 0x47 0x44 CRC:<0xb3> +[ INFO] [1555502887.273290840]: Receiving: sAN SetAccessMode \x01 +[ INFO] [1555502887.473927858]: Sending : sWN EIHstCola 0x01 CRC:<0x09> +[ INFO] [1555502887.475365983]: Receiving: sWA EIHstCola +[ INFO] [1555502887.675864993]: Sending : sMN LMCstopmeas CRC:<0x10> +[ INFO] [1555502888.199590269]: Receiving: sAN LMCstopmeas \x00 +[ INFO] [1555502888.400030148]: Sending : sRN DeviceIdent CRC:<0x25> +[ INFO] [1555502888.401393378]: Receiving: sRA DeviceIdent \x00\x08\x4d\x52\x53\x36\x31\x32\x34\x52\x00\x0a\x31\x2e\x31\x2e\x30\x2e\x35\x36\x35\x43 +[ INFO] [1555502888.401653485]: Deviceinfo MRS6124R V1.1.0.565C found and supported by this driver. +[ INFO] [1555502888.602062286]: Sending : sRN FirmwareVersion CRC:<0x24> +[ INFO] [1555502888.603444526]: Receiving: sRA FirmwareVersion \x00\x0a\x31\x2e\x31\x2e\x30\x2e\x35\x36\x35\x43 +[ INFO] [1555502888.804094446]: Sending : sRN SCdevicestate CRC:<0x30> +[ INFO] [1555502888.805521867]: Receiving: sRA SCdevicestate \x01 +[ INFO] [1555502889.006161400]: Sending : sRN ODoprh CRC:<0x41> +[ INFO] [1555502889.007613972]: Receiving: sRA ODoprh \x00\x00\x19\xf1 +[ INFO] [1555502889.209949897]: Sending : sRN ODpwrc CRC:<0x52> +[ INFO] [1555502889.211413041]: Receiving: sRA ODpwrc \x00\x00\x02\x55 +[ INFO] [1555502889.413742132]: Sending : sRN LocationName CRC:<0x55> +[ INFO] [1555502889.415205992]: Receiving: sRA LocationName \x00\x0b\x6e\x6f\x74\x20\x64\x65\x66\x69\x6e\x65\x64 +[ INFO] [1555502889.417205292]: Sending : sRN LMPoutputRange CRC:<0x5e> +[ INFO] [1555502889.418631134]: Receiving: sRA LMPoutputRange \x00\x01\x00\x00\x05\x15\x00\x04\xa3\x80\x00\x16\xe3\x60 +[ INFO] [1555502889.418830949]: Angle resolution of scanner is 0.13010 [deg] (in 1/10000th deg: 0x515) +[ INFO] [1555502889.418907556]: MIN_ANG: -1.040 [rad] -59.600 [deg] +[ INFO] [1555502889.418975818]: MAX_ANG: 1.047 [rad] 60.000 [deg] +[ INFO] [1555502889.419156102]: Sending : sWN LMPoutputRange 0x00 0x01 0x00 0x00 0x05 0x15 0x00 0x04 0xa3 0x80 0x00 0x16 0xe3 0x60 CRC:<0xd8> +[ INFO] [1555502889.420488646]: Receiving: sWA LMPoutputRange +[ INFO] [1555502889.420719836]: Sending : sRN LMPoutputRange CRC:<0x5e> +[ INFO] [1555502889.421994443]: Receiving: sRA LMPoutputRange \x00\x01\x00\x00\x05\x15\x00\x04\xa3\x80\x00\x16\xe3\x60 +[ INFO] [1555502889.422165198]: Angle resolution of scanner is 0.13010 [deg] (in 1/10000th deg: 0x515) +[ INFO] [1555502889.424815945]: MIN_ANG (after command verification): -1.040 [rad] -59.600 [deg] +[ INFO] [1555502889.424901901]: MAX_ANG (after command verification): 1.047 [rad] 60.000 [deg] +[ INFO] [1555502889.425102725]: Sending : sWN LMDscandatacfg 0x1f 0x00 0x01 0x01 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x01 CRC:<0x5c> +[ INFO] [1555502889.426373088]: Receiving: sWA LMDscandatacfg +[ INFO] [1555502889.426606493]: Sending : sRN LMDscandatacfg CRC:<0x67> +[ INFO] [1555502889.427933309]: Receiving: sRA LMDscandatacfg \x1f\x00\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x01 +[ INFO] [1555502889.430654546]: Sending : sWN FREchoFilter 0x00 CRC:<0x7f> +[ INFO] [1555502889.431952374]: Receiving: sWA FREchoFilter +[ INFO] [1555502889.432180430]: Sending : sMN LMCstartmeas CRC:<0x68> +[ INFO] [1555502889.963840302]: Receiving: sAN LMCstartmeas \x00 +[ INFO] [1555502889.964083670]: Sending : sMN Run CRC:<0x19> +[ INFO] [1555502889.965558914]: Receiving: sAN Run \x01 +[ INFO] [1555502889.965813465]: Sending : sEN LMDscandata 0x01 CRC:<0x33> +[ INFO] [1555502889.967297195]: Receiving: sEA LMDscandata \x01 +``` + +# Driver features and additional information + +## Software Overview + +The sick_scan_xd software is essentially affected by its use cases: + +* Implement the common tasks for different lidars: + * Provide driver software on Linux and Windows, generic, ROS 1 and ROS 2 + * Receive and convert scan data, publish point cloud + * Run startup, configuration and setup + +* Use cases: + * Provide a point cloud to the customer/application + * Provide a common high level interface for all supported lidars + * Hide datagram details, encodings and parsing knowhow + * The most common use case is to run lidar + sick_scan_xd to get a point cloud. + +* Software requirements: + * Support different lidars (LMS, LRS, LDMRS, MRS, NAV, TiM, RMS, multiScan, etc.) + * Support different OS (Linux, Windows) + * Support different targets (ROS 1, ROS 2, generic) + * Support different protocols (Cola-A, Cola-B, TCP, UDP, msgpack) + * Implement parser for different telegrams (scandata, scancfg, fields, etc.) + +This overview describes the most important modules and their relationship. + +### Software structure + +The following figures show the most important software blocks: + +![software_overview_01](doc/software_overview_01.png) + +![software_overview_02](doc/software_overview_02.png) + +sick_scan_xd contains 6 main functional blocks: + +* sick_generic_caller and sick_generic_laser for initialization and setup: + * Read configuration from launchfile: + * ROS 1: `ros::NodeHandle::getParam` + * ROS 2 and generic: `LaunchParser` (ros-wrapper) + * Lidar specific setup: + * class `sick_scan_xd::SickGenericParser`: lidar specific properties and messages parsing + * Set and get lidar specific properties: number of layers, angular resolution, etc. + * Parse and convert scan data, input: scan data (ascii or binary datagram), output: `ros::sensor_msgs::LaserScan` + * class `sick_scan_xd::SickScanCommonTcp`: receive TCP messages, convert and publish point cloud + * Start ros services: + * class `sick_scan_xd::SickScanServices`: register ros services, convert from/to SOPAS + * Start monitoring: + * class `sick_scan_xd::SickScanMonitor`: monitor scan data, reinit on timeout + * class `sick_scan_xd::PointCloudMonitor`: monitor point cloud, reinit on timeout +* sick_scan_common for the most common lidar devices (LMS, LRS, MRS, NAV, TiM, RMS, etc.): + * Implemention by SickScanCommon and SickScanCommonTcp + * Uses SickGenericParser for lidar specific properties and parsing + * Runs common tasks for LMS/LRS/MRS/NAV/TiM/RMS: + * Run SOPAS startup sequence + * Run TCP receiver thread + * Process telegrams: parse and convert to point cloud + * Publish point cloud +* sick_ldmrs for LD-MRS support using the ldmrs-library from https://github.com/SICKAG/libsick_ldmrs.git +* sick_scansegment_xd for multiScan136 and picoScan150 lidars using SOPAS, msgpack and UDP-communication +* sick_scan_services for ros services +* sick_generic_monitoring for monitoring and re-initialization in case of errors (e.g. network errors). + +The following figures show these 6 functional blocks: + +![software_overview_03](doc/software_overview_03.png) + +![software_overview_04](doc/software_overview_04.png) + +The function blocks depend on and use the underlying system (ROS, TCP, etc.): + +![driver_components_01](doc/driverComponentsDiagram1.png) + +### Message receiving and message handling + +Message receiving and message handling are decoupled, i.e. both tasks run in separate thread and exchange messages via a FIFO-buffer. This way, message handling cannot block tcp recv and vice versa. The following figure shows the message handling: + +![software_overview_05](doc/software_overview_05.png) + +The following figure shows the sequence diagram for a LMDscandata telegram: + +![messageSequenceDiagram1](doc/messageSequenceDiagram1.png) + +Incoming TCP messages and exported point cloud messages are monitored. sick_scan_xd reinitialises the lidar and the tcp connection in case of timeouts. + +### sick_scansegment_xd + +sick_scansegment_xd implements support for multiScan136 and picoScan150 lidars using SOPAS, msgpack and UDP-communication. It has 5 functional blocks: + +* class `sick_scansegment_xd::MsgPackThreads`: + * Init and run all sick_scansegment_xd components + * SOPAS startup (multiScan136, picoScan150) +* class `sick_scansegment_xd::UdpReceiver`: + * Run UDP receiver thread +* class `sick_scansegment_xd::MsgPackConverter`: + * Parse and convert msgpacks + * Collect scan segments +* class `sick_scansegment_xd::MsgPackValidator`: + * Validate msgpacks and scansegments +* class `sick_scansegment_xd::RosMsgpackPublisher`: + * Publish point cloud (single segments) + * Publish cloud_fullframe (fullframe pointcloud, 360 deg for Multiscan136 resp. 270 deg for picoscan) + +The following figure shows the compoenent diagram for sick_scansegment_xd: + +![driverComponentsDiagram2](doc/driverComponentsDiagram2.png) + +Message receiving, converting and publishing run in 3 separate threads and exchange their messages via a FIFO-buffer. + +The following figure shows the sequence diagram for a multiScan136 msgpack: + +![messageSequenceDiagram2](doc/messageSequenceDiagram2.png) + +The following figure shows the sequence diagram for a picoScan and compact format: + +![messageSequenceDiagram3](doc/messageSequenceDiagram3.png) + +### Files and folders + +The source files for the sick_scan_xd core can be found in the following folders: +* driver/src: source files +* include: header files +* launch: configuration +* msg: ros messages definitions +* srv: ros services definitions +* roswrap: ros wrapper (ROS 2 and generic) + +These folders are required to build sick_generic_caller. + +Additional folders for sick_scan_xd support, development and test are: +* test: test scripts and emulator +* tools: additional development tools + +## Generic Driver API + +### Overview + +A generic API for sick_scan_xd has the following goals: +* Easy integration of sick_scan_xd into customer systems with and without ROS +* Integrate SICK lidars with one API, independent of lidar types or underlying operating system +* Provide the same sick_scan_xd functionality on systems without ROS +* In particular: make the sick_scan_xd functionality available on non-ROS-systems without need to customize sources or configuration files. + +The generic sick_scan_xd API provides an interface to all lidars supported by sick_scan_xd. This API can be used in C, C++, Python, or any other language with support of C-bindings. + +The generic sick_scan_xd API ships with the API-header, the library (binary or sources) and usage examples for C, C++ and Python. The following component diagram shows the relationship between API, library, lidar and a customized application: + +![apiComponentsDiagram1.png](doc/sick_scan_api/apiComponentsDiagram1.png) + +Running multiple lidars simultaneously in a single process is not supported.** Currently the sick_scan_xd API does not support the single or multi-threaded use of 2 or more lidars in one process, since the sick_scan_xd library is not guaranteed to be thread-safe. To run multiple lidars simultaneously, we recommend using ROS or running sick_scan_xd in multiple and separate processes, so that each process serves one sensor. See [start multiple lidars](#start-multiple-nodes) for further information. + +### Build and test shared library + +The shared library, which implements the C-API, is built native on Linux or Windows (i.e. without ROS). Follow the instructions on [Build on Linux generic without ROS](#build-on-linux-generic-without-ros) for Linux resp. [Build on Windows](#build-on-windows-generic-without-ros) for Windows. + +#### Build the shared library on Linux + +Run the following commands to build the shared library `libsick_scan_xd_shared_lib.so` on Linux: +``` +# Clone repositories +git clone https://github.com/SICKAG/libsick_ldmrs.git +git clone -b master https://github.com/SICKAG/sick_scan_xd.git +# Build libsick_ldmrs library +mkdir -p ./build +mkdir -p ./libsick_ldmrs/build +pushd libsick_ldmrs/build +cmake -G "Unix Makefiles" .. +make -j4 +sudo make -j4 install +popd +# Build libsick_scan_xd_shared_lib.so +pushd ./build +export ROS_VERSION=0 +cmake -DROS_VERSION=0 -G "Unix Makefiles" ../sick_scan_xd +make -j4 +sudo make -j4 install +# Check build and library dependencies +ls -al ./sick_generic_caller +ls -al ./libsick_scan_xd_shared_lib.so +ls -al ./sick_scan_xd_api_test +ldd -r ./libsick_scan_xd_shared_lib.so +popd +``` +After successful build, the shared library `libsick_scan_xd_shared_lib.so` and a tiny test executable `sick_scan_xd_api_test` are created. + +#### Build the shared library on Windows + +Run the following commands to build the shared library `sick_scan_xd_shared_lib.dll` with Visual Studio 2019 on Windows: +``` +# Clone repository sick_scan_xd +git clone -b master https://github.com/SICKAG/sick_scan_xd.git +# Build libraries sick_scan_xd_shared_lib.dll +call "%ProgramFiles(x86)%\Microsoft Visual Studio\2019\Community\Common7\Tools\VsDevCmd.bat" -arch=amd64 -host_arch=amd64 +set _os=x64 +set _cmake_string=Visual Studio 16 +set _msvc=Visual Studio 2019 +set _cmake_build_dir=build +cd sick_scan_xd +if not exist %_cmake_build_dir% mkdir %_cmake_build_dir% +pushd %_cmake_build_dir% +cmake -DROS_VERSION=0 -G "%_cmake_string%" .. +if %ERRORLEVEL% neq 0 ( @echo ERROR building %_cmake_string% sick_scan_xd with cmake & @pause ) +cmake --build . --clean-first --config Debug +``` +After successful build, the shared library `sick_scan_xd_shared_lib.dll` and a tiny test executable `sick_scan_xd_api_test.exe` are created. To install the library and header in the system folder, run `cmake --build . --target install` with admin privileges. Note that LD-MRS is not supported on Windows. +privileges +> **_NOTE:_** sick_scan_xd builds and runs with both Visual Studio 2019 and 2022. Visual Studio 2019 is recommended, since ROS on Windows requires VS 2019. + +Replace `cmake -DROS_VERSION=0 -G "%_cmake_string%" ..` by `cmake -DROS_VERSION=0 -DCMAKE_ENABLE_EMULATOR=1 -G "%_cmake_string%" ..` to build emulators for unit tests without lidar hardware, see [Simulation](./CONTRIBUTING.md). + +#### Test the shared library + +The executable file `sick_scan_xd_api_test` provides a minimalistic API test. Run `sick_scan_xd_api_test hostname:=` to test the API against a lidar, e.g. on Linux: +``` +# export LD_LIBRARY_PATH=.:./build:$LD_LIBRARY_PATH # append relative path to build folder +export LD_LIBRARY_PATH=.:`pwd`/build:$LD_LIBRARY_PATH # append absolute path to build folder +./build/sick_scan_xd_api_test ./sick_scan_xd/launch/sick_tim_7xx.launch hostname:=192.168.0.1 +``` +On Windows, run e.g. +``` +set PATH=.;.\build;..\build\Debug;%PATH% +.\build\Debug\sick_scan_xd_api_test.exe launch/sick_lms_5xx.launch hostname:=192.168.0.1 +``` + +The executable binary `sick_scan_xd_api_test` will just load library `libsick_scan_xd_shared_lib.so` resp. `sick_scan_xd_shared_lib.dll`, start the lidar and print a message when receiving lidar messages, e.g. `sick_scan_xd_api_test: pointcloud callback`. Replace `sick_lms_1xx.launch` in the example by the launch file corresponding to your type of lidar. + +To load the library, the build folder has to be included in `LD_LIBRARY_PATH` (Linux) resp. `PATH` (Windows). Set this environment variable to your build folder, e.g. on Linux using +``` +# export LD_LIBRARY_PATH=.:./build:$LD_LIBRARY_PATH # append relative path to build folder +export LD_LIBRARY_PATH=.:`pwd`/build:$LD_LIBRARY_PATH # append absolute path to build folder +``` +resp. on Windows +``` +set PATH=.;.\build;.\build\Debug;%PATH% +``` + +### Usage example + +The sick_scan_xd API can be used on Linux or Windows in any language with support of C-bindings. There are 3 steps required to use the API: + +1. API- and lidar-initialization by + * SickScanApiLoadLibrary + * SickScanApiCreate + * SickScanApiInitByLaunchfile or SickScanApiInitByCli + +2. Receive messages by registration of callbacks using `SickScanApiRegisterMsg`-functions (recommended) or by polling using `SickScanApiWaitNextMsg`-functions. + + Alternative examples to receive lidar scan data as a point cloud: + * Register a callback for cartesian point cloud data using SickScanApiRegisterCartesianPointCloudMsg, or + * register a callback for polar point cloud data using SickScanApiRegisterPolarPointCloudMsg. + + The registered callback will be executed whenever the lidar has sent new scan data and receives the (cartesian or polar) point cloud by a parameter of type SickScanPointCloudMsg. The SickScanPointCloudMsg in sick_scan_xd API corresponds to ROS pointcloud: The cartesian point cloud (registered by SickScanApiRegisterCartesianPointCloudMsg) contains the fields (x, y, z, intensity). The polar point cloud (registered by SickScanApiRegisterPolarPointCloudMsg) contains the fields (range, azimuth, elevation, intensity). Each field contains its name (i.e. x, y, z, range, azimuth, elevation, or intensity) and offset. The scan data is a flat buffer of size width x height fields: + + ![apiPointCloudMsg](doc/sick_scan_api/apiPointCloudMsg.png) + + The following python code shows how to convert a cartesian point cloud to 3D points (x, y, z): + ``` + # Convert a SickScanCartesianPointCloudMsg to points + def pySickScanCartesianPointCloudMsgToXYZ(pointcloud_msg): + # get point cloud fields + num_fields = pointcloud_msg.fields.size + msg_fields_buffer = pointcloud_msg.fields.buffer + field_offset_x = -1 + field_offset_y = -1 + field_offset_z = -1 + for n in range(num_fields): + field_name = ctypesCharArrayToString(msg_fields_buffer[n].name) + field_offset = msg_fields_buffer[n].offset + if field_name == "x": + field_offset_x = msg_fields_buffer[n].offset + elif field_name == "y": + field_offset_y = msg_fields_buffer[n].offset + elif field_name == "z": + field_offset_z = msg_fields_buffer[n].offset + # Extract x,y,z + cloud_data_buffer_len = (pointcloud_msg.row_step * pointcloud_msg.height) # length of polar cloud data in byte + assert(pointcloud_msg.data.size == cloud_data_buffer_len and field_offset_x >= 0 and field_offset_y >= 0 and field_offset_z >= 0) + cloud_data_buffer = bytearray(cloud_data_buffer_len) + for n in range(cloud_data_buffer_len): + cloud_data_buffer[n] = pointcloud_msg.data.buffer[n] + points_x = np.zeros(pointcloud_msg.width * pointcloud_msg.height, dtype = np.float32) + points_y = np.zeros(pointcloud_msg.width * pointcloud_msg.height, dtype = np.float32) + points_z = np.zeros(pointcloud_msg.width * pointcloud_msg.height, dtype = np.float32) + point_idx = 0 + for row_idx in range(pointcloud_msg.height): + for col_idx in range(pointcloud_msg.width): + # Get lidar point in polar coordinates (range, azimuth and elevation) + pointcloud_offset = row_idx * pointcloud_msg.row_step + col_idx * pointcloud_msg.point_step + points_x[point_idx] = np.frombuffer(cloud_data_buffer, dtype = np.float32, count = 1, offset = pointcloud_offset + field_offset_x)[0] + points_y[point_idx] = np.frombuffer(cloud_data_buffer, dtype = np.float32, count = 1, offset = pointcloud_offset + field_offset_y)[0] + points_z[point_idx] = np.frombuffer(cloud_data_buffer, dtype = np.float32, count = 1, offset = pointcloud_offset + field_offset_z)[0] + point_idx = point_idx + 1 + return points_x, points_y, points_z + ``` + Exchange field names ("x", "y", "z") by ("range", "azimuth", "elevation") to get 3D polar points (range, azimuth, elevation). + + For further details, see minimalistic usage examples in C and Python. + + Note for multiScan100 and picoScan100 lidars: + + * The WaitNext-functions of the API return the next received message. For multiScan100 and picoScan, this can be a scan segment (i.e. a part of the full scan) or a fullframe point cloud (i.e. all scan points of a 360 degree scan). Depending on the timing, you may not receive all messages, i.e. you may e.g. receive scan points of different segments. We therefore recommend to register a message callback instead of a WaitNext-function. With a registered message callback, you will get all fullframe and segment point cloud messages. + + * For multiScan100 and picoScan, point cloud messages can contain a scan segment (i.e. a part of the full scan) or a fullframe point cloud (i.e. all scan points of a 360 degree scan). The type can be determined by the topic (default: "/cloud_unstructured_segments" for segments, "/cloud_unstructured_fullframe" for fullframe point clouds) or by segment index (-1 for fullframe, 0 up to 11 for segment point clouds). + + +3. Close lidar and API by + * `SickScanApiDeregisterMsg`-functions + * SickScanApiClose + * SickScanApiRelease + +It is recommended to store a deep copy of the point cloud data in a fifo buffer (first in, first out) for further data processing. After the registered callback is executed, the point cloud memory will be released. Make sure to store a deep copy of the point cloud, not a shallow copy. + +All functions named `SickScanApi` are implemented within the library file ("sick_scan_xd_shared_lib.dll" on Windows resp. "libsick_scan_xd_shared_lib.so" on Linux). A small wrapper is provided ([sick_scan_xd_api_wrapper.c](test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c) for C/C++, [sick_scan_api.py](python/api/sick_scan_api.py) for python), which loads and unloads the library (functions `SickScanApiLoadLibrary` and `SickScanApiUnloadLibrary`) and delegates the function calls to the binary. + +> **_NOTE:_** [sick_scan_api.py](python/api/sick_scan_api.py) requires python module numpy. On Windows, we recommend to install and use Python either with Visual Studio 2019 or by installing from https://www.python.org/downloads/windows/ (python installer, embedded version not recommended). Otherwise, please install numpy with `python -m pip install numpy` if numpy is not yet installed. + +#### Minimalistic usage example in C + +File [minimum_sick_scan_api_client.c](examples/c/minimum_sick_scan_api_client.c) shows a minimalistic example of a C client using the sick_scan_xd API. To build and run this example, open a command shell in folder `examples/scripts` and run `.\build_run_api_examples_linux.bash` on Linux resp. `build_run_api_examples_windows.cmd` on Windows. Make sure, that the shared library `libsick_scan_xd_shared_lib.so` resp. `sick_scan_xd_shared_lib.dll` has been successfully built in the build-folder. + +Alternatively, follow the build and run instructions on Linux: +``` +cd examples/c +mkdir -p ./build +cd ./build +cmake -G "Unix Makefiles" .. +make -j4 +cd ../.. +export LD_LIBRARY_PATH=.:./build:$LD_LIBRARY_PATH +./examples/c/build/minimum_sick_scan_api_client hostname:= +``` + +Alternatively, follow the build and run instructions on Windows: +``` +REM Set environment for Visual Studio 2019 (VS 16) +set _os=x64 +set _cmake_string=Visual Studio 16 +set _msvc=Visual Studio 2019 +set _cmake_build_dir=build +REM Build the minimalistic C usage example +cd examples\c +mkdir %_cmake_build_dir% +cd %_cmake_build_dir% +cmake -G "%_cmake_string%" .. +cmake --build . --clean-first --config Debug +REM Set environment: add build folder to LD_LIBRARY_PATH, add python/api to PYTHONPATH +cd ..\.. +set PATH=.;.\build;.\build\Debug;.\build_win64;.\build_win64\Debug;%PATH% +REM Run minimalistic C api example +.\examples\c\build\Debug\minimum_sick_scan_api_client.exe hostname:= +``` + +#### Minimalistic usage example in C++ + +File [minimum_sick_scan_api_client.cpp](examples/cpp/minimum_sick_scan_api_client.cpp) shows a minimalistic example of a C++ client using the sick_scan_xd API. To build and run this example, open a command shell in folder `examples/scripts` and run `.\build_run_api_examples_linux.bash` on Linux resp. `build_run_api_examples_windows.cmd` on Windows. Make sure, that the shared library `libsick_scan_xd_shared_lib.so` resp. `sick_scan_xd_shared_lib.dll` has been successfully built in the build-folder. + +Alternatively, follow the build and run instructions on Linux: +``` +cd examples/cpp +mkdir -p ./build +cd ./build +cmake -G "Unix Makefiles" .. +make -j4 +cd ../.. +export LD_LIBRARY_PATH=.:./build:$LD_LIBRARY_PATH +./examples/cpp/build/minimum_sick_scan_api_client hostname:= +``` + +Alternatively, follow the build and run instructions on Windows: +``` +REM Set environment for Visual Studio 2019 (VS 16) +set _os=x64 +set _cmake_string=Visual Studio 16 +set _msvc=Visual Studio 2019 +set _cmake_build_dir=build +REM Build the minimalistic C++ usage example +cd examples\cpp +mkdir %_cmake_build_dir% +cd %_cmake_build_dir% +cmake -G "%_cmake_string%" .. +cmake --build . --clean-first --config Debug +REM Set environment: add build folder to LD_LIBRARY_PATH, add python/api to PYTHONPATH +cd ..\.. +set PATH=.;.\build;.\build\Debug;.\build_win64;.\build_win64\Debug;%PATH% +REM Run minimalistic C++ api example +.\examples\cpp\build\Debug\minimum_sick_scan_api_client.exe hostname:= +``` + +#### Minimalistic usage example in Python + +File [minimum_sick_scan_api_client.py](examples/python/minimum_sick_scan_api_client.py) shows a minimalistic example of a python client using the sick_scan_xd API. To build and run this example, open a command shell in folder `examples/scripts` and run `.\build_run_api_examples_linux.bash` on Linux resp. `build_run_api_examples_windows.cmd` on Windows. Make sure, that the shared library `libsick_scan_xd_shared_lib.so` resp. `sick_scan_xd_shared_lib.dll` has been successfully built in the build-folder. + +Alternatively, follow the run instructions on Linux: +``` +export LD_LIBRARY_PATH=`pwd`:`pwd`/build:$LD_LIBRARY_PATH +export PYTHONPATH=`pwd`:`pwd`/python/api:$PYTHONPATH +python3 ./examples/python/minimum_sick_scan_api_client.py hostname:= +``` + +Alternatively, follow the run instructions on Windows: +``` +set PATH=.;.\build;.\build\Debug;.\build_win64;.\build_win64\Debug;%PATH% +set PYTHONPATH=.;.\python\api;%PATH% +python ./examples/python/minimum_sick_scan_api_client.py hostname:= +``` + +> **_NOTE:_** [sick_scan_api.py](python/api/sick_scan_api.py) requires python module numpy. On Windows, we recommend to install and use Python either with Visual Studio 2019 or by installing from https://www.python.org/downloads/windows/ (python installer, embedded version not recommended). Otherwise, please install numpy with `python -m pip install numpy` if numpy is not yet installed. + +#### Complete usage example in C++ + +A complete C/C++ usage example is implemented in [sick_scan_xd_api_test.cpp](test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp). Note that the shared library ("sick_scan_xd_shared_lib.dll" on Windows resp. "libsick_scan_xd_shared_lib.so" on Linux) has no dependencies to ROS. The usage example on the other hand supports both ROS 1, ROS 2 and native Linux or Windows. When build on ROS, it converts the SickScanApi-messages into ROS-messages. On ROS, they can be visualized by rviz. The following screenshot shows a point cloud published by `rosrun sick_scan_xd sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_tim_7xx.launch"`: + +![api_test_linux_ros1_tim7xx.png](doc/sick_scan_api/api_test_linux_ros1_tim7xx.png) + +Without ROS, sick_scan_xd_api_test plots a jpeg-file to enable a simple visualization of a point cloud. E.g.: +``` +firefox ./demo/image_viewer_api_test.html & +./build_linux/sick_scan_xd_api_test ./launch/sick_tim_7xx.launch +``` +![api_test_linux_tim7xx.png](doc/sick_scan_api/api_test_linux_tim7xx.png) + +#### Complete usage example in Python + +A complete python usage example is implemented in [sick_scan_xd_api_test.py](test/python/sick_scan_xd_api/sick_scan_xd_api_test.py). It is handy to test the sick_scan_xd library. Like its C++ counterpart [sick_scan_xd_api_test.cpp](test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp), it just loads library `libsick_scan_xd_shared_lib.so` resp. `sick_scan_xd_shared_lib.dll`, starts a lidar and receives the lidar point cloud and messages via API. On ROS 1, the lidar point cloud and messages are converted to ROS and published. The lidar point cloud can be visualized by rviz using topic "/sick_scan_xd_api_test/api_cloud". + +Run `python3 sick_scan_xd_api_test.py hostname:=` to test the API against a lidar. +On Linux e.g.: +``` +export PYTHONPATH=`pwd`:`pwd`/src/sick_scan_xd/python/api:$PYTHONPATH +source /opt/ros/noetic/setup.bash # replace by noetic by your ros version +python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./src/sick_scan_xd/launch/sick_lms_1xx.launch hostname:=192.168.0.1 +``` +On Windows e.g.: +``` +set PYTHONPATH=.;.\src\sick_scan_xd\python\api;%PYTHONPATH% +python ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./src/sick_scan_xd/launch/sick_lms_1xx.launch hostname:=192.168.0.1 +``` + +The pthon usage example [sick_scan_xd_api_test.py](test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) imports [sick_scan_api.py](python/api/sick_scan_api.py), which contains the python definitions of the sick_scan_xd API. Make sure that sick_scan_api.py can be imported, e.g. by including folder `python/api` in PYTHONPATH by: + +`export PYTHONPATH=`pwd`:`pwd`/src/sick_scan_xd/python/api:$PYTHONPATH` on Linux, resp.
+`set PYTHONPATH=.;.\src\sick_scan_xd\python\api;%PYTHONPATH%` on Windows + +> **_NOTE:_** The shared library ("sick_scan_xd_shared_lib.dll" on Windows resp. "libsick_scan_xd_shared_lib.so" on Linux) works standalone and does not have any ROS dependencies. The usage example [sick_scan_xd_api_test.py](test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) converts API- to ROS-messages for visualization and is therefore dependent on ROS, if ROS is installed. + +If ROS is not installed, [sick_scan_xd_api_test.py](test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) uses matplotlib to visualize the pointcloud. The following screenshot shows a TiM-7xx point cloud on Linux without ROS: + +![api_test_python_tim7xx.png](doc/sick_scan_api/api_test_python_tim7xx.png) + +> **_NOTE:_** [sick_scan_api.py](python/api/sick_scan_api.py) requires python module numpy. On Windows without ROS, [sick_scan_xd_api_test.py](test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) requires numpy and matplotlib. On Windows, we recommend to install and use Python either with Visual Studio 2019 or by installing from https://www.python.org/downloads/windows/ (python installer, embedded version not recommended). These python distributions provide the necessary packages and tools. Otherwise, please install numpy and matplotlib with `python -m pip install numpy` and `python -m pip install matplotlib` if not yet done. + +#### Diagnostic + +The API provides the following functions for diagnostics: + +* SickScanApiRegisterDiagnosticMsg and SickScanApiDeregisterDiagnosticMsg: Register resp. deregister a callback to receive diagnostic messages. Diagnostic messages contain a status code and status message. The status code is one of the following numbers: + * OK=0 (normal operation) + * WARN=1 (warning) + * ERROR=2 (error, should not occur) + * INIT=3 (initialization after startup or reconnection) + * EXIT=4 (sick_scan_xd exiting) + + The status message is descriptional C-string. + + A typical sequence of the status code is: + * INIT at startup, then + * after lidar initialization is completed: change to OK (normal operation) and run, and + * EXIT at shutdown. + Diagnostic messages are generated whenever the status changed or an ERROR occured. Status code 2 (i.e. error) should not occur under normal operation. + +* SickScanApiRegisterLogMsg and SickScanApiDeregisterLogMsg: Register resp. deregister a callback to receive log messages. This callback will receive all informational or error messages printed on console. The log messages contain a log level (Info=1, Warn=2, Error=3, Fatal=4) and the log message. + +* SickScanApiGetStatus queries the current status. This function returns the current status code (OK=0 i.e. normal operation, WARN=1, ERROR=2, INIT=3 i.e. initialization after startup or reconnection or EXIT=4) and the descriptional status message. + +* SickScanApiSendSOPAS sends a SOPAS command (Cola-A) to the lidar and returns the response from the device. + * C++ example: + ``` + char sopas_response_buffer[1024] = { 0 }; + SickScanApiSendSOPAS(apiHandle, "sRN SCdevicestate", &sopas_response_buffer[0], (int32_t)sizeof(sopas_response_buffer); // returns "sRA SCdevicestate \x00" in sopas_response_buffer + ``` + + * Python example: + ``` + sopas_response = SickScanApiSendSOPAS(sick_scan_library, api_handle, "sRN SCdevicestate")` # returns "sRA SCdevicestate \x00". + ``` + See the telegram listing for valid SOPAS commands. + +* SickScanApiSetVerboseLevel and SickScanApiGetVerboseLevel sets resp. returns the verbose level. The verbose level can be 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET (equivalent to ros\:\:console\:\:levels). Default verbose level is 1 (INFO), i.e. sick_scan_xd prints informational, warnings and error messages on the console. Logging callbacks registered with SickScanApiRegisterLogMsg will receive all informational, warnings and error messages independent of the verbose level. + +To monitor sick_scan_xd resp. the lidar, it is recommended to register a callback for diagnostic messages using SickScanApiRegisterDiagnosticMsg and to display the error message in case for status code 2 (error). See [sick_scan_xd_api_test.cpp](test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp) and [sick_scan_xd_api_test.py](test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) for an example. + +#### Simulation and unit test + +sick_scan_xd provides a tiny server for offline tests which simulates a basic lidar. It just accepts TCP connections, responds to sopas requests with predefined responses and sends lidar data from file. See [Simulation](./CONTRIBUTING.md) for further details. Note that the simulation does not emulate or replace a lidar, it just supports basic unit tests. + +Open a new terminal and run the following steps to test the api against a TiM7xx simulation using the python example mentioned above: + +1. Build library `libsick_scan_xd_shared_lib.so` incl. emulator with option `-DCMAKE_ENABLE_EMULATOR=1`: + ``` + mkdir -p ./src/build + pushd ./src/build + rm -rf ./* + cmake -DROS_VERSION=0 -DCMAKE_ENABLE_EMULATOR=1 -G "Unix Makefiles" ../sick_scan_xd + make -j4 + ls -al libsick_scan_xd_shared_lib.so sick_scan_xd_api_test sick_generic_caller sick_scan_emulator # list size and date of the binaries + popd + ``` + Building sick_scan_xd with option `-DCMAKE_ENABLE_EMULATOR=1` requires jsoncpp. Install libjsoncpp by running "sudo apt-get install libjsoncpp-dev" on Linux resp. "vcpkg install jsoncpp:x64-windows" on Windows (vcpkg required). Run the following steps to install Visual Studios package manager vcpkg on Windows: + * Download vcpkg-master.zip from https://github.com/microsoft/vcpkg/archive/master.zip and unzip to `c:\vcpkg`. Alternatively, run "git clone https://github.com/microsoft/vcpkg" + * Install vcpkg by running the following commands: + ``` + cd c:/vcpkg + bootstrap-vcpkg.bat + vcpkg integrate install + ``` + * Include vcpkg in your path: + ``` + set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH% + ``` + +2. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): + +3. Build sick_scan_xd for ROS 1 on Linux, see [Build on Linux ROS 1](#build-on-linux-ros1) + +4. Start the TiM7xx simulator: + ``` + cp -f ./src/sick_scan_xd/test/emulator/scandata/sopas_et_field_test_1_2_both_010.pcapng.json /tmp/lmd_scandata.pcapng.json + ./src/build/sick_scan_emulator ./src/sick_scan_xd/test/emulator/launch/emulator_01_default.launch & + sleep 1 + ``` + +5. Run sick_scan_xd_api_test.py against the TiM7xx simulator on localhost: + ``` + export PYTHONPATH=.:./src/sick_scan_xd/python/api:$PYTHONPATH + python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./src/sick_scan_xd/launch/sick_tim_7xx.launch hostname:=127.0.0.1 port:=2111 sw_pll_only_publish:=False + ``` + +6. Start rviz and visualize the point cloud on topic "/sick_scan_xd_api_test/api_cloud". + +> **_NOTE:_** The shared library ("sick_scan_xd_shared_lib.dll" on Windows resp. "libsick_scan_xd_shared_lib.so" on Linux) works standalone and does not have any ROS dependencies. The usage example [sick_scan_xd_api_test.py](test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) uses ROS for visualization. + +### C-API + +The header file [sick_scan_api.h](include/sick_scan_xd_api/sick_scan_api.h) defines the C-interface. It defines all datatypes, messages and functions of the generic sick_scan_xd API. To allow equal operations on all systems, the definition of datatypes and messages is as close as possible to their equivalents currently used on ROS. + +Python file [sick_scan_api.py](python/api/sick_scan_api.py) defines the same interface in python. + +### Useful links + +[ctypes](https://docs.python.org/3/library/ctypes.html) is used for data exchange and function calls between Python and C-libraries: +* https://docs.python.org/3/library/ctypes.html +* https://docs.python.org/3/library/ctypes.html#structures-and-unions +* https://docs.python.org/3/library/ctypes.html#callback-functions + +## Timestamps and synchronization (Software PLL) + +Often there is a requirement that the time stamp of the measurements should be calculated for each individual shot. This article explains some background information about the determination of these time stamps. Here the statements refer to the LMS511. However, they can be transferred to other lidars using the same logic. + +![lms511_scan.png](doc/lms511_scan.png) + +The lidar sends a pulsed beam onto a rotating mirror. Since the speed of rotation is relatively low, this mirror serves as transmitter and receiver. The direction of rotation can be seen in the drawing. + +25 scans per second means that the mirror makes 25 360° rounds per second. The actual laser unit is only active during the 190°. Therefore the so-called duty cycle is 190/360. The rotation is unaccelerated, so that the lidar arrives at the same angular direction again after 40 ms (1/25). From the angular distance from shot to shot you can calculate the pulse rate of the laser. For example, if the angular difference from shot to shot is 0.1 degree, the so-called shot rate would be: 360/0.1 * 25 shots/second. + +A scan means the group of all shots during one revolution. Just imagine the lidar as a lighthouse that rotates evenly and measures the distance values over time of flight in a certain sector (here 190°) with the above mentioned shot rate. Also you find some background material in the documentation of SICK. + +In the transmission protocol of the lidar two points in time are given in so-called ticks (resolution in microseconds): +a) Start of the scans in ticks +b) Start of IP data transfer from lidar to PC in ticks + +It is assumed that the transfer between lidar and PC is near latency-free. +The software PLL generates an assignment between the tick of the IP data transmission and the system time of the PC via an estimated line mapping. On the basis of this straight line equation, the start of the scan is then calculated relative to the system time. This generation time stamp is the time of the first shot of the scan. This timestamp is assigned to the point cloud timestamp. From there, the rotation speed and the angular distance from shot to shot can be used to approximately calculate the time for each shot. + +A software pll is used to convert LiDAR timestamps in ticks to the ros system time. + +Many sensor devices, e.g. lidar devices, provide sensor data with timestamps. These timestamps can be synchronized with the current system time by additional hardware, e.g. by GPS. But without specialized hardware, sensor timestamps and system time is normally unsynchronized. Sensor timestamps are often quite accurate, but have a different time base and a bias to the system time or to other sensor clocks. This difference is estimated and compensated by this Software PLL. + +The scanner has an internal time base of microseconds since system startup. Against this "tick" time base all time stamps are made in the scanner. When sending messages from the scanner, two time stamps are added: + +1. scan generation ticks--> timestamp at the time of the first shot +2. scan transmission ticks--> time stamp for the transmission of the data + +When data packets are received, they are timestamped by the driver against the systemtime in ros::time format. See following Fig. + +![timing_syn.png](doc/timing_sync.png) + +The relationship between system time and ticks is then derived from the time stamps and kept synchronous.The time required for the transmission of data over the network is assumed to be short and constant and is therefore neglected. The function of the algorithms is shown in the following Fig. + +![sequence_time_pll.png](doc/sequence_time_pll.png) + +To compensate the different time base and bias between sensor and system clock, the system time when receiving sensor data is gathered together with the sensor timestamp. While the system time is often measured in seconds resp. nanoseconds, the sensor timestamp is normally received in clock ticks. SoftwarePLL estimates the correlation between system time in secondsnanoseconds and sensor ticks, and computes a corrected time from ticks. This way you know at which time stamp the data +have been measured by your sensor. + +SoftwarePLL is a generic module and independant from specific sensor types. It just uses the system timestamps and ticks, estimates their correlation and predicts the time from sensor ticks.SoftwarePLL computes a linear regression between ticks and system timestamps. The system time is measured immediately after receiving new sensor data, while sensor ticks represent the sensor clock at the time of measurement. Thus we have three different times for each measurement + +- The time when the system receives the sensor data (receive time t_rec), measured in seconds resp. nanoseconds. +- The sensor ticks (or just ticks) at the time of the measurement. These ticks are contained in the sensor data and +received later by the system. +- The time of the measurement (measurement time t_mea). We don't know this time yet, but we estimate it from both the ticks +and their receive time t_rec using the SoftwarePLL. + +During initialization, ticks and system timestamps are stored in fifo buffer (first-in, first-out). After initialization, +typically after N=7 measurements, a regression line is computed, i.e. the slope `m` (gradient) of a function +`f(ticks) = m ticks + c` is estimated from ticks `x(i)` and timestamps `y(i)` by a linear regression +`m = (N sum(x(i) y(i)) - sum(x(i)) sum(y(i))) (N sum(x(i) x(i)) - sum(x(i))sum(x(i)))` with `0 = i N` and +unbiased values `x(i) = tick(i) - tick(0)`, `y(i) = t_rec(i) - t_rec(0)`. + +![pll_regression.png](doc/pll_regression.png) + +The estimated system time `t_esti(i)` of a measurement can be computed from its sensor tick by `t_esti(i) = m (ticks(i) - ticks(0)) + t_rec(0)`. +If the difference between estimated times `t_esti(i)` and the measured system timestamps `t_rec(i)` is small (typically +less than 100 milliseconds), the estimation can be considered to be valid. With a valid estimation of `m`, we can +get a corrected timestamp for new measurements by applying function `SoftwarePLLGetCorrectedTimeStamp`, which returns +the estimated system time of a measurement `t_esti = m (ticks - ticks(0)) + t_rec(0)`. + +If the estimation is not valid (i.e. the difference between estimated times and measured system timestamps in the buffer is +significant), we can't estimate system timestamps from sensor ticks. If this happens more than a given number of times +after initialization (typically 20 times), the fifo is reset and a new initialization is done. + +Use the following code snippet as an example: + +``` +#include softwarePLL.h + + Create an instance of SoftwarePLL +SoftwarePLL& software_pll = SoftwarePLLInstance(Sensor1); + + Get system time t_rec in seconds and nanoseconds when receiving sensor data +rosTime t_rec = rosTimenow(); +uint32_t sec = t_rec.nsec; +uint32_t nanosec = t_rec.nsec; + + Get sensor ticks from sensor data +uint32_t ticks = scanner_msg.ticks; + + Update SoftwarePLL +software_pll.UpdatePLL(sec, nanosec, ticks); + + Get corrected timestamp (time of measurement from ticks) +software_pll.GetCorrectedTimeStamp(sec, nanosec, ticks); +``` + +**Data buffering in MRS1000** + +Due to their construction the MRS1000 scanners generate different layers at the same time which are output sequentially by the scanner firmware. In order to ensure that only point cloud messages that follow one another in time are sent, buffering can be activated in the driver. +![scannbuffering.png](doc/scannbuffering.png) + +## Coordinate transforms + +Different lidars use different coordinate systems. sick_scan_xd transforms all points of the published pointclouds to the ROS coordinate system, independant of the lidar. The following figure shows the commonly used coordinate systems: + +![3d_coordinate_system_comp.png](doc/3d_coordinate_system_comp.png) + +An additional coordinate transform can be applied to the pointcloud. This optional transform can be used to transform the pointclouds into a user defined coordinate system. If the lidar is e.g. mounted on a vehicle, the pointclouds can be transformed into a vehicle coordinates. + +An additional coordinate transform can be configured by a 6D pose (x, y, z, roll, pitch, yaw) with a translational part (x, y, z) in [m] and a rotational part (roll, pitch, yaw) in [rad]. + +If configured, it will transform the point cloud from its "cloud" coordinates into user defined "world" coordinate system: + +`T[world,cloud] with P_world = T[world,cloud] * P_cloud` (parent: world, child: cloud) + +The final rotation is defined by: Rotation = Rot[yaw] * Rot[pitch] * Rot[roll] with roll = rotation about x-axis, pitch = rotation about y-axis and yaw = rotation about z-axis. + +An additional transform can be configured in the launchfile, e.g. + +``` + + + + + +``` + +Default value is `"0,0,0,0,0,0"`, i.e. no additional transform will be applied. + +The additional transform applies to cartesian lidar pointclouds and visualization marker (fields). +It is **NOT** applied to polar point clouds, radarscans, LD-MRS objects or other messages. + +Note that sick_scan_xd configures an additional transform using (x, y, z, roll, pitch, yaw). In contrast, the ROS static_transform_publisher uses commandline arguments in order x, y, z, yaw, pitch, roll. + +Example using ROS static_transform_publisher with x=0, y=0, z=0, roll=15, pitch=-10, yaw=5 [deg]: + +``` +source /opt/ros/noetic/setup.bash +# static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms +# tf_echo +# rot_x = 5 deg: 0.0872665, rot_y = -10 deg: -0.1745329, rot_z = 15 deg: 0.2617994 +rosrun tf static_transform_publisher 0 0 0 0.2617994 -0.1745329 0.0872665 parent_frame child_frame 100 +rosrun tf tf_echo parent_frame child_frame +``` + +File [trafo_example.py](doc/sick_scan_api/trafo_example.py) demonstrates how a transform can be computed. + +For upside down mounted devices, the point cloud can be 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"`. [sick_lrs_36x0_upside_down.launch](launch/sick_lrs_36x0_upside_down.launch) and [sick_lrs_36x1_upside_down.launch](launch/sick_lrs_36x1_upside_down.launch) show examples for compensating the point cloud of an upside down mounted device by a 180 deg rotation about the x axis. + + + +## IMU Support + +Devices of the MRS6xxx and MRS1xxx series are available with an optionally built-in IMU. + +For the IMU support of multiScan100 and picoScan100 refer to the device specific section. + +By setting the following config parameter in the launch file, the output of [imu messages](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html) can be enabled with a compatible scanner. Currently the messages are published in the /imu Topic. +```xml + + +``` +The imu Messages contain covariance matrices, these are currently determined from empirical values and are not measured specifically for each scanner. +The laser scanner provides additional information (tick timestamp and confidence) to the Imu messages these can be activated by activating the [SickImu messages](msg/SickImu.msg). + +```xml + +``` + +IMU messages are only supported in SOPAS binary mode. Due to the high data rate of the IMU messages (100 Hz and more) while sending the standard laser scanner messages at the same time, the ASCII mode is not supported. Please set the scanner to binary mode if you are using the IMU. + +## Encoders + +If the device is mounted for mobile use or if the objects to be measured are in motion,the application will usually also need position data to further process the measured valâ€ues.Encoders can be connected for this purpose. The encoder data is then available withthe other measured values in a single scan and at the same interface. A volume,for example, can be calculated by evaluating the measurement data. The input freâ€quency of the encoder signal must not exceed 50 kHz.The following encoders with push-pull output stage can be used: +1. Single-channel, only connected at encoder A, no direction detection. +2. Dual-channel (phase), connected at encoder A and encoder B; the pulses have aphase shift of 90°, making direction detection possible. By definition, during forâ€ward motion (CW = clockwise) phase A precedes phase B; conversely, duringreverse motion (CCW = counterclockwise) edge A rises before edge B. +3. Dual-channel (level), connected at encoder A and encoder B; the pulses are atencoder A; at encoder B, the direction is indicated by level 0 or level 1 (rarely). + +### Connecting encoders +![LMS4000 encoder connection](doc/lms4xxx_encoder_connection.png "LMS4000 encoder connection") +See also [LMS4000 Manual](https://cdn.sick.com/media/docs/0/90/790/Operating_instructions_LMS4000_2D_LiDAR_sensors_en_IM0079790.PDF) + +### Example circuit to trigger encoder counts +Whenever the switch is closed a potential of 24 V is applied to the encoder input A in mode (01 single-channel) this leads to an increase of the count by 1. + +![encoder trigger](doc/circuit.png "encoder trigger") + +### Activation of encoder information +If the parameter ```encoder_mode``` is set to 1-4 in the launch file, the encoder is activated in the laser scanner in the corresponding mode (see list above). + +The following encoder modes can be configured in the launch file or by commandline parameter: +* `encoder_mode:=-1`: Default value, i.e. encoder configuration not set +* `encoder_mode:=0`: Encoder off (supported by LMS1xx, LMS5xx, LMS4000, LRS4000) +* `encoder_mode:=1`: Single increment (supported by LMS1xx, LMS5xx, LMS4000, LRS4000) +* `encoder_mode:=2`: Direction recognition phase (supported by LMS1xx, LMS5xx, LMS4000, LRS4000) +* `encoder_mode:=3`: Direction recognition level (supported by LMS1xx, LMS5xx, LMS4000, LRS4000) +* `encoder_mode:=4`: Fixed increment speed/ticks (supported by LMS4000 only) + +Encoder messages are published on topic `/encoder` synchronously to the point cloud messages. They contain a timestamp and the encoder value, e.g.: +```console +foo@bar:~$rostopic echo /encoder +header: + seq: 20700 + stamp: + secs: 1570722972 + nsecs: 28866142 + frame_id: "Encoder" +enc_count: 836 +--- +header: + seq: 20701 + stamp: + secs: 1570722972 + nsecs: 30598181 + frame_id: "Encoder" +enc_count: 836 +--- +header: + seq: 20702 + stamp: + secs: 1570722972 + nsecs: 32138020 + frame_id: "Encoder" +enc_count: 836 +``` + +![Encoderdata in Sopas datagramm](doc/Encoder_data.png "Encoderdata in Sopas datagramm") + +![Set encoder config](doc/set_encoder_settings.png "Set encoder config") + +## Field Evaluation Information + +The LMS1xx, LMS5xx, TiM7xx and TiM7xxS families support extensions for field monitoring. + +### Field monitoring messages + +LMS1xx, LMS5xx, TiM7xx and TiM7xxS scanner support field monitoring. Fields can be configured by Sopas ET. Once they are configured, sick_scan_xd publishes ros messages containing the monitoring information from the lidar. + +By default, field monitoring is enabled in the launch files [sick_lms_1xx.launch](launch/sick_lms_1xx.launch), [sick_lms_5xx.launch](launch/sick_lms_5xx.launch), +[sick_tim_7xx.launch](launch/sick_tim_7xx.launch) resp. [sick_tim_7xxS.launch](launch/sick_tim_7xxS.launch) by following settings: +``` + + + +``` + +The driver queries the field configuration from the lidar and activates field monitoring by sending cola commands `"sEN LFErec 1"` and `"sEN LIDoutputstate 1"` at startup. Field monitoring is deactivated when driver exits. During runtime, it's possible to query, activate or deactivate monitoring using ros service ColaMsg with the following command (see section [Cola commands](#cola-commands)): +``` +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LFErec 1'}" # activate LFErec messages +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LFErec 0'}" # deactivate LFErec messages +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LFErec'}" # query activation status of LFErec messages +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDoutputstate 1'}" # activate LIDoutputstate messages +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDoutputstate 0'}" # deactivate LIDoutputstate messages +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LIDoutputstate'}" # query activation status of LIDoutputstate messages +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDinputstate 1'}" # activate LIDinputstate messages +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDinputstate 0'}" # deactivate LIDinputstate messages +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LIDinputstate'}" # query activation status of LIDinputstate messages +``` + +LFErec and LIDoutputstate messages are defined in [LFErecMsg.msg](msg/LFErecMsg.msg) and [LFErecFieldMsg.msg](msg/LFErecFieldMsg.msg) resp. [LIDoutputstateMsg.msg](msg/LIDoutputstateMsg.msg) and published on the following topics: `"/sick_tim_7xxS/lferec"` resp. `"/sick_tim_7xxS/lidoutputstate"`. + +| ** Lidar ** | ** lferec topic ** | ** lidoutputstate topic ** | +| ----------- | --------------------- | ----------------------------- | +| lms_1xx | /sick_lms_1xx/lferec | /sick_lms_1xx/lidoutputstate | +| lms_5xx | /sick_lms_5xx/lferec | /sick_lms_5xx/lidoutputstate | +| lms_7xx | /sick_tim_7xx/lferec | /sick_tim_7xx/lidoutputstate | +| lms_7xxS | /sick_tim_7xxS/lferec | /sick_tim_7xxS/lidoutputstate | + +To view the field monitoring messages, run +``` +rostopic echo "/sick_lms_1xx/lferec" +rostopic echo "/sick_lms_1xx/lidoutputstate" +rostopic echo "/sick_lms_5xx/lferec" +rostopic echo "/sick_lms_5xx/lidoutputstate" +rostopic echo "/sick_tim_7xx/lferec" +rostopic echo "/sick_tim_7xx/lidoutputstate" +rostopic echo "/sick_tim_7xxS/lferec" +rostopic echo "/sick_tim_7xxS/lidoutputstate" +``` +or use rviz to visualize monitored fields and their status (see section [Visualization with rviz](#visualization-with-rviz)) + +The most important values of the field monitoring messages are + +- `field_index` (uint8) and `field_result_mrs` (uint8) for each field of a LFErec message with result status
    +
  • 0: invalid / incorrect,
    • +
  • 1: free / clear, or
    • +
  • 2: infringed.
    • +
+ +- `output_state` (uint8) for each LIDoutputstate message with status 0 (not active), 1 (active) or 2 (not used). + +> **_NOTE:_** Field monitoring currently supports binary cola messages only, which is the default. If cola ascii is activated, please switch back to cola binary for field monitoring. + +### Visualization with rviz + +The point cloud, the monitored fields and their status can be visualized using rviz. Use the [rviz configuration file](test/emulator/config/rviz_emulator_cfg.rviz) +and run +``` +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg.rviz +``` + +Otherwise you can just add visualizations of type `/cloud/PointCloud2` and `/sick_tim_7xxS/marker` (resp. `/sick_tim_1xx/marker` for lms_1xx, `/sick_tim_5xx/marker` for lms_5xx and `/sick_tim_7xx/marker` for tim_7xx): + +![tim7xxs_screenshot01.png](doc/tim7xxs_screenshot01.png) + +The following screenshot shows a TiM781S example with 2 fields (the 3. field is not configured), the first field with status "Clear", the second with status "Infringed": + +![tim7xxs_screenshot02.png](doc/tim7xxs_screenshot02.png) + +The following screenshot shows a LMS511 example with a segmented field, two rectangular fields and a dynamic fields: + +![lms511_screenshot01.png](doc/lms511_screenshot01.png) + +> **_NOTE:_** Some combinations of rviz, OpenGL 3, VMware and graphic card drivers may cause visualization issues. In case of missing markers, try rviz with Open GL 2 using the command +``` +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg.rviz --opengl 210 +``` + +### Cola commands + +Cola commands can be sent for diagnosis and development using the ros service ColaMsg. This service is implemented in sick_scan_xd and started by +``` + +``` +in the launch file [sick_tim_7xxS.launch](launch/sick_tim_7xxS.launch) (resp. [sick_lms_1xx.launch](launch/sick_lms_1xx.launch), [sick_lms_5xx.launch](launch/sick_lms_5xx.launch), [sick_tim_7xx.launch](launch/sick_tim_7xx.launch) ). The ros service sends the given cola command to the lidar and returns its response. + +Example for cola command `"sRN SCdevicestate"` and response `"sRA SCdevicestate \\x00"` with error status 0 (no error): +``` +rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN SCdevicestate'}" +response: "sRA SCdevicestate \\x00" +``` + +### Tools, emulation and unittests + +Package sick_scan_xd implements some tools to support unittests, development and emulation of Tim781S devices: + +- sick_scan_emulator to emulate lidar devices and enable unittests (currently for Tim781S only) +- pcap_json_converter to convert pcapng-files to json. + +#### LMS and TiM emulation + +sick_scan_emulator implements a simple test server for cola commands. It rececives Cola-commands, returns Tim781S-like responses and sends Scandata from a json-file. Run +``` +roslaunch sick_scan_xd emulator_01_default.launch +``` +to emulate a local Tim781S device. Then start and connect the sick_scan_xd driver by +``` +roslaunch sick_scan_xd sick_tim_7xxS.launch hostname:=127.0.0.1 +``` + +Note that sick_scan_emulator just implements a simple server for offline tests. It does not emulate a lidar device completely and should only be used for development and testing. + +Scandata messages are parsed from json-file(s). These json-files are configured in the launch file [emulator.launch](test/emulator/launch/emulator_01_default.launch) and converted form wireshark-records (pcapng-files) using pcap_json_converter.py (see section Pcapng converter tool](#pcapng-converter-tool)). + +A LMS111 device can be emulated by running +``` +roslaunch sick_scan_xd emulator_lms1xx.launch & +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_lms1xx.rviz & +roslaunch sick_scan_xd sick_lms_1xx.launch hostname:=127.0.0.1 +``` + +A LMS511 device can be emulated by running +``` +roslaunch sick_scan_xd emulator_lms5xx.launch & +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_lms5xx.rviz & +roslaunch sick_scan_xd sick_lms_5xx.launch hostname:=127.0.0.1 +``` + +#### Unittests + +Folder `test/emulator/scandata` contains scandata examples for unittests. To run an offline unittest for LMS111, LMS511, TiM781, TiM781S enter the following commands: +``` +cd test/scripts +./run_simu_lms1xx.bash +./run_simu_lms5xx.bash +./run_simu_tim7xx_tim7xxS.bash +``` +or start emulator, driver and rviz by running +``` +source ./install/setup.bash +# Start sick_scan_xd emulator +roslaunch sick_scan_xd emulator_01_default.launch & +sleep 1 +# Start rviz +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg.rviz --opengl 210 & +sleep 1 +# Start sick_scan_xd driver for TiM871S +roslaunch sick_scan_xd sick_tim_7xxS.launch hostname:=127.0.0.1 +``` + +#### Pcapng converter tool + +The pcapng converter tool [pcap_json_converter.py](test/pcap_json_converter/pcap_json_converter.py) converts pcapng-files to json-files. Run the following steps to create a json-file with scandata for the emulator: + +1. Start wireshark and filter the tcp traffic on port 2112 with the filter expression `tcp and tcp.port==2112`. +2. Start TiM781S and run the sick_scan_xd driver. +3. Capture the network traffic for some time. +4. Stop capturing and save the network traffic in a pcapng-file. +5. Convert the pcapng-file to json by `python pcap_json_converter.py --pcap_filename=.pcapng`. Result is a jsonfile `.pcapng.json` +6. Set the resulting json-file in the emulator configuration [emulator.launch](test/emulator/launch/emulator_01_default.launch) by `` + +## SLAM Support + +### Introduction + +In robotic mapping and navigation, simultaneous localization and mapping (SLAM) is the computational problem of constructing or updating a map of an unknown environment while simultaneously keeping track of an agent's location within it. For further details please refer to https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping . + +### Measuring Principle + +The following assumes that the SLAM algorithm works with a laser scanner mounted on a mobile base. The mobile base (e.g. a robot) records the environment while driving and creates the map from it. The mobile base usually has a so-called intertial measurement unit (IMU). In principle, however, it is also possible to estimate the direction of movement from the chronological sequence of the laser scans by means of correlation observations. The laser scanner then virtually takes over the task of the IMU and other components (e.g. counting the wheel revolutions). The method of estimating the position and orientation (position estimation) of a mobile system based on data from its driving system is called odometry (cf. https://en.wikipedia.org/wiki/Odometry). + +The SLAM algorithm hector_slam (http://wiki.ros.org/hector_slam) supports odometry estimation directly from the laser scans and is therefore used as a reference implementation in the following. + +Other widely used SLAM algorithms such as gmapping (cf. http://wiki.ros.org/gmapping ) do not have this option. They depend on the data of an IMU. One possibility to use Gmapping nevertheless is the integration of the project laser_scan_matcher (https://answers.ros.org/question/63457/gmapping-without-odom/ and http://wiki.ros.org/laser_scan_matcher ). Here, however, the pose must still be converted into an odometry message (see https://answers.ros.org/question/12489/obtaining-nav_msgsodometry-from-a-laser_scan-eg-with-laser_scan_matcher/ ). + +### NAV350 ROS 1 SLAM example + +Build hector_slam and sick_scan_xd: +``` +cd src +git clone -b master https://github.com/SICKAG/sick_scan_xd.git +git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git +cd .. +catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DCMAKE_ENABLE_EMULATOR=1 -Wno-dev +``` + +Run rviz, sick_scan_xd with NAV350 and hector_slam: +``` +source ./devel_isolated/setup.bash +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_slam_nav350.rviz & +roslaunch sick_scan sick_nav_350.launch hostname:=192.168.0.1 tf_publish_rate:=0 & +roslaunch sick_scan test_200_slam_ros1_hector.launch scan_topic:=/scan scan_layer_0_frame_id:=cloud_POS_000_DIST1 cloud_frame_id:=cloud & +``` + +> **_NOTE:_** By default, sick_scan_xd publishes transform (TF) messages, which map frame id "map" to the point cloud frame id. To avoid conflicts with hector SLAM, it is recommended to disable these TF messages by commandline parameter **`tf_publish_rate:=0`** or by setting `` in the launchfile. + +The following rviz screenshot shows an example of a NAV350 pointcloud created by sick_scan_xd and its map generated by hector_slam on ROS 1: + +![slam_example_ros1_nav350.png](doc/screenshots/slam_example_ros1_nav350.png) + +### NAV350 ROS 2 SLAM example + +Install ths ROS 2 slam-toolbox with `sudo apt install ros-foxy-navigation2 ros-foxy-nav2-bringup ros-foxy-slam-toolbox` (replace `foxy` by your ros distribution). + +Build sick_scan_xd for ROS 2 as described in [INSTALL ROS 2](#install-on-ros-2) + +Run rviz2, sick_scan_xd, slam_toolbox and static transforms: +``` +source ./install/setup.bash +ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_slam_nav350.rviz & +ros2 launch sick_scan sick_nav_350.launch.py hostname:=192.168.0.1 tf_publish_rate:=0 & +ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link cloud & +ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_footprint base_link & +ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 odom base_footprint & +ros2 launch nav2_bringup navigation_launch.py & +ros2 launch slam_toolbox online_async_launch.py & +``` + +> **_NOTE:_** Laserscan messages need to be remapped to topic `/scan` (default is `/sick_nav_350/scan`). Use `remappings=[ ('/sick_nav_350/scan', '/scan'), ]` in the launchfile, e.g.: +``` +node = Node( + package='sick_scan', + executable='sick_generic_caller', + output='screen', + remappings=[ ('/sick_nav_350/scan', '/scan'), ], # remap laserscan messages to topic /scan +) +``` + +The following rviz2 screenshot shows an example of a NAV350 laserscan created by sick_scan_xd and its map generated by slam_toolbox on ROS 2: + +![slam_example_ros2_nav350.png](doc/screenshots/slam_example_ros2_nav350.png) + +### picoScan100 ROS 1 SLAM example + +Run rviz, sick_scan_xd with picoScan100 and hector_slam: +``` +source ./devel_isolated/setup.bash +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_slam_multiscan.rviz & +roslaunch sick_scan sick_nav_350.launch hostname:=192.168.0.1 hostname:=127.0.0.1 udp_receiver_ip:=192.168.0.100 tf_publish_rate:=0 & +roslaunch sick_scan test_200_slam_ros1_hector.launch scan_topic:=/sick_picoscan/scan_fullframe scan_layer_0_frame_id:=world_1 cloud_frame_id:=world & +``` +Replace ip address `192.168.0.100` with the ip address of your local machine running sick_scan_xd. + +The following rviz screenshot shows an example of a picoScan100 point cloud created by sick_scan_xd and its map generated by hector_slam on ROS 1: + +![slam_example_ros1_picoscan.png](doc/screenshots/slam_example_ros1_picoscan.png) + +### MRS1104 SLAM support + +MRS1104 provides 4 layers covering elevation angles at -2.5°, 0.0°, 2.5° and 5.0°. The layer with 0.0° is used for SLAM by default. The following rviz screenshot shows an example of a MRS1104 point cloud created by sick_scan_xd and its map generated by hector_slam on ROS 1: + +![slam_example_ros1_mrs1104.png](doc/screenshots/slam_example_ros1_mrs1104.png) + +Since Hector-Slam expects only one laser scan frame with a unique identifier for the laser scans, the following parameters were added to the driver. + +slam_echo: The name of the echo is entered here, which is filtered out of all possible 12 echoes. This should be "laser_POS_000_DIST1". This exports the first hit in the position with an elevation angle of 0°. If you want to use the layers with elevation angles -2.5°, 2.5° and 5.0°, you can set another flag with the name slam_bundle to True. If this flag is set, the oblique distances are multiplied by the cosine in this direction to obtain the projection onto the XY plane. This quadruples the number of points and increases the scan rate from 12.5 Hz to 50 Hz. However, for oblique impact surfaces (i.e. no vertical walls) this method can lead to larger estimation errors. In this case slam_bundle should be set to false. + +### Google Cartographer + +The support of Google Cartographer was made possible by a number of extensions to the driver. On the driver side, the MRS1104 is prepared to support the Google Cartographer. The Google Cartographer expects data packets at a high recording density (several hundred packets per second) to perform the SLAM algorithm. For this reason, an option has been introduced that allows the scans to be chopped into small angular ranges. The time stamps for these small ranges were converted accordingly. + +Setup Google Cartographer (these steps are for illustration only, you must adapt these lines to your local directory names) + +1. Login to Ubuntu. +2. Open multiple terminals. +3. Terminal 1: + . ros1_start.sh + roscore +4. Terminal 2: + . ros1_start.sh + cd ~/ros_catkin_ws + source ./devel/setup.bash +5. Terminal 3: + roslaunch sick_scan_xd sick_mrs_1xxx_cartographer.launch cloud_topic:=horizontal_laser_3d frame_id:=horizontal_vlp16_link +6. Terminal 4: + roslaunch sick_scan_xd sick_tim_5xx.launch cloud_topic:=vertical_laser_3d frame_id:=vertical_vlp16_link hostname:=192.168.0.71 +7. Terminal 5: + + * . ros1_start.sh + * cd ~/ros_cartographer_ws + * source ./install_isolated/setup.bash + * catkin_make_isolated + * roslaunch cartographer_ros live_demo_backpack_3d.launch + +**Example output** + +The following figure shows an example of an outdoor slam result using a MRS1104: + +![slam_example](doc/slam_example.png) + +### OctoMap + +[OctoMap](https://github.com/OctoMap) models a 3D occupancy map. The octomap_server builds and distributes volumetric 3D occupancy maps from a 3D point cloud. Tutorials and examples can be found e.g. in [3D Mapping with OctoMap](https://www.arminhornung.de/Research/pub/hornung13roscon.pdf), [octomap_tutorial](https://github.com/tejalbarnwal/octomap_tutorial) and [Basic usage of octomap_mapping](https://www.youtube.com/watch?v=dF2mlKJqkUg). Note that OctoMap is not a fully SLAM algorithm, but it can create 2D and 3D maps from point clouds. + +Run the following steps to build and run OctoMap and sick_scan_xd with a multiScan100 lidar on ROS 1: +1. Clone OctoMap + sick_scan_xd: + ``` + pushd src + git clone https://github.com/SICKAG/sick_scan_xd.git + git clone https://github.com/OctoMap/octomap_ros.git + git clone https://github.com/OctoMap/octomap_msgs.git + git clone https://github.com/OctoMap/octomap_mapping.git + popd + ``` +2. Set topic and frame_id for multiScan100 in octomap_mapping.launch: + ``` + + + ``` +3. Build: + ``` + rm -rf ./build ./devel ./install ./build_isolated ./devel_isolated ./install_isolated ./log + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -Wno-dev + ``` +4. Run OctoMap + sick_scan_xd: + ``` + # Run sick_scan_xd + multiScan100 + roslaunch sick_scan_xd sick_multiscan.launch hostname:="192.168.0.1" udp_receiver_ip:=" 192.168.0.100" + # Run octomap_server + roslaunch octomap_server octomap_mapping.launch + ``` + Replace parameter "hostname" with the ip address of the multiScan100 lidar and "udp_receiver_ip" with the ip address of the PC running sick_scan_xd. +5. Visualize OctoMap with rviz: + * Add MarkerArray topic "/occupied_cells_vis_array“ (colored voxels) + * Add Map topic "/projected_map“ (gray 2D Projection) +6. Save the OctoMap: + ``` + rosrun octomap_server octomap_saver -f ./octomap_multiscan.bt + ``` +7. Publish the saved OctoMap: + ``` + rosrun octomap_server octomap_server_node ./octomap_multiscan.bt + ``` +The following screenshot shows an example of an octomap created from a multiScan100 point cloud: + +![octomap_example_ros1_multiscan](doc/screenshots/octomap_example_ros1_multiscan.png) + +### RTAB-Map + +[RTAB-Map](https://introlab.github.io/rtabmap/) (Real-Time Appearance-Based Mapping) is a RGB-D, Stereo and Lidar Graph-Based SLAM approach, which can be used for 3D-SLAM in combination with multiScan100 or other SICK lidars. sick_scan_xd provides a 3D-SLAM example using RTAB-Map with the multiScan100 lidar. The following section describes how to install and run RTAB-Map with sick_scan_xd and a multiScan. + +#### Install on ROS 1 + +Run the following steps to build rtabmap and sick_scan_xd with on ROS 1: + +1. Build the prerequisites for RTAB-Map: + ``` + sudo apt-get install libboost-all-dev + sudo apt-get install libeigen3-dev + sudo apt-get install libsdl-image1.2-dev + sudo apt-get install libsdl-dev + sudo apt-get install ros-noetic-nav-msgs + sudo apt-get install ros-noetic-tf2-sensor-msgs + sudo apt-get install ros-noetic-imu-filter-madgwick + sudo apt-get install python3-wstool + sudo apt-get install ros-noetic-scan-tools + pushd /tmp + mkdir -p libnabo/build && pushd libnabo/build + cmake .. + cmake --build . + sudo cmake --build . --target install + popd + mkdir -p libpointmatcher/build && pushd libpointmatcher/build + cmake .. + make -j4 + sudo make install + popd + sudo ldconfig + mkdir -p rtabmap/build && pushd rtabmap/build + cmake .. + make -j4 + sudo make install + popd + sudo ldconfig + popd + ``` +2. Build RTAB-Map and sick_scan_xd in your workspace: + ``` + pushd src + git clone https://github.com/ros-planning/navigation.git + git clone https://github.com/ros-planning/navigation_msgs.git + git clone https://github.com/introlab/rtabmap_ros.git + git clone https://github.com/SICKAG/sick_scan_xd.git + popd + rm -rf ./build ./devel ./install ./build_isolated ./devel_isolated ./install_isolated ./log + catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -Wno-dev + sudo ldconfig + ``` + +Run `sudo ldconfig` if you encounter errors while loading shared libraries. + +Note that building rtabmap with libpointermatch is highly recommended. + +#### Run RTAB-MAP and multiScan100 on ROS 1 + +[sick_multiscan_rtabmap.launch](launch/sick_multiscan_rtabmap.launch) provides a launch file to run 3D-SLAM using rtabmap and sick_scan_xd with a multiScan100 lidar. The SLAM parameters are just examples taken from [rtabmap_examples/launch/test_ouster.launch](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_examples/launch/test_ouster.launch). Run `rosrun rtabmap_slam rtabmap --params` to see all RTAB-Map options, parameters and their meaning and adopt launch file [sick_multiscan_rtabmap.launch](launch/sick_multiscan_rtabmap.launch) if required. + +Run the following command to start rtabmap and sick_scan_xd with a multiScan100 lidar: +``` +source ./install_isolated/setup.bash +roslaunch sick_scan_xd sick_multiscan_rtabmap.launch hostname:="191.168.0.1" udp_receiver_ip:=" 191.168.0.100" +``` +Replace parameter "hostname" with the ip address of the multiScan100 lidar and "udp_receiver_ip" with the ip address of the PC running sick_scan_xd. The following screenshot shows an example of RTAB-MAP and a multiScan100 point cloud: + +![rtabmap_example_ros1_multiscan](doc/screenshots/rtabmap_example_ros1_multiscan.png) + +To visualize SLAM results, add e.g. topics `/rtabmap/grid_map`, `/rtabmap/localization_pose` and `/rtabmap/odom` in rviz. + +rtabmap provides services to switch to mapping or localization mode: +``` +rosservice call /rtabmap/resume # resume after pause +rosservice call /rtabmap/trigger_new_map # start a new map +rosservice call /rtabmap/set_mode_mapping # set mapping mode +rosservice call /rtabmap/set_mode_localization # set localization mode +``` + +Alternatively, you can use the options in rtabmap-viz: + +![rtabmap_viz_options](doc/screenshots/rtabmap_viz_options.png) + +#### Install on ROS 2 + +Building rtabmap and sick_scan_xd on ROS 2 is similar to ROS 1. Run the following steps to build rtabmap and sick_scan_xd with on ROS 2: + +1. Build the prerequisites for RTAB-Map: + ``` + sudo apt-get install libboost-all-dev + sudo apt-get install libeigen3-dev + sudo apt-get install libsdl-image1.2-dev + sudo apt-get install libsdl1.2-dev + sudo apt-get install ros-humble-nav-msgs + sudo apt-get install ros-humble-tf2-sensor-msgs + sudo apt-get install ros-humble-imu-filter-madgwick + sudo apt-get install python3-wstool + sudo apt-get install ros-humble-scan-tools + sudo apt install ros-humble-pcl-ros + pushd /tmp + git clone https://github.com/introlab/rtabmap.git rtabmap + git clone https://github.com/ethz-asl/libnabo.git libnabo + git clone https://github.com/ethz-asl/libpointmatcher.git libpointmatcher + mkdir -p libnabo/build && pushd libnabo/build + cmake .. + cmake --build . + sudo cmake --build . --target install + popd + mkdir -p libpointmatcher/build && pushd libpointmatcher/build + cmake .. + make -j4 + sudo make install + popd + sudo ldconfig + mkdir -p rtabmap/build && pushd rtabmap/build + cmake .. + make -j4 + sudo make install + popd + sudo ldconfig + popd + ``` +2. Build RTAB-Map and sick_scan_xd in your workspace: + ``` + pushd src + git clone --branch ros2 https://github.com/introlab/rtabmap_ros.git rtabmap_ros + git clone https://github.com/SICKAG/sick_scan_xd.git + popd + rosdep update && rosdep install --from-paths src --ignore-src -r -y + rm -rf ./build ./devel ./install ./build_isolated ./devel_isolated ./install_isolated ./log + colcon build --symlink-install --cmake-args " -DROS_VERSION=2" " -DCMAKE_ENABLE_EMULATOR=1" "-DCMAKE_BUILD_TYPE=Release" --event-handlers console_direct+ + sudo ldconfig + ``` + +#### Run RTAB-MAP and multiScan100 on ROS 2 + +[sick_multiscan_rtabmap.launch.py](launch/sick_multiscan_rtabmap.launch.py) provides a launch file to run 3D-SLAM using rtabmap and sick_scan_xd with a multiScan100 lidar. The SLAM parameters are examples taken from [rtabmap_examples/launch/test_ouster.launch](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_examples/launch/test_ouster.launch). + +Run the following command to start rtabmap and sick_scan_xd with a multiScan100 lidar: +``` +source ./install/setup.bash +ros2 launch sick_scan_xd sick_multiscan_rtabmap.launch.py hostname:="191.168.0.1" udp_receiver_ip:=" 191.168.0.100" +``` +Replace parameter "hostname" with the ip address of the multiScan100 lidar and "udp_receiver_ip" with the ip address of the PC running sick_scan_xd. + +rtabmap provides services to switch to mapping or localization mode: +``` +ros2 service call /rtabmap/resume std_srvs/srv/Empty # resume after pause +ros2 service call /rtabmap/trigger_new_map std_srvs/srv/Empty # start a new map +ros2 service call /rtabmap/set_mode_mapping std_srvs/srv/Empty # set mapping mode +ros2 service call /rtabmap/set_mode_localization std_srvs/srv/Empty # set localization mode +``` + + +## Raspberry Pi Support + +sick_scan_xd supports Linux on Raspberry Pi 4. Follow the build instructions for Linux to run sick_scan_xd on a Raspberry: +* [Build on Linux generic without ROS](#build-on-linux-generic-without-ros) +* [Build on Linux ROS 1](#build-on-linux-ros-1) +* [Build on Linux ROS 2](#build-on-linux-ros-2) + +Cmake option " -DRASPBERRY=1" activates compiler settings for the Raspberry. Laserscan messages and polar pointclouds are not published on the Raspberry due to performance reasons. + +### multiScan100 example + +The following screenshot shows sick_scan_xd running under ROS 1 on a Raspberry Pi 4 connected to a multiScan100 lidar. A Linux-PC uses rviz to display the fullframe point cloud generated on the Raspberry. The ssh-terminal shows the sick_scan_xd log messages on the Raspberry: +![screenshot raspberry performance test](doc/screenshots/raspberry-perftest-04.png) + +On a Raspberry Pi 4, sick_scan_xd processes 240 messages/second with a mean latency of 2.7 milliseconds/message. + +### Performance + +Due to the low power consumption of a Raspberry Pi, performance is critical for applications using sick_scan_xd, especially for multiScan100 lidars. + +Symptoms for performance problems can be e.g.: +* sick_scan_xd reports the loss of UDP packets or message drops +* sick_scan_xd does not publish the fullframe pointcloud +* rviz shows flickering segment pointclouds even with increased decay time +* low frequency of segment or fullframe pointcloud messages +* generally high system load + +Performance problems can have very different reasons. Notes to help with the elimination of performance issues: + +1. Use the latest Raspberry Pi 4. Previous Raspberry Pi models may work with sick_scan_xd, but are not supported officially. + +2. Eliminate multiple echos. For most lidars, the echo filter is activated by default and only the last echo is transmitted. Check the launch file configuration and set parameter `filter_echos` if not yet done: + ``` + + ``` + For multican lidars, the echo filter is activated in the launch file by parameter `host_FREchoFilter`: + ``` + + + ``` + +3. Run a basic performance test on ROS 2 using a tiny sopas test server and a udp player to emulate a multiscan: + ``` + # Start multiScan100 emulator (sopas test server) + python3 ./src/sick_scan_xd/test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & + # Start rviz + ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_multiscan_emu_360_perftest.rviz & + sleep 1 + # Start sick_generic_caller with sick_scansegment_xd + ros2 launch sick_scan sick_multiscan.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" & + sleep 3 + # Play udp packets to emulate multiScan + python3 ./src/sick_scan_xd/test/python/multiscan_perftest_player.py --udp_port=2115 --repeat=100 --send_rate=100 --verbose=0 --prompt=0 + ``` + The result should look like the follwing screenshot: + ![screenshot raspberry performance test](doc/screenshots/raspberry-perftest-01.png) + If you otherwise observe the loss of UDP packets, message drops, missing pointclouds or mean latency times significantly higher than 6 milliseconds/message, check the system load of your Raspberry and try to eliminate cpu or network intensive processes. + +4. Start sick_scan and the sopas test server on the Raspberry as above, but run the udp player `multiscan_perftest_player.py` on another PC in your local subnet, e.g. + ``` + python3 multiscan_perftest_player.py --dst_ip=192.168.1.27 --udp_port=2115 --repeat=1000 --send_rate=0 --force_delay=3.0e-3 --verbose=0 --prompt=0 + ``` + Replace the example ip adress `192.168.1.27` by the ip adress of your Raspberry. The result should look like the follwing screenshot: + ![screenshot raspberry performance test](doc/screenshots/raspberry-perftest-02.png) + If you otherwise observe the loss of UDP packets, message drops, missing pointclouds or mean latency times significantly higher than 6 milliseconds/message, check the system load of your Raspberry and try to eliminate cpu or network intensive processes. sick_scan_xd (i.e. process sick_generic_caller) should consume ca. 80% of one core resp. cause ca. 20% of the total cpu load. + +### Troubleshooting + +#### Endianess + +ARM processors support both little and big endian mode. sick_scan_xd has been tested on Raspberry Pi 4 using ROS 1 and ROS 2 on Linux in little endian mode. You can check the endianess of your system with `lscpu`. + +#### Build sick_scan_xd on a Raspberry without internet or github access + +Checkout sick_scan_xd and use `scp -rp` to copy files and directories recursively from local host to a Raspberry, e.g.: + +On your local Linux PC (Raspberry IP-address is 192.168.178.52 in this example): +``` +mkdir -p ./sick_scan_xd_raspberry_pi_pretest/src +pushd ./sick_scan_xd_raspberry_pi_pretest/src +git clone https://github.com/SICKAG/libsick_ldmrs.git +git clone -b master https://github.com/SICKAG/sick_scan_xd.git +popd +scp -rp ./sick_scan_xd_raspberry_pi_pretest 192.168.178.52:/home/rostest/sick_scan_xd_raspberry_pi_pretest +``` + +On your Raspberry Pi (ROS 1): +``` +cd /home/rostest/sick_scan_xd_raspberry_pi_pretest +pushd ./src/sick_scan_xd/test/scripts +chmod a+x ./*.bash +./makeall_ros1.bash +popd +source ./devel_isolated/setup.bash +``` + +To view the pointcloud on your local Linux PC (with Raspberry IP-address is 192.168.178.52 in this example): +``` +export ROS_MASTER_URI=http://192.168.178.52:11311/ +rviz +``` + +## More Tools + +Various tools exist in the repository to improve the operation of the scanners. It is also recommended to read the section [Troubleshooting](#Troubleshooting). +Overview of the tools: + +* Search for scanner in the network: + Use the Python3 tool "sick_generic_device_finder.py" in the tools/sick_generic_device_finder directory. + The tools will output the IP addresses of the connected scanners and some more information about the scanner. + Call it with python3, i.e. + `` + python3 sick_generic_device_finder.py + `` +* Setting new IP address: With the help of the parameter "new_IP" a new IP address can be assigned when calling the node sick_scan_xd. + The launch file sick_new_ip.launch in the launch directory shows an example of how to use this parameter. +* Converting of pointclouds to images: With the tool pcl_converter.cpp one can convert pointcloud2-data + to image. That is especial convenient for 24-layers scanners like the MRS6124. +* Setting up a brand new scanner: To set up a brand new scanner, + it is recommended to use the two tools "sick_generic_device_finder.py" to find the scanner in the network + and the launch file sick_new_ip.launch to set a new IP address. If further settings are to be saved that cannot be made via ROS parameters, we recommend using the Windows tool "Sopas ET" from SICK. +* Unit tests: For a quick unit test after installation without the sensor hardware, a test server is provided to simulate a scanner. See [Simulation](./CONTRIBUTING.md) for further details. +* Testing: The sick_scan_test program was developed for testing the driver. See [Testing](./CONTRIBUTING.md) for details. + + +# Device specific information + +## picoScan100/multiScan100 + +The multiScan100 and picoScan100 are new lidars from Sick. multiScan100 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 or compact format over UDP. + +multiScan100/picoScan100 lidars are supported by sick_scan_xd. +The following describes the configuration, validation and test in more detail. + +### Configuration + +multiScan100 is configured by launch file [sick_multiscan.launch](launch/sick_multiscan.launch). +picoScan100 is configured by launch file [sick_picoscan.launch](launch/sick_picoscan.launch). + +Modify file [sick_multiscan.launch](launch/sick_multiscan.launch) resp. [sick_picoscan.launch](launch/sick_picoscan.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. + +The ip address of the lidar and the udp receiver can be configured in the launch file by e.g. +``` + + +``` +or by command line by e.g. +``` +# Run sick_scansegment_xd generic without ROS: +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_xd 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_xd sick_multiscan.launch.py hostname:=192.168.0.1 udp_receiver_ip:=192.168.0.100 +``` + +### IMU support + +IMU support for multiScan100 and picoScan100 is enabled by default and can be configured in the launchfile: +``` + + + +``` + +sick_scan_xd receives IMU data by UDP and publishes [ROS 1 sensor_msgs/Imu](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Imu.html) resp. [ROS 2 sensor_msgs/msg/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html) messages. + +> **_NOTE:_** IMU support requires compact format, which is the default. If msgpack communication is configured, imu support is automatically disabled. + +IMU support for picoScan100 requires firmware version 1.1 or newer, see https://www.sick.com/de/en/downloads/media/swp680096 for firmware downloads. + +### SOPAS support + +On ROS 1 and ROS 2, service `ColaMsg` is provided to send CoLa commands to the lidar. Using this service, filters can be applied during runtime. + +[See](#sopas-support-for-sick_scan_segment_xd) here for examples. + +See the manual for further information of filter settings and parameter. + +The driver sends the following SOPAS start and stop sequence at program start resp. exit (example with default ip address 192.168.0.1): +``` +// Prerequirement: measurement is active, but no UDP data is sent +// Start sending scan data output +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 // for multiscan136 only +sWN ScanDataEnable 1 // enable scan data ouput +sMN LMCstartmeas // start measurement +sMN Run // apply the settings and logout +// ... +// UDP data is sent +// ... +// Stop sending scan data output +sMN SetAccessMode 3 F4724744 // set authorization level for writing settings +sWN ScanDataEnable 0 // disable scan data output +sMN Run // apply the settings and logout +// No UDP data is sent anymore +``` + +### Run multiScan100 and picoScan100 simultaneously + +The following example shows a multiScan100 and a picoScan100 device running simultaneously on ROS 1. The ip address of the multiScan100 is `192.168.0.1` (default), the ip address of the picoScan100 has been set to `192.168.0.2`. The Linux-PC running sick_scan_xd uses ip address `192.168.0.100`. `fping -a -q -g 192.168.0.0/24` shows all available devices in subnet `192.168.0.x`: + +![multiple_lidars_01.png](doc/screenshots/multiple_lidars_01.png) + +| device | ip | +| ------------- | ------------ | +| 192.168.0.1 | multiScan100 | +| 192.168.0.2 | picoScan100 | +| 192.168.0.100 | Linux-PC | + +Open 192.168.0.1 and 192.168.0.2 in a browser to view the network settings with SOPAS Air: + +![multiple_lidars_02.png](doc/screenshots/multiple_lidars_02.png) + +The frame ids and ros topics of both lidars should be configured differently. Copy both launchfiles (sick_multiscan.launch and sick_piocscan.launch in this example) e.g. to lidar1.launch and lidar2.launch and replace ros topics and frame ids, e.g. + * replace all "topic=/cloud_" by "topic=/cloud1_" in lidar1.launch + * replace all "topic=/cloud_" by "topic=/cloud2_" in lidar2.launch + * replace all "frameid=world" by "frameid=world1" in lidar1.launch + * replace all "frameid=world" by "frameid=world2" in lidar2.launch + +![multiple_lidars_03.png](doc/screenshots/multiple_lidars_03.png) + +![multiple_lidars_04.png](doc/screenshots/multiple_lidars_04.png) + +Provide the launchfiles with `catkin_make_isolated --install --cmake-args -DROS_VERSION=1`. + +Then launch sick_scan_xd twice with two different launchfiles, ip addresses, node names, udp ports, topic and frame ids. + +Example: + +` +roslaunch sick_scan_xd lidar1.launch hostname:=192.168.0.1 udp_receiver_ip:=192.168.0.100 nodename:=lidar1 udp_port:=2115 imu_udp_port:=7503 publish_frame_id:=world1 publish_laserscan_segment_topic:=scan1_segment publish_laserscan_fullframe_topic:=scan1_fullframe imu_topic:=imu1 & +` + +` +roslaunch sick_scan_xd lidar2.launch hostname:=192.168.0.2 udp_receiver_ip:=192.168.0.100 nodename:=lidar2 udp_port:=2116 imu_udp_port:=7504 publish_frame_id:=world2 publish_laserscan_segment_topic:=scan2_segment publish_laserscan_fullframe_topic:=scan2_fullframe imu_topic:=imu2 & +` + +Rviz shows the point clouds of both lidars running simultaneously, with frame id `world1` for lidar1 (multiScan) and frame id `world2` for lidar2 (picoScan): + +![multiple_lidars_05.png](doc/screenshots/multiple_lidars_05.png) + +![multiple_lidars_06.png](doc/screenshots/multiple_lidars_06.png) + +If the 6D poses of the lidars are known, their coordinates can be transformed to a common frame by a static_transform_publisher. Example: + +``` +rosrun tf static_transform_publisher 0 0 0 0 0 0 world world1 100 & +rosrun tf static_transform_publisher 0 0 0 0 0 0 world world2 100 & +``` + +![multiple_lidars_07.png](doc/screenshots/multiple_lidars_07.png) + +The big purple dots show the picoScan100 pointcloud, the other points are the multiScan100 point clouds. Both are transformed to the common frame id `world`. Note that both point clouds do not match exactly, because the 6D poses are just assumed to be (x=0, y=0, z=0, yaw=0, pitch=0, roll=0) in this example. + +### Visualization + +The multiScan100 and picoScan100 scans can be visualized by rviz. The following screenshots show two examples of a multiScan100 pointcloud: + +![msgpacks-emulator-rviz](doc/20210929-tokenized-msgpacks-emulator-rviz.png) +![msgpacks-emulator-rviz](doc/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_fullframe` collects all segments for a complete 360 degree full scan (360 degree for multiScan100, 276 degree for picoscan100). + +Pointcloud callbacks defined in the [API](#generic-driver-api) 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_unstructured_segments` (topic `/cloud` for sick_scan_xd version 2.10 or earlier) 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_unstructured_fullframe` (topic `/cloud_fullframe` for sick_scan_xd version 2.10 or earlier) 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](doc/sick_scan_segment_xd_01.png) + +* 1 full scan pointcloud concatenating all 12 segments:
+ ![sick_scan_segment_xd_02.png](doc/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. + +### Customized point clouds + +Pointclouds can be customized, i.e. the fields and points can be configured by launch file [sick_multiscan.launch](launch/sick_multiscan.launch). + +Parameter "custom_pointclouds" lists all customized pointclouds to be published. Each pointcloud is given by its name, e.g: +``` + +``` +This example publishes 5 types of pointclouds: +* Pointcloud for each segment in cartesian coordinates (x,y,z,i), named "cloud_unstructured_segments" +* Pointcloud for each fullframe in cartesian coordinates (x,y,z,i), named "cloud_unstructured_fullframe" +* Pointcloud for each segment in polar coordinates (azimuth,elevation,range,i), named "cloud_polar_unstructured_segments" +* Pointcloud for each fullframe in polar coordinates (azimuth,elevation,range,i), named "cloud_polar_unstructured_fullframe" +* Pointcloud for each fullframe with all available fields (x,y,z,i,range,azimuth,elevation,layer,echo,reflector), named "cloud_all_fields_fullframe" + +These 5 pointclouds are published by default. + +The properties of the pointcloud, i.e. their fields and points, are configured by the pointcloud name, e.g. pointcloud "cloud_unstructured_segments" (i.e. the segment pointcloud in cartesian coordinates): +``` + + +``` +The cloud property configuration is a list of key-value-pairs, where each key-value-pair specifies a property and its value. E.g. `topic=/cloud_unstructured_segments frameid=world` defines ros topic "/cloud_unstructured_segments" and frame id "world" for the pointcloud named "cloud_unstructured_segments". + +The following key-value-pairs of a customized pointcloud are currently supported: + +* Parameter "coordinateNotation" is an enum to configure pointcloud coordinates: + * coordinateNotation=0: cartesian (default, pointcloud has fields x,y,z,i), identical to customized with fields=x,y,z,i + * coordinateNotation=1: polar (pointcloud has fields azimuth,elevation,r,i), identical to customized with fields=azimuth,elevation,range,i + * coordinateNotation=2: both cartesian and polar (pointcloud has fields x,y,z,azimuth,elevation,r,i), identical to customized with fields=x,y,z,azimuth,elevation,range,i + * coordinateNotation=3: customized pointcloud fields, i.e. the pointcloud has fields configured by parameter "fields" + +* Parameter "updateMethod" is an enum to configure fullframe pointclouds versus segmented pointcloud: + * updateMethod=0: fullframe pointcloud (default) + * updateMethod=1: segmented pointcloud + +* Parameter "fields" defines the fields of the pointcloud for coordinateNotation == 3 (customized pointcloud fields), e.g. + * fields=x,y,z,i: cartesian pointcloud + * fields=range,azimuth,elevation: polar pointcloud + * or any other combination of x,y,z,i,range,azimuth,elevation,t,ts,lidar_sec,lidar_nsec,ring,layer,echo,reflector + + These fields have the following meaning: + * field "x": cartesian x coordinate in meter in ROS coordinates (4 byte, float32) + * field "y": cartesian y coordinate in meter in ROS coordinates (4 byte, float32) + * field "z": cartesian z coordinate in meter in ROS coordinates (4 byte, float32) + * field "i": intensity (4 byte, float32) + * field "range": polar coordinate range in meter (4 byte, float32) + * field "azimuth": polar coordinate azimuth in radians (4 byte, float32) + * field "elevation": polar coordinate elevation in radians (4 byte, float32) + * field "t": time offset in nano seconds relative to the header timestamp in the point cloud (4 byte, uint32), used by rtabmap for deskewing + * field "ts": time offset in seconds relative to the header timestamp (4 byte, float32) + * field "lidar_sec": uint32 seconds part of the lidar timestamp in microseconds (lidar time), lidar_sec = (uint32_t)(lidar_timestamp_microsec / 1000000) + * field "lidar_nsec": uint32nano seconds part of the lidar timestamp in microseconds (lidar time), lidar_nsec = (uint32_t)(1000 * (lidar_timestamp_microsec % 1000000)) + * field "ring": layer id (1 byte, int8), identical to field "layer" + * field "layer": layer (group) index (4 byte, int32), 0 <= layer < 16 for multiScan100 (16 layer), 0 for picoScan100 (1 layer) + * field "echo": echo index (4 byte, int32) + * field "reflector": optional reflector bit (1 byte, uint8), 0 or 1, default: 0 + +* Parameter "echos" defines which echos are included in the pointcloud, e.g. + * echos=0,1,2: all echos + * echos=2: last echo + or any other combination of 0,1,2 + +* Parameter "layers" defines which echos are included in the pointcloud, e.g + * layers=0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 for all layers + * layers=5 for the 0 degree layer + or any other combination of 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 + +* Parameter "reflectors" filters the points by the reflector bit, i.e. + * reflectors=0,1 for points with reflector bit set or not set + * reflectors=0 for points with reflector bit not set + * reflectors=1 for points with reflector bit set + +* Parameter "infringed" defines filters the points by infringement, i.e. + * infringed=0,1 for points with infringement bit set or not set + * infringed=0 for points with infringement bit not set + * infringed=1 for points with infringement bit set + Parameter "infringed" is currently not supported (reserved for future use) + +* Parameter "topic" defines the ros topic, e.g. topic=/cloud_fullframe for cartesian fullframe pointclouds + +* Parameter "frameid" defines the ros frame of the pointcloud, e.g. frameid=world, frameid=map or frameid=base_link + +* Parameter "publish" activates or deactivates the pointcloud, e.g. publish=1 to generate and publish, or publish=0 to deactivate that pointcloud + +To add a new pointcloud, define a pointcloud name (e.g. "cloud_layer7_cartesian"), add "cloud_layer7_cartesian" in parameter "custom_pointclouds" and specify a new parameter "cloud_layer7_cartesian" with the new cloud properties, e.g. +``` + + +``` + +The following pointclouds are currently predefined in launch file [sick_multiscan.launch](launch/sick_multiscan.launch): +``` + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +``` + +> **_NOTE:_** The sick_scan_xd API callback functions `SickScanApiRegisterCartesianPointCloudMsg` and `SickScanApiRegisterPolarPointCloudMsg` provide cartesian and polar pointclouds, i.e. pointclouds configured with `coordinateNotation=0` (cartesian) or `coordinateNotation=1` (polar). Pointclouds with `coordinateNotation=2` (cartesian + polar) or `coordinateNotation=3` (customized fields) are currently not supported by the generic API. + +### Customized point clouds on a Raspberry Pi + +Performance is critical on a Raspberry. To reduce the cpu load, you may restrict the number of pointclouds to the minimum required for your application. E.g. if you just need the cartesian fullframe pointcloud, you can use +``` + +``` +to decrease the cpu usage. + +### 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 for multiScan100, 276 degree for picoScan100) + +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. + +The msgpack validation is configured in file [sick_multiscan.launch](launch/sick_multiscan.launch) resp. [sick_picoscan.launch](launch/sick_picoscan.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. + +### Firewall configuration + +By default, UDP communication is allowed on localhosts. To enable udp communication between 2 different machines, firewalls have to be configured. + +On Windows: Setup the windows firewall to allow sick_scan_xd to receive udp packages on port 2115. +To pass udp packages from a remote sender, the default rule for incoming udp packages has to be configured in the windows firewall: +1. Run "wf.msc" as admin, +2. Click Inbound Rules and locate the rule(s) for lidar3d_msr100_recv (resp. python to allow python test scripts), and +3. Deactivate the UDP-rule for this process(es) or configure exceptions for remote computers. +4. Alternatively, you can create a new rule allowing udp communication on port 2115. + +On Linux: Run the following commands to allow any udp communication on port 2115: +``` +sudo iptables -A INPUT -p udp -m udp --dport 2115 -j ACCEPT +sudo iptables -A OUTPUT -p udp -m udp --sport 2115 -j ACCEPT +sudo iptables-save +``` +Alternatively, you can also use +``` +sudo ufw allow from any to any port 2115 proto udp +``` +to allow all udp traffic on port 2115. + +> **_NOTE:_** If Linux or Windows is running in a virtual machine, make sure UDP port 2115 is forwarded. With VMware Workstation Pro, you can configure port forwarding +using the Virtual Network Editor. Udp echos, delays, drops and other unexpected errors might occur when more than one network card is configured in VMware. +Make sure you have only one network adapter activated with custom NAT: + +![vmware_network_settings](doc/vmware_network_settings.png) + +### SOPAS support for sick_scan_segment_xd + +On ROS 1 and ROS 2, sick_scan_segment_xd provides ros service `ColaMsg` to send CoLa commands to the lidar. Using this service, filters can be applied to multiScan136 and picoScan150 lidars during runtime. + +Examples on ROS 1: +``` +rosservice list +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN IsSystemReady'}" # response: "sAN IsSystemReady 1" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115'}" # response: "sWA ScanDataEthSettings" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataFormat 1'}" # response: "sWA ScanDataFormat" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataPreformatting 1'}" # response: "sWA ScanDataPreformatting" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataEnable 1'}" # response: "sWA ScanDataEnable" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN LMCstartmeas'}" # response: "sAN LMCstartmeas" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN Run'}" # response: "sAN Run 1" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 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" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN FREchoFilter 1'}" # response: "sWA FREchoFilter" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1'}" # response: "sWA LFPangleRangeFilter" +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" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN Run'}" # response: "sAN Run 1" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" +rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 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" +``` + +> **_NOTE:_** LFPlayerFilter is not supported by picoscan150. + +Examples on ROS 2: +``` +ros2 service list +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN IsSystemReady'}" # response: "sAN IsSystemReady 1" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115'}" # response: "sWA ScanDataEthSettings" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN ScanDataFormat 1'}" # response: "sWA ScanDataFormat" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN ScanDataPreformatting 1'}" # response: "sWA ScanDataPreformatting" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN ScanDataEnable 1'}" # response: "sWA ScanDataEnable" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN LMCstartmeas'}" # response: "sAN LMCstartmeas" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN Run'}" # response: "sAN Run 1" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN LFPlayerFilter'}" # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN FREchoFilter 1'}" # response: "sWA FREchoFilter" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1'}" # response: "sWA LFPangleRangeFilter" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1'}" # response: "sWA LFPlayerFilter" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN Run'}" # response: "sAN Run 1" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1" +ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN LFPlayerFilter'}" # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" +``` + +> **_NOTE:_** LFPlayerFilter is not supported by picoscan150. + + +## TiM781 and TiM781S + +For TiM781S lidars, the initial lidar configuration can be deactivated using optional argument initialize_scanner:=0. +Note that this mode does not initialize the lidar. The mode assumes that the scanner is in an appropriate state corresponding to the properties configured in the launchfile. It is not recommended to use this option unless for specific tasks in a controlled environment. + +**Do not use this mode except the lidar has been configured properly and initialized successfully and is in the same state as after initialization by the launchfile! This option is for advanced users only!** + +Example: `roslaunch sick_scan_xd sick_tim_7xxS.launch hostname:=192.168.0.1 initialize_scanner:=0` + +### Field evaluation TiM7xx + +The field evaluation for TiM7xx lidars support two additional options to configure the active field set: FieldSetSelectionMethod and ActiveFieldSet. These options allow to set the active field set during runtime, see the operation manual for details. + +Options FieldSetSelectionMethod and ActiveFieldSet can be configured in the launch file and by ros services "FieldSetRead" and "FieldSetWrite". + +Initial values for FieldSetSelectionMethod and ActiveFieldSet can be configured in the launch file, e.g.: +``` + + +``` +By default, options FieldSetSelectionMethod and ActiveFieldSet are not written by the driver, i.e. the default values apply (factory defaults or settings by SOPAS ET). + +Example service call for ROS1 to read resp. write options FieldSetSelectionMethod and ActiveFieldSet: +``` +rosservice call /sick_tim_7xx/FieldSetRead "{}" # returns field_set_selection_method and active_field_set +rosservice call /sick_tim_7xx/FieldSetWrite "{field_set_selection_method_in: -1, active_field_set_in: 1}" # write active_field_set +``` +Example service call for ROS1 to read resp. write options FieldSetSelectionMethod and ActiveFieldSet: +``` +ros2 service call /FieldSetRead sick_scan_xd/srv/FieldSetReadSrv "{}" # returns field_set_selection_method and active_field_set +ros2 service call /FieldSetWrite sick_scan_xd/srv/FieldSetWriteSrv "{field_set_selection_method_in: -1, active_field_set_in: 1}" # write active_field_set +``` + +Parameter active_field_set < 0: do not set (default), active_field_set > 0: index of active field otherwise (see operation manual for details about ActiveFieldSet telegram) + +Parameter field_set_selection_method < 0: do not set (default), field_set_selection_method = 0: active field selection by digital inputs, field_set_selection_method = 1: active field selection by telegram + +Note that FieldSetSelectionMethod (parameter field_set_selection_method) requires a higher authorization level and should be configured in the launch file. It is therefore recommended to set `field_set_selection_method_in: -1` when using ros service FieldSetWrite. + +## TiM240 + +The TiM240 is a new scanner that fits seamlessly into the family of other TiM devices. The TiM240 has an opening angle of 240 degrees. In contrast to the previous scanners from SICK, the coordinate system used corresponds directly to the ROS convention. For this reason, this scanner does not require a coordinate conversion of 90 degrees around the Z-axis. However, this is taken into account in the driver code, so that the user will not notice any difference in the setting of the angular ranges during use. +The angular position according to the data sheet can be taken from the drawings below. + +The following figures show the difference between the TiM5xx family and the TiM240 device. + +![TiM5xx scan area](doc/tim240/tim_5xx_scanarea.jpg) + +TiM5xx scanning area + +![TiM240 scan area](doc/tim240/tim_240_scanarea.jpg) + +TiM240 scanning area + + +## NAV350 + +NAV350 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 NAV350 support in sick_scan_xd. Please refer to the manuals for further information. + +### Process loop + +Scan data, landmarks and poses of NAV350 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. See the NAV350 manual for details about imk-files. + +The settings are configured in launch file [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_xd/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_xd/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 NAV350: + +![nav350_ros1_screenshot2.jpg](doc/nav350_ros1_screenshot2.jpg) + +### Odometry messages + +Odometry messages can be sent to the NAV350 device using ROS messages `nav_msgs/Odometry` on topic "/sick_nav_350/odom" or `sick_scan_xd/NAVOdomVelocity` on topic "/sick_nav_350/nav_odom_velocity". Odometry messages `sick_scan_xd/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_xd/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}}}}' +``` + +### Angle compensation + +#### Introduction + +For measurements with the highest demands on the accuracy of the angle measurement, the devices of the NAV series allow compensation of slight angle deviations during a rotation. The compensation is determined by the three parameters. +* Additive compensation by an angle offset +* Sinusoidal correction by specifying the amplitude and phase of compensation + +The three parameters are then used to calculate the compensation as follows: + + ![Formula for angle compensation](doc/angle_compensation/angle_compensation_000.png) + +Offset and phase are given in [deg] + +#### Example + +The information is read from lidar by using the command `sRN MCAngleCompSin`. +The answer gives one amplitude, phase and offset compensation in tens of thousands. + +The function reads +* Amplitude-Parameter: +1893 +* Phase-Parameter: -210503 +* Offset-Parameter: -245 + +These corresponds to: +* Amplitude-compensation: +0.1893 +* Phase-Compensation: -21.0503 [deg] +* Offset-Compensation: -0.0245 [deg] + +**Compensation formula for example for NAV210/NAV245** + +``` +Angle[comp.] = Angle[Raw] - 0.189300 * sin(Angle[Raw] + -21.050300 [deg]) - -0.024500 +``` + +**Compensation formula for example for NAV310** +``` +Angle[comp.] = Angle[Raw] + 0.189300 * sin(Angle[Raw] - -21.050300 [deg]) + -0.024500 +``` +**Example lookup values for NAV310 for this example (first entries)** + +| IN [Deg] | Out [Deg] | Correction [Deg] | +| --------- | --------- | ---------------- | +| 0.000000 | 0.043494 | 0.043494 | +| 1.000000 | 1.046567 | 0.046567 | +| 2.000000 | 2.049618 | 0.049618 | +| 3.000000 | 3.052647 | 0.052647 | +| 4.000000 | 4.055652 | 0.055652 | +| 5.000000 | 5.058633 | 0.058633 | +| 6.000000 | 6.061588 | 0.061588 | +| 7.000000 | 7.064518 | 0.064518 | +| 8.000000 | 8.067420 | 0.067420 | +| 9.000000 | 9.070294 | 0.070294 | +| 10.000000 | 10.073139 | 0.073139 | + +#### Comparing compensated vs. raw values + +For the example the compensation function looks like this (X-Axis: measured angle [deg], Y-Axis: compensation in [deg]) + + ![Plot of compensation function (example)](doc/angle_compensation/angle_compensation_001.png) + + +#### Coordinate systems + +For a better understanding of the data sheets of the different lidar systems the following drawings compare the different coordinate systems. Usually the scanners rotate counter-clockwise. The scanners of the NAV3xx series rotate clockwise. All coordinate systems following the right-hand rule, if the axis definition as shown in the picture is used. + ![Used coordinate systems](doc/angle_compensation/3d_coordinate_system_comp.png) + +By means of simple matrix operations all axis orientations can be transformed into each other. But since we are only interested in the angle around the Z-axis, the conversions can be done as follows (CS = Coordinate System): + +| IN From | Out To | Operation | Additional info | +| ------- | ------ | ------------ | ---------------------------------- | +| ROS | NAV3xx | Out=-In+180° | maps [-180°...180°] to [360°...0°] | +| NAV3xx | ROS | Out=-In+180° | maps [0°...360°] to [180°...-180°] | +| ROS | NAV2XX | Out=In+90° | | +| NAV2xx | ROS | Out=In-90° | | + +#### Check compensation function + +By using `Octave` ones can check the compensation function against the given values by exporting the value via a testbed function. + + + + +## MRS6124 + +### Timing between Layers + +The layers are taken up by the scanner in packs of 6. The scanner delivers at an output data rate of 10 Hz and 24 layers 24/6*10=40 scan packets of 6 layers per second. The following table shows an example of the timing for a complete 24 layer recording + +| Raw Time /µs | Delta Time /µs | Elevation Angle /Deg | +| ------------ | :------------: | -------------------: | +| 2551706348 | 0 | 13.19 | +| 2551706348 | 0 | 12.565 | +| 2551706348 | 0 | 11.940 | +| 2551706348 | 0 | 11.315 | +| 2551706348 | 0 | 10.690 | +| 2551706348 | 0 | 10.065 | +| 2551731348 | 25000 | 9.440 | +| 2551731348 | 25000 | 8.815 | +| 2551731348 | 25000 | 8.190 | +| 2551731348 | 25000 | 7.565 | +| 2551731348 | 25000 | 6.940 | +| 2551731348 | 25000 | 6.315 | +| 2551756348 | 50000 | 5.690 | +| 2551756348 | 50000 | 5.065 | +| 2551756348 | 50000 | 4.440 | +| 2551756348 | 50000 | 3.815 | +| 2551756348 | 50000 | 3.190 | +| 2551756348 | 50000 | 2.565 | +| 2551781348 | 75000 | 1.940 | +| 2551781348 | 75000 | 1.315 | +| 2551781348 | 75000 | 0.690 | +| 2551781348 | 75000 | 0.065 | +| 2551781348 | 75000 | -0.560 | +| 2551781348 | 75000 | -1.185 | +| NEW SCAN | +| 2551807862 | 101514 | 13.190 | +| 2551807862 | 101514 | 12.565 | +| 2551807862 | 101514 | 11.940 | +| 2551807862 | 101514 | 11.315 | +| 2551807862 | 101514 | 10.690 | +| 2551807862 | 101514 | 10.065 | +| 2551832862 | 126514 | 9.440 | + +The time stamps between the layers are interpolated by the scanner. The time stamps of the first layer (Ang.=13.19°) are measured and show jitter accordingly. + +### MRS600 with RMS1000 + +1. Setup environment and power supply +2. roslaunch sick_scan_xd test_0002_combi_live.launch +3. Check setup using rviz +4. Close all applications, which are not necessary (like IDE, browser, git client) +5. Setup Tracking algorithm + +``` +top +``` +6. Record data +``` +rosrun rosbag record record -o combi -a +``` + +## RMS1000 + +This driver supports the radar type RMS1000. 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. + +The RMS1000 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, the signal reflected by the object and received by the radar is time-shifted to the original transmitted frequency. By mixing the transmitted signal with the received signal, the frequency shift and thus the time shift can be determined. Based on the known modulation parameters of the transmitter, the propagation time of the signal can be determined, which in turn is proportional to the distance of the object. For precise distance measurement, therefore, the transmission frequency must be modulated as precisely as possible in linear fashion, since any non-linearity impairs the distance accuracy of the radar. + +Through this indirect time measurement via the frequency change of the transmitter, even very close targets can be measured with high accuracy and cost-efficiency using the FMCW method, provided that the modulation parameters are selected appropriately. The distance resolution is determined by the bandwidth of the transmitted signal. + +### Raw Targets + +Raw targets correspond to individual reflectors that are detected by the radar. Each individual reflector carries the following information: +* Range +* Horizontal angle (azimuth) +* Doppler speed +* Reflectivity of the target (aka rcs - radar cross section) + +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 + +Tracking objects are determined from the raw targets via a tracking procedure over the spatial and temporal +distribution of the raw targets. The track method estimates the location, direction and speed of the object based on an initial estimate. After initialization, new raw targets are assigned to the track if they "fit" to the track. This process is called "gating". Once these raw targets have been assigned to the track, +the track is updated and the new estimate is used for further processing. + +The distribution of raw targets over the object also determines the object length during the tracking process. + +The tracking object therefore has the following properties: +* Distance from radar in Cartesian coordinates +* Direction vector in Cartesian coordinates +* Direction of travel as an angle in the X/Y plane +* Vehicle speed +* Vehicle length + +### ROS message for Radar + +After parsing the telegram, the driver sends an ROS message of type RadarScan. RadarScan consists of the following components: +``` +Header header +RadarPreHeader radarPreHeader +sensor_msgs/PointCloud2 targets +sick_scan_xd/RadarObject[] objects +``` +#### RadarPreHeader +The radar preheader contains information that provides general information about the radar. This data record can usually be ignored for object recognition with regard to raw targets and tracking objects. +For details please refer to the message specification of Sick. + +#### targets + +The list with the raw targets of type sick_scan_xd/targets contains the information about the raw targets. +Each raw target contains the following data fields in a pointcloud2-message (height: 1, width: number of raw targets): +``` + std::string channelRawTargetId[] = { "x", "y", "z", "vrad","amplitude" }; +``` +This raw target contains cartesian coordinates, which are derived from range and azimuth angle (horizontal angle) in the following way (code snippet): +``` +valSingle[0] = rawTargetList[i].Dist() cos(angle); // x +valSingle[1] = rawTargetList[i].Dist() * sin(angle); // y +valSingle[2] = 0.0; // z +valSingle[3] = rawTargetList[i].Vrad(); // vrad +valSingle[4] = rawTargetList[i].Ampl(); // amplitude +``` + +#### objects + +The list with the objects of type sick_scan_xd/RadarObject[] contains the information about the track objects. + +``` +int32 id + +time tracking_time // valid +time last_seen // not set + +geometry_msgs/TwistWithCovariance velocity // valid + +geometry_msgs/Pose bounding_box_center // valid +geometry_msgs/Vector3 bounding_box_size // valid + +geometry_msgs/PoseWithCovariance object_box_center // valid +geometry_msgs/Vector3 object_box_size // valid + +geometry_msgs/Point[] contour_points // not set +``` + +Please note that not all fields are filled in the object messages. The message specification contains valid ones in the areas marked here in the code section. + +The corresponding code fills the object list in the following manner: + +``` + float heading = atan2( objectList[i].V3Dy(), objectList[i].V3Dx()); + + 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; + + radarMsg_.objects[i].bounding_box_center.position.x = objectList[i].P3Dx(); + radarMsg_.objects[i].bounding_box_center.position.y = objectList[i].P3Dy(); + radarMsg_.objects[i].bounding_box_center.position.z = 0.0; + radarMsg_.objects[i].bounding_box_center.orientation.x = cos(heading); + radarMsg_.objects[i].bounding_box_center.orientation.y = sin(heading); + radarMsg_.objects[i].bounding_box_center.orientation.z = 0.0; + radarMsg_.objects[i].bounding_box_center.orientation.w = 1.0; // homogeneous coordinates + + + radarMsg_.objects[i].bounding_box_size.x = objectList[i].ObjLength(); + radarMsg_.objects[i].bounding_box_size.y = 1.7; + radarMsg_.objects[i].bounding_box_size.z = 1.7; + for (int ii = 0; ii < 6; ii++) + { + int mainDiagOffset = ii * 6 + ii; // build eye-matrix + radarMsg_.objects[i].object_box_center.covariance[mainDiagOffset] = 1.0; // it is a little bit hacky ... + radarMsg_.objects[i].velocity.covariance[mainDiagOffset] = 1.0; + } + radarMsg_.objects[i].object_box_center.pose = radarMsg_.objects[i].bounding_box_center; + radarMsg_.objects[i].object_box_size= radarMsg_.objects[i].bounding_box_size; + +``` +As you can see there are default values for object height and object width of 1.7 (typical private vehicle) + + + +### Launch Files + +The following launch files serve as examples for use: + +* 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 + +#### Data visualization example video + +[A video example can be found here](doc/200326_5_video_track.mp4). + +The following figure shows a viz-screenshot of the pointcloud: + +![radar_rviz](doc/radar_rviz.png) + +### Parameter for Radar Usage + +The following parameters are support by the node **sick_generic_caller** in combination with the RADAR RMS1000 / RMS2000: + +* scanner_type (string, default: "") --> Must be set to **sick_rms_xxxx** +* range_max (double, default: 25.0) +* Maximum range +* hostname +* port +* timelimit +* tracking_mode + 0: BASIC-Tracking - use for tracking smaller objects + 1: TRAFFIC-Tracking - use for tracking larger objects like vehicles +* transmit_raw_targets (bool, default: true) +* transmit_objects (bool, default: true) +* emul_sensor (bool, default: false) + +### Radar Datagram + +The message sick_scan_xd/RadarScan consists of four parts: +- Header in standard format +- radarPreHeader with higher-level information +- targets: Raw targets output from radar +- objects: Tracking objects that are determined based on the raw targets using the internal tracking algorithm. + +The complete structure can be determined using the command: +``` +rosmsg show sick_scan_xd/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. +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** + +``` +sSN LMDradardata 1 1 112F6E9 0 0 BCC DC0C 730E9D16 730EA06D 0 0 0 0 0 0 1 0 0 4 DIST1 42200000 00000000 0 AZMT1 3C23D70A 00000000 0 VRAD1 3C23D70A 00000000 0 AMPL1 3DCCCCCD 00000000 0 1 MODE1 3F800000 00000000 0 0 0 0 0 0 +``` + +In the following, the individual tokens are numbered one after another and their meaning is explained: + + +``` + 0: sSN + 1: LMDradardata + + MeasurementData + =============== + 2: 1 MeasurementData.uiVersionNo : Version Information for this while structureValue + Value Range: 0 ... 65535 + DeviceBlock + =========== + 3: 1 DeviceBlock.uiIdent : Logical number of the device + Value Range: 0 ... 65535 + 4: 112F6E9 DeviceBlock.udiSerialNo : Serial number of the device + Value Range : 0..4294967295 + + + 5: 0 DeviceBlock.xbState : State of the device + Bit length : 16 + + 0.0 Bool : Value Range False, True + Initialisation: False + Meaning : bDeviceError + + 0.1 Bool : Value Range False, True + Initialisation: False + Meaning : bContaminationWarning + + 0.2 Bool : Value Range False, True + Initialisation: False + Meaning : bContaminationError + + 0.3 ... 0.7 + 6: 0 1.0 ... 1.7 Bool : Value Range False, True + Reserved + +StatusBlock +=========== + 7: BCC uiTelegramCount + 8: DC0C uiCycleCount (or uiScanCount???) + 9: 730E9D16 udiSystemCountScan + 10: 730EA06D udiSystemCountTransmit + 11: 0 xbInputs (Bit 0.0 .. 0.7) + 12: 0 xbInputs (Bit 1.0 .. 1.7) + 13: 0 xbOutputs (Bit 0.0 .. 0.7) + 14: 0 xbOutputs (Bit 1.0 .. 1.7) + +MeasurementParam1Block +====================== + 15: 0 MeasurementParam1Block.uiCycleDuration + 16: 0 MeasurementParam1Block.uiNoiseLevel + +aEncoderBlock +============= + 17: 1 Number of aEncoderBlocks + + + 18: 0 aEncoderBlock[0].udiEncoderPos + 19: 0 aEncoderBlock[0].iEncoderSpeed + + 20: 4 Number of following data channels + 21: DIST1 + 22: 42200000 + 23: 00000000 + 24: 0 + 25: AZMT1 + 26: 3C23D70A + 27: 00000000 + 28: 0 + 29: VRAD1 + 30: 3C23D70A + 31: 00000000 + 32: 0 + 33: AMPL1 + 34: 3DCCCCCD + 35: 00000000 + 36: 0 + 37: 1 + 38: MODE1 + 39: 3F800000 + 40: 00000000 + 41: 0 + 42: 0 + 43: 0 + 44: 0 + 45: 0 + 46: 0 +``` + +## RMS1000 and LMS1000 combination + +This tutorial shows how to combine a RMS1000radar with a LMS1000 lidar. + +To demonstrate the lidar/radar combination, a RMS1000and a LMS1000 device were put into operation. The sick_scan_xd driver and rviz were started on ROS 1 Linux. Bagfiles have been recorded to demonstrate the required transform (rms_1xxx_lms_1xx_movement_off.bag and rms_1xxx_lms_1xx_movement_on.bag). + +Run the following steps: + +1. Connect RMS1000and LMS1000 and start sick_scan_xd with launchfiles sick_lms_1xxx.launch and sick_rms_xxxx.launch: + ``` + roslaunch sick_scan_xd sick_lms_1xxx.launch + roslaunch sick_scan_xd sick_rms_xxxx.launch + ``` + Make sure, that different ros node names and different IP-addresses are used. + + The following rviz screenshot shows both pointclouds: + ![rms_1xxx_lms_1xx_combi_screenshot01.png](doc/combination_rms_1xxx_lms_1xx/rms_1xxx_lms_1xx_combi_screenshot01.png) + + Note that each sensor has its own frame id and coordinate system. The RMS1000uses the frame id "radar", the LMS1000 uses the frame id "cloud". To combine both sensor, we have to transform the radar frame and coordinates to the lidar frame and coordinates. + Radar targets have multiple echos due to reflection. + +2. Start a ros static_transform_publisher to convert radar frames (frame id `/radar`) to lidar frames (frame id `/cloud`): + ``` + rosrun tf static_transform_publisher 0 0 0 0 0 0 /cloud /radar 100 + ``` + Using this transform, rviz displays both the radar and lidar pointcloud: + ![rms_1xxx_lms_1xx_combi.png](doc/combination_rms_1xxx_lms_1xx/rms_1xxx_lms_1xx_combi.png) + +> **_NOTE:_** If you use this example with a playback of bagfiles (e.g. `rosbag play --loop ./rms_1xxx_lms_1xx_movement_off.bag`), you might encounter errors due to different timestamps (the recorded timestamps in the bagfiles are different from the timestamps by the static_transform_publisher). + +Alternatively, the radar frame id and an optional transform can be configured in the radar launch file (parameter "frame_id" and "add_transform_xyz_rpy"). + +## LMS1000 and MRS1000 interlacing + +Most lidars create non interlaced scan data by default, i.e. a scan data telegram contains all scan points measured during a full 360 degree circulation. For MRS1xxx and LMS1000 lidars, parameter `ang_res` can be configured to increase the angular resolution. In this case, the scan data and point clouds are measured and published interlaced. + +The following table shows valid combinations of the angular resolution (parameter `ang_res`) and scan frequency (parameter `scan_freq`) for MRS1000 and LMS1000 lidars: + +| lidar | ang_res [deg] | scan_freq [Hz] | interlacing | +| ------- | ------------- | -------------- | -------------- | +| MRS1000 | 0.25 | 50 | non-interlaced | +| MRS1000 | 0.125 | 25 | 2 x interlaced | +| MRS1000 | 0.0625 | 12.5 | 4 x interlaced | +| LMS1000 | 0.75 | 150 | non-interlaced | +| LMS1000 | 0.375 | 75 | 2 x interlaced | +| LMS1000 | 0.1875 | 37.5 | 4 x interlaced | + +**MRS1000 angular resolution** + +By default, MRS1000 lidars create non-interlaced scans with an angular resolution 0.25 deg. +Using higher resolutions, the MRS1000 sends scan data interlaced. + +MRS1000 lidars measure 4 consecutive scans with identical starting angles for each of its 4 layers, where each layer has a different elevation angle. This results in (12.5 Hz) * (4 layers) * (360 deg / 0.25 deg) * (275 deg / 360 deg) = 55000 shots per second. + +With default configuration ang_res=0.25, the angular resolution of each scan is 0.25 [deg] and scans and point clouds are non-interlaced. The sequence of scan data telegrams is repeating with 4 consecutive telegrams (resp. 4 point cloud messages): +* layer[0] starting at -137.5 deg, angle inc = 0.25 deg +* layer[1] starting at -137.5 deg, angle inc = 0.25 deg +* layer[2] starting at -137.5 deg, angle inc = 0.25 deg +* layer[3] starting at -137.5 deg, angle inc = 0.25 deg + +With configuration ang_res=0.125, the angular resolution of each scan is 0.125 [deg] and scans and point clouds are 2 x interlaced. The sequence of scan data telegrams is repeating with 8 consecutive telegrams (resp. 8 point cloud messages): +* layer[0] starting at -137.500 deg, angle inc = 0.25 deg +* layer[1] starting at -137.500 deg, angle inc = 0.25 deg +* layer[2] starting at -137.500 deg, angle inc = 0.25 deg +* layer[3] starting at -137.500 deg, angle inc = 0.25 deg +* layer[0] starting at -137.625 deg, angle inc = 0.25 deg +* layer[1] starting at -137.625 deg, angle inc = 0.25 deg +* layer[2] starting at -137.625 deg, angle inc = 0.25 deg +* layer[3] starting at -137.625 deg, angle inc = 0.25 deg + +With configuration ang_res=0.0625, the angular resolution of each scan is 0.0625 [deg] and scans and point clouds are 4 x interlaced. The sequence of scan data telegrams is repeating with 16 consecutive telegrams (resp. 16 point cloud messages): +* layer[0] starting at -137.4375 deg, angle inc = 0.25 deg +* layer[1] starting at -137.4375 deg, angle inc = 0.25 deg +* layer[2] starting at -137.4375 deg, angle inc = 0.25 deg +* layer[3] starting at -137.4375 deg, angle inc = 0.25 deg +* layer[0] starting at -137.5000 deg, angle inc = 0.25 deg +* layer[1] starting at -137.5000 deg, angle inc = 0.25 deg +* layer[2] starting at -137.5000 deg, angle inc = 0.25 deg +* layer[3] starting at -137.5000 deg, angle inc = 0.25 deg +* layer[0] starting at -137.5625 deg, angle inc = 0.25 deg +* layer[1] starting at -137.5625 deg, angle inc = 0.25 deg +* layer[2] starting at -137.5625 deg, angle inc = 0.25 deg +* layer[3] starting at -137.5625 deg, angle inc = 0.25 deg +* layer[0] starting at -137.6250 deg, angle inc = 0.25 deg +* layer[1] starting at -137.6250 deg, angle inc = 0.25 deg +* layer[2] starting at -137.6250 deg, angle inc = 0.25 deg +* layer[3] starting at -137.6250 deg, angle inc = 0.25 deg + +To use the full angular resolution of one 360 degree circulation, the point cloud must be accumulated by 8 resp. 16 messages in interlaced mode. + +**LMS1000 angular resolution** + +By default, LMS1000 lidars create non-interlaced scans with an angular resolution 0.75 deg. +Using higher resolutions, the LMS1000 sends scan data interlaced. + +With configuration ang_res=0.75, the angular resolution of each scan is 0.75 [deg]. This means that each point cloud message also has a resolution of 0.75 [deg]. With configuration ang_res=0.375, the scan is generated interlaced: Each scan still has 0.75 [deg] resolution, but 2 x 4 = 8 consecutive scans are rotated by 0.375 [deg] against each other. I.e. 8 consecutive point cloud messages each have an angular resolution of 0.375 [deg] at half the frequency. Within a point cloud message the angular resolution is still 0.75 [deg]. + +With ang_res=0.375, scan data are two times interlaced. The sequence of scan data telegrams is repeating with 8 consecutive telegrams: +* 4 scans starting at -138.000 deg, angle inc = 0.75 deg, then +* 4 scans starting at -138.375 deg, angle inc = 0.75 deg + +Thus the start angles of the received scan data telegrams are +{ ..., -138.375, -138.375, -138.375, -138.375, -138.000, -138.000, -138.000, -138.000, ... } + +With ang_res=0.1875 the scan is generated quadruple interlaced, i.e. 4 x 4 = 16 consecutive scans are each rotated by 0.1875 [deg]. Each scan is resolved with 0.75 [deg]; 4 x 4 = 16 scans resp. 16 pointclouds together (accumulated) result in a resolution of 0.1875 [deg] at a quarter of the frequency. + +With ang_res=0.1875, scan data are four times interlaced. The sequence of scan data telegrams is repeating with 16 consecutive telegrams: +* 4 scans starting at -137.8125 deg, angle inc = 0.75 deg, then +* 4 scans starting at -138.0000 deg, angle inc = 0.75 deg, then +* 4 scans starting at -138.1875 deg, angle inc = 0.75 deg, then +* 4 scans starting at -138.3750 deg, angle inc = 0.75 deg, then + +Thus the start angles of the received scan data telegrams are +{ ..., -137.8125, -137.8125, -137.8125, -137.8125, -138.0000, -138.0000, -138.0000, -138.0000, -138.1875, -138.1875, -138.1875, -138.1875, -138.3750, -138.3750, -138.3750, -138.3750, ... } + +You can see this in rviz by increasing the decay time to e.g. 4/75=0.054 or higher. The screenshot shows an example with the default setting ang_res=0.75: + +![LMS1xxx_0.7500_deg.png](doc/screenshots/LMS1xxx_0.7500_deg.png) + +The angular resolution is (just roughly measured) about atan(0.11/0.9) / 9 points = 0.77 [deg]. With ang_res=0.375 and decay=0.1 rviz shows twice the resolution: + +![LMS1xxx_0.7500_deg.png](doc/screenshots/LMS1xxx_0.3750_deg.png) + +Correspondingly, rviz shows four times the resolution with ang_res=0.1875 and decay=0.1: + +![LMS1xxx_0.7500_deg.png](doc/screenshots/LMS1xxx_0.1875_deg.png) + +To use the full angular resolution of one 360 degree circulation, the pointcloud must be accumulated by 8 resp. 16 messages. + +The active configuration can be seen in the log output during scanner initialization, e.g.: +``` +[ INFO] [1669294673.078608968]: sRA LMPscancfg: scan frequency = 75 Hz, angular resolution = 0.375 deg. +``` + +> **_NOTE:_** LMS creates 4 consecutive scans with identical starting angles for each of its 4 laser LEDs. The 4 laser LEDs are mounted in one plane. All together, this gives you (4 scans) * (37.5 Hz) * (1 layer) * (360 deg / 0.75 deg) * (275 deg / 360 deg) = 55000 shots per second. +> + +# FAQ + +## General + +**How to run multiple sensors concurrently?** + +:question: How can I run multiple sensors concurrently with sick_scan_xd ? + +:white_check_mark: To support multiple sensors, sick_scan_xd has to be started multiple times, with one sick_scan_xd-node for each sensor. See [start multiple nodes](#start-multiple-nodes) for details. + +**Driver restarts again and again after "sFA" message** + +:question: The sick_scan_xd driver restarts again and again after an error message "sFA". + +:white_check_mark: The behaviour is intentional. The error message "sFA" can be caused by faulty configuration or errors in the lidar. Correct operation after this error message is not guaranteed. In this case, the driver restarts itself. It is recommended to identify and correct the error using its error number ("`sFA`"). The SOPAS error codes are listed in the manual. + +**Driver restarts after timeout error** + +:question: The sick_scan_xd driver changes the communication protocol and restarts after a timeout error. + +:white_check_mark: The use of binary communication (Cola-B) is highly recommended due to better compatibility, lower network traffic and general support. +Recommendation: +1. Set parameter "use_binary_protocol" to "true" in the launch file, and +2. Set the lidar communication mode with the SOPAS ET software to binary and save this setting in the scanner's EEPROM. + +**Changes in launchfiles are ignored** + +:question: roslaunch still uses an old version after modifying the launch-file. + +:white_check_mark: After modifying a launch-file, it has to be installed by running `catkin_make_isolated --install --cmake-args -DROS_VERSION=1` +to be located and used by `roslaunch`. + +**ROS 2 launch file support** + +:question: How can I create a ROS 2 node in python to run sick_generic_caller from a launch.py-file in ROS 2? + +:white_check_mark: Example to launch a TiM-7xx node in ROS 2: +``` + sick_scan_pkg_prefix = get_package_share_directory('sick_scan_xd') + tim_launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/sick_tim_7xx.launch') + tim_top_node = Node( + package='sick_scan_xd', + executable='sick_generic_caller', + output='screen', + arguments=[ + tim_launch_file_path, + 'nodename:=/lidars/tim_top', + 'hostname:=192.168.0.110', + 'cloud_topic:=/lidars/tim_top/cloud', + 'frame_id:=tim_top' + ] + ) +``` +Thanks to user JWhitleyWork. + +**Timestamps** + +:question: What timestamp is provided in the pointcloud and laserscan messages? + +:white_check_mark: Details about timestamps are given in [timing](#time-synchronization) and [software_pll](#software-pll). + +In a nutshell: +The lidars do not work with absolute time stamps but with "ticks". There are two types of tick timestamps: +* Tick timestamps for the generation +* Tick timestamps for sending the message +To match the tick timestamps against the system time, a straight line equation is estimated via the so-called software PLL, which can be used to calculate from the system time to the ticks in the lidar (and vice versa). The assumption is that the tick timestamp for sending the message corresponds to the system time when receiving the message. In reality there will be a tiny delay here. This delay can be ignored. + +With the help of this straight line equation, one can now calculate the system time at which the data is generated in the lidar. + +Summary: +1. lidar: stamp with ticks the generation (first shot in the scan) (TICK_GEN) +2. lidar: stamp with ticks the sending of the scan message (TICK_TRANSMIT) +3. PC: Stamp with system time the receiving of the message +4. PC: Calculate back to system time of generation: +System time of generation = System time of receiving - (TICK_TRANSMIT - TICK_GEN)/TICK_FREQUENCY + +**Laserscan messages with multiple frame ids** + +:question: sick_scan_xd publishes laserscan messages for multiScan100 and picoScan100 with multiple frame ids and possibly inconsistent data. Which frame id is correct? + +:white_check_mark: By default, an echo filter is activated in the multiScan100 and picoScan100 launchfile. This echo filter suppresses multiple echos, e.g. echos from an object and a protective glass pane. The default configuration is "last echo only". In this case (i.e. one echo only), the fullframe laserscan messages on topic scan_fullframe all have identical frame ids for each layer, i.e. "world_\". For the multiScan100 lidars with 16 layers, sick_scan_xd publishes laserscan messages with frame ids "world_1", "world_2" up to "world_16". For picoScan100 lidars with 1 layer, there is just one frame id "world_1". + +In case of multiple echos (i.e. echo filter is deactivated), each echo is published by a laserscan message with different frame ids "world_\_\". For picoScan100 lidars with 3 echos, there are 3 frame ids "world_1_0", "world_1_1", "world_1_2" published. For multiScan100 lidars with 16 layers and 3 echos, there are 48 different frame ids published "world_1_0", "world_1_1", "world_1_2", "world_2_0", "world_2_1", "world_2_2", ... , "world_16_0", "world_16_1", "world_16_2". + +This behaviour is intended, since a laserscan message can not contain multiple ranges for a single scan point at one azimuth angle. Therefore, there have to be different laserscan messages for each layer and each echo. Layer and echo of a laserscan message are identified by the frame id. + +**Compilation errors** + +:question: Compiler reports errors in file `/opt/ros//include/sick_scan` + +:white_check_mark: If sick_scan was previously installed using `apt-get install ros--sick-scan`, you have to remove previous versions using `apt-get purge ros--sick-scan`. Run the following steps for a complete rebuild: +1. Run `sudo apt-get purge ros--sick-scan` (e.g. `sudo apt-get purge ros-noetic-sick-scan`) to remove previously installed sick_scan-packages +2. Remove the folders sick_scan_xd/build, sick_scan_xd/build_isolated, sick_scan_xd/devel, sick_scan_xd/devel_isolated, sick_scan_xd/install and sick_scan_xd/install_isolated +3. Rebuild + +:question: cmake cannot find diagnostic_updater + +:white_check_mark: On ROS 2 foxy, package diagnostic_updater needs to be installed by +``` +sudo apt-get update +sudo apt-get install ros-$ROS_DISTRO-diagnostic-updater # install diagnostic_updater +# E.g. to install diagnostic_updater on foxy, run +# sudo apt-get install ros-foxy-diagnostic-updater +``` + +:question: catkin gives me the following error message: +`By not providing "FindSICKLDMRS.cmake" in CMAKE_MODULE_PATH this project ..., but CMake did not find one."` + +:white_check_mark: One problem with ROS is that it doesn't automatically rebuild everything if you just append "-DLMRRS=0". +If you accidentally did the following call before + +``` +catkin_make_isolated --install --cmake-args -DROS_VERSION=1 +``` +you must remove the build/devel/install-directories created by the ROS build process. +For this please run the following commands to remove the directories, which holds the previous build results: +``` +cd ~/ros_catkin_ws +rm -rf build_isolated +rm -rf devel_isolated +rm -rf install_isolated +rm -rf devel +``` +It is possible that not all directories are present in this list. But that does not matter. +The only subdirectory left should be "src". +You can check this with the following command: + +``` +ls */ -d +``` +The output should be: +``` +src/ +``` +After doing this please rerun the command +catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 + +**rviz or rviz2 do not work as expected** + +:question: rviz shows a grey point cloud. The size of points can be adjusted. + +:white_check_mark: Check in the launch file that the intensity flag is set to True. + +:question: rviz shows a grey point cloud and the size of points can not be adjusted. + +:white_check_mark: Probably in this case you are running Linux in a virtual machine. In this case, OpenGL may not work correctly in the VM. rviz then chooses a kind of "fallback solution" and deactivates the colors. +Also, changing the "Size" and "Style" display in rviz has no effect on the display of the pointcloud data. + +The problem can be avoided by starting rviz with the following sequence: + +``` +export LIBGL_ALWAYS_SOFTWARE=1 +rosrun rviz rviz +``` + +:question: rviz2 on Ubuntu 24 with ROS 2 jazzy crashes immediately after start + +:white_check_mark: This can be a wayland vs. X11 problem. Try `export QT_QPA_PLATFORM=xcb` before starting rviz2. See https://github.com/ros-visualization/rviz/issues/1442#issuecomment-553900795 and https://blog.martin-graesslin.com/blog/2015/07/porting-qt-applications-to-wayland/ for further information. + +**Angular resolution and scan frequency** + +:question: The angular resolution/ number of shots is too small + +:white_check_mark: Possibly Mean or Median filters are activated. Use Sopas ET to deactivate them and store this settings permanent on the device, see picture. + +![Sopas_filter](doc/tim5xxx_filter.PNG) + +Further information can be found at http://wiki.ros.org/rviz/Troubleshooting . + +:question: The scanner and message frequency is lower than expected + +:white_check_mark: Mean or median filters decrease the scanner frequency. Check and configure filter settings with SOPAS ET or deactivate them in the launch file (MRS1xxx, LMS1xxx, LMS4xxx, LRS4xxx only): +``` + + +``` +By default, filter settings are not overwritten, i.e. the filter settings stored in the lidars EEPROM apply (factory defaults resp. SOPAS ET filter settings). + +**LMS1xxx angular resolution** + +:question: Independent of the configuration, the LMS1xxx pointcloud always displays 0.75 [deg] angular resolution + +:white_check_mark: Using higher resolutions, the LMS1xxx sends scan data interlaced. See [LMS1000 and MRS1000 interlacing](#lms1000-and-mrs1000-interlacing) for details. + +**"ERROR: Tcp::open: Failed to open TCP connection to 192.168.0.1, aborting."** + +:question: Question: +sick_generic_caller gives you an answer like: +```bash +"ERROR: Tcp::open: Failed to open TCP connection to 192.168.0.1, aborting." +``` + +:white_check_mark: Answer: See FAQ for network diagnosis and recommended configuration. + +**IP Address of Lidar** + +:question: Question: +My scanner does not use the default ip address. What shall I do? + +:white_check_mark: Answer: +There are two options doing this: + +* Permanently: +Replace the following entry with your ip address. +```bash + +``` +* Temporarily +Use a command line argument in addition to the launch file argument: +```bash + hostname:=192.168.0.2 +``` + +**Timeout Warning** + +:question: Question: +During start phase the are warning/error message like + +```bash +no answer received after 5000 ms. Maybe sopas mode is wrong. +``` +and some more warning/error messages: + +:white_check_mark: Answer: +In this case the driver tries to start the scanner in binary mode. If this is not possible, warnings and error messages are generated. +The driver switches the scanner from ASCII mode to binary mode and then restarts communication. The messages can therefore be ignored. +For a long-term solution, we recommend switching from ASCII to binary communication with SOPAS ET under Windows. + +**Own Data Handling** + +:question: Question: +I would like to process data with my own methods. + +:white_check_mark: Answer: +Search for keyword "PUBLISH_DATA:" in the code and replace the code for writing +jpeg-files and CSV-files with your own source code. + +**Occasionally missing scan data** + +:question: Question: +Occasionally, no scan data appear, but the scanner is still reachable using `ping ` or `nc -z -v ` + +:white_check_mark: Answer: +This is likely to be a data capture issue. In any case it's recommend (if not already set) to use SOPAS-Binary instead of SOPAS-ASCII, because here the data rate is lower. + +In addition, the following measures can help to determine the source of the problems: +a) Exchange the cables to the lidar(s) incl. the network cables +b) Exchange the power supply to the lidar(s) +c) avoidance of interconnection of other components (like switch etc.) +d) upgrade hardware and firmware of devices (if new versions available) + +If it is not a hardware problem (e.g. cable), check if there are other software components using the network interface. + +If there are still problems, use Wireshark to see if there are any communication problems reported. Wireshark can export the recorded network traffic as text (see screenshot). Search for entries like "LMDscandata" in the generated text file. This marks the beginning of a new scan message. You can then compare the order of the timestamps of these packets to see if there were any failures. + +![Wireshark screenshot](https://user-images.githubusercontent.com/33296049/124088216-01aa2280-da53-11eb-91ae-2b88b37e08eb.png) + +**python_d.exe not found** + +:question: Question: +On Windows with ROS, cmake complains "python_d.exe not found" when running rosidl generator + +:white_check_mark: Answer: +Workaround: Copy python.exe to python_d.exe in the python folder. + +**Debugging** + +:question: Question: +How can I debug sick_generic_caller on ROS 1? + +:white_check_mark: Answer: +Build with compiler option `-g` and run sick_generic_caller as described using a launchfile. Stop sick_generic_caller (Ctrl-C or kill) and save the current ros parameter using `rosparam dump .yaml`. Load these parameter with `rosparam load .yaml` and restart sick_generic_caller in gdb or in your IDE. + +**Curved lines on a straight wall** + +:question: Question: +The X,Y points of the lidar show a curved line even though the lidar is scanning a straight wall. How can this be? + +:white_check_mark: Answer: +This effect occurs when the lidar has multiple planes that are tilted up or down. In this case, the laser beams of this plane do not lie on a flat plane. Rather, the beams lie on a cone. If the laser beams then hit a wall, the result is a curved course of the lidar points. If the lidar is horizontal and the wall is vertical, this is a hyperbola (see following figure): + +![cone_section](doc/cone_section.png) + +This image is generated using the website https://www.intmath.com/plane-analytic-geometry/conic-sections-summary-interactive.php. +Thus, the mathematical laws for a conic section apply, as they are explained e.g. at Wikipedia at https://en.wikipedia.org/wiki/Conic_section. + +**Interlacing** + +:question: Question: +How should I interpret the scan rate and lidar resolution from the manual? What is the relationship between ROS point cloud publishing rate and scan frequency here? + +:white_check_mark: Answer: + +The angular resolution and scan frequency is configurable in many lidars such as the LRS-4xxx or MRS1000. Depending on the lidar type, angular resolution and scan frequency can be set in the launch file either via the parameter "scan_cfg_list_entry" or the parameters "ang_res" and "scan_freq". Angular resolution and scan frequency are not independent of each other. If no default settings are used, the values must be selected according to the manual for the respective lidar and set in launch file. + +An increase in resolution is achieved by interlacing by a factor of N. This means that N consecutive scans are rotated by a constant angular offset. Each scan in itself still has the physically given angular resolution and frequency. By concatenating N interlaced scans, the angular resolution is increased by the factor N. + +Example: The default setting of an MRS1000 is 0.25 degrees horizontal angular resolution at 50 Hz scan frequency without interlacing and an angular range of 275 degrees in total. I.e. each scan measures the distance at the hor. angles [ ..., 0.000, 0.250, 0.500, 0.750, ... ]. + +If 0.125 degrees horizontal angular resolution is configured at 25 Hz scan frequency, the scans are performed with 2 times interlacing (N=2). Every 2nd scan is horizontally shifted by 0.125 degrees. I.e. each scan measures alternately at the hor. angles [ ..., 0.000, 0.250, 0.500, 0.750, ... ] and [ ... , 0.125, 0.375, 0.625, 0.875 ... ]. 50 single scans per second resp. 25 interlaced scans per second are sent. + +If 0.0625 degrees horizontal angular resolution at 12.5 Hz scan frequency is configured, the scans are performed with 4 times interlacing (N=4). Successive scans are shifted horizontally by 0.0625 degrees each. That is, each scan measures alternately at the hor. angles [ ..., 0.000, 0.250, 0.500, 0.750, ... ], [... , 0.0625, 0.3125, 0.5625, 0.8125 ... ], [... , 0.125, 0.375, 0.625, 0.875 ... ] and [... , 0.1875, 0.4375, 0.6875, 0.9375 ... ]. 50 single scans per second resp. 12.5 interlaced scans per second are sent. + +In interlacing mode, laser scan and point cloud messages are published interlaced, too. In rviz, the higher angular resolution is clearly visible when the decay time is increased. + +With a scan frequency of 50 Hz and 4 active layers, the lidar will send a new scan line each 0.02 seconds. Each layer is scanned after 0.08 seconds resp. with 12.5 Hz (4 layers multiplied by 12.5 Hz = 50 Hz scan frequency). The point cloud accumulates all active layers and is therefore published with 12.5 Hz (i.e. scan frequency divided by number of layers). + +If you check the publishing rate of the point cloud messages of a MRS-1104, you will measure 12.4 Hz, since the scans of 4 layers are accumulated in 1 point cloud message (50 hz scan frequency divided by 4 layers = 12.5 Hz point cloud publishing frequency). The resolution of each single point cloud message is 0.125 [deg]. Only by interleaving 4 consecutive messages you get the high resolution of 0.0625 [deg]. + +See [LMS1000 and MRS1000 interlacing](#lms1000-and-mrs1000-interlacing) for further informations. + + +## Networking + +A TCP connection is needed to receive scan data from a lidar, which requires an appropriate network setup. + +Common errors when establishing the TCP connection and possible solutions are described below. If you encounter network errors like "Failed to open TCP connection" when running sick_scan_xd, follow these recommendations. + +**Static IP addresses** + +Most lidars have the default IP address 192.168.0.1. It is highly recommended to use a static IPv4 network with IP addresses 192.168.x.y. PC and lidar should use the same subnet. **Avoid using DHCP.** + +Example IP address of the lidar: 192.168.0.1 (default) + +Example IP address of the PC running sick_scan_xd: 192.168.0.100 + +The IP address of the PC running sick_scan_xd and the lidar must not be identical! + +Use `ifconfig -a` on Linux resp. `ipconfig /all` on Windows to view network settings and lcoal IP addresses. + +1. Try to ping your device: + ```bash + ping 192.168.0.1 + ``` +2. Disconnect your scanner and retry ping + + ```bash + ping 192.168.0.1 + ``` + The result of ping contains a pattern like + ```bash + ... Destination Host Unreachable + ``` +3. Reconnect your device and try to ping: + ```bash + ping 192.168.0.1 + ``` + +If you do not know the IP addresses, try to find the IP addresses of your PC and your lidar in your subnet: + +1. Install fping: `apt-get install fping` + +2. Scan your network (for example, subnet 192.168.10.0/24): `fping -a 192.168.0.1\24` to +search for all IP addresses from 192.168.0.0 to 192.168.0.255. + +The result is similar to: +```bash +192.168.0.4 is alive +192.168.0.22 is alive +``` +and a lot of unreachable entries. +In the example the IP address 192.168.0.4 is the laserscanner MRS1104 and the IP address 192.168.0.22 is the computer running linux. Check this with `ifconfig|grep 192.168.0.22`. + +We recommend to use wired Ethernet. On Ubuntu, use the "Wired Settings" menu to check the network profile. Network profiles sometimes change automatically when multiple profiles are in use or the lidar has been switched off and on. + +![network_setup_02.png](doc/screenshots/network_setup_02.png) + + +**Network configuration with SOPAS ET** + +The [SOPAS Engineering Tool](https://www.sick.com/de/en/catalog/digital-services-and-solutions/software/sopas-engineering-tool/p/p367244) (SOPAS ET) allows a detailed lidar configuration incl. network settings. We recommend to use SOPAS ET in case of network problems or to change the lidars network configuration. The operation manual contains further details. + +**Diagnostic tools** + +Network tools can help in case of connection errors. Examples are: + +* `ping ` to check if the lidar is reachable +* `fping -a 192.168.0.1\24` to see ip addresses available in the subnet +* `netcat ` to read or write network data +* `wireshark` to monitor and record the network traffic between PC and lidar. +* `sudo ufw status` to see if a firewall is active. If active, disable firewalls with `sudo ufw disable`. + +## multiScan100 / picoScan100 + +**Visual Studio: Breakpoints in Debug Mode disabled** + +:question: In Windows debug version the compiler does not stop at breakpoints. + +:white_check_mark: Check, that you are using the Debug Version. At '/Zi' to compiler settings. Disable optimization. +(see `https://stackoverflow.com/questions/865546/generating-symbols-in-release-binaries-with-visual-studio` for details). + +**Packages lost in benchmark** + +:question: sick_scan_xd seems to drop packages, when sending 10000 msgpacks with polarscan_sender_test.py from another computer + +:white_check_mark: There can be a number of reasons for dropped messages (udp or msgpacks). Besides slow network connection, there can be other pitfalls depending on the system: + +- If Linux or Windows is running in a virtual machine, make sure UDP port 2115 is forwarded. See [Firewall configuration](#firewall-configuration). + +- Depending on ROS 2 system settings, log messages might be buffered. To really see all log messages of sick_generic_caller, terminate sick_scan_xd/sick_generic_caller (Ctrl-C or kill) and view the ros logfile by `cat ~/.ros/log/sick_scan_*.log` + +**Convert pcapng-files to msgpack or json** + +:question: How can I convert a pcapng-file with scandata to a msgpack- or json-file? + +:white_check_mark: Run the following steps: +* Install python msgpack package with `pip install msgpack` +* 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 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_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_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_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. + +# Troubleshooting and technical support + +The software is based on the ROS drivers sick_scan, sick_scan_base and sick_scan2. For FAQ and troubleshooting please also have a look at https://github.com/SICKAG/sick_scan , https://github.com/SICKAG/sick_scan_base and https://github.com/SICKAG/sick_scan2 . +Common problems might be solved in closed issues. + +1. Check Scanner IP in the launch file. +2. Check Ethernet connection to scanner with netcat e.g. ```nc -z -v -w5 $SCANNERIPADDRESS 2112```. + For further details about setting up the correct ip settings see [IP configuration](#ip-address-configuration)) +3. View node startup output wether the IP connection could be established +4. Check the scanner status using the LEDs on the device. The LED codes are described in the above mentioned operation manuals. +5. Further testing and troubleshooting informations can found in the file test/readme_testplan.txt +6. If you stop the scanner in your debugging IDE or by other hard interruption (like Ctrl-C), you must wait until 60 sec. before + the scanner is up and running again. During this time the MRS6124 reconnects twice. + If you do not wait this waiting time you could see one of the following messages: + * TCP connection error + * Error-Message 0x0d +7. Amplitude values in rviz: If you see only one color in rviz try the following: + Set the min/max-Range of intensity display in the range [0...200] and switch on the intensity flag in the launch file +8. In case of network problems check your own ip address and the ip address of your laser scanner (by using SOPAS ET). + * List of own IP-addresses: ifconfig|grep "inet addr" + * Try to ping scanner ip address (used in launch file) +9. If the driver stops during init phase please stop the driver with ctrl-c and restart (could be caused due to protocol ASCII/Binary cola-dialect) + +In case of technical support please open a new issue. For optimal support, add the following information to your request: + + 1. Scanner model name, + 2. Ros node startup log, + 3. Sopas file of your scanner configuration. The instructions at https://sickconnect.com/create-and-download-a-sopas-file/ show how to create the Sopas file. + +In case of application support please use [https://support.sick.com](https://support.sick.com). + +# Creators and contribution + +Michael Lehning (http://www.lehning.de) on behalf of SICK AG (https://www.sick.com/) + +This [documentation](/CONTRIBUTING.md) is intended to provide background information on the maintenance and extension of the repository. + +# Keywords -An overview over the software and its modules can be found in [software_overview](doc/software_overview.md). - -## FAQ - -* FAQ: [FAQ](FAQ.md) - -## Keywords - -MRS1000 -MRS1104 -LMS1000 -LMS1104 -MRS6000 -MRS6124 -RMS1xxx -RMS1000 -RMSxxxx -ROS LiDAR -SICK LiDAR -SICK Laser -SICK Laserscanner -SICK Radar -LMS1xx -MRS1xxx -LMS1xxx -MRS6xxx -TiM5xx -TiM551 -TiM561 -TiM571 -TiM781 -TiM781S -LMS5xx -LMS511 -NAV210 -NAV245 -NAV310 -LDMRS -LRS4000 -LD-LRS3600 -LD-LRS3601 -LD-LRS3611 -LD-OEM1500 -LD-OEM1501 -multiScan100 -multiScan -picoScan100 -picoScan \ No newline at end of file +MRS1000, MRS1104, LMS1000, LMS1104, MRS6000, MRS6124, RMS1xxx, RMS1000, RMSxxxx, ROS LiDAR, SICK LiDAR, SICK Laser, SICK Laserscanner, SICK Radar, LMS1xx, MRS1xxx, LMS1xxx, MRS6xxx, TiM5xx, TiM551, TiM561, TiM571, TiM781, TiM781S, LMS5xx, LMS511, NAV210, NAV245, NAV310, LD-MRS, LRS4000, LD-LRS3600, LD-LRS3601, LD-LRS3611, LD-OEM1500, LD-OEM1501, multiScan100, multiScan, picoScan100, picoScan150, picoScan120, multiScan136, multiScan165 \ No newline at end of file diff --git a/REQUIREMENTS.md b/REQUIREMENTS.md deleted file mode 100644 index 776cf5c8..00000000 --- a/REQUIREMENTS.md +++ /dev/null @@ -1,44 +0,0 @@ -## Supported SICK Devices - -This driver works with all of the following products. - -| Type | Part no. | -| ------------------- | ---------------------------------------------------------------------------------------------------------------------------------- | -| MRS6124 | [6065086](https://www.sick.com/de/en/detection-and-ranging-solutions/3d-lidar-sensors/mrs6000/c/g448151) | -| MRS1000 | [e.g. 1081208](https://www.sick.com/sg/en/detection-and-ranging-solutions/3d-lidar-sensors/mrs1000/mrs1104c-111011/p/p495044) | -| TiM2xx | [1104981](https://www.sick.com/ag/en/detection-and-ranging-solutions/2d-lidar-sensors/tim2xx/tim240-2050300/p/p654443) | -| TiM5xx | [e.g. 1060445](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/tim5xx/tim551-2050001/p/p343045) | -| TiM7xxS | [e.g. 1105052](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim771s-2174104/p/p660929) | -| TiM7xx | [e.g 1096807](https://www.sick.com/de/de/mess-und-detektionsloesungen/2d-lidar-sensoren/tim7xx/tim781-2174101/p/p594148) | -| LMS5xx | [e.g. 1046135](https://www.sick.com/de/en/detection-and-ranging-solutions/2d-lidar-sensors/lms5xx/c/g179651) | -| LMS1000 | [1092445](https://www.sick.com/ag/en/detection-and-ranging-solutions/2d-lidar-sensors/lms1000/c/g387151) | -| 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) | -| RMS1000 | [1107598](https://www.sick.com/de/en/detection-and-ranging-solutions/radar-sensors/rms1000/rms1731c-636111/p/p660833) | -| RMS2000 | BETA | -| multiScan100 | [e.g. 1131164](https://www.sick.com/1131164) | -| picoScan100 | [e.g. 1134610](https://www.sick.com/1134610) | - -Note: -* LDMRS family is currently not supported on Windows. -* ROS services require installation of ROS-1 or ROS-2, i.e. services for Cola commands are currently not supported on native Linux or native Windows. -* ROS services are currently not available for LDMRS. -* dynamic reconfiguration of sick_scan_xd parameter is supported on ROS-1 or ROS-2 only, neither under Linux nor under Windows. -* Publishing pointcloud data requires ROS-1 or ROS-2. On native Linux resp. native Windows, pointcloud data are currently saved to jpg- and csv-files for demonstration purposes. - -## Supported platforms - -The driver is developed and tested for x86 architecture. Use for system with ARM architecture (e.g. Raspberry platform) is not officially supported. - -## Supported interfaces - -Only Ethernet-IPv4-based communication with the sensor is supported by this driver. \ No newline at end of file diff --git a/SUPPORT.md b/SUPPORT.md deleted file mode 100644 index 6d29bf0d..00000000 --- a/SUPPORT.md +++ /dev/null @@ -1,42 +0,0 @@ -## Bugs and Feature Requests - -- Sopas protocol mapping: --- All scanners: COLA-B (Binary) -- Software should be further tested, documented and beautified - -## Troubleshooting - -The software is based on the ROS drivers sick_scan, sick_scan_base and sick_scan2. For FAQ and troubleshooting please also have a look at https://github.com/SICKAG/sick_scan , https://github.com/SICKAG/sick_scan_base and https://github.com/SICKAG/sick_scan2 . -Common problems might be solved in closed issues. - -### General troubleshooting - -1. Check Scanner IP in the launch file. -2. Check Ethernet connection to scanner with netcat e.g. ```nc -z -v -w5 $SCANNERIPADDRESS 2112```. - For further details about setting up the correct ip settings see [IP configuration](doc/ipconfig/ipconfig.md) -3. View node startup output wether the IP connection could be established -4. Check the scanner status using the LEDs on the device. The LED codes are described in the above mentioned operation manuals. -5. Further testing and troubleshooting informations can found in the file test/readme_testplan.txt -6. If you stop the scanner in your debugging IDE or by other hard interruption (like Ctrl-C), you must wait until 60 sec. before - the scanner is up and running again. During this time the MRS6124 reconnects twice. - If you do not wait this waiting time you could see one of the following messages: - * TCP connection error - * Error-Message 0x0d -7. Amplitude values in rviz: If you see only one color in rviz try the following: - Set the min/max-Range of intensity display in the range [0...200] and switch on the intensity flag in the launch file -8. In case of network problems check your own ip address and the ip address of your laser scanner (by using SOPAS ET). - * List of own IP-addresses: ifconfig|grep "inet addr" - * Try to ping scanner ip address (used in launch file) -9. If the driver stops during init phase please stop the driver with ctrl-c and restart (could be caused due to protocol ASCII/Binary cola-dialect). - -## Support - -* In case of technical support please open a new issue. For optimal support, add the following information to your request: - 1. Scanner model name, - 2. Ros node startup log, - 3. Sopas file of your scanner configuration. - The instructions at http://sickusablog.com/create-and-download-a-sopas-file/ show how to create the Sopas file. -* In case of application support please use [https://supportportal.sick.com ](https://supportportal.sick.com). -* Issue Handling: Issues, for which no reply was received from the questioner for more than 7 days, - are closed by us because we assume that the user has solved the problem. - diff --git a/USAGE.md b/USAGE.md deleted file mode 100644 index 3e3d5ef0..00000000 --- a/USAGE.md +++ /dev/null @@ -1,542 +0,0 @@ -## Run sick_scan_xd driver - -The sick_scan_xd driver can be started on the command line by `sick_generic_caller [hostname:=]`. The start process varies slightly depending on the target OS: - -- On native Linux without ROS, call - - ```sick_generic_caller ``` - -- On Linux with ROS-1, call - - ```./devel_isolated/setup.bash``` - - ```roslaunch sick_scan_xd ``` - -- On Linux with ROS-2, call - - ```source ./install/setup.bash``` - - ```ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/``` - -- On native Windows without ROS, call - - ```sick_generic_caller ``` - -- On Windows with ROS-2, call - - ```call .\install\setup.bat``` - - ```ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/``` - -Use the following commands to run the sick_scan_xd driver for a specific scanner type: - -- For MRS6124: - * Linux native: `sick_generic_caller sick_mrs_6xxx.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_mrs_6xxx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_mrs_6xxx.launch` - * Windows native: `sick_generic_caller sick_mrs_6xxx.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_mrs_6xxx.launch` -- For MRS1104: - * Linux native: `sick_generic_caller sick_mrs_1xxx.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_mrs_1xxx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd 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_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_mrs_1xxx.launch` -- For LMS1104 with firmware 1.x: - * Linux native: `sick_generic_caller sick_lms_1xxx.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_lms_1xxx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd 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_xd 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_xd sick_lms_1xxx_v2.launch` - * Linux ROS-2: `ros2 run sick_scan_xd 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_xd 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_xd sick_tim_240.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_240.launch` - * Windows native: `sick_generic_caller sick_tim_240.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_240.launch` -- For TiM5xx-family: - * Linux native: `sick_generic_caller sick_tim_5xx.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_tim_5xx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_5xx.launch` - * Windows native: `sick_generic_caller sick_tim_5xx.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_5xx.launch` -- For TiM7xx-family (no safety scanner): - * Linux native: `sick_generic_caller sick_tim_7xx.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_tim_7xx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch` - * Windows native: `sick_generic_caller sick_tim_7xx.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch` -- For TiM7xxS-family (safety scanner): - * Linux native: `sick_generic_caller sick_tim_7xxS.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_tim_7xxS.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xxS.launch` - * Windows native: `sick_generic_caller sick_tim_7xxS.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xxS.launch` -- For LMS1xx-family: - * Linux native: `sick_generic_caller sick_lms_1xx.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_lms_1xx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_1xx.launch` - * Windows native: `sick_generic_caller sick_lms_1xx.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_1xx.launch` -- For LMS5xx-family: - * Linux native: `sick_generic_caller sick_lms_5xx.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_lms_5xx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_5xx.launch` - * Windows native: `sick_generic_caller sick_lms_5xx.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_5xx.launch` -- For LMS4xxx-family: - * Linux native: `sick_generic_caller sick_lms_4xxx.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_lms_4xxx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_4xxx.launch` - * Windows native: `sick_generic_caller sick_lms_4xxx.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_4xxx.launch` -- For LRS4000: - * Linux native: `sick_generic_caller sick_lrs_4xxx.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_lrs_4xxx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_4xxx.launch` - * Windows native: `sick_generic_caller sick_lrs_4xxx.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_4xxx.launch` -- For LDMRS-family: - * Linux native: `sick_generic_caller sick_ldmrs.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_ldmrs.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_ldmrs.launch` - * Note that LDMRS are currently not supported on Windows -- For LRS36x0: - * Linux native: `sick_generic_caller sick_lrs_36x0.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_lrs_36x0.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x0.launch` - * Windows native: `sick_generic_caller sick_lrs_36x0.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x0.launch` -- For LRS36x0 mounted upside down: - * Linux native: `sick_generic_caller sick_lrs_36x0_upside_down.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_lrs_36x0_upside_down.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x0_upside_down.launch` - * Windows native: `sick_generic_caller sick_lrs_36x0_upside_down.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x0_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 LRS36x1: - * Linux native: `sick_generic_caller sick_lrs_36x1.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_lrs_36x1.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x1.launch` - * Windows native: `sick_generic_caller sick_lrs_36x1.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x1.launch` -- For LRS36x1 mounted upside down: - * Linux native: `sick_generic_caller sick_lrs_36x1_upside_down.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_lrs_36x1_upside_down.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lrs_36x1_upside_down.launch` - * Windows native: `sick_generic_caller sick_lrs_36x1_upside_down.launch` - * Windows ROS-2: `ros2 run sick_scan_xd 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_xd sick_oem_15xx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd 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_xd 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_xd sick_nav_2xx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_2xx.launch` - * Windows native: `sick_generic_caller sick_nav_2xx.launch` - * Windows ROS-2: `ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_nav_2xx.launch` -- For NAV310: - * Linux native: `sick_generic_caller sick_nav_31x.launch` - * Linux ROS-1: `roslaunch sick_scan_xd sick_nav_31x.launch` - * Linux ROS-2: `ros2 run sick_scan_xd 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_xd 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_xd sick_nav_350.launch` - * Linux ROS-2: `ros2 run sick_scan_xd 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_xd 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_xd sick_rms_xxxx.launch` - * Linux ROS-2: `ros2 run sick_scan_xd 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_xd 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_xd sick_multiscan.launch hostname:= udp_receiver_ip:=` - * Linux ROS-2: `ros2 launch sick_scan_xd 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_xd 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). -- For picoScan150: - * Linux native: `sick_generic_caller sick_picoscan.launch hostname:= udp_receiver_ip:=` - * Linux ROS-1: `roslaunch sick_scan_xd sick_picoscan.launch hostname:= udp_receiver_ip:=` - * Linux ROS-2: `ros2 launch sick_scan_xd sick_picoscan.launch.py hostname:= udp_receiver_ip:=` - * Windows native: `sick_generic_caller sick_picoscan.launch hostname:= udp_receiver_ip:=` - * Windows ROS-2: `ros2 launch sick_scan_xd sick_picoscan.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 - -- `hostname:=` to connect to a sensor with a given IP address. Default value is always the factory default IP address of the scanner. - -Further (common and scanner specific) options can be set via launchfile, see [Common parameters](#common-parameters) and configure the settings in the launchfile corresponding to the scanner type. - -Note: After modifying a launch-file, it has to be installed by running `catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -Wno-dev` -to be located and used by `roslaunch`. - -On ROS-2 you can launch sick_generic_caller by python-launchfiles, too. Use -``` -ros2 launch sick_scan_xd .launch.py := -``` -E.g. for LMS-5xx: `ros2 launch sick_scan_xd sick_lms_5xx.launch.py hostname:=192.168.0.1` - -The launch.py-files on ROS-2 passes the corresponding launch-file to the driver: [sick_lms_5xx.launch.py](launch/sick_lms_5xx.launch.py) gives an example for LMS-5xx. Parameter can be overwritten -* either by commandline, e.g.
`ros2 launch sick_scan_xd sick_lms_5xx.launch.py hostname:=192.168.0.1`, -* or by passing additional arguments in the launch.py-file, e.g.
`node = Node(package='sick_scan_xd', executable='sick_generic_caller', arguments=[launch_file_path, 'hostname:=192.168.0.1'])` - - -### Start Multiple Nodes - -Multiple nodes can be started to support multiple sensors. In this case, multiple instances of sick_scan_xd have to be started, each node with different name and topic. ROS-1 example to run two TiM 7xx devices with ip address `192.168.0.1` and `192.168.0.2`: - -``` -roslaunch sick_scan_xd sick_tim_7xx.launch nodename:=sick_tim_7xx_1 hostname:=192.168.0.1 cloud_topic:=cloud_1 & -roslaunch sick_scan_xd sick_tim_7xx.launch nodename:=sick_tim_7xx_2 hostname:=192.168.0.2 cloud_topic:=cloud_2 & -``` - -On Linux with ROS-1, multiple nodes to support multiple sensors can be started by one launch file, too. -Take the launchfile [sick_tim_5xx_twin.launch](launch/sick_tim_5xx_twin.launch) as an example. -Remapping the scan and cloud topics is essential to distinguish the scandata and provide TF information. - -ROS-2 example to run two TiM 7xx devices with ip address `192.168.0.1` and `192.168.0.2`: - -``` -ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch nodename:=sick_tim_7xx_1 hostname:=192.168.0.1 cloud_topic:=cloud_1 & -ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch nodename:=sick_tim_7xx_2 hostname:=192.168.0.2 cloud_topic:=cloud_2 & -``` - -### Common parameters - -For the launch-file settings and the tag/values pairs the following keywords are supported: - -| Keyword | Meaning | Default value | Hint | -|--------------|-----------------|----------------|---------------| -| scanner_type | Scanner family | ??? | see list above | -| min_ang | Start scan angle in [rad] | -2.3998277 | | -| max_ang | End scan angle in [rad] | +2.3998277 | | -| intensity_resolution_16bit | Switch between 8Bit/16Bit| "false" | do not change| -| hostname | Ip address of scanner | 192.168.0.1 | change to scanner ip address in your network (see faq) | -| port | port number | 2112 | do not change, check firewall rules if there is blocking traffic | -| timelimit | Timelimit in [sec] | 5 | do not change | - -- `scanner_type` - Name of the used scanner. Usually this is also the name of the launch file. This entry is used to differentiate - between the various scanner properties within the software code. - -- `hostname` - IP-address of the scanner (default: 192.168.0.1) - -- `port` - IP-port of the scanner (default: 2112) - -- `min_ang` - Start angle in [rad] - -- `max_ang` - End angle in [rad] - -- `use_binary_protocol` - Switch between SOPAS Binary and SOPAS ASCII protocol - -- `intensity` - Enable or disable transport of intensity values - -- `intensity_resolution_16bit` - If true, the intensity values is transferred as 16 bit value. If false, as 8 bit value. - -- `min_intensity` - If min_intensity > 0, all range values in a LaserScan message are set to infinity, if their intensity value is below min_intensity - -- `cloud_topic` - Topic name of the published pointcloud2 data - -- `frame_id` - Frame id used for the published data - -Tag/value pairs of the commandline overwrite settings in the launch file. -The use of the parameters can be looked up in the launch files. This is also recommended as a starting point. - -### Starting Scanner with Specific Ip Address - -To start the scanner with a specific IP address, option `hostname:=` can be used. -The hostname is the ip-address of the scanner, e.g. -``` -sick_generic_caller sick_tim_5xx.launch hostname:=192.168.0.71 # Linux native -roslaunch sick_scan_xd sick_tim_5xx.launch hostname:=192.168.0.71 # Linux ROS-1 -ros2 run sick_scan_xd sick_generic_caller sick_tim_5xx.launch hostname:=192.168.0.71 # Linux ROS-2 -sick_generic_caller sick_tim_5xx.launch hostname:=192.168.0.71 # Windows native -ros2 run sick_scan_xd sick_generic_caller sick_tim_5xx.launch hostname:=192.168.0.71 # Windows ROS-2 -``` - -### Further useful parameters and features - -- **Timestamps**: If parameter`sw_pll_only_publish` is true (default), an internal Software PLL is used to sync the scan generation timestamps to system timestamps. See [software_pll.md](./doc/software_pll.md) for details. - -- **Angle compensation**: For highest angle accuracy the NAV-Lidar series supports an [angle compensation mechanism](./doc/angular_compensation.md). - -- **Angle correction**: MRS-1xxx lidars transmit accurate azimuth angles for each scan point. Therefore, the stride (angle increment) of the MRS-1xxx azimuth angles in polar and cartesian point clouds is not exactly constant. Since laserscan messages assume a constant angle increment, scan points in point cloud and laserscan messages have slightly different azimuth angles. - -- **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 (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: - ``` - - - ``` - By default, radar objects are tracked. - -- **Coordinate transform**: An optional coordinate transform can be applied to the pointcloud. See [coordinate transforms](./doc/coordinate_transforms.md) for details. - -### Lidar specific parameters - -Some lidars have specific parameters and options. - -- **TiM781S**: For TiM781S lidars, the initial lidar configuration can be deactivated using optional argument initialize_scanner:=0. - Note that this mode does not initialize the lidar. The mode assumes that the scanner is in an appropriate state corresponding to the properties configured in the launchfile. It is not recommended to use this option unless for specific tasks in a controlled environment.
- **Do not use this mode except the lidar has been configured properly and initialized successfully and is in the same state as after initialization by the launchfile! This option is for advanced users only!**
- Example: `roslaunch sick_scan_xd sick_tim_7xxS.launch hostname:=192.168.0.1 initialize_scanner:=0` - -### ROS services - -On ROS-1 and ROS-2, services can be used to send COLA commands to the sensor. This can be very helpful for diagnosis, e.g. by querying the device status or its id. - -Use the following examples to run a cola commond on ROS-1: -``` -rosservice call /sick_lms_5xx/ColaMsg "{request: 'sMN IsSystemReady'}" -rosservice call /sick_lms_5xx/ColaMsg "{request: 'sRN SCdevicestate'}" -rosservice call /sick_lms_5xx/ColaMsg "{request: 'sEN LIDinputstate 1'}" -rosservice call /sick_lms_5xx/ColaMsg "{request: 'sEN LIDoutputstate 1'}" -rosservice call /sick_lms_5xx/ColaMsg "{request: 'sMN LMCstartmeas'}" -rosservice call /sick_lms_5xx/SCdevicestate "{}" # query device state -rosservice call /sick_lms_5xx/SCreboot "{}" # execute a software reset on the device -rosservice call /sick_lms_5xx/SCsoftreset "{}" # save current parameter and shut down device -``` - -Use the following examples to run a cola commond on ROS-2: -``` -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN IsSystemReady'}" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN SCdevicestate'}" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sEN LIDinputstate 1'}" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sEN LIDoutputstate 1'}" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN LMCstartmeas'}" -ros2 service call /SCdevicestate sick_scan_xd/srv/SCdevicestateSrv "{}" # query device state -ros2 service call /SCreboot sick_scan_xd/srv/SCrebootSrv "{}" # execute a software reset on the device -ros2 service call /SCsoftreset sick_scan_xd/srv/SCsoftresetSrv "{}" # save current parameter and shut down device -``` - -Use ros service `SickScanExit` to stop the scanner and driver: -``` -rosservice call /sick_nav_31x/SickScanExit "{}" # stop scanner and driver on ROS-1 -ros2 service call /SickScanExit sick_scan_xd/srv/SickScanExitSrv "{}" # stop scanner and driver on ROS-2 -``` - -Note: -* The COLA commands are sensor specific. See the user manual and telegram listing for further details. -* ROS services require installation of ROS-1 or ROS-2, i.e. services for Cola commands are currently not supported on native Linux or native Windows. -* ROS services are currently not available for the LDMRS. -* Some SOPAS commands like `sMN SetAccessMode 3 F4724744` stop the current measurement. In this case, the driver restarts after a timeout (5 seconds by default). To process those SOPAS commands without restart, you can - * send `sMN LMCstartmeas` and `sMN Run` to switch again into measurement mode within the timeout, or - * increase the driver timeout `read_timeout_millisec_default` in the launch-file. - -Additional services can be available for specific lidars. Service "GetContaminationResult" is e.g. available for MRS-1xxx, LMS-1xxx and multiScan: -``` -# ROS-1 example for service GetContaminationResult (LMS 1xxx) -rosservice call /sick_lms_1xxx/GetContaminationResult "{}" -# ROS-2 example for service GetContaminationResult (LMS 1xxx) -ros2 service call /GetContaminationResult sick_scan_xd/srv/GetContaminationResultSrv "{}" -``` - -Example sequence with stop and start measurement to set a particle filter (TiM-7xxx on ROS-1): -``` -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LFPparticle'}" # response: "sRA LFPparticle \\x00\\x01\\xf4" -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sWN LFPparticle 0101F4'}" # response: "sWA LFPparticle" -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN LMCstartmeas'}" -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN Run'}" -``` - -### Driver states, timeouts - -The driver runs in two different states: - -1. Initialization: The scanner is initialized and configured by a list of sopas commands - -2. Measurement: The scanner is operational, scandata are transmitted and the point cloud is published. -After start, the driver enters initialization mode. After successful initialization, the driver switches automatically into measurement mode. - -The communication between driver and scanner is monitored. In case of communication timeouts, e.g. due to network problems, the TCP connection is reset and the scanner is re-initialized. The driver uses 3 different timeouts (i.e time since last message received from lidar): - -1. In measurement mode: If no messages arrive for 5 seconds [timeout 0], the TCP/IP connection is closed. After a short delay, the tcp connection is reopened and the driver switches to initialisation mode and reinitialises the Lidar. - -2. In initialisation mode: If no messages received after 120 sec [Timeout 1] the TCP/IP connection is closed. After a short delay, the tcp connection is reopened and the driver switches to initialisation mode and reinitialises the Lidar. - -3. In any mode: If no messages received after 150 sec [Timeout 2] the driver terminates. - -Note: The internal timer is reset on successful communication. i.e. the timeout refers to the time of the last message from the Lidar. If there was no message yet, then the time of programme start is used. - -All timeouts can be configured in the launchfile: -``` - - - - -``` - -The following diagram shows the transition between the driver states: - -![driverStatesDiagram](./doc/driverStatesDiagram1.png) - -Note: Timeout 2 (i.e. no lidar message after 150 seconds) terminates the driver. By default, the driver does not restart automatically. It is therefor recommended to run the driver within an endless loop, e.g. in bash: - -``` -while(true) ; do roslaunch sick_scan_xd [] ; done -``` - -The following table summarizes the timeout parameter: - -![timeout_parameter](./doc/timeout_parameter.png) - -Details of timeout settings: - -* message_monitoring_enabled: Enable or disable timeouts and monitoring. Disabling deactivates any error handling in case of network problems. Recommended default value: True - -* read_timeout_millisec_default: Read timeout in milliseconds in operational (measurement) mode. If no datagrams are received from lidar within 5 seconds (default), the TCP socket is closed and the lidar is reinitialized. - -* read_timeout_millisec_startup: Read timeout in milliseconds during initialization after startup. If SOPAS commands are not responded within 120 seconds (default), the TCP socket is closed and lidar is reinitialized. - -* read_timeout_millisec_kill_node: Pointcloud timeout in milliseconds in operational (measurement) mode. If the sick_scan_xd does not publish a pointcloud within the last 150 seconds, the sick_scan_xd process is killed. Should never happen, but is the “last resort” to exit after any kind of error (e.g. socket hangs up and blocks after network trouble). - -* All timeouts configured in milliseconds - -* To disable timeouts (not recommended): - * Set message_monitoring_enabled = false, or - * Set timeouts to "infinite" values, i.e MAX_INT = 2147483647 milliseconds (24.9 days) - -* To disable pointcloud monitoring (not recommended): - * read_timeout_millisec_kill_node <= 0 deactivates pointcloud monitoring - -* Parameter read_timeout_millisec_default and read_timeout_millisec_startup: value 0 and negative values are currently NOT mapped to other values, i.e. will cause an immediately timeout error. Use value 2147483647 or message_monitoring_enabled = false to deactivate read timeouts (not recommended) - -## Sopas Mode - -This driver supports both COLA-B (binary) and COLA-A (ASCII) communication with the laser scanner. Binary mode is activated by default, since this mode generates less network traffic and enables more compatibility to all scanners. -If the communication mode set in the scanner memory is different from that used by the driver, the scanner's communication mode is changed. This requires a restart of the TCP-IP connection, which can extend the start time by up to 30 seconds. -There are two ways to prevent this: -1. Recommended: - * Set the communication mode with the SOPAS ET software to binary and save this setting in the scanner's EEPROM. - * Set "use_binary_protocol" to default value "true". -2. Use the parameter "use_binary_protocol" to overwrite the default settings of the driver. - -## Tools - -Various tools exist in the repository to improve the operation of the scanners. It is also recommended to read the following section "Troubleshooting". -Overview of the tools: - -* Search for scanner in the network: - Use the Python3 tool "sick_generic_device_finder.py" in the tools/sick_generic_device_finder directory. - The tools will output the IP addresses of the connected scanners and some more information about the scanner. - Call it with python3, i.e. - `` - python3 sick_generic_device_finder.py - `` -* Setting new IP address: With the help of the parameter "new_IP" a new IP address can be assigned when calling the node sick_scan_xd. - The launch file sick_new_ip.launch in the launch directory shows an example of how to use this parameter. -* Converting of pointclouds to images: With the tool pcl_converter.cpp one can convert pointcloud2-data - to image. That is especial convenient for 24-layers scanners like the MRS6124. -* Setting up a brand new scanner: To set up a brand new scanner, - it is recommended to use the two tools "sick_generic_device_finder.py" to find the scanner in the network - and the launch file sick_new_ip.launch to set a new IP address. If further settings are to be saved that cannot be made via ROS parameters, we recommend using the Windows tool "Sopas ET" from SICK. -* Unit tests: For a quick unit test after installation without the sensor hardware, a test server is provided to simulate a scanner. See [emulator](doc/emulator.md) for further details. -* Testing: The sick_scan_test program was developed for testing the driver. See [test/sick_scan_test.md](test/sick_scan_test.md) for details. - -## Simulation - -For unittests without sensor hardware, a simple test server is provided. To build the test server, call either cmake with option `-DCMAKE_ENABLE_EMULATOR=1`, or activate cmake option `ENABLE_EMULATOR` in CMakeLists.txt. Then rebuild sick_scan_xd. By default, option `ENABLE_EMULATOR` is switched off. - -Please note that this just builds a simple test server for basic unittests of sick_scan_xd drivers. Its purpose is to run basic tests and to help with diagnosis in case of issues. It does not emulate a real scanner! - -Simulation requires jsoncpp. Install with `sudo apt-get install libjsoncpp-dev` on Linux and with `vcpkg install jsoncpp:x64-windows` on Windows. - -You can find examples to test and run sick_scan_xd in offline mode in folder `test/scripts`. Their purpose is to demonstrate the usage of the sick_scan_xd driver. Please feel free to customize the scripts or use them as a starting point for own projects. - -### Simulation on Windows - -Run script `run_simu_lms_5xx.cmd` in folder `test/scripts` or execute the following commands: - -1. Start the test server: - ``` - cd .\build - start "testserver" cmd /k python ../test/emulator/test_server.py --scandata_file=../test/emulator/scandata/20210302_lms511.pcapng.scandata.txt --scandata_frequency=20.0 --tcp_port=2112 - @timeout /t 1 - ``` - -2. Run sick_generic_caller. On native Windows: - ``` - .\Debug\sick_generic_caller.exe ../launch/sick_lms_5xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False - ``` - On Windows with ROS-2: - ``` - ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_5xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False - ``` - -3. Open file `image_viewer.html` in folder `demo` in your browser to view a jpg-image of the current scan. - -Note, that python version 3 incl. runtime dlls must be accessable, f.e. by extending the PATH environment variable: -``` -set PYTHON_DIR=%ProgramFiles(x86)%/Microsoft Visual Studio/Shared/Python37_64 -set PATH=%PYTHON_DIR%;%PYTHON_DIR%/Scripts;c:\vcpkg\installed\x64-windows\bin;%PATH% -``` - -Further examples are provided in folder `test/scripts`. - -### Simulation on Linux - -Run script `run_simu_lms_5xx.bash` in folder `test/scripts` or execute the following commands: - -1. Start the test server: - ``` - python3 ./test/emulator/test_server.py --scandata_file=./test/emulator/scandata/20210302_lms511.pcapng.scandata.txt --scandata_frequency=20.0 --tcp_port=2112 & - sleep 1 - ``` - -2. Run sick_generic_caller. - - On native Linux: - ``` - ./build/sick_generic_caller ./launch/sick_lms_5xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & - ``` - - On Linux with ROS-1: - ``` - roslaunch sick_scan_xd sick_lms_5xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & - ``` - - On Linux with ROS-2: - ``` - ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_lms_5xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & - ``` - -3. View the point cloud. - - On native Linux:
- Open file `image_viewer.html` in folder `demo` in a browser (f.e. firefox) to view a jpg-image of the current scan. - - On Linux with ROS-1: - ``` - rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_lms5xx.rviz & - ``` - - On Linux with ROS-2: - ``` - rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_lms5xx.rviz & - ``` - -Further examples are provided in folder `test/scripts`. - diff --git a/doc/IMU.md b/doc/IMU.md deleted file mode 100644 index a41c5668..00000000 --- a/doc/IMU.md +++ /dev/null @@ -1,20 +0,0 @@ -## IMU Support - -The scanners of the MRS6xxx and MRS1xxx series will be optionally available with an IMU in 2019. - -See [IMU support](sick_scan_segment_xd#imu-support) for the IMU support of multiScan and picoScan. - -## Activating IMU Messages -By setting the following config parameter in the launch file, the output of [imu messages](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html) can be enabled with a compatible scanner. Currently the messages are published in the /imu Topic. -```xml - - -``` -The imu Messages contain covariance matrices, these are currently determined from empirical values and are not measured specifically for each scanner. -The laser scanner provides additional information (tick timestamp and confidence) to the Imu messages these can be activated by activating the [SickImu messages](../msg/SickImu.msg). - -```xml - -``` - -IMU messages are only supported in SOPAS binary mode. Due to the high data rate of the IMU messages (100 Hz and more) while sending the standard laser scanner messages at the same time, the ASCII mode is not supported. Please set the scanner to binary mode if you are using the IMU. diff --git a/doc/addDevice.md b/doc/addDevice.md deleted file mode 100644 index 270b3b21..00000000 --- a/doc/addDevice.md +++ /dev/null @@ -1,62 +0,0 @@ -# Adding new Device -## Table of contents - -- [Introduction](#introduction) -- [Launch Files](#launch-files) - -## Introduction - -This driver is designed to support several different scanner types (including radar) from Sick. -This documentation describes how to add additional devices to the driver. - -## Naming Convention - -For each device type a name pattern is assigned as follows: -`` -sick__ -`` - -The name type is used in the code to decide which scanner-specific parameters are set. -The name type is passed as a parameter as follows: -``` - -``` - -## Launch Files - -A launch file is created for each device type, -which usually has the same naming convention as the scanner type. -To create a new device, it is recommended to copy, rename and edit an existing launch file. - -## Code Modification - -1. Hint: Construction of parser: -``` - sick_scan_xd::SickGenericParser *parser = new sick_scan_xd::SickGenericParser(scannerName); -``` -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_XXXX_NAME); - in the file sick_generic_parser.cpp - -4. Add new parameter block like - ``` - if (basicParams[i].getScannerName().compare(SICK_SCANNER_MRS_1XXX_NAME) == 0) - {... - } in the file sick_generic_parser.cpp - ``` - -5. Copy the file sick_generic_radar.cpp and add a new class following the structure -of this file. - - - - - - - - - - - diff --git a/doc/angular_compensation.md b/doc/angular_compensation.md deleted file mode 100644 index 87b55beb..00000000 --- a/doc/angular_compensation.md +++ /dev/null @@ -1,116 +0,0 @@ -# Angle Compensation -## Table of contents - -- [Introduction](#introduction) -- [Example](#example) -- [Comparing compensated vs. raw values](#comparing-compensated-vs-raw-values) -- [Check compensation function](#check-compensation-function) - -## Introduction - -For measurements with the highest demands on the accuracy of the angle measurement, the devices of the NAV series allow compensation of slight angle deviations during a rotation. The compensation is determined by the three parameters. -* Additive compensation by an angle offset -* Sinusoidal correction by specifying the amplitude and phase of compensation - -The three parameters are then used to calculate the compensation as follows: - - ![Formula for angle compensation](angle_compensation/angle_compensation_000.png) - -Offset and phase are given in [deg] - -## Example - -The information is read from lidar by using the command `sRN MCAngleCompSin`. -The answer gives one amplitude, phase and offset compensation in tens of thousands. - -The function reads -* Amplitude-Parameter: +1893 -* Phase-Parameter: -210503 -* Offset-Parameter: -245 - -These corresponds to: -* Amplitude-compensation: +0.1893 -* Phase-Compensation: -21.0503 [deg] -* Offset-Compensation: -0.0245 [deg] - -### Compensation formula for example for NAV210/NAV245 -``` -Angle[comp.] = Angle[Raw] - 0.189300 * sin(Angle[Raw] + -21.050300 [deg]) - -0.024500 -``` - -### Compensation formula for example for NAV310 -``` -Angle[comp.] = Angle[Raw] + 0.189300 * sin(Angle[Raw] - -21.050300 [deg]) + -0.024500 -``` -#### Example lookup values for NAV310 for this example (first entries) - -|IN [Deg] | Out [Deg]| Correction [Deg] | -|----------|----------|--------------------| -| 0.000000| 0.043494| 0.043494 | -| 1.000000| 1.046567| 0.046567 | -| 2.000000| 2.049618| 0.049618 | -| 3.000000| 3.052647| 0.052647 | -| 4.000000| 4.055652| 0.055652 | -| 5.000000| 5.058633| 0.058633 | -| 6.000000| 6.061588| 0.061588 | -| 7.000000| 7.064518| 0.064518 | -| 8.000000| 8.067420| 0.067420 | -| 9.000000| 9.070294| 0.070294 | -| 10.000000| 10.073139| 0.073139 | - - - - -## Comparing compensated vs. raw values - -For the example the compensation function looks like this (X-Axis: measured angle [deg], Y-Axis: compensation in [deg]) - - ![Plot of compensation function (example)](angle_compensation/angle_compensation_001.png) - - -## Coordinate systems - -For a better understanding of the data sheets of the different lidar systems the following drawings compare the different coordinate systems. Usually the scanners rotate counter-clockwise. The scanners of the NAV3xx series rotate clockwise. All coordinate systems following the right-hand rule, if the axis definition as shown in the picture is used. - ![Used coordinate systems](angle_compensation/3d_coordinate_system_comp.png) - -By means of simple matrix operations all axis orientations can be transformed into each other. But since we are only interested in the angle around the Z-axis, the conversions can be done as follows (CS = Coordinate System): - -|IN From | Out To | Operation | Additional info | -|--------|--------|--------------|------------------------------------| -| ROS | NAV3xx | Out=-In+180° | maps [-180°...180°] to [360°...0°] | -| NAV3xx | ROS | Out=-In+180° | maps [0°...360°] to [180°...-180°] | -| ROS | NAV2XX | Out=In+90° | | -| NAV2xx | ROS | Out=In-90° | | - -## Check compensation function - -By using `Octave` ones can check the compensation function against the given values by exporting the value via a testbed function. - - - - - - - - - diff --git a/doc/bloom.md b/doc/bloom.md deleted file mode 100644 index 70474c0a..00000000 --- a/doc/bloom.md +++ /dev/null @@ -1,230 +0,0 @@ -# sick_scan_xd releases using bloom: - -## First time installation of toolchain - -1. Install on Linux: - * Install bloom: - ``` - sudo apt-get update - sudo apt-get install python3-bloom python3-catkin-pkg - ``` - * Install docker: - ``` - pushd /tmp - curl -fsSL https://get.docker.com -o get-docker.sh - sudo sh get-docker.sh - sudo usermod -aG docker $USER - popd - shutdown -r now # reboot - # short quicktest - docker --version - docker info - docker run hello-world - ``` - * Install ros-buildfarm: - ``` - # sudo apt-get install python3-ros-buildfarm # not successfully, unable to locate - pip3 install ros-buildfarm # installs ros-buildfarm 3.0 successfully - ``` - -2. Build the prerelease: - * Short version to build a prerelase: - * Run the following commands: - ``` - mkdir -p ./ws_sick_scan_xd_bloom/src - cd ./ws_sick_scan_xd_bloom/src - git clone -b master https://github.com/SICKAG/sick_scan_xd.git - cd ./sick_scan_xd/test/scripts - ./run_linux_ros1_bloom.bash - ``` - * Fix any errors during the prerelease build and check in - * Repeat `./run_linux_ros1_bloom.bash` until the the prerelease build finishes without errors - * Alternative version: - * Open http://prerelease.ros.org/noetic in the brower - * Add a custom repository: `sick_scan_xd` , `https://github.com/SICKAG/sick_scan_xd` , `master` (or `feature/bloom_pretest` or any other branch to test) - * Add a custom repository: `msgpack11` , `https://github.com/SICKAG/msgpack11` , `master` - * Add a custom repository: `libsick_ldmrs` , `https://github.com/SICKAG/libsick_ldmrs` , `master` - * Confirm next steps (i.e. URL of build farm: https://raw.githubusercontent.com/ros-infrastructure/ros_buildfarm_config/production/index.yaml, Ubuntu focal) - * Click on `Generate command` - * Run the generated command, i.e.: - ``` - source /opt/ros/noetic/setup.bash - mkdir -p /tmp/prerelease_job - cd /tmp/prerelease_job - generate_prerelease_script.py \ - https://raw.githubusercontent.com/ros-infrastructure/ros_buildfarm_config/production/index.yaml \ - noetic default ubuntu focal amd64 \ - --custom-repo \ - sick_scan_xd:git:https://github.com/SICKAG/sick_scan_xd:master \ - msgpack11:git:https://github.com/SICKAG/msgpack11:master \ - libsick_ldmrs:git:https://github.com/SICKAG/libsick_ldmrs:master \ - --level 1 \ - --output-dir ./ - ``` - * Run `printf "\033c" ; rm -rf ~/.ccache ; mkdir -p ~/.ccache ; ./prerelease.sh` in folder `/tmp/prerelease_job` - * In case of error message `/usr/lib/ccache/cc is not able to compile a simple test program`: - * Remove folder `~/.ccache` before running `./prerelease.sh` - * See https://answers.ros.org/question/347063/error-pre-release-melodic/ - * Fix any errors during the prerelease build and check in - * Remove the temporary build folder by `rm -rf /tmp/prerelease_job` - * Repeat until `prerelease.sh` finishes without errors. - -3. Submit package sick_scan_xd for indexing (noetic) - * Fork `https://github.com/ros/rosdistro` -> `https://github.com//rosdistro.git` - * `git clone https://github.com//rosdistro.git` - * Edit file `rosdistro/noetic/distribution.yaml` and add after `sick_scan`: - ``` - sick_scan_xd: - doc: - type: git - url: https://github.com/SICKAG/sick_scan_xd.git - version: master - ``` - * `cd rosdistro ; source /opt/ros/noetic/setup.bash ; rosdistro_reformat file://"$(pwd)"/index.yaml` - * git commit: `git commit -m "Adding sick_scan_xd to documentation index for distro noetic" distribution.yaml` - * git push: `git push origin master` - * Submit a pull request on `https://github.com//rosdistro` - -4. For ROS-2 humble: Follow instructions on https://docs.ros.org/en/humble/How-To-Guides/Releasing/Releasing-a-Package.html - * Note: Bloom releases for ros2 foxy are not longer supported (Pull request failed, "This pull request changes files for a ROS distribution that is no longer supported (End Of Life)") - * Submit package sick_scan_xd for indexing (ros2 humble) - * Reset fork `https://github.com//rosdistro.git` to origin/master or delete the fork and create a new one -> `https://github.com//rosdistro.git` - * `git clone https://github.com//rosdistro.git` - * Edit file `rosdistro/humble/distribution.yaml` and add after `sick_safevisionary_ros2`: - ``` - sick_scan_xd: - doc: - type: git - url: https://github.com/SICKAG/sick_scan_xd.git - version: develop - status: developed - ``` - * git commit and push ("Adding sick_scan_xd to documentation index for distro humble") - * Submit a pull request on `https://github.com//rosdistro` - * Do the same for any new ROS2 version, e.g. iron and jazzy (`rosdistro/iron/distribution.yaml`, `rosdistro/jazzy/distribution.yaml`) - * [Start a new release team](https://github.com/ros2-gbp/ros2-gbp-github-org/issues/new?assignees=&labels=&template=new_release_team.md&title=Add+release+team) - * ROS-2 sick_scan_xd team: https://github.com/orgs/ros2-gbp/teams/sick_scan_xd - * ROS-2 sick_scan_xd release repository: https://github.com/ros2-gbp/sick_scan_xd-release - -## Release build for ROS-1 noetic - -* Build a prerelease (dry run in a docker container): - * Run the following commands: - ``` - git clone -b master https://github.com/SICKAG/sick_scan_xd.git - cd ./sick_scan_xd/test/scripts - sudo dos2unix ./*.bash ; sudo chmod a+x ./*.bash - ./run_linux_ros1_bloom.bash - ``` - * Fix any errors during the prerelease build and check in - * Repeat `./run_linux_ros1_bloom.bash` until the the prerelease build finishes without errors - -* Build a binary release: follow https://wiki.ros.org/bloom/Tutorials/FirstTimeRelease - * Update version number in package.xml, minor version number should be incremented at least - * Create resp. update CHANGELOG.rst: - ``` - source /opt/ros/noetic/setup.bash - cd ./src/sick_scan_xd - rm ./CHANGELOG.rst - catkin_generate_changelog --all # create CHANGELOG.rst - ``` - * Commit and pull all changes incl. CHANGELOG.rst and package.xml: - ``` - git add CHANGELOG.rst package.xml - git commit -m "Update CHANGELOG.rst and package version" - git push - ``` - * Run `catkin_prepare_release` and `bloom-release` in folder `src/sick_scan_xd`: - ``` - source /opt/ros/noetic/setup.bash - catkin_prepare_release -y - bloom-release --rosdistro noetic --track noetic sick_scan_xd # at first time: call with option --edit for configuration - ``` - * For the initial release (first time): Run `bloom-release` in folder `src/sick_scan_xd` with option `--edit`: - ``` - source /opt/ros/noetic/setup.bash - catkin_prepare_release -y - bloom-release --rosdistro noetic --track noetic sick_scan_xd --edit - Release repository url: https://github.com/SICKAG/sick_scan_xd-release.git - upstream: - Upstream Repository URI: https://github.com/SICKAG/sick_scan_xd.git - Upstream VCS Type: - Version: - Release Tag: - Upstream Devel Branch: feature/bloom_pretest - ROS Distro: noetic - Patches Directory: - Release Repository Push URL: - ``` - * Check status: https://index.ros.org/p/sick_scan_xd/#noetic - * Install binary release: `sudo apt update ; sudo apt-get install ros-noetic-sick-scan-xd`. Note from https://wiki.ros.org/bloom/Tutorials/FirstTimeRelease: Packages built are periodically synchronized over to the shadow-fixed and public repositories, so it might take as long as a month before your package is available on the public ROS debian repositories (i.e. available via apt-get). - -## Release build for ROS-2 - -For ROS-2 follow the instructions on https://docs.ros.org/en/humble/How-To-Guides/Releasing/Releasing-a-Package.html: -* Checkout the sick_scan_xd version to be released and run: - ``` - git clone -b master https://github.com/SICKAG/sick_scan_xd.git - cd ./sick_scan_xd - rm ./CHANGELOG.rst - catkin_generate_changelog --all # create CHANGELOG.rst - ``` -* Commit CHANGELOG.rst and optional modifications: - ``` - git add CHANGELOG.rst - git commit -m "Update CHANGELOG.rst" - git push - ``` -* Run `catkin_prepare_release` and `bloom-release`: - ``` - bloom-release --rosdistro humble --track humble sick_scan_xd # at first time: call with option --new-track - ``` - For the initial release (i.e. at the first time): Run bloom-relase configuration with option --new-track: - `bloom-release --new-track --rosdistro humble --track humble sick_scan_xd` - * Release repository url: https://github.com/ros2-gbp/sick_scan_xd-release.git - * Upstream: - * Upstream Repository URI: https://github.com/SICKAG/sick_scan_xd.git - * Upstream Devel Branch: develop - * ROS Distro: humble - After the initial release has been approved: Run - ``` - sudo rosdep init - rosdep update - ``` - -## Check status - -* ROS-1 release repository: https://github.com/SICKAG/sick_scan_xd-release -* ROS-2 release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git -* ROS-1 jenkins build status: https://build.ros.org/job/Ndev__sick_scan_xd__ubuntu_focal_amd64/lastBuild/ -* ROS-2 jenkins build status: https://build.ros2.org/job/Hdev__sick_scan_xd__ubuntu_jammy_amd64/lastBuild/ -* ROS-1 jenkins: https://build.ros.org/search/?q=sick_scan_xd -* ROS-2 jenkins: https://build.ros2.org/search/?q=sick_scan_xd -* Documentation: https://index.ros.org/p/sick_scan_xd/#noetic , http://wiki.ros.org/sick_scan_xd - -## Troubleshooting, FAQ: - -* Bloom builds an old sick_scan_xd version (ROS1): - * Check `devel_branch` in https://github.com/SICKAG/sick_scan_xd-release/blob/master/tracks.yaml. If devel_branch is an old branch, replace it with e.g. `develop` or `master`, or update the `` to a new version. - -* Bloom builds an old sick_scan_xd version (ROS2): - * Check `devel_branch` in https://github.com/ros2-gbp/sick_scan_xd-release/blob/master/tracks.yaml. If devel_branch is an old branch, replace it with e.g. `develop` or `master`, or update the `` to a new version. - -* Bloom builds a new sick_scan_xd version, but apt still installs an old version: - * Check the sick_scan_xd version in the release repositories https://github.com/SICKAG/sick_scan_xd-release.git (ROS1) and https://github.com/ros2-gbp/sick_scan_xd-release.git (ROS2) - * Install bloom (if not yet done) using `sudo apt-get install python-bloom` on Linux or `pip install -U bloom` on Windows - * Run - ``` - bloom-release --rosdistro noetic -d sick_scan_xd # release repository: https://github.com/SICKAG/sick_scan_xd-release.git, argument -d enables debug infos - bloom-release --rosdistro humble -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos - bloom-release --rosdistro iron -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos - bloom-release --rosdistro jazzy -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos - ``` - * In case of github 2FA errors: Follow http://wiki.ros.org/bloom/Tutorials/GithubManualAuthorization to create a 2FA token and configure the token in file `~/.config/bloom`. - - -## Useful links and tutorials - -* http://wiki.ros.org/bloom -* https://wiki.ros.org/bloom/Tutorials/FirstTimeRelease -* https://docs.ros.org/en/humble/How-To-Guides/Releasing/Releasing-a-Package.html diff --git a/doc/combi_recording.md b/doc/combi_recording.md deleted file mode 100644 index 98d83b3c..00000000 --- a/doc/combi_recording.md +++ /dev/null @@ -1,24 +0,0 @@ -# Checking driver in combination of MRS6xxx and RMSxxxx - -1. Setup environment - * 230 V Voltage converter - * Power supply notebook - * Power bank - * Tripod - * Measurement Unit - * Switch - * Permit for measurement in public area -2. roslaunch sick_scan_xd test_0002_combi_live.launch -3. Check setup using rviz -4. Close all applications, which are not necessary (like IDE, browser, git client) -5. Setup Tracking algorithm - -``` -top -``` -6. Record data -``` -rosrun rosbag record record -o combi -a -``` - - diff --git a/doc/combination_rms_1xxx_lms_1xx/readme.docx b/doc/combination_rms_1xxx_lms_1xx/readme.docx deleted file mode 100644 index 77a62889..00000000 Binary files a/doc/combination_rms_1xxx_lms_1xx/readme.docx and /dev/null differ 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 deleted file mode 100644 index 7b126beb..00000000 --- a/doc/combination_rms_1xxx_lms_1xx/tutorial_rms1_lms1_combination.md +++ /dev/null @@ -1,31 +0,0 @@ -# RMS_1xxx and LMS_1xxx combination - -This tutorial shows how to combine a RMS_1xxx radar with a LMS_1xxx lidar. - -To demonstrate the lidar/radar combination, a RMS_1xxx and a LMS_1xxx device were put into operation. The sick_scan_xd driver and rviz were started on ROS-1 Linux. Bagfiles have been recorded to demonstrate the required transform (rms_1xxx_lms_1xx_movement_off.bag and rms_1xxx_lms_1xx_movement_on.bag). - -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_xxxx.launch: - ``` - roslaunch sick_scan_xd sick_lms_1xxx.launch - roslaunch sick_scan_xd sick_rms_xxxx.launch - ``` - Make sure, that different ros node names and different IP-addresses are used. - - The following rviz screenshot shows both pointclouds: - ![rms_1xxx_lms_1xx_combi_screenshot01.png](rms_1xxx_lms_1xx_combi_screenshot01.png) - - Note that each sensor has its own frame id and coordinate system. The RMS_1xxx uses the frame id "radar", the LMS_1xxx uses the frame id "cloud". To combine both sensor, we have to transform the radar frame and coordinates to the lidar frame and coordinates. - Radar targets have multiple echos due to reflection. - -2. Start a ros static_transform_publisher to convert radar frames (frame id `/radar`) to lidar frames (frame id `/cloud`): - ``` - rosrun tf static_transform_publisher 0 0 0 0 0 0 /cloud /radar 100 - ``` - Using this transform, rviz displays both the radar and lidar pointcloud: - ![rms_1xxx_lms_1xx_combi.png](rms_1xxx_lms_1xx_combi.png) - -Note: If you use this example with a playback of bagfiles (e.g. `rosbag play --loop ./rms_1xxx_lms_1xx_movement_off.bag`), you might encounter errors due to different timestamps (the recorded timestamps in the bagfiles are different from the timestamps by the static_transform_publisher). - -Alternatively, the radar frame id and an optional transform can be configured in the radar launchfile (parameter "frame_id" and "add_transform_xyz_rpy"). diff --git a/doc/cone_section.png b/doc/cone_section.png index 5a1d3902..2845d917 100644 Binary files a/doc/cone_section.png and b/doc/cone_section.png differ diff --git a/doc/coordinate_transforms.md b/doc/coordinate_transforms.md deleted file mode 100644 index 481b9c21..00000000 --- a/doc/coordinate_transforms.md +++ /dev/null @@ -1,48 +0,0 @@ -# Coordinate transforms - -Different lidars use different coordinate systems. sick_scan_xd transforms all points of the published pointclouds to the ROS coordinate system, independant of the lidar. The following figure shows the commonly used coordinate systems: - -![3d_coordinate_system_comp.png](3d_coordinate_system_comp.png) - -An additional coordinate transform can be applied to the pointcloud. This optional transform can be used to transform the pointclouds into a user defined coordinate system. If the lidar is e.g. mounted on a vehicle, the pointclouds can be transformed into a vehicle coordinates. - -An additional coordinate transform can be configured by a 6D pose (x, y, z, roll, pitch, yaw) with a translational part (x, y, z) in [m] and a rotational part (roll, pitch, yaw) in [rad]. - -If configured, it will transform the pointcloud from its "cloud" coordinates into user defined "world" coordinate system: - -`T[world,cloud] with P_world = T[world,cloud] * P_cloud` (parent: world, child: cloud) - -The final rotation is defined by: Rotation = Rot[yaw] * Rot[pitch] * Rot[roll] with roll = rotation about x-axis, pitch = rotation about y-axis and yaw = rotation about z-axis. - -An additional transform can be configured in the launchfile, e.g. - -``` - - - - - -``` - -Default value is `"0,0,0,0,0,0"`, i.e. no additional transform will be applied. - -The additional transform applies to cartesian lidar pointclouds and visualization marker (fields). -It is **NOT** applied to polar pointclouds, radarscans, ldmrs objects or other messages. - -Note that sick_scan_xd configures an additional transform using (x, y, z, roll, pitch, yaw). In contrast, the ROS static_transform_publisher uses commandline arguments in order x, y, z, yaw, pitch, roll. - -Example using ROS static_transform_publisher with x=0, y=0, z=0, roll=15, pitch=-10, yaw=5 [deg]: - -``` -source /opt/ros/noetic/setup.bash -# static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms -# tf_echo -# rot_x = 5 deg: 0.0872665, rot_y = -10 deg: -0.1745329, rot_z = 15 deg: 0.2617994 -rosrun tf static_transform_publisher 0 0 0 0.2617994 -0.1745329 0.0872665 parent_frame child_frame 100 -rosrun tf tf_echo parent_frame child_frame -``` - -File [trafo_example.py](sick_scan_api/trafo_example.py) demonstrates how a transform can be computed. - -For upside down mounted devices, the pointcloud can be 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"`. [sick_lrs_36x0_upside_down.launch](../launch/sick_lrs_36x0_upside_down.launch) and [sick_lrs_36x1_upside_down.launch](../launch/sick_lrs_36x1_upside_down.launch) show examples for compensating the pointcloud of an upside down mounted device by a 180 deg rotation about the x axis. - diff --git a/doc/emulator.md b/doc/emulator.md deleted file mode 100644 index fbc92622..00000000 --- a/doc/emulator.md +++ /dev/null @@ -1,110 +0,0 @@ -# Unit tests - -For a quick unit test after installation without the sensor hardware, a test server is provided to simulate a scanner. It implements a simple tcp server, which responds to binary cola messages and sends predefined LMDscandata to a tcp-client. The sick_scan_xd driver can connect to the local test server instead of the lidar device for offline-tests. Please note, that this test server does not emulate a Lidar sensor. It just sends some simple scan data and response messages to a tcp client. It can be used for a quick unit test after build and install. - -## Build - -To build the test server, activate cmake option `ENABLE_EMULATOR` in CMakeLists.txt and rebuild sick_scan_xd. By default, option `ENABLE_EMULATOR` is switched off. - -## Run - -For a unit test of LMS1xx, run the following commands in different terminals: - -``` -cd sick_scan_xd -source ./install/setup.bash - -# Start sick_scan_xd emulator -roslaunch sick_scan_xd emulator_lms1xx.launch & -sleep 1 - -# Start rviz -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_lms1xx.rviz & -sleep 1 - -# Start sick_scan_xd driver -roslaunch sick_scan_xd sick_lms_1xx.launch hostname:=127.0.0.1 -``` - -For a unit test of LMS5xx, run the following commands in different terminals: - -``` -cd sick_scan_xd -source ./install/setup.bash - -# Start sick_scan_xd emulator -roslaunch sick_scan_xd emulator_lms5xx.launch & -sleep 1 - -# Start rviz -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_lms5xx.rviz & -sleep 1 - -# Start sick_scan_xd driver -roslaunch sick_scan_xd sick_lms_5xx.launch hostname:=127.0.0.1 -``` - -For a unit test of LMS7xx, run the following commands in different terminals: - -``` -cd sick_scan_xd -source ./install/setup.bash - -# Start sick_scan_xd emulator -roslaunch sick_scan_xd emulator_01_default.launch & -sleep 1 - -# Start rviz -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg.rviz & -sleep 1 - -# Start sick_scan_xd driver -roslaunch sick_scan_xd sick_tim_7xx.launch hostname:=127.0.0.1 -``` - -For a unit test of LMS7xxS, run the following commands in different terminals: - -``` -cd sick_scan_xd -source ./install/setup.bash - -# Start sick_scan_xd emulator -roslaunch sick_scan_xd emulator_01_default.launch & -sleep 1 - -# Start rviz -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg.rviz & -sleep 1 - -# Start sick_scan_xd driver -roslaunch sick_scan_xd sick_tim_7xxS.launch hostname:=127.0.0.1 -``` - -Alternatively, you can just run the test scripts provided in folder `sick_scan_xd/test/scripts`: - -``` -cd sick_scan_xd/test/scripts -./makeall.bash -./run_simu_lms1xx.bash -./run_simu_lms5xx.bash -./run_simu_tim7xx_tim7xxS.bash -``` - -Make sure to finish all sick_scan_xd nodes after a test. All nodes can be killed by -``` -rosnode kill -a ; sleep 1 -killall sick_generic_caller ; sleep 1 -killall sick_scan_emulator ; sleep 1 -``` - -## Examples - -rviz example screenshots using sick_scan_xd with LMS1xx and LMS5xx test server: - -![emulator_lms1xx_screenshot.png](emulator_lms1xx_screenshot.png) - -rviz example screenshots using sick_scan_xd with LMS7xx and LMS7xxS test server: - -![emulator_lms1xx_screenshot.png](emulator_lms7xx_screenshot.png) - -Further examples are provided in folder `test/scripts`. diff --git a/doc/encoder.md b/doc/encoder.md deleted file mode 100644 index 2450656a..00000000 --- a/doc/encoder.md +++ /dev/null @@ -1,67 +0,0 @@ -# Encoder in Laserscanners -See also [LMS4xxx Manual](https://cdn.sick.com/media/docs/0/90/790/Operating_instructions_LMS4000_2D_LiDAR_sensors_en_IM0079790.PDF) - -3.5.3.3 - -Connecting encodersIf the device is mounted for mobile use or if the objects to be measured are in motion,the application will usually also need position data to further process the measured valâ€ues.Encoders can be connected for this purpose. The encoder data is then available withthe other measured values in a single scan and at the same interface. A volume,for example, can be calculated by evaluating the measurement data. The input freâ€quency of the encoder signal must not exceed 50 kHz.The following encoders with push-pull output stage can be used: -1. Single-channel, only connected at encoder A, no direction detection. -2. Dual-channel (phase), connected at encoder A and encoder B; the pulses have aphase shift of 90°, making direction detection possible. By definition, during forâ€ward motion (CW = clockwise) phase A precedes phase B; conversely, duringreverse motion (CCW = counterclockwise) edge A rises before edge B. -3. Dual-channel (level), connected at encoder A and encoder B; the pulses are atencoder A; at encoder B, the direction is indicated by level 0 or level 1 (rarely). - - -## connecting encoders - -![LMS 4xxx encoder connection](./lms4xxx_encoder_connection.png "LMS 4xxx encoder connection") - - - -### Example Circuit to trigger encoder counts -Whenever the switch is closed a potential of 24 V is applied to the encoder input A in mode (01 single-channel) this leads to an increase of the count by 1. - -![encoder trigger](./circuit.png "encoder trigger") - -### Activation of Encoder Informations -If the parameter ```encoder_mode``` is set to 1-4 in the launch file, the encoder is activated in the laser scanner in the corresponding mode (see list above). - -The following encoder modes can be configured in the launchfile or by commandline parameter: -* `encoder_mode:=-1`: Default value, i.e. encoder configuration not set -* `encoder_mode:=0`: Encoder off (supported by LMS1xx, LMS5xx, LMS4000, LRS4000) -* `encoder_mode:=1`: Single increment (supported by LMS1xx, LMS5xx, LMS4000, LRS4000) -* `encoder_mode:=2`: Direction recognition phase (supported by LMS1xx, LMS5xx, LMS4000, LRS4000) -* `encoder_mode:=3`: Direction recognition level (supported by LMS1xx, LMS5xx, LMS4000, LRS4000) -* `encoder_mode:=4`: Fixed increment speed/ticks (supported by LMS4000 only) - -Encoder messages are published on topic `/encoder` synchronously to the PointCloud messages. They contain a timestamp and the encoder value, e.g.: -```console -foo@bar:~$rostopic echo /encoder -header: - seq: 20700 - stamp: - secs: 1570722972 - nsecs: 28866142 - frame_id: "Encoder" -enc_count: 836 ---- -header: - seq: 20701 - stamp: - secs: 1570722972 - nsecs: 30598181 - frame_id: "Encoder" -enc_count: 836 ---- -header: - seq: 20702 - stamp: - secs: 1570722972 - nsecs: 32138020 - frame_id: "Encoder" -enc_count: 836 -``` - - - - -![Encoderdata in Sopas datagramm](./Encoder_data.png "Encoderdata in Sopas datagramm") - -![Set encoder config](./set_encoder_settings.png "Set encoder config") diff --git a/doc/field_monitoring_extensions.md b/doc/field_monitoring_extensions.md deleted file mode 100644 index 291dd200..00000000 --- a/doc/field_monitoring_extensions.md +++ /dev/null @@ -1,171 +0,0 @@ -# Field monitoring for LMS1xx, LMS5xx, TiM7xx and TiM7xxS - -The LMS1xx, LMS5xx, TiM7xx and TiM7xxS families have the following extended settings for field monitoring: - -## Field monitoring messages - -LMS1xx, LMS5xx, TiM7xx and TiM7xxS scanner support field monitoring. Fields can be configured by Sopas ET. Once they are configured, sick_scan_xd publishes ros messages containing the monitoring information from the lidar. - -By default, field monitoring is enabled in the launch files [sick_lms_1xx.launch](../launch/sick_lms_1xx.launch), [sick_lms_5xx.launch](../launch/sick_lms_5xx.launch), -[sick_tim_7xx.launch](../launch/sick_tim_7xx.launch) resp. [sick_tim_7xxS.launch](../launch/sick_tim_7xxS.launch) by following settings: -``` - - - -``` - -The driver queries the field configuration from the lidar and activates field monitoring by sending cola commands `"sEN LFErec 1"` and `"sEN LIDoutputstate 1"` at startup. Field monitoring is deactivated when driver exits. During runtime, it's possible to query, activate or deactivate monitoring using ros service ColaMsg with the following command (see section [Cola commands](#cola-commands)): -``` -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LFErec 1'}" # activate LFErec messages -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LFErec 0'}" # deactivate LFErec messages -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LFErec'}" # query activation status of LFErec messages -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDoutputstate 1'}" # activate LIDoutputstate messages -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDoutputstate 0'}" # deactivate LIDoutputstate messages -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LIDoutputstate'}" # query activation status of LIDoutputstate messages -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDinputstate 1'}" # activate LIDinputstate messages -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sEN LIDinputstate 0'}" # deactivate LIDinputstate messages -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LIDinputstate'}" # query activation status of LIDinputstate messages -``` - -LFErec and LIDoutputstate messages are defined in [LFErecMsg.msg](../msg/LFErecMsg.msg) and [LFErecFieldMsg.msg](../msg/LFErecFieldMsg.msg) resp. [LIDoutputstateMsg.msg](../msg/LIDoutputstateMsg.msg) and published on the following topics: `"/sick_tim_7xxS/lferec"` resp. `"/sick_tim_7xxS/lidoutputstate"`. - -| ** Lidar ** | ** lferec topic ** | ** lidoutputstate topic ** | -|---------------|--------------------|-----------------------------| -| lms_1xx | /sick_lms_1xx/lferec | /sick_lms_1xx/lidoutputstate | -| lms_5xx | /sick_lms_5xx/lferec | /sick_lms_5xx/lidoutputstate | -| lms_7xx | /sick_tim_7xx/lferec | /sick_tim_7xx/lidoutputstate | -| lms_7xxS | /sick_tim_7xxS/lferec | /sick_tim_7xxS/lidoutputstate | - -To view the field monitoring messages, run -``` -rostopic echo "/sick_lms_1xx/lferec" -rostopic echo "/sick_lms_1xx/lidoutputstate" -rostopic echo "/sick_lms_5xx/lferec" -rostopic echo "/sick_lms_5xx/lidoutputstate" -rostopic echo "/sick_tim_7xx/lferec" -rostopic echo "/sick_tim_7xx/lidoutputstate" -rostopic echo "/sick_tim_7xxS/lferec" -rostopic echo "/sick_tim_7xxS/lidoutputstate" -``` -or use rviz to visualize monitored fields and their status (see section [Visualization with rviz](#visualization-with-rviz)) - -The most important values of the field monitoring messages are - -- `field_index` (uint8) and `field_result_mrs` (uint8) for each field of a LFErec message with result status
    -
  • 0: invalid / incorrect,
    • -
  • 1: free / clear, or
    • -
  • 2: infringed.
    • -
- -- `output_state` (uint8) for each LIDoutputstate message with status 0 (not active), 1 (active) or 2 (not used). - -Note: Field monitoring currently supports binary cola messages only, which is the default. If cola ascii is activated, please switch back to cola binary for field monitoring. - -## Visualization with rviz - -The point cloud, the monitored fields and their status can be visualized using rviz. Use the [rviz configuration file](../test/emulator/config/rviz_emulator_cfg.rviz) -and run -``` -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg.rviz -``` - -Otherwise you can just add visualizations of type `/cloud/PointCloud2` and `/sick_tim_7xxS/marker` (resp. `/sick_tim_1xx/marker` for lms_1xx, `/sick_tim_5xx/marker` for lms_5xx and `/sick_tim_7xx/marker` for tim_7xx): - -![tim7xxs_screenshot01.png](tim7xxs_screenshot01.png) - -The following screenshot shows a TiM781S example with 2 fields (the 3. field is not configured), the first field with status "Clear", the second with status "Infringed": - -![tim7xxs_screenshot02.png](tim7xxs_screenshot02.png) - -The following screenshot shows a LMS511 example with a segmented field, two rectangular fields and a dynamic fields: - -![lms511_screenshot01.png](lms511_screenshot01.png) - -Note: Some combinations of rviz, OpenGL 3, VMware and graphic card drivers may cause visualization issues. In case of missing markers, try rviz with Open GL 2 using the command -``` -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg.rviz --opengl 210 -``` - -## Cola commands - -Cola commands can be sent for diagnosis and development using the ros service ColaMsg. This service is implemented in sick_scan_xd and started by -``` - -``` -in the launch file [sick_tim_7xxS.launch](../launch/sick_tim_7xxS.launch) (resp. [sick_lms_1xx.launch](../launch/sick_lms_1xx.launch), [sick_lms_5xx.launch](../launch/sick_lms_5xx.launch), [sick_tim_7xx.launch](../launch/sick_tim_7xx.launch) ). The ros service sends the given cola command to the lidar and returns its response. - -Example for cola command `"sRN SCdevicestate"` and response `"sRA SCdevicestate \\x00"` with error status 0 (no error): -``` -rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN SCdevicestate'}" -response: "sRA SCdevicestate \\x00" -``` - -## Tools, emulation and unittests - -Package sick_scan_xd implements some tools to support unittests, development and emulation of Tim781S devices: - -- sick_scan_emulator to emulate lidar devices and enable unittests (currently for Tim781S only) - -- pcap_json_converter to convert pcapng-files to json. - -### LMS and TiM emulation - -sick_scan_emulator implements a simple test server for cola commands. It rececives Cola-commands, returns Tim781S-like responses and sends Scandata from a json-file. Run -``` -roslaunch sick_scan_xd emulator_01_default.launch -``` -to emulate a local Tim781S device. Then start and connect the sick_scan_xd driver by -``` -roslaunch sick_scan_xd sick_tim_7xxS.launch hostname:=127.0.0.1 -``` - -Note that sick_scan_emulator just implements a simple server for offline tests. It does not emulate a lidar device completely and should only be used for development and testing. - -Scandata messages are parsed from json-file(s). These json-files are configured in the launch file [emulator.launch](../test/emulator/launch/emulator_01_default.launch) and converted form wireshark-records (pcapng-files) using pcap_json_converter.py (see section Pcapng converter tool](#pcapng-converter-tool)). - -A LMS111 device can be emulated by running -``` -roslaunch sick_scan_xd emulator_lms1xx.launch & -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_lms1xx.rviz & -roslaunch sick_scan_xd sick_lms_1xx.launch hostname:=127.0.0.1 -``` - -A LMS511 device can be emulated by running -``` -roslaunch sick_scan_xd emulator_lms5xx.launch & -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_lms5xx.rviz & -roslaunch sick_scan_xd sick_lms_5xx.launch hostname:=127.0.0.1 -``` - -### Unittests - -Folder `test/emulator/scandata` contains scandata examples for unittests. To run an offline unittest for LMS111, LMS511, TiM781, TiM781S enter the following commands: -``` -cd test/scripts -./run_simu_lms1xx.bash -./run_simu_lms5xx.bash -./run_simu_tim7xx_tim7xxS.bash -``` -or start emulator, driver and rviz by running -``` -source ./install/setup.bash -# Start sick_scan_xd emulator -roslaunch sick_scan_xd emulator_01_default.launch & -sleep 1 -# Start rviz -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg.rviz --opengl 210 & -sleep 1 -# Start sick_scan_xd driver for TiM871S -roslaunch sick_scan_xd sick_tim_7xxS.launch hostname:=127.0.0.1 -``` - -### Pcapng converter tool - -The pcapng converter tool [pcap_json_converter.py](../test/pcap_json_converter/pcap_json_converter.py) converts pcapng-files to json-files. Run the following steps to create a json-file with scandata for the emulator: - -1. Start wireshark and filter the tcp traffic on port 2112 with the filter expression `tcp and tcp.port==2112`. -2. Start TiM781S and run the sick_scan_xd driver. -3. Capture the network traffic for some time. -4. Stop capturing and save the network traffic in a pcapng-file. -5. Convert the pcapng-file to json by `python pcap_json_converter.py --pcap_filename=.pcapng`. Result is a jsonfile `.pcapng.json` -6. Set the resulting json-file in the emulator configuration [emulator.launch](../test/emulator/launch/emulator_01_default.launch) by `` diff --git a/doc/git_lfs.md b/doc/git_lfs.md deleted file mode 100644 index 9ff840c2..00000000 --- a/doc/git_lfs.md +++ /dev/null @@ -1,39 +0,0 @@ -# Git Large File Storage - -Large scanner datafiles (i.e. converted pcapng-files) for test and development of sick_scan_xd have been added using the "Git Large File Storage" (git lfs). - -1. Run the following steps to install git lfs: - - Download and install git lfs from https://git-lfs.github.com/ - - Integrate to git by running `git lfs install` - -2. Add folder "scandata" with large files: - ``` - git add ./test/emulator/scandata/readme.txt - git commit -m "added scandata folder" - git push - ``` - -3. Add large files to folder "scandata" using git lfs: - ``` - git lfs track "./test/emulator/scandata/*.*" - git add .gitattributes - ``` - Note: If your current working directory is on a Windows-drive generated by subst, change to the original folder first. - -4. Add, commit and push the new files normally: - ``` - git add ./test/emulator/scandata/*.* - git commit -m "added scandata files using git lfs" - git push - ``` - -5. Test by cloning sick_scan_xd into a new folder: - ``` - cd /tmp - git clone https://github.com/michael1309/sick_scan_xd - ``` - Note: Use `git clone` to make a local copy of the git lfs files. Downloading the zip file https://github.com/SICKAG/sick_scan_xd/archive/refs/heads/main.zip just creates link-files. - -Helpful links: -- https://git-lfs.github.com/ -- https://docs.github.com/en/github/managing-large-files/versioning-large-files/configuring-git-large-file-storage diff --git a/doc/google_cartographer.md b/doc/google_cartographer.md deleted file mode 100644 index 33b99bb2..00000000 --- a/doc/google_cartographer.md +++ /dev/null @@ -1,42 +0,0 @@ -# Google Cartographer -## Table of contents - -- [Supported Hardware](#supported-hardware) -- [Setup](#setup) - -## Supported Hardware - -## Setup - -Setup Google Cartographer (these steps are for illustration only, you must adapt these lines to your local directory names) - -1. Login to Ubuntu. -2. Open multiple terminals. -3. Terminal 1: - . ros1_start.sh - roscore -4. Terminal 2: - . ros1_start.sh - cd ~/ros_catkin_ws - source ./devel/setup.bash -5. Terminal 3: - roslaunch sick_scan_xd sick_mrs_1xxx_cartographer.launch cloud_topic:=horizontal_laser_3d frame_id:=horizontal_vlp16_link -6. Terminal 4: - roslaunch sick_scan_xd sick_tim_5xx.launch cloud_topic:=vertical_laser_3d frame_id:=vertical_vlp16_link hostname:=192.168.0.71 -7. Terminal 5: - - * . ros1_start.sh - * cd ~/ros_cartographer_ws - * source ./install_isolated/setup.bash - * catkin_make_isolated - - * roslaunch cartographer_ros live_demo_backpack_3d.launch - - -## Example output - -The following figure shows an example of an outdoor slam result using a MRS1104-scanner: - -![slam_example](slam_example.png) - - diff --git a/doc/interlacing.md b/doc/interlacing.md deleted file mode 100644 index d80db5d3..00000000 --- a/doc/interlacing.md +++ /dev/null @@ -1,105 +0,0 @@ -# Interlacing - -Most lidars create non interlaced scan data by default, i.e. a scan data telegram contains all scan points measured during a full 360 degree circulation. For MRS-1xxx and LMS-1xxx lidars, parameter `ang_res` can be configured to increase the angular resolution. In this case, the scan data and point clouds are measured and published interlaced. - -The following table shows valid combinations of the angular resolution (parameter `ang_res`) and scan frequency (parameter `scan_freq`) for MRS-1xxx and LMS-1xxx lidars: - -| lidar | ang_res [deg] | scan_freq [Hz] | interlacing | -| ----- | ----- | ----- | ----- | -| MRS-1xxx | 0.25 | 50 | non-interlaced | -| MRS-1xxx | 0.125 | 25 | 2 x interlaced | -| MRS-1xxx | 0.0625 | 12.5 | 4 x interlaced | -| LMS-1xxx | 0.75 | 150 | non-interlaced | -| LMS-1xxx | 0.375 | 75 | 2 x interlaced | -| LMS-1xxx | 0.1875 | 37.5 | 4 x interlaced | - -## MRS-1xxx angular resolution - -By default, MRS-1xxx lidars create non-interlaced scans with an angular resolution 0.25 deg. -Using higher resolutions, the MRS-1xxx sends scan data interlaced. - -MRS-1xxx lidars measure 4 consecutive scans with identical starting angles for each of its 4 layers, where each layer has a different elevation angle. This results in (12.5 Hz) * (4 layers) * (360 deg / 0.25 deg) * (275 deg / 360 deg) = 55000 shots per second. - -With default configuration ang_res=0.25, the angular resolution of each scan is 0.25 [deg] and scans and point clouds are non-interlaced. The sequence of scan data telegrams is repeating with 4 consecutive telegrams (resp. 4 point cloud messages): -* layer[0] starting at -137.5 deg, angle inc = 0.25 deg -* layer[1] starting at -137.5 deg, angle inc = 0.25 deg -* layer[2] starting at -137.5 deg, angle inc = 0.25 deg -* layer[3] starting at -137.5 deg, angle inc = 0.25 deg - -With configuration ang_res=0.125, the angular resolution of each scan is 0.125 [deg] and scans and point clouds are 2 x interlaced. The sequence of scan data telegrams is repeating with 8 consecutive telegrams (resp. 8 point cloud messages): -* layer[0] starting at -137.500 deg, angle inc = 0.25 deg -* layer[1] starting at -137.500 deg, angle inc = 0.25 deg -* layer[2] starting at -137.500 deg, angle inc = 0.25 deg -* layer[3] starting at -137.500 deg, angle inc = 0.25 deg -* layer[0] starting at -137.625 deg, angle inc = 0.25 deg -* layer[1] starting at -137.625 deg, angle inc = 0.25 deg -* layer[2] starting at -137.625 deg, angle inc = 0.25 deg -* layer[3] starting at -137.625 deg, angle inc = 0.25 deg - -With configuration ang_res=0.0625, the angular resolution of each scan is 0.0625 [deg] and scans and point clouds are 4 x interlaced. The sequence of scan data telegrams is repeating with 16 consecutive telegrams (resp. 16 point cloud messages): -* layer[0] starting at -137.4375 deg, angle inc = 0.25 deg -* layer[1] starting at -137.4375 deg, angle inc = 0.25 deg -* layer[2] starting at -137.4375 deg, angle inc = 0.25 deg -* layer[3] starting at -137.4375 deg, angle inc = 0.25 deg -* layer[0] starting at -137.5000 deg, angle inc = 0.25 deg -* layer[1] starting at -137.5000 deg, angle inc = 0.25 deg -* layer[2] starting at -137.5000 deg, angle inc = 0.25 deg -* layer[3] starting at -137.5000 deg, angle inc = 0.25 deg -* layer[0] starting at -137.5625 deg, angle inc = 0.25 deg -* layer[1] starting at -137.5625 deg, angle inc = 0.25 deg -* layer[2] starting at -137.5625 deg, angle inc = 0.25 deg -* layer[3] starting at -137.5625 deg, angle inc = 0.25 deg -* layer[0] starting at -137.6250 deg, angle inc = 0.25 deg -* layer[1] starting at -137.6250 deg, angle inc = 0.25 deg -* layer[2] starting at -137.6250 deg, angle inc = 0.25 deg -* layer[3] starting at -137.6250 deg, angle inc = 0.25 deg - -To use the full angular resolution of one 360 degree circulation, the point cloud must be accumulated by 8 resp. 16 messages in interlaced mode. - -See [Data buffering in MRS 1xxx](https://github.com/SICKAG/sick_scan_xd/blob/develop/doc/timing.md#data-buffering-in-mrs-1xxx) for timing and timestamp. - -## LMS-1xxx angular resolution - -By default, LMS-1xxx lidars create non-interlaced scans with an angular resolution 0.75 deg. -Using higher resolutions, the LMS-1xxx sends scan data interlaced. - -With configuration ang_res=0.75, the angular resolution of each scan is 0.75 [deg]. This means that each point cloud message also has a resolution of 0.75 [deg]. With configuration ang_res=0.375, the scan is generated interlaced: Each scan still has 0.75 [deg] resolution, but 2 x 4 = 8 consecutive scans are rotated by 0.375 [deg] against each other. I.e. 8 consecutive point cloud messages each have an angular resolution of 0.375 [deg] at half the frequency. Within a point cloud message the angular resolution is still 0.75 [deg]. - -With ang_res=0.375, scan data are two times interlaced. The sequence of scan data telegrams is repeating with 8 consecutive telegrams: -* 4 scans starting at -138.000 deg, angle inc = 0.75 deg, then -* 4 scans starting at -138.375 deg, angle inc = 0.75 deg - -Thus the start angles of the received scan data telegrams are -{ ..., -138.375, -138.375, -138.375, -138.375, -138.000, -138.000, -138.000, -138.000, ... } - -With ang_res=0.1875 the scan is generated quadruple interlaced, i.e. 4 x 4 = 16 consecutive scans are each rotated by 0.1875 [deg]. Each scan is resolved with 0.75 [deg]; 4 x 4 = 16 scans resp. 16 pointclouds together (accumulated) result in a resolution of 0.1875 [deg] at a quarter of the frequency. - -With ang_res=0.1875, scan data are four times interlaced. The sequence of scan data telegrams is repeating with 16 consecutive telegrams: -* 4 scans starting at -137.8125 deg, angle inc = 0.75 deg, then -* 4 scans starting at -138.0000 deg, angle inc = 0.75 deg, then -* 4 scans starting at -138.1875 deg, angle inc = 0.75 deg, then -* 4 scans starting at -138.3750 deg, angle inc = 0.75 deg, then - -Thus the start angles of the received scan data telegrams are -{ ..., -137.8125, -137.8125, -137.8125, -137.8125, -138.0000, -138.0000, -138.0000, -138.0000, -138.1875, -138.1875, -138.1875, -138.1875, -138.3750, -138.3750, -138.3750, -138.3750, ... } - -You can see this in rviz by increasing the decay time to e.g. 4/75=0.054 or higher. The screenshot shows an example with the default setting ang_res=0.75: - -![LMS1xxx_0.7500_deg.png](screenshots/LMS1xxx_0.7500_deg.png) - -The angular resolution is (just roughly measured) about atan(0.11/0.9) / 9 points = 0.77 [deg]. With ang_res=0.375 and decay=0.1 rviz shows twice the resolution: - -![LMS1xxx_0.7500_deg.png](screenshots/LMS1xxx_0.3750_deg.png) - -Correspondingly, rviz shows four times the resolution with ang_res=0.1875 and decay=0.1: - -![LMS1xxx_0.7500_deg.png](screenshots/LMS1xxx_0.1875_deg.png) - -To use the full angular resolution of one 360 degree circulation, the pointcloud must be accumulated by 8 resp. 16 messages. - -The active configuration can be seen in the log output during scanner initialization, e.g.: -``` -[ INFO] [1669294673.078608968]: sRA LMPscancfg: scan frequency = 75 Hz, angular resolution = 0.375 deg. -``` - -Note: LMS creates 4 consecutive scans with identical starting angles for each of its 4 laser LEDs. The 4 laser LEDs are mounted in one plane. All together, this gives you (4 scans) * (37.5 Hz) * (1 layer) * (360 deg / 0.75 deg) * (275 deg / 360 deg) = 55000 shots per second. diff --git a/doc/ipconfig/ipconfig.md b/doc/ipconfig/ipconfig.md deleted file mode 100644 index ebc4f167..00000000 --- a/doc/ipconfig/ipconfig.md +++ /dev/null @@ -1,43 +0,0 @@ -# IP address configuration -## Detecting SICK devices in the network -The Python script -```sick_scan_xd/tools/sick_generic_device_finder/sick_generic_device_finder.py``` -sends a UDP broadcast to which all available scanners respond with a device description. -The varibale ```UDP_IP = "192.168.0.255"``` defines the broardcast address used by the script. -If you are using a different IP address configuration on your host pc you have to change this variable according to the brodcast address of your network card. -```ifconfig``` shows the broadcast address for every network adapter. -## Change IP address -The IP address of the device can be changed with a customized launch file. -The following launch sequence is an example: - -```roslaunch sick_scan_xd sick_new_ip.launch hostname:=192.168.0.1 new_IP:=192.168.0.100``` - -The launchfile restarts the lidar after the address change and stops the sick_scan_xd node. After a few seconds of booting time the scanner is reachable under the new IP address. The Python script is experimental. It is known that some ethernet adapter are not fully supported. As a fallback solution you can always use the SOPAS ET software under windows. - -## Starting with a new lidar -The lidar is delivered with a standard IP address, to read or change it the [SICK SOPAS ET](https://www.sick.com/de/de/sopas-engineering-tool-2018/p/p367244) for windows can be used. -When the tool is started, a search is performed which lists all scanners available in the network. -![SOPAS start](./scanner_found.PNG "SOPAS start") -Double-click to select the scanner for the project -![SOPAS select](./scanner_added.PNG "SOPAS select") -Double click on the lidar icon to open the configuration menu of the scanner -Select here the network configuration menu and set the parameters by clicking on the save icon (red arrow) -![SOPAS save network](./set_config.PNG "SOPAS save network") -To ensure that the settings are stored even after a power cycle, they must be stored in the eeprom. To do this, click on the eeprom icon and confirm the save. -![SOPAS save eeprom](./save_permanent.PNG "SOPAS save eeprom") -To test the settings under Windows use the commands ```ipconfig``` and ```ping``` in the ```cmd.exe```. Make sure that the lidar and host pc have different ip addresses e.g. 192.168.0.110 for the pc and 192.168.0.71 for the scanner. -![Windows ping](./ipconfig_windows.PNG "windows ip config") -## test connection under Linux -to test the settings under the Linux target system you can use netcat to check if a TCP connection to the scanner can be established -```nc -z -v -w5 $SCANNERIPADDRESS 2112``` -the connection can be successfully established - -![Linux netcat scanner](./nc_scanner.PNG "linux netcat scanner") - -unlike a ping, the connection attempt to the hostpc is not successful - -![Linux netcat host](./nc_win_host.PNG "linux netcat host") ------------------------------------------------------------------------- - -![SICK Logo](https://sick-syd.data.continum.net/static_2018013123/_ui/desktop/common/images/base/pics/logo.png "SICK Logo") -![Lehning Logo](http://www.lehning.de/style/banner.jpg "LEHNING Logo") diff --git a/doc/ipconfig/ipconfig_windows.PNG b/doc/ipconfig/ipconfig_windows.PNG deleted file mode 100644 index eb2721df..00000000 Binary files a/doc/ipconfig/ipconfig_windows.PNG and /dev/null differ diff --git a/doc/ipconfig/scanner_added.PNG b/doc/ipconfig/scanner_added.PNG index d0f0c2b9..49635304 100644 Binary files a/doc/ipconfig/scanner_added.PNG and b/doc/ipconfig/scanner_added.PNG differ diff --git a/doc/ipconfig/scanner_found.PNG b/doc/ipconfig/scanner_found.PNG index e523d01c..f04663b0 100644 Binary files a/doc/ipconfig/scanner_found.PNG and b/doc/ipconfig/scanner_found.PNG differ diff --git a/doc/lms4xxx_encoder_connection.png b/doc/lms4xxx_encoder_connection.png index bef4a34d..2de388e6 100644 Binary files a/doc/lms4xxx_encoder_connection.png and b/doc/lms4xxx_encoder_connection.png differ diff --git a/doc/lms511_screenshot01.png b/doc/lms511_screenshot01.png index 281fef5d..4e53bd97 100644 Binary files a/doc/lms511_screenshot01.png and b/doc/lms511_screenshot01.png differ diff --git a/doc/messageSequenceDiagram3.png b/doc/messageSequenceDiagram3.png new file mode 100644 index 00000000..1c2fd74b Binary files /dev/null and b/doc/messageSequenceDiagram3.png differ diff --git a/doc/messageSequenceDiagram3.txt b/doc/messageSequenceDiagram3.txt new file mode 100644 index 00000000..dc5ff826 --- /dev/null +++ b/doc/messageSequenceDiagram3.txt @@ -0,0 +1,50 @@ +@startuml + +"picoScan" -> "udp receiver thread": UDP packet +"picoScan" -> "udp receiver thread": UDP packet +"picoScan" -> "udp receiver thread": UDP packet + +"udp receiver thread" -> "input FIFO": push telegram +"input FIFO" -> "process thread": pop telegram + +"picoScan" -> "udp receiver thread": UDP packet +"picoScan" -> "udp receiver thread": UDP packet +"picoScan" -> "udp receiver thread": UDP packet + +"process thread" -> "export FIFO": push segment point cloud +"process thread" -> "SegmentCollector": push segment point cloud +"SegmentCollector" -> "SegmentCollector": Segments = [ 0 ] +"export FIFO" -> "ROS-/API-Export": segment 0 point cloud +"ROS-/API-Export" -> "Application": segment 0 point cloud + +"udp receiver thread" -> "input FIFO": push telegram +"input FIFO" -> "process thread": pop telegram + +"process thread" -> "export FIFO": push segment point cloud +"process thread" -> "SegmentCollector": push segment point cloud +"SegmentCollector" -> "SegmentCollector": Segments = [ 0, 1 ] +"export FIFO" -> "ROS-/API-Export": segment 1 point cloud +"ROS-/API-Export" -> "Application": segment 1 point cloud + +... + +"picoScan" -> "udp receiver thread": UDP packet +"picoScan" -> "udp receiver thread": UDP packet +"picoScan" -> "udp receiver thread": UDP packet + +"udp receiver thread" -> "input FIFO": push telegram +"input FIFO" -> "process thread": pop telegram + +"process thread" -> "export FIFO": push segment point cloud +"process thread" -> "SegmentCollector": push segment point cloud +"SegmentCollector" -> "SegmentCollector": Segments = [ 0, 1, ... 8 ] +"SegmentCollector" -> "export FIFO": full frame point cloud +"SegmentCollector" -> "SegmentCollector": Segments = [] +"export FIFO" -> "ROS-/API-Export": segment 8 point cloud +"ROS-/API-Export" -> "Application": segment 8 point cloud +"export FIFO" -> "ROS-/API-Export": full frame point cloud +"ROS-/API-Export" -> "Application": full frame point cloud + +... + +@enduml \ No newline at end of file diff --git a/doc/mrs6xxx_timing.md b/doc/mrs6xxx_timing.md deleted file mode 100644 index 04a29b91..00000000 --- a/doc/mrs6xxx_timing.md +++ /dev/null @@ -1,54 +0,0 @@ -# MRS6xxx Timing -## Timing between Layers -The layers are taken up by the scanner in packs of 6. The scanner delivers at an output data rate of 10 Hz and 24 layers 24/6*10=40 scan packets of 6 layers per second. The following table shows an example of the timing for a complete 24 layer recording - -|Raw Time /µs|Delta Time /µs|Elevation Angle /Deg| -|----------|:-------------:|------:| -|2551706348|0|13.19 -|2551706348 |0 |12.565 -|2551706348 |0 |11.940 -|2551706348 |0 |11.315 -|2551706348 |0 |10.690 -|2551706348 |0 |10.065 -|2551731348 |25000 |9.440 -|2551731348 |25000 |8.815 -|2551731348 |25000 |8.190 -|2551731348 |25000 |7.565 -|2551731348 |25000 |6.940 -|2551731348 |25000 |6.315 -|2551756348 |50000 |5.690 -|2551756348 |50000 |5.065 -|2551756348 |50000 |4.440 -|2551756348 |50000 |3.815 -|2551756348 |50000 |3.190 -|2551756348 |50000 |2.565 -|2551781348 |75000 |1.940 -|2551781348 |75000 |1.315 -|2551781348 |75000 |0.690 -|2551781348 |75000 |0.065 -|2551781348 |75000 |-0.560 -|2551781348 |75000 |-1.185 -|NEW SCAN| -|2551807862| 101514| 13.190 -|2551807862| 101514| 12.565 -|2551807862| 101514| 11.940 -|2551807862| 101514| 11.315 -|2551807862| 101514| 10.690 -|2551807862| 101514| 10.065 -|2551832862| 126514| 9.440 - - -## Jitter -The time stamps between the layers are interpolated by the scanner. The time stamps of the first layer (Ang.=13.19°) are measured and show jitter accordingly. -The jitter of the time stamps of the first layer was measured and is shown in the following fig. - -![mrs6xxx_jitter_002](mrs_6xxx_timing.png) - -In the spectrogram of the sound generated by the scanner, an amplitude modulation appears (e.g. at 6 kHz Band) that roughly matches the fluctuation of the scanning frequencies, compare the following figure. -![mrs6xxx_audio](mrs6xxx_audio_fft.jpg) - - - - - - diff --git a/doc/multiple_lidars.md b/doc/multiple_lidars.md deleted file mode 100644 index 2c00d38c..00000000 --- a/doc/multiple_lidars.md +++ /dev/null @@ -1,107 +0,0 @@ -# How to run multiple sensors concurrently - -To support multiple sensors, sick_scan_xd has to be started multiple times, with one sick_scan_xd-node for each sensor. By default, each sick_scan_xd-node connects to "192.168.0.1" and publishes its pointcloud on topic "cloud". Therefore both the node name, the ip-address of the sensor and the pointcloud topic have to be configured differently for each node. - -Node name, ip-address and pointcloud topic can be configured in the launch-file or by commandline argument: - -* Topic, nodename and ip configuration in a launch-file (example for TiM7xx): - ``` - - - - - - - - - - ``` - -* Topic, node name and ip configuration by commandline (ROS1-example for TiM7xx): - ``` - roslaunch sick_scan_xd sick_tim_7xx.launch nodename:=sick_tim_7xx_1 hostname:=192.168.0.1 cloud_topic:=cloud_1 - roslaunch sick_scan_xd sick_tim_7xx.launch nodename:=sick_tim_7xx_2 hostname:=192.168.0.2 cloud_topic:=cloud_2 - ``` - -* Topic, node name and ip configuration by commandline (ROS2-example for TiM7xx): - ``` - ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch nodename:=sick_tim_7xx_1 hostname:=192.168.0.1 cloud_topic:=cloud_1 - ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_tim_7xx.launch nodename:=sick_tim_7xx_2 hostname:=192.168.0.2 cloud_topic:=cloud_2 - ``` - -Scripts [run_linux_ros1_simu_tim7xx_twin.bash](../test/scripts/run_linux_ros1_simu_tim7xx_twin.bash) and [run_linux_ros2_simu_tim7xx_twin.bash](../test/scripts/run_linux_ros2_simu_tim7xx_twin.bash) show a complete example with emulation of two TiM7xx sensors and two sick_scan_xd nodes running concurrently using different nodenames and topics. - -To run two multiScan or picoScan devices simultanously, each sick_scan_xd node must be configured with different lidar ip addresses and udp ports, different node names, different ros topics and frame ids for each point cloud. Therefore the following launchfile parameter should be overwritten by individual settings for each lidar: -* "hostname": e.g. "192.168.0.190" and "192.168.0.98" -* "nodename": e.g. sick_picoscan0" and "sick_picoscan1" -* "publish_frame_id": e.g. "world0" and "world1" -* "publish_laserscan_segment_topic": e.g. "scan0_segment" and "scan1_segment" -* "publish_laserscan_fullframe_topic": e.g. "scan0_fullframe" and "scan1_fullframe" -* "imu_topic": e.g. "imu0" and "imu1" -* "udp_port": e.g. "56661" and "56662" -* "imu_udp_port": e.g. "7503" and "7504" -* individual topics and frame ids for each customized point cloud, e.g. - * replace all "topic=/cloud_" by "topic=/cloud0_" resp. "topic=/cloud1_" - * replace all "frameid=world" by "frameid=world0" resp. "frameid=world1" -It is recommend to first verify the launchfile configurations separately for each picoScan before running them simultanously. - -For picoScan and multiScan, parameter udp_receiver_ip must be set to the ip address of the PC running sick_scan_xd. It is recommend to use ip addresses in the same subnet. - -**Note: The sick_scan_xd API does not support running multiple lidars simultaneously in a single process.** Currently the sick_scan_xd API does not support the single or multi-threaded use of 2 or more lidars in one process, since the sick_scan_xd library is not guaranteed to be thread-safe. To run multiple lidars simultaneously, we recommend using ROS or running sick_scan_xd in multiple and separate processes, so that each process serves one sensor. - -## Example: Run multiScan and picoScan simultaneously - -The following example shows a multiScan and a picoScan device running simultaneously on ROS-1. The ip address of the multiScan is `192.168.0.1` (default), the ip address of the picoScan has been set to `192.168.0.2`. The Linux-PC running sick_scan_xd uses ip address `192.168.0.100`. `fping -a -q -g 192.168.0.0/24` shows all available devices in subnet `192.168.0.x`: - -![multiple_lidars_01.png](screenshots/multiple_lidars_01.png) - -| device | ip | -|--------|----| -| 192.168.0.1 | multiScan | -| 192.168.0.2 | picoScan | -| 192.168.0.100 | Linux-PC | - -Open 192.168.0.1 and 192.168.0.2 in a browser to view the network settings with SOPAS Air: - -![multiple_lidars_02.png](screenshots/multiple_lidars_02.png) - -The frame ids and ros topics of both lidars should be configured differently. Copy both launchfiles (sick_multiscan.launch and sick_piocscan.launch in this example) e.g. to lidar1.launch and lidar2.launch and replace ros topics and frame ids, e.g. - * replace all "topic=/cloud_" by "topic=/cloud1_" in lidar1.launch - * replace all "topic=/cloud_" by "topic=/cloud2_" in lidar2.launch - * replace all "frameid=world" by "frameid=world1" in lidar1.launch - * replace all "frameid=world" by "frameid=world2" in lidar2.launch - -![multiple_lidars_03.png](screenshots/multiple_lidars_03.png) - -![multiple_lidars_04.png](screenshots/multiple_lidars_04.png) - -Provide the launchfiles with `catkin_make_isolated --install --cmake-args -DROS_VERSION=1`. - -Then launch sick_scan_xd twice with two different launchfiles, ip addresses, node names, udp ports, topic and frame ids. - -Example: - -` -roslaunch sick_scan_xd lidar1.launch hostname:=192.168.0.1 udp_receiver_ip:=192.168.0.100 nodename:=lidar1 udp_port:=2115 imu_udp_port:=7503 publish_frame_id:=world1 publish_laserscan_segment_topic:=scan1_segment publish_laserscan_fullframe_topic:=scan1_fullframe imu_topic:=imu1 & -` - -` -roslaunch sick_scan_xd lidar2.launch hostname:=192.168.0.2 udp_receiver_ip:=192.168.0.100 nodename:=lidar2 udp_port:=2116 imu_udp_port:=7504 publish_frame_id:=world2 publish_laserscan_segment_topic:=scan2_segment publish_laserscan_fullframe_topic:=scan2_fullframe imu_topic:=imu2 & -` - -Rviz shows the point clouds of both lidars running simultaneously, with frame id `world1` for lidar1 (multiScan) and frame id `world2` for lidar2 (picoScan): - -![multiple_lidars_05.png](screenshots/multiple_lidars_05.png) - -![multiple_lidars_06.png](screenshots/multiple_lidars_06.png) - -If the 6D poses of the lidars are known, their coordinates can be transformed to a common frame by a static_transform_publisher. Example: - -``` -rosrun tf static_transform_publisher 0 0 0 0 0 0 world world1 100 & -rosrun tf static_transform_publisher 0 0 0 0 0 0 world world2 100 & -``` - -![multiple_lidars_07.png](screenshots/multiple_lidars_07.png) - -The big purple dots show the picoScan pointcloud, the other points are the multiScan point clouds. Both are transformed to the common frame id `world`. Note that both point clouds do not match exactly, because the 6D poses are just assumed to be (x=0, y=0, z=0, yaw=0, pitch=0, roll=0) in this example. \ No newline at end of file diff --git a/doc/nav350.md b/doc/nav350.md deleted file mode 100644 index 513fa3fc..00000000 --- a/doc/nav350.md +++ /dev/null @@ -1,84 +0,0 @@ -# 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_xd/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_xd/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_xd/NAVOdomVelocity` on topic "/sick_nav_350/nav_odom_velocity". Odometry messages `sick_scan_xd/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_xd/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/network.md b/doc/network.md deleted file mode 100644 index 16b376a1..00000000 --- a/doc/network.md +++ /dev/null @@ -1,69 +0,0 @@ -# sick_scan_xd networking FAQ - -A TCP connection is needed to receive scan data from a lidar, which requires an appropriate network setup. - -Common errors when establishing the TCP connection and possible solutions are described below. If you encounter network errors like "Failed to open TCP connection" when running sick_scan_xd, follow these recommendations. - -## Static IP addresses - -Most lidars have the default IP address 192.168.0.1. It is highly recommended to use a static IPv4 network with IP addresses 192.168.x.y. PC and lidar should use the same subnet. **Avoid using DHCP.** - -Example IP address of the lidar: 192.168.0.1 (default) - -Example IP address of the PC running sick_scan_xd: 192.168.0.100 - -The IP address of the PC running sick_scan_xd and the lidar must not be identical! - -Use `ifconfig -a` on Linux resp. `ipconfig /all` on Windows to view network settings and lcoal IP addresses. - -1. Try to ping your device: - ```bash - ping 192.168.0.1 - ``` -2. Disconnect your scanner and retry ping - - ```bash - ping 192.168.0.1 - ``` - The result of ping contains a pattern like - ```bash - ... Destination Host Unreachable - ``` -3. Reconnect your device and try to ping: - ```bash - ping 192.168.0.1 - ``` - -If you do not know the IP addresses, try to find the IP addresses of your PC and your lidar in your subnet: - -1. Install fping: `apt-get install fping` - -2. Scan your network (for example, subnet 192.168.10.0/24): `fping -a 192.168.0.1\24` to -search for all IP addresses from 192.168.0.0 to 192.168.0.255. - -The result is similar to: -```bash -192.168.0.4 is alive -192.168.0.22 is alive -``` -and a lot of unreachable entries. -In the example the IP address 192.168.0.4 is the laserscanner MRS1104 and the IP address 192.168.0.22 is the computer running linux. Check this with `ifconfig|grep 192.168.0.22`. - -We recommend to use wired Ethernet. On Ubuntu, use the "Wired Settings" menu to check the network profile. Network profiles sometimes change automatically when multiple profiles are in use or the lidar has been switched off and on. - -![network_setup_02.png](screenshots/network_setup_02.png) - - -## Network configuration with SOPAS ET - -The [SOPAS Engineering Tool](https://www.sick.com/de/en/catalog/digital-services-and-solutions/software/sopas-engineering-tool/p/p367244) (SOPAS ET) allows a detailed lidar configuration incl. network settings. We recommend to use SOPAS ET in case of network problems or to change the lidars network configuration. The operation manual contains further details. - -## Diagnostic tools - -Network tools can help in case of connection errors. Examples are: - -* `ping ` to check if the lidar is reachable -* `fping -a 192.168.0.1\24` to see ip addresses available in the subnet -* `netcat ` to read or write network data -* `wireshark` to monitor and record the network traffic between PC and lidar. -* `sudo ufw status` to see if a firewall is active. If active, disable firewalls with `sudo ufw disable`. diff --git a/doc/profile_002.png b/doc/profile_002.png index 6b0ab829..9f71a01c 100644 Binary files a/doc/profile_002.png and b/doc/profile_002.png differ diff --git a/doc/profiling.md b/doc/profiling.md deleted file mode 100644 index f16194df..00000000 --- a/doc/profiling.md +++ /dev/null @@ -1,89 +0,0 @@ -# Profiling - -## Table of contents - -- [Introduction](#introduction) -- [Installation](#installation) -- [Usage](#Usage) -## Introduction - -Since the existing node can basically be used on different platforms, bottlenecks can occur with weak hardware. To better analyze these bottlenecks, software profiling can be performed. -The following example shows how to perform profiling. -For further details on profiling, please refer to https://baptiste-wicht.com/posts/2011/09/profile-c-application-with-callgrind-kcachegrind.html, for example. - -## Installation - -First of all, you need to install Callgrind and KCachegrind. -You also need to install graphviz in order to view the call graph in KCachegrind. The applications are already packaged for the most important Linux distributions. You can just use apt-get to install them: -``` -sudo apt-get install valgrind kcachegrind graphviz -``` - -## Usage -We have to start by profiling the application with Callgrind. To profile an application with Callgrind, you just have to prepend the Callgrind invocation in front of your normal program invocation: -``` -valgrind --tool=callgrind program [program_options] -``` -### Starting profiling for sick_scan_xd -In order to establish a reference to the source code during profiling, the program must be compiled with debug symbols, this can be done with catkin_make -``` -catkin_make install -DCMAKE_BUILD_TYPE=Debug -``` -It is necessary to create a rosmaster so that the sick_scan_xd node can connect to it because we can't use roslaunch during profiling. -``` -roscore -``` -To set the parameters we start a node as usual with roslaunch. -``` -roslaunch sick_scan_xd sick_lms_5xx.launch hostname:=192.168.0.151 -``` -While this node is running we can use ```ps -aef| grep sick_scan_xd``` to determine the program path and the call parameters. -``` -rosuser@ROS-NB:~$ ps -aef|grep sick_scan_xd -rosuser 4839 2443 0 14:43 pts/1 00:00:00 /usr/bin/python /opt/ros/melodic/bin/roslaunch sick_scan_xd sick_lms_5xx.launch hostname:=192.168.0.151 -rosuser 4854 4839 1 14:43 ? 00:00:03 /home/rosuser/ros_catkin_ws/devel/lib/sick_scan_xd/sick_generic_caller __name:=sick_lms_5xx __log:=/home/rosuser/.ros/log/f9861670-304c-11e9-9839-54e1ad2921b6/sick_lms_5xx-1.log -rosuser 4910 4875 0 14:46 pts/6 00:00:00 grep --color=auto sick_scan_xd -``` -now we can close the node and restart with callgrid -``` -valgrind --tool=callgrind program /home/rosuser/ros_catkin_ws/devel/lib/sick_scan_xd/sick_generic_caller __name:=sick_lms_5xx -``` -The result will be stored in a callgrind.out.XXX file where XXX will be the process identifier. -You can read this file using a text editor, but it won't be very useful because it's very cryptic. -That's here that KCacheGrind will be useful. You can launch KCacheGrind using command line -or in the program menu if your system installed it here. Then, you have to open your profile file. - -The first view present a list of all the profiled functions. You can see the inclusive -and the self cost of each function and the location of each one. - -![src_view.png](src_view.png) - -Once you click on a function, the other views are filled with information. The view in uppper right part of the window gives some information about the selected function. - -![profile_002](profile_002.png) - -The view have several tabs presenting different information: - -Types : Present the types of events that have been recorded. In our case, it's not really interesting, it's just the number of instructions fetch -Callers : List of the direct callers -All Callers : List of all the callers, it seems the callers and the callers of the callers -Callee Map : A map of the callee, personally, I do not really understand this view, but it's a kind of call graph representing the cost of the functions -Source code : The source code of the function if the application has been compiled with the debug symbol -And finally, you have another view with data about the selected function. - -![profile_003](profile_003.png) - -Again, several tabs: - -* Callees : The direct callees of the function -* Call Graph : The call graph from the function to the end -* All Callees : All the callees and the callees of the callees -* Caller Map : All functions are represented as blocks the size corresponds to their CPU time. Callees are stacked on the callers. -* Machine Code : The machine code of the function if the application has been profiled with --dump-instr=yes option -You have also several display options and filter features to find exactly what you want and display it the way you want. - -The information provided by KCacheGrind can be very useful to find which functions takes too much time or which functions are called too much. - -This text is an adopted version of https://baptiste-wicht.com/posts/2011/09/profile-c-application-with-callgrind-kcachegrind.html . - -Thanks to Baptiste Wicht. diff --git a/doc/radar.md b/doc/radar.md deleted file mode 100644 index 407a72b3..00000000 --- a/doc/radar.md +++ /dev/null @@ -1,203 +0,0 @@ -# Radar - -## Table of contents - -- [Introduction](#introduction) -- [Measuring Principle](#measuring-principle) -- [Raw Targets](#raw-targets) -- [Tracking Objects](#tracking-objects) -- [ROS message for Radar](#ros-message-for-radar) -- [Parameter for Radar Usage](#parameter_for_radar_usage) -- [Radar Datagram](radar_datagram.md) -- [Visualization](#visualization) -- [Launch Files](#launch-files) - -## Introduction - -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 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, -the signal reflected by the object and received by the radar is time-shifted to the original transmitted frequency. -By mixing the transmitted signal with the received signal, the frequency shift and thus the time shift can be determined. -Based on the known modulation parameters of the transmitter, the propagation time of the signal can be determined, which in turn is proportional to the distance of the object. -For precise distance measurement, therefore, the transmission frequency must be modulated as precisely as possible in linear fashion, -since any non-linearity impairs the distance accuracy of the radar. - -Through this indirect time measurement via the frequency change of the transmitter, even very close targets can be measured with high accuracy and cost-efficiency using the FMCW method, provided that the modulation parameters are selected appropriately. The distance resolution is determined by the bandwidth of the transmitted signal. - -## Raw Targets - -Raw targets correspond to individual reflectors that are detected by the radar. Each individual reflector carries the following information: -* Range -* Horizontal angle (azimuth) -* Doppler speed -* Reflectivity of the target (aka rcs - radar cross section) - -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 - -Tracking objects are determined from the raw targets via a tracking procedure over the spatial and temporal -distribution of the raw targets. The track method estimates the location, direction and speed of the object based on an initial estimate. After initialization, new raw targets are assigned to the track if they "fit" to the track. This process is called "gating". Once these raw targets have been assigned to the track, -the track is updated and the new estimate is used for further processing. - -The distribution of raw targets over the object also determines the object length during the tracking process. - -The tracking object therefore has the following properties: -* Distance from radar in Cartesian coordinates -* Direction vector in Cartesian coordinates -* Direction of travel as an angle in the X/Y plane -* Vehicle speed -* Vehicle length - -## ROS message for Radar - -After parsing the telegram, the driver sends an ROS message of type RadarScan. RadarScan consists of the following components: -``` -Header header -RadarPreHeader radarPreHeader -sensor_msgs/PointCloud2 targets -sick_scan_xd/RadarObject[] objects -``` -### RadarPreHeader -The radar preheader contains information that provides general information about the radar. This data record can usually be ignored for object recognition with regard to raw targets and tracking objects. -For details please refer to the message specification of Sick. - -### targets - -The list with the raw targets of type sick_scan_xd/targets contains the information about the raw targets. -Each raw target contains the following data fields in a pointcloud2-message (height: 1, width: number of raw targets): -``` - std::string channelRawTargetId[] = { "x", "y", "z", "vrad","amplitude" }; -``` -This raw target contains cartesian coordinates, which are derived from range and azimuth angle (horizontal angle) in the following way (code snippet): -``` -valSingle[0] = rawTargetList[i].Dist() cos(angle); // x -valSingle[1] = rawTargetList[i].Dist() * sin(angle); // y -valSingle[2] = 0.0; // z -valSingle[3] = rawTargetList[i].Vrad(); // vrad -valSingle[4] = rawTargetList[i].Ampl(); // amplitude -``` - - -### objects - -The list with the objects of type sick_scan_xd/RadarObject[] contains the information about the track objects. - -``` -int32 id - -time tracking_time // valid -time last_seen // not set - -geometry_msgs/TwistWithCovariance velocity // valid - -geometry_msgs/Pose bounding_box_center // valid -geometry_msgs/Vector3 bounding_box_size // valid - -geometry_msgs/PoseWithCovariance object_box_center // valid -geometry_msgs/Vector3 object_box_size // valid - -geometry_msgs/Point[] contour_points // not set -``` - -Please note that not all fields are filled in the object messages. The message specification contains valid ones in the areas marked here in the code section. - -The corresponding code fills the object list in the following manner: - -``` - float heading = atan2( objectList[i].V3Dy(), objectList[i].V3Dx()); - - 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; - - radarMsg_.objects[i].bounding_box_center.position.x = objectList[i].P3Dx(); - radarMsg_.objects[i].bounding_box_center.position.y = objectList[i].P3Dy(); - radarMsg_.objects[i].bounding_box_center.position.z = 0.0; - radarMsg_.objects[i].bounding_box_center.orientation.x = cos(heading); - radarMsg_.objects[i].bounding_box_center.orientation.y = sin(heading); - radarMsg_.objects[i].bounding_box_center.orientation.z = 0.0; - radarMsg_.objects[i].bounding_box_center.orientation.w = 1.0; // homogeneous coordinates - - - radarMsg_.objects[i].bounding_box_size.x = objectList[i].ObjLength(); - radarMsg_.objects[i].bounding_box_size.y = 1.7; - radarMsg_.objects[i].bounding_box_size.z = 1.7; - for (int ii = 0; ii < 6; ii++) - { - int mainDiagOffset = ii * 6 + ii; // build eye-matrix - radarMsg_.objects[i].object_box_center.covariance[mainDiagOffset] = 1.0; // it is a little bit hacky ... - radarMsg_.objects[i].velocity.covariance[mainDiagOffset] = 1.0; - } - radarMsg_.objects[i].object_box_center.pose = radarMsg_.objects[i].bounding_box_center; - radarMsg_.objects[i].object_box_size= radarMsg_.objects[i].bounding_box_size; - -``` -As you can see there are default values for object height and object width of 1.7 (typical private vehicle) - - - -## Launch Files - -The following launch files serve as examples for use: - -* 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 - -### Data visualization example video - -[A video example can be found here](200326_5_video_track.mp4). - -The following figure shows a viz-screenshot of the pointcloud: - -![radar_rviz](radar_rviz.png) - -## Parameter for Radar Usage -The following parameters are support by the node **sick_generic_caller** in -combination with the RADAR RMSxxxx: - -* ~scanner_type (string, default: "")
- Must be set to **sick_rms_xxxx** -* ~range_max (double, default: 25.0)
- Maximum range -* ~hostname -* ~port -* ~timelimit -* ~tracking_mode
- 0: BASIC-Tracking - use for tracking smaller objects
- 1: TRAFFIC-Tracking - use for tracking larger objects like vehicles
-* transmit_raw_targets (bool, default: true) -* transmit_objects (bool, default: true) -* emul_sensor (bool, default: false) - - diff --git a/doc/radar_datagram.md b/doc/radar_datagram.md deleted file mode 100644 index 2f11c314..00000000 --- a/doc/radar_datagram.md +++ /dev/null @@ -1,131 +0,0 @@ -# Radar Datagram -## Table of contents - -- [Introduction](#introduction) -- [Structure of Radar Datagram](#structure_of_radar_datagram) -## Introduction - -This documentation gives an -overview of the structure of the radar datagrams. -With regard to the PreHeader, final consultations will be carried out with SICK, so that slight changes may -still occur here in Q3/2018. - -## Structure of Radar Datagram - -composition - -The message sick_scan_xd/RadarScan consists of four parts: -- Header in standard format -- radarPreHeader with higher-level information -- targets: Raw targets output from radar -- objects: Tracking objects that are determined based on the raw targets using the internal tracking algorithm. - -The complete structure can be determined using the command: -``` -rosmsg show sick_scan_xd/RadarScan' -``` - - -## Structure of Radar Preheader - -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. -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: - -``` -sSN LMDradardata 1 1 112F6E9 0 0 BCC DC0C 730E9D16 730EA06D 0 0 0 0 0 0 1 0 0 4 DIST1 42200000 00000000 0 AZMT1 3C23D70A 00000000 0 VRAD1 3C23D70A 00000000 0 AMPL1 3DCCCCCD 00000000 0 1 MODE1 3F800000 00000000 0 0 0 0 0 0 -``` - -In the following, the individual tokens are numbered one after another and their meaning is explained: - - -``` - 0: sSN - 1: LMDradardata - - MeasurementData - =============== - 2: 1 MeasurementData.uiVersionNo : Version Information for this while structureValue - Value Range: 0 ... 65535 - DeviceBlock - =========== - 3: 1 DeviceBlock.uiIdent : Logical number of the device - Value Range: 0 ... 65535 - 4: 112F6E9 DeviceBlock.udiSerialNo : Serial number of the device - Value Range : 0..4294967295 - - - 5: 0 DeviceBlock.xbState : State of the device - Bit length : 16 - - 0.0 Bool : Value Range False, True - Initialisation: False - Meaning : bDeviceError - - 0.1 Bool : Value Range False, True - Initialisation: False - Meaning : bContaminationWarning - - 0.2 Bool : Value Range False, True - Initialisation: False - Meaning : bContaminationError - - 0.3 ... 0.7 - 6: 0 1.0 ... 1.7 Bool : Value Range False, True - Reserved - -StatusBlock -=========== - 7: BCC uiTelegramCount - 8: DC0C uiCycleCount (or uiScanCount???) - 9: 730E9D16 udiSystemCountScan - 10: 730EA06D udiSystemCountTransmit - 11: 0 xbInputs (Bit 0.0 .. 0.7) - 12: 0 xbInputs (Bit 1.0 .. 1.7) - 13: 0 xbOutputs (Bit 0.0 .. 0.7) - 14: 0 xbOutputs (Bit 1.0 .. 1.7) - -MeasurementParam1Block -====================== - 15: 0 MeasurementParam1Block.uiCycleDuration - 16: 0 MeasurementParam1Block.uiNoiseLevel - -aEncoderBlock -============= - 17: 1 Number of aEncoderBlocks - - - 18: 0 aEncoderBlock[0].udiEncoderPos - 19: 0 aEncoderBlock[0].iEncoderSpeed - - 20: 4 Number of following data channels - 21: DIST1 - 22: 42200000 - 23: 00000000 - 24: 0 - 25: AZMT1 - 26: 3C23D70A - 27: 00000000 - 28: 0 - 29: VRAD1 - 30: 3C23D70A - 31: 00000000 - 32: 0 - 33: AMPL1 - 34: 3DCCCCCD - 35: 00000000 - 36: 0 - 37: 1 - 38: MODE1 - 39: 3F800000 - 40: 00000000 - 41: 0 - 42: 0 - 43: 0 - 44: 0 - 45: 0 - 46: 0 -``` diff --git a/doc/raspberry.md b/doc/raspberry.md deleted file mode 100644 index 1cf9f947..00000000 --- a/doc/raspberry.md +++ /dev/null @@ -1,102 +0,0 @@ -# Raspberry Pi - -sick_scan_xd supports Linux on Raspberry Pi 4. Follow the build instructions for Linux to run sick_scan_xd on a Raspberry: -* [Build on Linux generic without ROS](../INSTALL-GENERIC.md#build-on-linux-generic-without-ros) -* [Build on Linux ROS1](../INSTALL-ROS1.md#build-on-linux-ros1) -* [Build on Linux ROS2](../INSTALL-ROS2.md#build-on-linux-ros2) -* [USAGE](USAGE.md) - -Cmake option " -DRASPBERRY=1" activates compiler settings for the Raspberry. Laserscan messages and polar pointclouds are not published on the Raspberry due to performance reasons. - -## Multiscan example - -The following screenshot shows sick_scan_xd running under ROS-1 on a Raspberry Pi 4 connected to a multiscan lidar. A Linux-PC uses rviz to display the fullframe pointcloud generated on the Raspberry. The ssh-terminal shows the sick_scan_xd log messages on the Raspberry: -![screenshot raspberry performance test](screenshots/raspberry-perftest-04.png) - -On a Raspberry Pi 4, sick_scan_xd processes 240 messages/second with a mean latency of 2.7 milliseconds/message. - -## Performance - -Due to the low power consumption of a Raspberry Pi, performance is critical for applications using sick_scan_xd, especially for multiscan lidars. - -Symptoms for performance problems can be e.g.: -* sick_scan_xd reports the loss of UDP packets or message drops -* sick_scan_xd does not publish the fullframe pointcloud -* rviz shows flickering segment pointclouds even with increased decay time -* low frequency of segment or fullframe pointcloud messages -* generally high system load - -Performance problems can have very different reasons. Notes to help with the elimination of performance issues: - -1. Use the latest Raspberry Pi 4. Previous Raspberry Pi models may work with sick_scan_xd, but are not supported officially. - -2. Eliminate multiple echos. For most lidars, the echo filter is activated by default and only the last echo is transmitted. Check the launchfile configuration and set parameter `filter_echos` if not yet done: - ``` - - ``` - For multican lidars, the echo filter is activated in the launchfile by parameter `host_FREchoFilter`: - ``` - - - ``` - -3. Run a basic performance test on ROS-2 using a tiny sopas test server and a udp player to emulate a multiscan: - ``` - # Start multiscan emulator (sopas test server) - python3 ./src/sick_scan_xd/test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & - # Start rviz - ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_multiscan_emu_360_perftest.rviz & - sleep 1 - # Start sick_generic_caller with sick_scansegment_xd - ros2 launch sick_scan sick_multiscan.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" & - sleep 3 - # Play udp packets to emulate multiScan - python3 ./src/sick_scan_xd/test/python/multiscan_perftest_player.py --udp_port=2115 --repeat=100 --send_rate=100 --verbose=0 --prompt=0 - ``` - The result should look like the follwing screenshot: - ![screenshot raspberry performance test](screenshots/raspberry-perftest-01.png) - If you otherwise observe the loss of UDP packets, message drops, missing pointclouds or mean latency times significantly higher than 6 milliseconds/message, check the system load of your Raspberry and try to eliminate cpu or network intensive processes. - -4. Start sick_scan and the sopas test server on the Raspberry as above, but run the udp player `multiscan_perftest_player.py` on another PC in your local subnet, e.g. - ``` - python3 multiscan_perftest_player.py --dst_ip=192.168.1.27 --udp_port=2115 --repeat=1000 --send_rate=0 --force_delay=3.0e-3 --verbose=0 --prompt=0 - ``` - Replace the example ip adress `192.168.1.27` by the ip adress of your Raspberry. The result should look like the follwing screenshot: - ![screenshot raspberry performance test](screenshots/raspberry-perftest-02.png) - If you otherwise observe the loss of UDP packets, message drops, missing pointclouds or mean latency times significantly higher than 6 milliseconds/message, check the system load of your Raspberry and try to eliminate cpu or network intensive processes. sick_scan_xd (i.e. process sick_generic_caller) should consume ca. 80% of one core resp. cause ca. 20% of the total cpu load. - -## Troubleshooting - -### Endianess - -ARM processors support both little and big endian mode. sick_scan_xd has been tested on Raspberry Pi 4 using ROS1 and ROS2 on Linux in little endian mode. You can check the endianess of your system with `lscpu`. - -### Build sick_scan_xd on a Raspberry without internet or github access - -Checkout sick_scan_xd and use `scp -rp` to copy files and directories recursively from local host to a Raspberry, e.g.: - -On your local Linux PC (Raspberry IP-address is 192.168.178.52 in this example): -``` -mkdir -p ./sick_scan_xd_raspberry_pi_pretest/src -pushd ./sick_scan_xd_raspberry_pi_pretest/src -git clone https://github.com/SICKAG/libsick_ldmrs.git -git clone -b master https://github.com/SICKAG/sick_scan_xd.git -popd -scp -rp ./sick_scan_xd_raspberry_pi_pretest 192.168.178.52:/home/rostest/sick_scan_xd_raspberry_pi_pretest -``` - -On your Raspberry Pi (ROS-1): -``` -cd /home/rostest/sick_scan_xd_raspberry_pi_pretest -pushd ./src/sick_scan_xd/test/scripts -chmod a+x ./*.bash -./makeall_ros1.bash -popd -source ./devel_isolated/setup.bash -``` - -To view the pointcloud on your local Linux PC (with Raspberry IP-address is 192.168.178.52 in this example): -``` -export ROS_MASTER_URI=http://192.168.178.52:11311/ -rviz -``` diff --git a/doc/sick_scan_api/api_test_linux_tim7xx.png b/doc/sick_scan_api/api_test_linux_tim7xx.png index a2761804..8629956b 100644 Binary files a/doc/sick_scan_api/api_test_linux_tim7xx.png and b/doc/sick_scan_api/api_test_linux_tim7xx.png differ diff --git a/doc/sick_scan_api/sick_scan_api.md b/doc/sick_scan_api/sick_scan_api.md deleted file mode 100644 index 097fd619..00000000 --- a/doc/sick_scan_api/sick_scan_api.md +++ /dev/null @@ -1,425 +0,0 @@ -# Generic API for sick_scan_xd - -## Overview - -A generic API for sick_scan_xd has the following goals: -* Easy integration of sick_scan_xd into customer systems with and without ROS -* Integrate SICK lidars with one API, independent of lidar types or underlying operating system -* Provide the same sick_scan_xd functionality on systems without ROS -* In particular: make the sick_scan_xd functionality available on non-ROS-systems without need to customize sources or configuration files. - -The generic sick_scan_xd API provides an interface to all lidars supported by sick_scan_xd. This API can be used in C, C++, Python, or any other language with support of C-bindings. - -The generic sick_scan_xd API ships with the API-header, the library (binary or sources) and usage examples for C, C++ and Python. The following component diagram shows the relationship between API, library, lidar and a customized application: - -![apiComponentsDiagram1.png](apiComponentsDiagram1.png) - -**Note: Running multiple lidars simultaneously in a single process is not supported.** Currently the sick_scan_xd API does not support the single or multi-threaded use of 2 or more lidars in one process, since the sick_scan_xd library is not guaranteed to be thread-safe. To run multiple lidars simultaneously, we recommend using ROS or running sick_scan_xd in multiple and separate processes, so that each process serves one sensor. See [multiple_lidars.md](../multiple_lidars.md) for further information. - -## Build and test shared library - -The shared library, which implements the C-API, is built native on Linux or Windows (i.e. without ROS). Follow the instructions on [Build on Linux generic without ROS](../../INSTALL-GENERIC.md#build-on-linux-generic-without-ros) for Linux resp. [Build on Windows](../../INSTALL-GENERIC.md#build-on-windows) for Windows. - -### Build the shared library on Linux - -Run the following commands to build the shared library `libsick_scan_xd_shared_lib.so` on Linux: -``` -# Clone repositories -git clone https://github.com/SICKAG/libsick_ldmrs.git -git clone -b master https://github.com/SICKAG/sick_scan_xd.git -# Build libsick_ldmrs library -mkdir -p ./build -mkdir -p ./libsick_ldmrs/build -pushd libsick_ldmrs/build -cmake -G "Unix Makefiles" .. -make -j4 -sudo make -j4 install -popd -# Build libsick_scan_xd_shared_lib.so -pushd ./build -export ROS_VERSION=0 -cmake -DROS_VERSION=0 -G "Unix Makefiles" ../sick_scan_xd -make -j4 -sudo make -j4 install -# Check build and library dependencies -ls -al ./sick_generic_caller -ls -al ./libsick_scan_xd_shared_lib.so -ls -al ./sick_scan_xd_api_test -ldd -r ./libsick_scan_xd_shared_lib.so -popd -``` -After successful build, the shared library `libsick_scan_xd_shared_lib.so` and a tiny test executable `sick_scan_xd_api_test` are created. - -### Build the shared library on Windows - -Run the following commands to build the shared library `sick_scan_xd_shared_lib.dll` with Visual Studio 2019 on Windows: -``` -# Clone repository sick_scan_xd -git clone -b master https://github.com/SICKAG/sick_scan_xd.git -# Build libraries sick_scan_xd_shared_lib.dll -call "%ProgramFiles(x86)%\Microsoft Visual Studio\2019\Community\Common7\Tools\VsDevCmd.bat" -arch=amd64 -host_arch=amd64 -set _os=x64 -set _cmake_string=Visual Studio 16 -set _msvc=Visual Studio 2019 -set _cmake_build_dir=build -cd sick_scan_xd -if not exist %_cmake_build_dir% mkdir %_cmake_build_dir% -pushd %_cmake_build_dir% -cmake -DROS_VERSION=0 -G "%_cmake_string%" .. -if %ERRORLEVEL% neq 0 ( @echo ERROR building %_cmake_string% sick_scan_xd with cmake & @pause ) -cmake --build . --clean-first --config Debug -``` -After successful build, the shared library `sick_scan_xd_shared_lib.dll` and a tiny test executable `sick_scan_xd_api_test.exe` are created. To install the library and header in the system folder, run `cmake --build . --target install` with admin priviledges. Note that LDMRS is not supported on Windows. - -Note: sick_scan_xd builds and runs with both Visual Studio 2019 and 2022. Visual Studio 2019 is recommended, since ROS on Windows requires VS 2019. - -Replace `cmake -DROS_VERSION=0 -G "%_cmake_string%" ..` by `cmake -DROS_VERSION=0 -DCMAKE_ENABLE_EMULATOR=1 -G "%_cmake_string%" ..` to build emulators for unittests without lidar hardware, see [Simulation and unittest](#simulation-and-unittest). - -### Test the shared library - -The executable file `sick_scan_xd_api_test` provides a minimalistic API test. Run `sick_scan_xd_api_test hostname:=` to test the API against a lidar, e.g. on Linux: -``` -# export LD_LIBRARY_PATH=.:./build:$LD_LIBRARY_PATH # append relative path to build folder -export LD_LIBRARY_PATH=.:`pwd`/build:$LD_LIBRARY_PATH # append absolute path to build folder -./build/sick_scan_xd_api_test ./sick_scan_xd/launch/sick_tim_7xx.launch hostname:=192.168.0.1 -``` -On Windows, run e.g. -``` -set PATH=.;.\build;..\build\Debug;%PATH% -.\build\Debug\sick_scan_xd_api_test.exe launch/sick_lms_5xx.launch hostname:=192.168.0.1 -``` - -The executable binary `sick_scan_xd_api_test` will just load library `libsick_scan_xd_shared_lib.so` resp. `sick_scan_xd_shared_lib.dll`, start the lidar and print a message when receiving lidar messages, e.g. `sick_scan_xd_api_test: pointcloud callback`. Replace `sick_lms_1xx.launch` in the example by the launchfile corresponding to your type of lidar. - -To load the library, the build folder has to be included in `LD_LIBRARY_PATH` (Linux) resp. `PATH` (Windows). Set this environment variable to your build folder, e.g. on Linux using -``` -# export LD_LIBRARY_PATH=.:./build:$LD_LIBRARY_PATH # append relative path to build folder -export LD_LIBRARY_PATH=.:`pwd`/build:$LD_LIBRARY_PATH # append absolute path to build folder -``` -resp. on Windows -``` -set PATH=.;.\build;.\build\Debug;%PATH% -``` - -## Usage example - -The sick_scan_xd API can be used on Linux or Windows in any language with support of C-bindings. There are 3 steps required to use the API: - -1. API- and lidar-initialization by - * SickScanApiLoadLibrary - * SickScanApiCreate - * SickScanApiInitByLaunchfile or SickScanApiInitByCli - -2. Receive messages by registration of callbacks using `SickScanApiRegisterMsg`-functions (recommended) or by polling using `SickScanApiWaitNextMsg`-functions. - - Alternative examples to receive lidar scan data as a pointcloud: - * Register a callback for cartesian pointcloud data using SickScanApiRegisterCartesianPointCloudMsg, or - * register a callback for polar pointcloud data using SickScanApiRegisterPolarPointCloudMsg. - - The registered callback will be executed whenever the lidar has sent new scan data and receives the (cartesian or polar) pointcloud by a parameter of type SickScanPointCloudMsg. The SickScanPointCloudMsg in sick_scan_xd API corresponds to ROS pointcloud: The cartesian pointcloud (registered by SickScanApiRegisterCartesianPointCloudMsg) contains the fields (x, y, z, intensity). The polar pointcloud (registered by SickScanApiRegisterPolarPointCloudMsg) contains the fields (range, azimuth, elevation, intensity). Each field contains its name (i.e. x, y, z, range, azimuth, elevation, or intensity) and offset. The scan data is a flat buffer of size width x height fields: - - ![apiPointCloudMsg](apiPointCloudMsg.png) - - The following python code shows how to convert a cartesian pointcloud to 3D points (x, y, z): - ``` - # Convert a SickScanCartesianPointCloudMsg to points - def pySickScanCartesianPointCloudMsgToXYZ(pointcloud_msg): - # get pointcloud fields - num_fields = pointcloud_msg.fields.size - msg_fields_buffer = pointcloud_msg.fields.buffer - field_offset_x = -1 - field_offset_y = -1 - field_offset_z = -1 - for n in range(num_fields): - field_name = ctypesCharArrayToString(msg_fields_buffer[n].name) - field_offset = msg_fields_buffer[n].offset - if field_name == "x": - field_offset_x = msg_fields_buffer[n].offset - elif field_name == "y": - field_offset_y = msg_fields_buffer[n].offset - elif field_name == "z": - field_offset_z = msg_fields_buffer[n].offset - # Extract x,y,z - cloud_data_buffer_len = (pointcloud_msg.row_step * pointcloud_msg.height) # length of polar cloud data in byte - assert(pointcloud_msg.data.size == cloud_data_buffer_len and field_offset_x >= 0 and field_offset_y >= 0 and field_offset_z >= 0) - cloud_data_buffer = bytearray(cloud_data_buffer_len) - for n in range(cloud_data_buffer_len): - cloud_data_buffer[n] = pointcloud_msg.data.buffer[n] - points_x = np.zeros(pointcloud_msg.width * pointcloud_msg.height, dtype = np.float32) - points_y = np.zeros(pointcloud_msg.width * pointcloud_msg.height, dtype = np.float32) - points_z = np.zeros(pointcloud_msg.width * pointcloud_msg.height, dtype = np.float32) - point_idx = 0 - for row_idx in range(pointcloud_msg.height): - for col_idx in range(pointcloud_msg.width): - # Get lidar point in polar coordinates (range, azimuth and elevation) - pointcloud_offset = row_idx * pointcloud_msg.row_step + col_idx * pointcloud_msg.point_step - points_x[point_idx] = np.frombuffer(cloud_data_buffer, dtype = np.float32, count = 1, offset = pointcloud_offset + field_offset_x)[0] - points_y[point_idx] = np.frombuffer(cloud_data_buffer, dtype = np.float32, count = 1, offset = pointcloud_offset + field_offset_y)[0] - points_z[point_idx] = np.frombuffer(cloud_data_buffer, dtype = np.float32, count = 1, offset = pointcloud_offset + field_offset_z)[0] - point_idx = point_idx + 1 - return points_x, points_y, points_z - ``` - Exchange field names ("x", "y", "z") by ("range", "azimuth", "elevation") to get 3D polar points (range, azimuth, elevation). - - For further details, see - * [Minimalistic usage example in C](#minimalistic-usage-example-in-c) - * [Minimalistic usage example in C++](#minimalistic-usage-example-in-c-1) - * [Minimalistic usage example in Python](#minimalistic-usage-example-in-python) - * [Complete usage example in C++](#complete-usage-example-in-c) - * [Complete usage example in Python](#complete-usage-example-in-python) - - Note for multiScan and picoScan lidars: - - * The WaitNext-functions of the API return the next received message. For multiScan and picoScan, this can be a scan segment (i.e. a part of the full scan) or a fullframe poincloud (i.e. all scan points of a 360 degree scan). Depending on the timing, you may not receive all messages, i.e. you may e.g. receive scan points of different segments. We therefore recommend to register a message callback instead of a WaitNext-function. With a registered message callback, you will get all fullframe and segment pointcloud messages. - - * For multiScan and picoScan, pointcloud messages can contain a scan segment (i.e. a part of the full scan) or a fullframe poincloud (i.e. all scan points of a 360 degree scan). The type can be determined by the topic (default: "/cloud_unstructured_segments" for segments, "/cloud_unstructured_fullframe" for fullframe pointclouds) or by segment index (-1 for fullframe, 0 up to 11 for segment pointclouds). - - -3. Close lidar and API by - * `SickScanApiDeregisterMsg`-functions - * SickScanApiClose - * SickScanApiRelease - -All functions named `SickScanApi` are implemented within the library file ("sick_scan_xd_shared_lib.dll" on Windows resp. "libsick_scan_xd_shared_lib.so" on Linux). A small wrapper is provided ([sick_scan_xd_api_wrapper.c](../../test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c) for C/C++, [sick_scan_api.py](../../python/api/sick_scan_api.py) for python), which loads and unloads the library (functions `SickScanApiLoadLibrary` and `SickScanApiUnloadLibrary`) and delegates the function calls to the binary. - -Note: [sick_scan_api.py](../../python/api/sick_scan_api.py) requires python module numpy. On Windows, we recommend to install and use Python either with Visual Studio 2019 or by installing from https://www.python.org/downloads/windows/ (python installer, embedded version not recommended). Otherwise, please install numpy with `python -m pip install numpy` if numpy is not yet installed. - - -### Minimalistic usage example in C - -File [minimum_sick_scan_api_client.c](../../examples/c/minimum_sick_scan_api_client.c) shows a minimalistic example of a C client using the sick_scan_xd API. To build and run this example, open a command shell in folder `examples/scripts` and run `.\build_run_api_examples_linux.bash` on Linux resp. `build_run_api_examples_windows.cmd` on Windows. Make sure, that the shared library `libsick_scan_xd_shared_lib.so` resp. `sick_scan_xd_shared_lib.dll` has been successfully built in the build-folder. - -Alternatively, follow the build and run instructions on Linux: -``` -cd examples/c -mkdir -p ./build -cd ./build -cmake -G "Unix Makefiles" .. -make -j4 -cd ../.. -export LD_LIBRARY_PATH=.:./build:$LD_LIBRARY_PATH -./examples/c/build/minimum_sick_scan_api_client hostname:= -``` - -Alternatively, follow the build and run instructions on Windows: -``` -REM Set environment for Visual Studio 2019 (VS 16) -set _os=x64 -set _cmake_string=Visual Studio 16 -set _msvc=Visual Studio 2019 -set _cmake_build_dir=build -REM Build the minimalistic C usage example -cd examples\c -mkdir %_cmake_build_dir% -cd %_cmake_build_dir% -cmake -G "%_cmake_string%" .. -cmake --build . --clean-first --config Debug -REM Set environment: add build folder to LD_LIBRARY_PATH, add python/api to PYTHONPATH -cd ..\.. -set PATH=.;.\build;.\build\Debug;.\build_win64;.\build_win64\Debug;%PATH% -REM Run minimalistic C api example -.\examples\c\build\Debug\minimum_sick_scan_api_client.exe hostname:= -``` - -### Minimalistic usage example in C++ - -File [minimum_sick_scan_api_client.cpp](../../examples/cpp/minimum_sick_scan_api_client.cpp) shows a minimalistic example of a C++ client using the sick_scan_xd API. To build and run this example, open a command shell in folder `examples/scripts` and run `.\build_run_api_examples_linux.bash` on Linux resp. `build_run_api_examples_windows.cmd` on Windows. Make sure, that the shared library `libsick_scan_xd_shared_lib.so` resp. `sick_scan_xd_shared_lib.dll` has been successfully built in the build-folder. - -Alternatively, follow the build and run instructions on Linux: -``` -cd examples/cpp -mkdir -p ./build -cd ./build -cmake -G "Unix Makefiles" .. -make -j4 -cd ../.. -export LD_LIBRARY_PATH=.:./build:$LD_LIBRARY_PATH -./examples/cpp/build/minimum_sick_scan_api_client hostname:= -``` - -Alternatively, follow the build and run instructions on Windows: -``` -REM Set environment for Visual Studio 2019 (VS 16) -set _os=x64 -set _cmake_string=Visual Studio 16 -set _msvc=Visual Studio 2019 -set _cmake_build_dir=build -REM Build the minimalistic C++ usage example -cd examples\cpp -mkdir %_cmake_build_dir% -cd %_cmake_build_dir% -cmake -G "%_cmake_string%" .. -cmake --build . --clean-first --config Debug -REM Set environment: add build folder to LD_LIBRARY_PATH, add python/api to PYTHONPATH -cd ..\.. -set PATH=.;.\build;.\build\Debug;.\build_win64;.\build_win64\Debug;%PATH% -REM Run minimalistic C++ api example -.\examples\cpp\build\Debug\minimum_sick_scan_api_client.exe hostname:= -``` - -### Minimalistic usage example in Python - -File [minimum_sick_scan_api_client.py](../../examples/python/minimum_sick_scan_api_client.py) shows a minimalistic example of a python client using the sick_scan_xd API. To build and run this example, open a command shell in folder `examples/scripts` and run `.\build_run_api_examples_linux.bash` on Linux resp. `build_run_api_examples_windows.cmd` on Windows. Make sure, that the shared library `libsick_scan_xd_shared_lib.so` resp. `sick_scan_xd_shared_lib.dll` has been successfully built in the build-folder. - -Alternatively, follow the run instructions on Linux: -``` -export LD_LIBRARY_PATH=`pwd`:`pwd`/build:$LD_LIBRARY_PATH -export PYTHONPATH=`pwd`:`pwd`/python/api:$PYTHONPATH -python3 ./examples/python/minimum_sick_scan_api_client.py hostname:= -``` - -Alternatively, follow the run instructions on Windows: -``` -set PATH=.;.\build;.\build\Debug;.\build_win64;.\build_win64\Debug;%PATH% -set PYTHONPATH=.;.\python\api;%PATH% -python ./examples/python/minimum_sick_scan_api_client.py hostname:= -``` - -Note: [sick_scan_api.py](../../python/api/sick_scan_api.py) requires python module numpy. On Windows, we recommend to install and use Python either with Visual Studio 2019 or by installing from https://www.python.org/downloads/windows/ (python installer, embedded version not recommended). Otherwise, please install numpy with `python -m pip install numpy` if numpy is not yet installed. - -### Complete usage example in C++ - -A complete C/C++ usage example is implemented in [sick_scan_xd_api_test.cpp](../../test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp). Note that the shared library ("sick_scan_xd_shared_lib.dll" on Windows resp. "libsick_scan_xd_shared_lib.so" on Linux) has no dependencies to ROS. The usage example on the other hand supports both ROS-1, ROS-2 and native Linux or Windows. When build on ROS, it converts the SickScanApi-messages into ROS-messages. On ROS, they can be visualized by rviz. The following screenshot shows a pointcloud published by `rosrun sick_scan_xd sick_scan_xd_api_test _sick_scan_args:="./src/sick_scan_xd/launch/sick_tim_7xx.launch"`: - -![api_test_linux_ros1_tim7xx.png](api_test_linux_ros1_tim7xx.png) - -Without ROS, sick_scan_xd_api_test plots a jpeg-file to enable a simple visualization of a pointcloud. E.g.: -``` -firefox ./demo/image_viewer_api_test.html & -./build_linux/sick_scan_xd_api_test ./launch/sick_tim_7xx.launch -``` -![api_test_linux_tim7xx.png](api_test_linux_tim7xx.png) - -### Complete usage example in Python - -A complete python usage example is implemented in [sick_scan_xd_api_test.py](../../test/python/sick_scan_xd_api/sick_scan_xd_api_test.py). It is handy to test the sick_scan_xd library. Like its C++ counterpart [sick_scan_xd_api_test.cpp](../../test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp), it just loads library `libsick_scan_xd_shared_lib.so` resp. `sick_scan_xd_shared_lib.dll`, starts a lidar and receives the lidar pointcloud and messages via API. On ROS-1, the lidar pointcloud and messages are converted to ROS and published. The lidar pointcloud can be visualized by rviz using topic "/sick_scan_xd_api_test/api_cloud". - -Run `python3 sick_scan_xd_api_test.py hostname:=` to test the API against a lidar. -On Linux e.g.: -``` -export PYTHONPATH=`pwd`:`pwd`/src/sick_scan_xd/python/api:$PYTHONPATH -source /opt/ros/noetic/setup.bash # replace by noetic by your ros version -python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./src/sick_scan_xd/launch/sick_lms_1xx.launch hostname:=192.168.0.1 -``` -On Windows e.g.: -``` -set PYTHONPATH=.;.\src\sick_scan_xd\python\api;%PYTHONPATH% -python ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./src/sick_scan_xd/launch/sick_lms_1xx.launch hostname:=192.168.0.1 -``` - -The pthon usage example [sick_scan_xd_api_test.py](../../test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) imports [sick_scan_api.py](../../python/api/sick_scan_api.py), which contains the python definitions of the sick_scan_xd API. Make sure that sick_scan_api.py can be imported, e.g. by including folder `python/api` in PYTHONPATH by: - -`export PYTHONPATH=`pwd`:`pwd`/src/sick_scan_xd/python/api:$PYTHONPATH` on Linux, resp.
-`set PYTHONPATH=.;.\src\sick_scan_xd\python\api;%PYTHONPATH%` on Windows - -Note: The shared library ("sick_scan_xd_shared_lib.dll" on Windows resp. "libsick_scan_xd_shared_lib.so" on Linux) works standalone and does not have any ROS dependancies. The usage example [sick_scan_xd_api_test.py](../../test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) converts API- to ROS-messages for visualization and is therefore dependent on ROS, if ROS is installed. - -If ROS is not installed, [sick_scan_xd_api_test.py](../../test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) uses matplotlib to visualize the pointcloud. The following screenshot shows a TiM-7xx pointcloud on Linux without ROS: - -![api_test_python_tim7xx.png](api_test_python_tim7xx.png) - -Note: [sick_scan_api.py](../../python/api/sick_scan_api.py) requires python module numpy. On Windows without ROS, [sick_scan_xd_api_test.py](../../test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) requires numpy and matplotlib. On Windows, we recommend to install and use Python either with Visual Studio 2019 or by installing from https://www.python.org/downloads/windows/ (python installer, embedded version not recommended). These python distributions provide the necessary packages and tools. Otherwise, please install numpy and matplotlib with `python -m pip install numpy` and `python -m pip install matplotlib` if not yet done. - -### Diagnostic - -The API provides the following functions for diagnostics: - -* SickScanApiRegisterDiagnosticMsg and SickScanApiDeregisterDiagnosticMsg: Register resp. deregister a callback to receive diagnostic messages. Diagnostic messages contain a status code and status message. The status code is one of the following numbers: - * OK=0 (normal operation) - * WARN=1 (warning) - * ERROR=2 (error, should not occure) - * INIT=3 (initialization after startup or reconnection) - * EXIT=4 (sick_scan_xd exiting) - - The status message is descriptional C-string. - - A typical sequence of the status code is: - * INIT at startup, then - * after lidar initialization is completed: change to OK (normal operation) and run, and - * EXIT at shutdown. - Diagnostic messages are generated whenever the status changed or an ERROR occured. Status code 2 (i.e. error) should not occure under normal operation. - -* SickScanApiRegisterLogMsg and SickScanApiDeregisterLogMsg: Register resp. deregister a callback to receive log messages. This callback will receive all informational or error messages printed on console. The log messages contain a log level (Info=1, Warn=2, Error=3, Fatal=4) and the log message. - -* SickScanApiGetStatus queries the current status. This function returns the current status code (OK=0 i.e. normal operation, WARN=1, ERROR=2, INIT=3 i.e. initialization after startup or reconnection or EXIT=4) and the descriptional status message. - -* SickScanApiSendSOPAS sends a SOPAS command (Cola-A) to the lidar and returns the response from the device. - * C++ example: - ``` - char sopas_response_buffer[1024] = { 0 }; - SickScanApiSendSOPAS(apiHandle, "sRN SCdevicestate", &sopas_response_buffer[0], (int32_t)sizeof(sopas_response_buffer); // returns "sRA SCdevicestate \x00" in sopas_response_buffer - ``` - - * Python example: - ``` - sopas_response = SickScanApiSendSOPAS(sick_scan_library, api_handle, "sRN SCdevicestate")` # returns "sRA SCdevicestate \x00". - ``` - See the telegram listing for valid SOPAS commands. - -* SickScanApiSetVerboseLevel and SickScanApiGetVerboseLevel sets resp. returns the verbose level. The verbose level can be 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET (equivalent to ros\:\:console\:\:levels). Default verbose level is 1 (INFO), i.e. sick_scan_xd prints informational, warnings and error messages on the console. Logging callbacks registered with SickScanApiRegisterLogMsg will receive all informational, warnings and error messages independant of the verbose level. - -To monitor sick_scan_xd resp. the lidar, it is recommended to register a callback for diagnostic messages using SickScanApiRegisterDiagnosticMsg and to display the error message in case for status code 2 (error). See [sick_scan_xd_api_test.cpp](../../test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp) and [sick_scan_xd_api_test.py](../../test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) for an example. - -### Simulation and unittest - -sick_scan_xd provides a tiny server for offline tests which simulates a basic lidar. It just accepts TCP connections, responds to sopas requests with predefined responses and sends lidar data from file. See [Simulation](../../USAGE.md#simulation) for further details. Note that the simulation does not emulate or replace a lidar, it just supports basic unittests. - -Open a new terminal and run the following steps to test the api against a TiM7xx simulation using the python example mentioned above: - -1. Build library `libsick_scan_xd_shared_lib.so` incl. emulator with option `-DCMAKE_ENABLE_EMULATOR=1`: - ``` - mkdir -p ./src/build - pushd ./src/build - rm -rf ./* - cmake -DROS_VERSION=0 -DCMAKE_ENABLE_EMULATOR=1 -G "Unix Makefiles" ../sick_scan_xd - make -j4 - ls -al libsick_scan_xd_shared_lib.so sick_scan_xd_api_test sick_generic_caller sick_scan_emulator # list size and date of the binaries - popd - ``` - Building sick_scan_xd with option `-DCMAKE_ENABLE_EMULATOR=1` requires jsoncpp. Install libjsoncpp by running "sudo apt-get install libjsoncpp-dev" on Linux resp. "vcpkg install jsoncpp:x64-windows" on Windows (vcpkg required). Run the following steps to install Visual Studios package manager vcpkg on Windows: - * Download vcpkg-master.zip from https://github.com/microsoft/vcpkg/archive/master.zip and unzip to `c:\vcpkg`. Alternatively, run "git clone https://github.com/microsoft/vcpkg" - * Install vcpkg by running the following commands: - ``` - cd c:/vcpkg - bootstrap-vcpkg.bat - vcpkg integrate install - ``` - * Include vcpkg in your path: - ``` - set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH% - ``` - -3. Create a workspace folder, e.g. `sick_scan_ws` (or any other name): - - -2. Build sick_scan_xd for ROS-1 on Linux, see [Build on Linux ROS1](../../INSTALL-ROS1.md#build-on-linux-ros1) - -3. Start the TiM7xx simulator: - ``` - cp -f ./src/sick_scan_xd/test/emulator/scandata/sopas_et_field_test_1_2_both_010.pcapng.json /tmp/lmd_scandata.pcapng.json - ./src/build/sick_scan_emulator ./src/sick_scan_xd/test/emulator/launch/emulator_01_default.launch & - sleep 1 - ``` - -4. Run sick_scan_xd_api_test.py against the TiM7xx simulator on localhost: - ``` - export PYTHONPATH=.:./src/sick_scan_xd/python/api:$PYTHONPATH - python3 ./src/sick_scan_xd/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./src/sick_scan_xd/launch/sick_tim_7xx.launch hostname:=127.0.0.1 port:=2111 sw_pll_only_publish:=False - ``` - -5. Start rviz and visualize the pointcloud on topic "/sick_scan_xd_api_test/api_cloud". - -Note: The shared library ("sick_scan_xd_shared_lib.dll" on Windows resp. "libsick_scan_xd_shared_lib.so" on Linux) works standalone and does not have any ROS dependancies. The usage example [sick_scan_xd_api_test.py](../../test/python/sick_scan_xd_api/sick_scan_xd_api_test.py) uses ROS for visualization. - -## C-API - -The header file [sick_scan_api.h](../../include/sick_scan_xd_api/sick_scan_api.h) defines the C-interface. It defines all datatypes, messages and functions of the generic sick_scan_xd API. To allow equal operations on all systems, the definition of datatypes and messages is as close as possible to their equivalents currently used on ROS. - -Python file [sick_scan_api.py](../../python/api/sick_scan_api.py) defines the same interface in python. - -## Useful links - -[ctypes](https://docs.python.org/3/library/ctypes.html) is used for data exchange and function calls between Python and C-libraries: -* https://docs.python.org/3/library/ctypes.html -* https://docs.python.org/3/library/ctypes.html#structures-and-unions -* https://docs.python.org/3/library/ctypes.html#callback-functions diff --git a/doc/sick_scan_segment_xd.md b/doc/sick_scan_segment_xd.md deleted file mode 100644 index a05574ae..00000000 --- a/doc/sick_scan_segment_xd.md +++ /dev/null @@ -1,373 +0,0 @@ -# picoScan100/multiScan100 - -The multiScan100 and picoScan100 are new lidars from Sick. multiScan100 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 or compact format over UDP. - -multiScan100/picoScan100 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 - -multiScan100 is configured by launch file [sick_multiscan.launch](../launch/sick_multiscan.launch). -picoScan100 is configured by launch file [sick_picoscan.launch](../launch/sick_picoscan.launch). - -Modify file [sick_multiscan.launch](../launch/sick_multiscan.launch) resp. [sick_picoscan.launch](../launch/sick_picoscan.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. - -The ip address of the lidar and the udp receiver can be configured in the launch file by e.g. -``` - - -``` -or by command line by e.g. -``` -# Run sick_scansegment_xd generic without ROS: -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_xd 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_xd sick_multiscan.launch.py hostname:=192.168.0.1 udp_receiver_ip:=192.168.0.100 -``` - -## IMU support - -IMU support for multiScan and picoScan is enabled by default and can be configured in the launchfile: -``` - - - -``` - -sick_scan_xd receives IMU data by UDP and publishes [ROS-1 sensor_msgs/Imu](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Imu.html) resp. [ROS-2 sensor_msgs/msg/Imu](https://docs.ros2.org/latest/api/sensor_msgs/msg/Imu.html) messages. - -Note: IMU support requires compact format, which is the default. If msgpack communication is configured, imu support is automatically disabled. - -IMU support for picoScan requires firmware version 1.1 or newer, see https://www.sick.com/de/en/downloads/media/swp680096 for firmware downloads. - -## SOPAS support - -On ROS-1 and ROS-2, service `ColaMsg` is provided to send CoLa commands to the lidar. Using this service, filters can be applied during runtime. - -See [sick_scansegment_xd_sopas_examples.md](sick_scansegment_xd_sopas_examples.md) for examples. - -See the manual for further information of filter settings and parameter. - - -The driver sends the following SOPAS start and stop sequence at program start resp. exit (example with default ip address 192.168.0.1): -``` -// Prerequirement: measurement is active, but no UDP data is sent -// Start sending scan data output -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 // for multiscan136 only -sWN ScanDataEnable 1 // enable scan data ouput -sMN LMCstartmeas // start measurement -sMN Run // apply the settings and logout -// ... -// UDP data is sent -// ... -// Stop sending scan data output -sMN SetAccessMode 3 F4724744 // set authorization level for writing settings -sWN ScanDataEnable 0 // disable scan data output -sMN Run // apply the settings and logout -// No UDP data is sent anymore -``` - -## Visualization - -The multiScan100 and picoScan100 scans can be visualized by rviz. The following screenshots show two examples of a multiScan100 pointcloud: - -![msgpacks-emulator-rviz](20210929-tokenized-msgpacks-emulator-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_fullframe` collects all segments for a complete 360 degree full scan (360 degree for multiScan100, 276 degree for picoscan100). - -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_unstructured_segments` (topic `/cloud` for sick_scan_xd version 2.10 or earlier) 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_unstructured_fullframe` (topic `/cloud_fullframe` for sick_scan_xd version 2.10 or earlier) 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. - -## Customized pointclouds - -Pointclouds can be customized, i.e. the fields and points can be configured by launchfile [sick_multiscan.launch](../launch/sick_multiscan.launch). - -Parameter "custom_pointclouds" lists all customized pointclouds to be published. Each pointcloud is given by its name, e.g: -``` - -``` -This example publishes 5 types of pointclouds: -* Pointcloud for each segment in cartesian coordinates (x,y,z,i), named "cloud_unstructured_segments" -* Pointcloud for each fullframe in cartesian coordinates (x,y,z,i), named "cloud_unstructured_fullframe" -* Pointcloud for each segment in polar coordinates (azimuth,elevation,range,i), named "cloud_polar_unstructured_segments" -* Pointcloud for each fullframe in polar coordinates (azimuth,elevation,range,i), named "cloud_polar_unstructured_fullframe" -* Pointcloud for each fullframe with all available fields (x,y,z,i,range,azimuth,elevation,layer,echo,reflector), named "cloud_all_fields_fullframe" - -These 5 pointclouds are published by default. - -The properties of the pointcloud, i.e. their fields and points, are configured by the pointcloud name, e.g. pointcloud "cloud_unstructured_segments" (i.e. the segment pointcloud in cartesian coordinates): -``` - - -``` -The cloud property configuration is a list of key-value-pairs, where each key-value-pair specifies a property and its value. E.g. `topic=/cloud_unstructured_segments frameid=world` defines ros topic "/cloud_unstructured_segments" and frame id "world" for the pointcloud named "cloud_unstructured_segments". - -The following key-value-pairs of a customized pointcloud are currently supported: - -* Parameter "coordinateNotation" is an enum to configure pointcloud coordinates: - * coordinateNotation=0: cartesian (default, pointcloud has fields x,y,z,i), identical to customized with fields=x,y,z,i - * coordinateNotation=1: polar (pointcloud has fields azimuth,elevation,r,i), identical to customized with fields=azimuth,elevation,range,i - * coordinateNotation=2: both cartesian and polar (pointcloud has fields x,y,z,azimuth,elevation,r,i), identical to customized with fields=x,y,z,azimuth,elevation,range,i - * coordinateNotation=3: customized pointcloud fields, i.e. the pointcloud has fields configured by parameter "fields" - -* Parameter "updateMethod" is an enum to configure fullframe pointclouds versus segmented pointcloud: - * updateMethod=0: fullframe pointcloud (default) - * updateMethod=1: segmented pointcloud - -* Parameter "fields" defines the fields of the pointcloud for coordinateNotation == 3 (customized pointcloud fields), e.g. - * fields=x,y,z,i: cartesian pointcloud - * fields=range,azimuth,elevation: polar pointcloud - * or any other combination of x,y,z,i,range,azimuth,elevation,t,ts,ring,layer,echo,reflector - - These fields have the following meaning: - * field "x": cartesian x coordinate in meter in ROS coordinates (4 byte, float32) - * field "y": cartesian y coordinate in meter in ROS coordinates (4 byte, float32) - * field "z": cartesian z coordinate in meter in ROS coordinates (4 byte, float32) - * field "i": intensity (4 byte, float32) - * field "range": polar coordinate range in meter (4 byte, float32) - * field "azimuth": polar coordinate azimuth in radians (4 byte, float32) - * field "elevation": polar coordinate elevation in radians (4 byte, float32) - * field "t": time offset in nano seconds relative to the header timestamp in the point cloud (4 byte, uint32), used by rtabmap for deskewing - * field "ts": time offset in seconds relative to the header timestamp (4 byte, float32) - * field "ring": layer id (1 byte, int8), identical to field "layer" - * field "layer": layer (group) index (4 byte, int32), 0 <= layer < 16 for multiScan (16 layer), 0 for picoScan (1 layer) - * field "echo": echo index (4 byte, int32) - * field "reflector": optional reflector bit (1 byte, uint8), 0 or 1, default: 0 - -* Parameter "echos" defines which echos are included in the pointcloud, e.g. - * echos=0,1,2: all echos - * echos=2: last echo - or any other combination of 0,1,2 - -* Parameter "layers" defines which echos are included in the pointcloud, e.g - * layers=0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 for all layers - * layers=5 for the 0 degree layer - or any other combination of 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 - -* Parameter "reflectors" filters the points by the reflector bit, i.e. - * reflectors=0,1 for points with reflector bit set or not set - * reflectors=0 for points with reflector bit not set - * reflectors=1 for points with reflector bit set - -* Parameter "infringed" defines filters the points by infringement, i.e. - * infringed=0,1 for points with infringement bit set or not set - * infringed=0 for points with infringement bit not set - * infringed=1 for points with infringement bit set - Parameter "infringed" is currently not supported (reserved for future use) - -* Parameter "topic" defines the ros topic, e.g. topic=/cloud_fullframe for cartesian fullframe pointclouds - -* Parameter "frameid" defines the ros frame of the pointcloud, e.g. frameid=world, frameid=map or frameid=base_link - -* Parameter "publish" activates or deactivates the pointcloud, e.g. publish=1 to generate and publish, or publish=0 to deactivate that pointcloud - -To add a new pointcloud, define a pointcloud name (e.g. "cloud_layer7_cartesian"), add "cloud_layer7_cartesian" in parameter "custom_pointclouds" and specify a new parameter "cloud_layer7_cartesian" with the new cloud properties, e.g. -``` - - -``` - -The following pointclouds are currently predefined in launchfile [sick_multiscan.launch](../launch/sick_multiscan.launch): -``` - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -``` - -Note: The sick_scan_xd API callback functions `SickScanApiRegisterCartesianPointCloudMsg` and `SickScanApiRegisterPolarPointCloudMsg` provide cartesian and polar pointclouds, i.e. pointclouds configured with `coordinateNotation=0` (cartesian) or `coordinateNotation=1` (polar). Pointclouds with `coordinateNotation=2` (cartesian + polar) or `coordinateNotation=3` (customized fields) are currently not supported by the generic API. - -### Customized pointclouds on a Raspberry - -Performance is critical on a Raspberry. To reduce the cpu load, you may restrict the number of pointclouds to the minimum required for your application. E.g. if you just need the cartesian fullframe pointcloud, you can use -``` - -``` -to decrease the cpu usage. - -## 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 for multiScan100, 276 degree for picoScan100) - -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. - -The msgpack validation is configured in file [sick_multiscan.launch](../launch/sick_multiscan.launch) resp. [sick_picoscan.launch](../launch/sick_picoscan.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. - -## Firewall configuration - -By default, UDP communication is allowed on localhosts. To enable udp communication between 2 different machines, firewalls have to be configured. - -On Windows: Setup the windows firewall to allow sick_scan_xd to receive udp packages on port 2115. -To pass udp packages from a remote sender, the default rule for incoming udp packages has to be configured in the windows firewall: -1. Run "wf.msc" as admin, -2. Click Inbound Rules and locate the rule(s) for lidar3d_msr100_recv (resp. python to allow python test scripts), and -3. Deactivate the UDP-rule for this process(es) or configure exceptions for remote computers. -4. Alternatively, you can create a new rule allowing udp communication on port 2115. - -On Linux: Run the following commands to allow any udp communication on port 2115: -``` -sudo iptables -A INPUT -p udp -m udp --dport 2115 -j ACCEPT -sudo iptables -A OUTPUT -p udp -m udp --sport 2115 -j ACCEPT -sudo iptables-save -``` -Alternatively, you can also use -``` -sudo ufw allow from any to any port 2115 proto udp -``` -to allow all udp traffic on port 2115. - -Note: If Linux or Windows is running in a virtual machine, make sure UDP port 2115 is forwarded. With VMware Workstation Pro, you can configure port forwarding -using the Virtual Network Editor. Udp echos, delays, drops and other unexpected errors might occure when more than one network card is configured in VMware. -Make sure you have only one network adapter activated with custom NAT: -![vmware_network_settings](vmware_network_settings.png) - -## FAQ - -### Visual Studio: Breakpoints in Debug Mode disabled - -:question: In Windows debug version the compiler does not stop at breakpoints. - -:white_check_mark: Check, that you are using the Debug Version. At '/Zi' to compiler settings. Disable optimization. -(see `https://stackoverflow.com/questions/865546/generating-symbols-in-release-binaries-with-visual-studio` for details). - -### Packages lost in benchmark - -:question: sick_scan_xd seems to drop packages, when sending 10000 msgpacks with polarscan_sender_test.py from another computer - -:white_check_mark: There can be a number of reasons for dropped messages (udp or msgpacks). Besides slow network connection, there can be other pitfalls depending on the system: - -- If Linux or Windows is running in a virtual machine, make sure UDP port 2115 is forwarded. See [Firewall configuration](#firewall__configuration). - -- Depending on ROS2 system settings, log messages might be buffered. To really see all log messages of sick_generic_caller, terminate sick_scan_xd/sick_generic_caller (Ctrl-C or kill) and view the ros logfile by `cat ~/.ros/log/sick_scan_*.log` - -### Convert pcapng-files to msgpack or json - -:question: How can I convert a pcapng-file with scandata to a msgpack- or json-file? - -:white_check_mark: Run the following steps: -* Install python msgpack package with `pip install msgpack` -* 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 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_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_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_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_scansegment_xd_sopas_examples.md b/doc/sick_scansegment_xd_sopas_examples.md deleted file mode 100644 index f4fae51b..00000000 --- a/doc/sick_scansegment_xd_sopas_examples.md +++ /dev/null @@ -1,55 +0,0 @@ -# SOPAS support for sick_scan_segment_xd - -On ROS-1 and ROS-2, sick_scan_segment_xd provides ros service `ColaMsg` to send CoLa commands to the lidar. Using this service, filters can be applied to multiScan136 and picoScan150 lidars during runtime. - -Examples on ROS-1: -``` -rosservice list -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN IsSystemReady'}" # response: "sAN IsSystemReady 1" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115'}" # response: "sWA ScanDataEthSettings" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataFormat 1'}" # response: "sWA ScanDataFormat" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataPreformatting 1'}" # response: "sWA ScanDataPreformatting" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataEnable 1'}" # response: "sWA ScanDataEnable" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN LMCstartmeas'}" # response: "sAN LMCstartmeas" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN Run'}" # response: "sAN Run 1" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 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" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN FREchoFilter 1'}" # response: "sWA FREchoFilter" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1'}" # response: "sWA LFPangleRangeFilter" -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" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN Run'}" # response: "sAN Run 1" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" -rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 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" -``` - -Note: LFPlayerFilter and LFPangleRangeFilter are not supported by picoscan150. - -Examples on ROS-2: -``` -ros2 service list -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN IsSystemReady'}" # response: "sAN IsSystemReady 1" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115'}" # response: "sWA ScanDataEthSettings" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN ScanDataFormat 1'}" # response: "sWA ScanDataFormat" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN ScanDataPreformatting 1'}" # response: "sWA ScanDataPreformatting" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN ScanDataEnable 1'}" # response: "sWA ScanDataEnable" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN LMCstartmeas'}" # response: "sAN LMCstartmeas" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN Run'}" # response: "sAN Run 1" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN LFPlayerFilter'}" # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN SetAccessMode 3 F4724744'}" # response: "sAN SetAccessMode 1" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN FREchoFilter 1'}" # response: "sWA FREchoFilter" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1'}" # response: "sWA LFPangleRangeFilter" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sWN LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1'}" # response: "sWA LFPlayerFilter" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sMN Run'}" # response: "sAN Run 1" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN FREchoFilter'}" # response: "sRA FREchoFilter 1" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN LFPangleRangeFilter'}" # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1" -ros2 service call /ColaMsg sick_scan_xd/srv/ColaMsgSrv "{request: 'sRN LFPlayerFilter'}" # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" -``` - -Note: LFPlayerFilter and LFPangleRangeFilter are not supported by picoscan150. diff --git a/doc/slam.md b/doc/slam.md deleted file mode 100644 index 63d7bb94..00000000 --- a/doc/slam.md +++ /dev/null @@ -1,323 +0,0 @@ -# Slam - -## Table of contents - -- [Introduction](#introduction) -- [Principle](#measuring-principle) -- [NAV-350 ROS-1 SLAM example](#nav-350-ros-1-slam-example) -- [NAV-350 ROS-2 SLAM example](#nav-350-ros-2-slam-example) -- [picoScan ROS-1 SLAM example](#picoscan-ros-1-slam-example) -- [MRS-1104 SLAM support](#mrs-1104-slam-support) -- [Google Cartographer](#google-cartographer) -- [OctoMap](#octomap) -- [RTAB-Map](#rtab-map) - -## Introduction - -In robotic mapping and navigation, simultaneous localization and mapping (SLAM) is the computational problem of constructing -or updating a map of an unknown environment while simultaneously keeping track of an agent's location within it. -For further details please refer to https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping. - -## Measuring Principle - -The following assumes that the SLAM algorithm works with a laser scanner mounted on a mobile base. The mobile base (e.g. a robot) records the environment while driving and creates the map from it. The mobile base usually has a so-called intertial measurement unit (IMU). In principle, however, it is also possible to estimate the direction of movement from the chronological sequence of the laser scans by means of correlation observations. -The laser scanner then virtually takes over the task of the IMU and other components (e.g. counting the wheel revolutions). The method of estimating the position and orientation (position estimation) of a mobile system based on data from its driving system is called odometry (cf. https://en.wikipedia.org/wiki/Odometry). - -The SLAM algorithm hector_slam (http://wiki.ros.org/hector_slam) supports odometry estimation directly from the laser scans and is therefore used as a reference implementation in the following. - -Other widely used SLAM algorithms such as gmapping (cf. http://wiki.ros.org/gmapping ) do not have this option. They depend on the data of an IMU. One possibility to use Gmapping nevertheless is the integration of the project laser_scan_matcher (https://answers.ros.org/question/63457/gmapping-without-odom/ and http://wiki.ros.org/laser_scan_matcher ). Here, however, the pose must still be converted into an odometry message (see https://answers.ros.org/question/12489/obtaining-nav_msgsodometry-from-a-laser_scan-eg-with-laser_scan_matcher/ ). - -## NAV-350 ROS-1 SLAM example - -Build hector_slam and sick_scan_xd: -``` -cd src -git clone -b master https://github.com/SICKAG/sick_scan_xd.git -git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git -cd .. -catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DCMAKE_ENABLE_EMULATOR=1 -Wno-dev -``` - -Run rviz, sick_scan_xd with NAV-350 and hector_slam: -``` -source ./devel_isolated/setup.bash -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_slam_nav350.rviz & -roslaunch sick_scan sick_nav_350.launch hostname:=192.168.0.1 tf_publish_rate:=0 & -roslaunch sick_scan test_200_slam_ros1_hector.launch scan_topic:=/scan scan_layer_0_frame_id:=cloud_POS_000_DIST1 cloud_frame_id:=cloud & -``` - -**Note:** By default, sick_scan_xd publishes transform (TF) messages, which map frame id "map" to the point cloud frame id. To avoid conflicts with hector SLAM, it is recommended to disable these TF messages by commandline parameter **`tf_publish_rate:=0`** or by setting `` in the launchfile. - -The following rviz screenshot shows an example of a NAV-350 pointcloud created by sick_scan_xd and its map generated by hector_slam on ROS1: - -![slam_example_ros1_nav350.png](screenshots/slam_example_ros1_nav350.png) - -## NAV-350 ROS-2 SLAM example - -Install ths ROS2 slam-toolbox with `sudo apt install ros-foxy-navigation2 ros-foxy-nav2-bringup ros-foxy-slam-toolbox` (replace `foxy` by your ros distribution). - -Build sick_scan_xd for ROS-2 as described in [INSTALL-ROS2.md](../INSTALL-ROS2.md) - -Run rviz2, sick_scan_xd, slam_toolbox and static transforms: -``` -source ./install/setup.bash -ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_slam_nav350.rviz & -ros2 launch sick_scan sick_nav_350.launch.py hostname:=192.168.0.1 tf_publish_rate:=0 & -ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link cloud & -ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_footprint base_link & -ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 odom base_footprint & -ros2 launch nav2_bringup navigation_launch.py & -ros2 launch slam_toolbox online_async_launch.py & -``` - -Note: Laserscan messages need to be remapped to topic `/scan` (default is `/sick_nav_350/scan`). Use `remappings=[ ('/sick_nav_350/scan', '/scan'), ]` in the launchfile, e.g.: -``` -node = Node( - package='sick_scan', - executable='sick_generic_caller', - output='screen', - remappings=[ ('/sick_nav_350/scan', '/scan'), ], # remap laserscan messages to topic /scan -) -``` - -The following rviz2 screenshot shows an example of a NAV-350 laserscan created by sick_scan_xd and its map generated by slam_toolbox on ROS2: - -![slam_example_ros2_nav350.png](screenshots/slam_example_ros2_nav350.png) - -## picoScan ROS-1 SLAM example - -Run rviz, sick_scan_xd with picoScan and hector_slam: -``` -source ./devel_isolated/setup.bash -rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_slam_multiscan.rviz & -roslaunch sick_scan sick_nav_350.launch hostname:=192.168.0.1 hostname:=127.0.0.1 udp_receiver_ip:=192.168.0.100 tf_publish_rate:=0 & -roslaunch sick_scan test_200_slam_ros1_hector.launch scan_topic:=/sick_picoscan/scan_fullframe scan_layer_0_frame_id:=world_1 cloud_frame_id:=world & -``` -Replace ip address `192.168.0.100` with the ip address of your local machine running sick_scan_xd. - -The following rviz screenshot shows an example of a picoScan pointcloud created by sick_scan_xd and its map generated by hector_slam on ROS1: - -![slam_example_ros1_picoscan.png](screenshots/slam_example_ros1_picoscan.png) - - -## MRS-1104 SLAM support - -MRS-1104 provides 4 layers covering elevation angles at -2.5°, 0.0°, 2.5° and 5.0°. The layer with 0.0° is used for SLAM by default. - -The following rviz screenshot shows an example of a MRS1104 pointcloud created by sick_scan_xd and its map generated by hector_slam on ROS1: - -![slam_example_ros1_mrs1104.png](screenshots/slam_example_ros1_mrs1104.png) - -Since Hector-Slam expects only one laser scan frame with a unique identifier for the laser scans, the following parameters were added to the driver. - -slam_echo: The name of the echo is entered here, which is filtered out of all possible 12 echoes. This should be "laser_POS_000_DIST1". This exports the first hit in the position with an elevation angle of 0°. If you want to use the layers with elevation angles -2.5°, 2.5° and 5.0°, you can set another flag with the name slam_bundle to True. If this flag is set, the oblique distances are multiplied by the cosine in this direction to obtain the projection onto the XY plane. This quadruples the number of points and increases the scan rate from 12.5 Hz to 50 Hz. However, for oblique impact surfaces (i.e. no vertical walls) this method can lead to larger estimation errors. In this case slam_bundle should be set to false. - -## Google Cartographer - -The support of Google Cartographer was made possible by a number of extensions to the driver. On the driver side, the MRS1104 is prepared to support the Google Cartographer. The Google Cartographer expects data packets at a high recording density (several hundred packets per second) to perform the SLAM algorithm. For this reason, an option has been introduced that allows the scans to be chopped into small angular ranges. The time stamps for these small ranges were converted accordingly. You can find results and further information [here](./google_cartographer.md) - -## OctoMap - -[OctoMap](https://github.com/OctoMap) models a 3D occupancy map. The octomap_server builds and distributes volumetric 3D occupancy maps from a 3D point cloud. Tutorials and examples can be found e.g. in -[3D Mapping with OctoMap](https://www.arminhornung.de/Research/pub/hornung13roscon.pdf), -[octomap_tutorial](https://github.com/tejalbarnwal/octomap_tutorial) and -[Basic usage of octomap_mapping](https://www.youtube.com/watch?v=dF2mlKJqkUg). Note that OctoMap is not a fully SLAM algorithm, but it can create 2D and 3D maps from point clouds. - -Run the following steps to build and run OctoMap and sick_scan_xd with a multiScan lidar on ROS-1: -1. Clone OctoMap + sick_scan_xd: - ``` - pushd src - git clone https://github.com/SICKAG/sick_scan_xd.git - git clone https://github.com/OctoMap/octomap_ros.git - git clone https://github.com/OctoMap/octomap_msgs.git - git clone https://github.com/OctoMap/octomap_mapping.git - popd - ``` -2. Set topic and frame_id for multiScan in octomap_mapping.launch: - ``` - - - ``` -3. Build: - ``` - rm -rf ./build ./devel ./install ./build_isolated ./devel_isolated ./install_isolated ./log - catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -Wno-dev - ``` -4. Run OctoMap + sick_scan_xd: - ``` - # Run sick_scan_xd + multiScan - roslaunch sick_scan_xd sick_multiscan.launch hostname:="192.168.0.1" udp_receiver_ip:=" 192.168.0.100" - # Run octomap_server - roslaunch octomap_server octomap_mapping.launch - ``` - Replace parameter "hostname" with the ip address of the multiScan lidar and "udp_receiver_ip" with the ip address of the PC running sick_scan_xd. -5. Visualize OctoMap with rviz: - * Add MarkerArray topic "/occupied_cells_vis_array“ (colored voxels) - * Add Map topic "/projected_map“ (gray 2D Projection) -6. Save the OctoMap: - ``` - rosrun octomap_server octomap_saver -f ./octomap_multiscan.bt - ``` -7. Publish the saved OctoMap: - ``` - rosrun octomap_server octomap_server_node ./octomap_multiscan.bt - ``` -The following screenshot shows an example of an octomap created from a multiScan point cloud: - -![octomap_example_ros1_multiscan](screenshots/octomap_example_ros1_multiscan.png) - -## RTAB-Map - -[RTAB-Map](https://introlab.github.io/rtabmap/) (Real-Time Appearance-Based Mapping) is a RGB-D, Stereo and Lidar Graph-Based SLAM approach, which can be used for 3D-SLAM in combination with multiScan or other SICK lidars. sick_scan_xd provides a 3D-SLAM example using RTAB-Map with the multiScan lidar. The following section describes how to install and run RTAB-Map with sick_scan_xd and a multiScan. - -### Install on ROS-1 - -Run the following steps to build rtabmap and sick_scan_xd with on ROS-1: - -1. Build the prerequisites for RTAB-Map: - ``` - sudo apt-get install libboost-all-dev - sudo apt-get install libeigen3-dev - sudo apt-get install libsdl-image1.2-dev - sudo apt-get install libsdl-dev - sudo apt-get install ros-noetic-nav-msgs - sudo apt-get install ros-noetic-tf2-sensor-msgs - sudo apt-get install ros-noetic-imu-filter-madgwick - sudo apt-get install python3-wstool - sudo apt-get install ros-noetic-scan-tools - pushd /tmp - mkdir -p libnabo/build && pushd libnabo/build - cmake .. - cmake --build . - sudo cmake --build . --target install - popd - mkdir -p libpointmatcher/build && pushd libpointmatcher/build - cmake .. - make -j4 - sudo make install - popd - sudo ldconfig - mkdir -p rtabmap/build && pushd rtabmap/build - cmake .. - make -j4 - sudo make install - popd - sudo ldconfig - popd - ``` -2. Build RTAB-Map and sick_scan_xd in your workspace: - ``` - pushd src - git clone https://github.com/ros-planning/navigation.git - git clone https://github.com/ros-planning/navigation_msgs.git - git clone https://github.com/introlab/rtabmap_ros.git - git clone https://github.com/SICKAG/sick_scan_xd.git - popd - rm -rf ./build ./devel ./install ./build_isolated ./devel_isolated ./install_isolated ./log - catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -Wno-dev - sudo ldconfig - ``` - -Run `sudo ldconfig` if you encounter errors while loading shared libraries. - -Note that building rtabmap with libpointermatch is highly recommended. - -### Run RTAB-MAP and multiScan on ROS-1 - -[sick_multiscan_rtabmap.launch](../launch/sick_multiscan_rtabmap.launch) provides a launchfile to run 3D-SLAM using rtabmap and sick_scan_xd with a multiScan lidar. The SLAM parameters are just examples taken from [rtabmap_examples/launch/test_ouster.launch](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_examples/launch/test_ouster.launch). Run `rosrun rtabmap_slam rtabmap --params` to see all RTAB-Map options, parameters and their meaning and adopt launchfile [sick_multiscan_rtabmap.launch](../launch/sick_multiscan_rtabmap.launch) if required. - -Run the following command to start rtabmap and sick_scan_xd with a multiscan lidar: -``` -source ./install_isolated/setup.bash -roslaunch sick_scan_xd sick_multiscan_rtabmap.launch hostname:="191.168.0.1" udp_receiver_ip:=" 191.168.0.100" -``` -Replace parameter "hostname" with the ip address of the multiScan lidar and "udp_receiver_ip" with the ip address of the PC running sick_scan_xd. - -The following screenshot shows an example of RTAB-MAP and a multiScan point cloud: - -![rtabmap_example_ros1_multiscan](screenshots/rtabmap_example_ros1_multiscan.png) - -To visualize SLAM results, add e.g. topics `/rtabmap/grid_map`, `/rtabmap/localization_pose` and `/rtabmap/odom` in rviz. - -rtabmap provides services to switch to mapping or localization mode: -``` -rosservice call /rtabmap/resume # resume after pause -rosservice call /rtabmap/trigger_new_map # start a new map -rosservice call /rtabmap/set_mode_mapping # set mapping mode -rosservice call /rtabmap/set_mode_localization # set localization mode -``` - -Alternatively, you can use the options in rtabmap-viz: - -![rtabmap_viz_options](screenshots/rtabmap_viz_options.png) - -### Install on ROS-2 - -Building rtabmap and sick_scan_xd on ROS-2 is similar to ROS-1. Run the following steps to build rtabmap and sick_scan_xd with on ROS-2: - -1. Build the prerequisites for RTAB-Map: - ``` - sudo apt-get install libboost-all-dev - sudo apt-get install libeigen3-dev - sudo apt-get install libsdl-image1.2-dev - sudo apt-get install libsdl1.2-dev - sudo apt-get install ros-humble-nav-msgs - sudo apt-get install ros-humble-tf2-sensor-msgs - sudo apt-get install ros-humble-imu-filter-madgwick - sudo apt-get install python3-wstool - sudo apt-get install ros-humble-scan-tools - sudo apt install ros-humble-pcl-ros - pushd /tmp - git clone https://github.com/introlab/rtabmap.git rtabmap - git clone https://github.com/ethz-asl/libnabo.git libnabo - git clone https://github.com/ethz-asl/libpointmatcher.git libpointmatcher - mkdir -p libnabo/build && pushd libnabo/build - cmake .. - cmake --build . - sudo cmake --build . --target install - popd - mkdir -p libpointmatcher/build && pushd libpointmatcher/build - cmake .. - make -j4 - sudo make install - popd - sudo ldconfig - mkdir -p rtabmap/build && pushd rtabmap/build - cmake .. - make -j4 - sudo make install - popd - sudo ldconfig - popd - ``` -2. Build RTAB-Map and sick_scan_xd in your workspace: - ``` - pushd src - git clone --branch ros2 https://github.com/introlab/rtabmap_ros.git rtabmap_ros - git clone https://github.com/SICKAG/sick_scan_xd.git - popd - rosdep update && rosdep install --from-paths src --ignore-src -r -y - rm -rf ./build ./devel ./install ./build_isolated ./devel_isolated ./install_isolated ./log - colcon build --symlink-install --cmake-args " -DROS_VERSION=2" " -DCMAKE_ENABLE_EMULATOR=1" "-DCMAKE_BUILD_TYPE=Release" --event-handlers console_direct+ - sudo ldconfig - ``` - -### Run RTAB-MAP and multiScan on ROS-2 - -[sick_multiscan_rtabmap.launch.py](../launch/sick_multiscan_rtabmap.launch.py) provides a launchfile to run 3D-SLAM using rtabmap and sick_scan_xd with a multiScan lidar. The SLAM parameters are examples taken from [rtabmap_examples/launch/test_ouster.launch](https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_examples/launch/test_ouster.launch). - -Run the following command to start rtabmap and sick_scan_xd with a multiscan lidar: -``` -source ./install/setup.bash -ros2 launch sick_scan_xd sick_multiscan_rtabmap.launch.py hostname:="191.168.0.1" udp_receiver_ip:=" 191.168.0.100" -``` -Replace parameter "hostname" with the ip address of the multiScan lidar and "udp_receiver_ip" with the ip address of the PC running sick_scan_xd. - -rtabmap provides services to switch to mapping or localization mode: -``` -ros2 service call /rtabmap/resume std_srvs/srv/Empty # resume after pause -ros2 service call /rtabmap/trigger_new_map std_srvs/srv/Empty # start a new map -ros2 service call /rtabmap/set_mode_mapping std_srvs/srv/Empty # set mapping mode -ros2 service call /rtabmap/set_mode_localization std_srvs/srv/Empty # set localization mode -``` - diff --git a/doc/software_overview.md b/doc/software_overview.md deleted file mode 100644 index ae08d2d8..00000000 --- a/doc/software_overview.md +++ /dev/null @@ -1,128 +0,0 @@ -# Software Overview - -The sick_scan_xd software is essentially affected by its use cases: - -* Implement the common tasks for different lidars: - * Provide driver software on Linux and Windows, generic, ROS-1 and ROS-2 - * Receive and convert scan data, publish pointcloud - * Run startup, configuration and setup - -* Use cases: - * Provide a pointcloud to the customer/application - * Provide a common highlevel interface for all supported lidars - * Hide datagram details, encodings and parsing knowhow - * The most common use case is to run lidar + sick_scan_xd to get a pointcloud. - -* Software requirements: - * Support different lidars (LMS, LRS, LDMRS, MRS, NAV, TiM, RMS, multiScan, etc.) - * Support different OS (Linux, Windows) - * Support different targets (ROS-1, ROS-2, generic) - * Support different protocols (Cola-A, Cola-B, TCP, UDP, msgpack) - * Implement parser for different telegrams (scandata, scancfg, fields, etc.) - -This overview describes the most important modules and their relationship. - -## Software structure - -The following figures show the most important software blocks: - -![software_overview_01](software_overview_01.png) - -![software_overview_02](software_overview_02.png) - -sick_scan_xd contains 6 main functional blocks: - -* sick_generic_caller and sick_generic_laser for initialization and setup: - * Read configuration from launchfile: - * ROS1: `ros::NodeHandle::getParam` - * ROS2 and generic: `LaunchParser` (ros-wrapper) - * Lidar specific setup: - * class `sick_scan_xd::SickGenericParser`: lidar specific properties and messages parsing - * Set and get lidar specific properties: number of layers, angular resolution, etc. - * Parse and convert scan data, input: scan data (ascii or binary datagram), output: `ros::sensor_msgs::LaserScan` - * class `sick_scan_xd::SickScanCommonTcp`: receive TCP messages, convert and publish pointcloud - * Start ros services: - * class `sick_scan_xd::SickScanServices`: register ros services, convert from/to SOPAS - * Start monitoring: - * class `sick_scan_xd::SickScanMonitor`: monitor scan data, reinit on timeout - * class `sick_scan_xd::PointCloudMonitor`: monitor pointcloud, reinit on timeout -* sick_scan_common for the most common lidar devices (LMS, LRS, MRS, NAV, TiM, RMS, etc.): - * Implemention by SickScanCommon and SickScanCommonTcp - * Uses SickGenericParser for lidar specific properties and parsing - * Runs common tasks for LMS/LRS/MRS/NAV/TiM/RMS: - * Run SOPAS startup sequence - * Run TCP receiver thread - * Process telegrams: parse and convert to pointcloud - * Publish pointcloud -* sick_ldmrs for LDMRS support using the ldmrs-library from https://github.com/SICKAG/libsick_ldmrs.git -* sick_scansegment_xd for multiScan136 and picoScan150 lidars using SOPAS, msgpack and UDP-communication -* sick_scan_services for ros services -* sick_generic_monitoring for monitoring and re-initialization in case of errors (e.g. network errors). - -The following figures show these 6 functional blocks: - -![software_overview_03](software_overview_03.png) - -![software_overview_04](software_overview_04.png) - -The function blocks depend on and use the underlying system (ROS, TCP, etc.): - -![driver_components_01](driverComponentsDiagram1.png) - -## Message receiving and message handling - -Message receiving and message handling are decoupled, i.e. both tasks run in separate thread and exchange messages via a FIFO-buffer. This way, message handling cannot block tcp recv and vice versa. The following figure shows the message handling: - -![software_overview_05](software_overview_05.png) - -The following figure shows the sequence diagram for a LMDscandata telegram: - -![messageSequenceDiagram1](messageSequenceDiagram1.png) - -Incoming TCP messages and exported pointcloud messages are monitored. sick_scan_xd reinitialises the lidar and the tcp connection in case of timeouts. - -## sick_scansegment_xd - -sick_scansegment_xd implements support for multiScan136 and picoScan150 lidars using SOPAS, msgpack and UDP-communication. It has 5 functional blocks: - -* class `sick_scansegment_xd::MsgPackThreads`: - * Init and run all sick_scansegment_xd components - * SOPAS startup (multiScan136, picoScan150) -* class `sick_scansegment_xd::UdpReceiver`: - * Run UDP receiver thread -* class `sick_scansegment_xd::MsgPackConverter`: - * Parse and convert msgpacks - * Collect scan segments -* class `sick_scansegment_xd::MsgPackValidator`: - * Validate msgpacks and scansegments -* class `sick_scansegment_xd::RosMsgpackPublisher`: - * Publish pointcloud (single segments) - * Publish cloud_fullframe (fullframe pointcloud, 360 deg for Multiscan136 resp. 270 deg for picoscan) - -The following figure shows the compoenent diagram for sick_scansegment_xd: - -![driverComponentsDiagram2](driverComponentsDiagram2.png) - -Message receiving, converting and publishing run in 3 separate threads and exchange their messages via a FIFO-buffer. - -The following figure shows the sequence diagram for a multiScan136 msgpacks: - -![messageSequenceDiagram2](messageSequenceDiagram2.png) - -## Files and folders - -The source files for the sick_scan_xd core can be found in the following folders: -* driver/src: source files -* include: header files -* launch: configuration -* msg: ros messages definitions -* srv: ros services definitions -* roswrap: ros wrapper (ROS-2 and generic) - -These folders are required to build sick_generic_caller. - -Besides [README.md](../README.md) and [CHANGELOG.md](../CHANGELOG.md), all documentation can be found in the doc folder. - -Additional folders for sick_scan_xd support, development and test are: -* test: test scripts and emulator -* tools: additional development tools diff --git a/doc/software_overview_01.png b/doc/software_overview_01.png index 89d5325c..1115bc99 100644 Binary files a/doc/software_overview_01.png and b/doc/software_overview_01.png differ diff --git a/doc/software_overview_03.png b/doc/software_overview_03.png index 99e15b6f..140b6845 100644 Binary files a/doc/software_overview_03.png and b/doc/software_overview_03.png differ diff --git a/doc/software_overview_04.png b/doc/software_overview_04.png index 7084a60f..2afabf40 100644 Binary files a/doc/software_overview_04.png and b/doc/software_overview_04.png differ diff --git a/doc/software_pll.md b/doc/software_pll.md deleted file mode 100644 index 55b792ac..00000000 --- a/doc/software_pll.md +++ /dev/null @@ -1,77 +0,0 @@ -# SoftwarePLL - -Software PLL for synchronisation between ticks and timestamps. - -## Introduction - -Many sensor devices, e.g. lidar devices, provide sensor data with timestamps. These timestamps can be synchronized with -the current system time by additional hardware, e.g. by GPS. But without specialized hardware, sensor timestamps and -system time is normally unsynchronized. Sensor timestamps are often quite accurate, but have a different time base and -a bias to the system time or to other sensor clocks. This difference is estimated and compensated by this Software PLL. - -To compensate the different time base and bias between sensor and system clock, the system time when receiving sensor data -is gathered together with the sensor timestamp. While the system time is often measured in seconds resp. nanoseconds, -the sensor timestamp is normally received in clock ticks. SoftwarePLL estimates the correlation between system time in -secondsnanoseconds and sensor ticks, and computes a corrected time from ticks. This way you know at which time stamp the data -have been measured by your sensor. - -SoftwarePLL is a generic module and independant from specific sensor types. It just uses the system timestamps and ticks, -estimates their correlation and predicts the time from sensor ticks. See [timing.md](timing.md) for an application -example using SICK laser scanner. - -## How Software PLL works - -SoftwarePLL computes a linear regression between ticks and system timestamps. The system time is measured immediately -after receiving new sensor data, while sensor ticks represent the sensor clock at the time of measurement. Thus we have -three different times for each measurement - -- The time when the system receives the sensor data (receive time t_rec), measured in seconds resp. nanoseconds. - -- The sensor ticks (or just ticks) at the time of the measurement. These ticks are contained in the sensor data and -received later by the system. - -- The time of the measurement (measurement time t_mea). We don't know this time yet, but we estimate it from both the ticks -and their receive time t_rec using the SoftwarePLL. - -During initialization, ticks and system timestamps are stored in fifo buffer (first-in, first-out). After initialization, -typically after N=7 measurements, a regression line is computed, i.e. the slope `m` (gradient) of a function -`f(ticks) = m ticks + c` is estimated from ticks `x(i)` and timestamps `y(i)` by a linear regression -`m = (N sum(x(i) y(i)) - sum(x(i)) sum(y(i))) (N sum(x(i) x(i)) - sum(x(i))sum(x(i)))` with `0 = i N` and -unbiased values `x(i) = tick(i) - tick(0)`, `y(i) = t_rec(i) - t_rec(0)`. - -![pll_regression.png](pll_regression.png) - -The estimated system time `t_esti(i)` of a measurement can be computed from its sensor tick by `t_esti(i) = m (ticks(i) - ticks(0)) + t_rec(0)`. -If the difference between estimated times `t_esti(i)` and the measured system timestamps `t_rec(i)` is small (typically -less than 100 milliseconds), the estimation can be considered to be valid. With a valid estimation of `m`, we can -get a corrected timestamp for new measurements by applying function `SoftwarePLLGetCorrectedTimeStamp`, which returns -the estimated system time of a measurement `t_esti = m (ticks - ticks(0)) + t_rec(0)`. - -If the estimation is not valid (i.e. the difference between estimated times and measured system timestamps in the buffer is -significant), we can't estimate system timestamps from sensor ticks. If this happens more than a given number of times -after initialization (typically 20 times), the fifo is reset and a new initialization is done. - -## Source code example - -Use the following code snippet as an example - -``` -#include softwarePLL.h - - Create an instance of SoftwarePLL -SoftwarePLL& software_pll = SoftwarePLLInstance(Sensor1); - - Get system time t_rec in seconds and nanoseconds when receiving sensor data -rosTime t_rec = rosTimenow(); -uint32_t sec = t_rec.nsec; -uint32_t nanosec = t_rec.nsec; - - Get sensor ticks from sensor data -uint32_t ticks = scanner_msg.ticks; - - Update SoftwarePLL -software_pll.UpdatePLL(sec, nanosec, ticks); - - Get corrected timestamp (time of measurement from ticks) -software_pll.GetCorrectedTimeStamp(sec, nanosec, ticks); -``` diff --git a/doc/startup_demo.md b/doc/startup_demo.md deleted file mode 100644 index 42dbca0a..00000000 --- a/doc/startup_demo.md +++ /dev/null @@ -1,126 +0,0 @@ -# Startup Sequence - -## Table of contents - -- [Introduction](#introduction) -- [Quick Check of Firmware](#quick-check-of-firmware) -- [Startup Sequence](#startup-sequence) - -## Introduction - -The following ROS boot protocol shows the typical start sequence when starting a SICK laser scanner. The MRS6124 is shown here as an example. However, the startup sequence is generally similar for all scanners. - -## Quick Check of firmware - -After a firmware update, the following Quickcheck is performed: - -1. Is the device accessible via ping? -2. Can the device be started with the corresponding generic launch file? -3. At the end of the launch process, the device switches to receive mode - for scan data? Typically the last command sent is ```sEA LMDscandata \x01```. -4. Check with rviz: Is it possible to see the Pointcloud2 data or similar? Is the shown data reasonable? -5. Check the scan rate with the command -``` -rostopic hz /cloud -``` - -6. Further inspection, if any, by dumping Pointcloud2 data. -The header is of particular interest here. A typical call can therefore look as follows: -``` -rostopic echo /cloud|grep frame -B 7 -A 26 -``` - - -## Startup Sequence -``` - -roslaunch sick_scan_xd sick_mrs_6xxx.launch hostname:=192.168.0.25 -... logging to /home/rosuser/.ros/log/75631922-6109-11e9-b76f-54e1ad2921b6/roslaunch-ROS-NB-10680.log -Checking log directory for disk usage. This may take awhile. -Press Ctrl-C to interrupt -Done checking log file disk usage. Usage is <1GB. - -started roslaunch server http://ROS-NB:40757/ - -SUMMARY -======== - -PARAMETERS - * /rosdistro: melodic - * /rosversion: 1.14.3 - * /sick_mrs_6xxx/filter_echos: 0 - * /sick_mrs_6xxx/hostname: 192.168.0.25 - * /sick_mrs_6xxx/max_ang: 1.047197333 - * /sick_mrs_6xxx/min_ang: -1.040216 - * /sick_mrs_6xxx/port: 2112 - * /sick_mrs_6xxx/range_max: 250.0 - * /sick_mrs_6xxx/range_min: 0.1 - * /sick_mrs_6xxx/scanner_type: sick_mrs_6xxx - * /sick_mrs_6xxx/timelimit: 5 - * /sick_mrs_6xxx/use_binary_protocol: True - -NODES - / - sick_mrs_6xxx (sick_scan_xd/sick_generic_caller) - -auto-starting new master -process[master]: started with pid [10690] -ROS_MASTER_URI=http://localhost:11311 - -setting /run_id to 75631922-6109-11e9-b76f-54e1ad2921b6 -process[rosout-1]: started with pid [10701] -started core service [/rosout] -process[sick_mrs_6xxx-2]: started with pid [10708] -[ INFO] [1555502887.036684738]: sick_generic_caller V. 001.003.016 -[ INFO] [1555502887.036717573]: Program arguments: /home/rosuser/ros_catkin_ws/devel/lib/sick_scan_xd/sick_generic_caller -[ INFO] [1555502887.036725741]: Program arguments: __name:=sick_mrs_6xxx -[ INFO] [1555502887.036731933]: Program arguments: __log:=/home/rosuser/.ros/log/75631922-6109-11e9-b76f-54e1ad2921b6/sick_mrs_6xxx-2.log -[ INFO] [1555502887.048425000]: Found sopas_protocol_type param overwriting default protocol: -[ INFO] [1555502887.048956468]: Binary protocol activated -[ INFO] [1555502887.048984179]: Start initialising scanner [Ip: 192.168.0.25] [Port: 2112] -[ INFO] [1555502887.067528995]: Publishing laserscan-pointcloud2 to cloud -[ INFO] [1555502887.071035827]: Parameter setting for -[ INFO] [1555502887.271739084]: Sending : sMN SetAccessMode 0x03 0xf4 0x72 0x47 0x44 CRC:<0xb3> -[ INFO] [1555502887.273290840]: Receiving: sAN SetAccessMode \x01 -[ INFO] [1555502887.473927858]: Sending : sWN EIHstCola 0x01 CRC:<0x09> -[ INFO] [1555502887.475365983]: Receiving: sWA EIHstCola -[ INFO] [1555502887.675864993]: Sending : sMN LMCstopmeas CRC:<0x10> -[ INFO] [1555502888.199590269]: Receiving: sAN LMCstopmeas \x00 -[ INFO] [1555502888.400030148]: Sending : sRN DeviceIdent CRC:<0x25> -[ INFO] [1555502888.401393378]: Receiving: sRA DeviceIdent \x00\x08\x4d\x52\x53\x36\x31\x32\x34\x52\x00\x0a\x31\x2e\x31\x2e\x30\x2e\x35\x36\x35\x43 -[ INFO] [1555502888.401653485]: Deviceinfo MRS6124R V1.1.0.565C found and supported by this driver. -[ INFO] [1555502888.602062286]: Sending : sRN FirmwareVersion CRC:<0x24> -[ INFO] [1555502888.603444526]: Receiving: sRA FirmwareVersion \x00\x0a\x31\x2e\x31\x2e\x30\x2e\x35\x36\x35\x43 -[ INFO] [1555502888.804094446]: Sending : sRN SCdevicestate CRC:<0x30> -[ INFO] [1555502888.805521867]: Receiving: sRA SCdevicestate \x01 -[ INFO] [1555502889.006161400]: Sending : sRN ODoprh CRC:<0x41> -[ INFO] [1555502889.007613972]: Receiving: sRA ODoprh \x00\x00\x19\xf1 -[ INFO] [1555502889.209949897]: Sending : sRN ODpwrc CRC:<0x52> -[ INFO] [1555502889.211413041]: Receiving: sRA ODpwrc \x00\x00\x02\x55 -[ INFO] [1555502889.413742132]: Sending : sRN LocationName CRC:<0x55> -[ INFO] [1555502889.415205992]: Receiving: sRA LocationName \x00\x0b\x6e\x6f\x74\x20\x64\x65\x66\x69\x6e\x65\x64 -[ INFO] [1555502889.417205292]: Sending : sRN LMPoutputRange CRC:<0x5e> -[ INFO] [1555502889.418631134]: Receiving: sRA LMPoutputRange \x00\x01\x00\x00\x05\x15\x00\x04\xa3\x80\x00\x16\xe3\x60 -[ INFO] [1555502889.418830949]: Angle resolution of scanner is 0.13010 [deg] (in 1/10000th deg: 0x515) -[ INFO] [1555502889.418907556]: MIN_ANG: -1.040 [rad] -59.600 [deg] -[ INFO] [1555502889.418975818]: MAX_ANG: 1.047 [rad] 60.000 [deg] -[ INFO] [1555502889.419156102]: Sending : sWN LMPoutputRange 0x00 0x01 0x00 0x00 0x05 0x15 0x00 0x04 0xa3 0x80 0x00 0x16 0xe3 0x60 CRC:<0xd8> -[ INFO] [1555502889.420488646]: Receiving: sWA LMPoutputRange -[ INFO] [1555502889.420719836]: Sending : sRN LMPoutputRange CRC:<0x5e> -[ INFO] [1555502889.421994443]: Receiving: sRA LMPoutputRange \x00\x01\x00\x00\x05\x15\x00\x04\xa3\x80\x00\x16\xe3\x60 -[ INFO] [1555502889.422165198]: Angle resolution of scanner is 0.13010 [deg] (in 1/10000th deg: 0x515) -[ INFO] [1555502889.424815945]: MIN_ANG (after command verification): -1.040 [rad] -59.600 [deg] -[ INFO] [1555502889.424901901]: MAX_ANG (after command verification): 1.047 [rad] 60.000 [deg] -[ INFO] [1555502889.425102725]: Sending : sWN LMDscandatacfg 0x1f 0x00 0x01 0x01 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x01 CRC:<0x5c> -[ INFO] [1555502889.426373088]: Receiving: sWA LMDscandatacfg -[ INFO] [1555502889.426606493]: Sending : sRN LMDscandatacfg CRC:<0x67> -[ INFO] [1555502889.427933309]: Receiving: sRA LMDscandatacfg \x1f\x00\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x01 -[ INFO] [1555502889.430654546]: Sending : sWN FREchoFilter 0x00 CRC:<0x7f> -[ INFO] [1555502889.431952374]: Receiving: sWA FREchoFilter -[ INFO] [1555502889.432180430]: Sending : sMN LMCstartmeas CRC:<0x68> -[ INFO] [1555502889.963840302]: Receiving: sAN LMCstartmeas \x00 -[ INFO] [1555502889.964083670]: Sending : sMN Run CRC:<0x19> -[ INFO] [1555502889.965558914]: Receiving: sAN Run \x01 -[ INFO] [1555502889.965813465]: Sending : sEN LMDscandata 0x01 CRC:<0x33> -[ INFO] [1555502889.967297195]: Receiving: sEA LMDscandata \x01 -``` diff --git a/doc/tim240/tim240.md b/doc/tim240/tim240.md deleted file mode 100644 index 31b7c24c..00000000 --- a/doc/tim240/tim240.md +++ /dev/null @@ -1,22 +0,0 @@ -# TiM240 -## Table of contents - -- [Introduction](#introduction) -- [Scan Area](#scan-area) - -## Introduction - -The TiM240 is a new scanner that fits seamlessly into the family of other TiM devices. The TiM240 has an opening angle of 240 degrees. In contrast to the previous scanners from SICK, the coordinate system used corresponds directly to the ROS convention. For this reason, this scanner does not require a coordinate conversion of 90 degrees around the Z-axis. However, this is taken into account in the driver code, so that the user will not notice any difference in the setting of the angular ranges during use. -The angular position according to the data sheet can be taken from the drawings below. - -## Scan Area - -The following figures show the difference between the TiM5xx family and the TiM240 device. -![TiM5xx scan area](tim_5xx_scanarea.jpg) TiM 5xx scanning area -![TiM240 scan area](tim_240_scanarea.jpg) TiM 240 scanning area - - - - - - diff --git a/doc/tim7xxs_screenshot02.png b/doc/tim7xxs_screenshot02.png index 0e28ac00..1e6d1481 100644 Binary files a/doc/tim7xxs_screenshot02.png and b/doc/tim7xxs_screenshot02.png differ diff --git a/doc/timestamping.md b/doc/timestamping.md deleted file mode 100644 index b484357e..00000000 --- a/doc/timestamping.md +++ /dev/null @@ -1,30 +0,0 @@ -# Timestamping - -Often there is a requirement that the time stamp of the measurements should be calculated for each individual shot. -This article explains some background information about the determination of these time stamps. Here the statements refer to the LMS511. However, they can be transferred to other lidars using the same logic. - -![](lms511_scan.png) - - - -The lidar sends a pulsed beam onto a rotating mirror. Since the speed of rotation is relatively low, this mirror serves as transmitter and receiver. - - -The direction of rotation can be seen in the drawing. - - -25 scans per second means that the mirror makes 25 360° rounds per second. The actual laser unit is only active during the 190°. Therefore the so-called duty cycle is 190/360. The rotation is unaccelerated, so that the lidar arrives at the same angular direction again after 40 ms (1/25). From the angular distance from shot to shot you can calculate the pulse rate of the laser. For example, if the angular difference from shot to shot is 0.1 degree, the so-called shot rate would be: 360/0.1 * 25 shots/second. - - -A scan means the group of all shots during one revolution. Just imagine the lidar as a lighthouse that rotates evenly and measures the distance values over time of flight in a certain sector (here 190°) with the above mentioned shot rate. Also you find some background material in the documentation of SICK. - - -In the transmission protocol of the lidar two points in time are given in so-called ticks (resolution in microseconds): -a) Start of the scans in ticks -b) Start of IP data transfer from lidar to PC in ticks - -It is assumed that the transfer between lidar and PC is near latency-free. -The software PLL generates an assignment between the tick of the IP data transmission and the system time of the PC via an estimated line mapping. On the basis of this straight line equation, the start of the scan is then calculated relative to the system time. This generation time stamp is the time of the first shot of the scan. This timestamp is assigned to the pointcloud timestamp. From there, the rotation speed and the angular distance from shot to shot can be used to approximately calculate the time for each shot. - - -More details can be found [here](https://github.com/SICKAG/sick_scan_xd/blob/master/doc/timing.md) diff --git a/doc/timing.md b/doc/timing.md deleted file mode 100644 index 4986e4a9..00000000 --- a/doc/timing.md +++ /dev/null @@ -1,19 +0,0 @@ -## Time synchronization - -Many laser scanner applications require accurate timing down to the level of individual points, so time synchronization is implemented in this ROS driver. - The scanner has an internal time base of microseconds since system startup. Against this "tick" time base all time stamps are made in the scanner. - When sending messages from the scanner, two time stamps are added: -1. scan generation ticks--> timestamp at the time of the first shot -2. scan transmission ticks--> time stamp for the transmission of the data - -When data packets are received, they are timestamped by the driver against the systemtime in ros::time format. See following Fig. - -![timing_syn.png](timing_sync.png) -The relationship between system time and ticks is then derived from the time stamps and kept synchronous.The time required for the transmission of data over the network is assumed to be short and constant and is therefore neglected. -The function of the algorithms is shown in the following Fig. -![sequence_time_pll.png](sequence_time_pll.png) - -# Data buffering in MRS 1xxx - -Due to their construction the MRS 1xxx scanners generate different layers at the same time which are output sequentially by the scanner firmware. In order to ensure that only point cloud messages that follow one another in time are sent, buffering can be activated in the driver. -![scannbuffering.png](scannbuffering.png) diff --git a/driver/src/sick_generic_field_mon.cpp b/driver/src/sick_generic_field_mon.cpp index 403620ae..34c2a8d3 100644 --- a/driver/src/sick_generic_field_mon.cpp +++ b/driver/src/sick_generic_field_mon.cpp @@ -59,6 +59,7 @@ #include #include +#include "sick_scan/sick_scan_messages.h" namespace sick_scan_xd { @@ -164,8 +165,6 @@ namespace sick_scan_xd SickScanFieldMonSingleton::SickScanFieldMonSingleton() { this->monFields.resize(48); - // just for debugging, but very helpful for the start - this->active_mon_fieldset = 0; } /*! @@ -185,18 +184,66 @@ namespace sick_scan_xd \param datagramm: Pointer to datagram data \param datagram_length: Number of bytes in datagram */ - int SickScanFieldMonSingleton::parseBinaryLIDinputstateMsg(unsigned char* datagramm, int datagram_length) + int SickScanFieldMonSingleton::parseBinaryLIDinputstateMsg(unsigned char* datagramm, int datagram_length, sick_scan_msg::LIDinputstateMsg& inputstate_msg) { int exitCode=ExitSuccess; if(datagram_length > 36) { + // Read N output states + int number_of_input_states = 4; // LMS5xx, TiM7xx and TiM7xxS have 4 digital inputs (M12 connector) + inputstate_msg.header.stamp = rosTimeNow(); + inputstate_msg.input_state.clear(); + inputstate_msg.input_state.reserve(number_of_input_states); int fieldset = 0; for(int offset = 35; offset >= 32; offset--) // datagramm[32]=INT1, datagramm[33]=INT2, datagramm[34]=INT3, datagramm[35]=INT4 { fieldset = (fieldset << 1); - fieldset |= ((datagramm[offset] != 0) ? 1 : 0); + fieldset |= ((datagramm[offset] == 1) ? 1 : 0); + inputstate_msg.input_state.push_back(datagramm[offset]); + } + fieldset += 1; // FieldSet in range 1 to 16, i.e. 0x00000000 is fieldset 1, 0x00000001 is fieldset 2, ..., 0x01000000 is fieldset 9, ..., 0x01010101 is fieldset 16 + if (getFieldSelectionMethod() == 1) // ActiveFieldset from telegram + { + fieldset = getActiveFieldset(); + } + else // default: ActiveFieldset from LIDinputstate + { + setActiveFieldset(fieldset); + } + inputstate_msg.active_fieldset = fieldset; + // Read 2 byte version + 4 byte system counter + int msg_start_idx = 26; // start after 4 byte STX + payload length + "... LIDinputstate " + datagramm += msg_start_idx; + datagram_length -= msg_start_idx; + if( !readBinaryBuffer(datagramm, datagram_length, inputstate_msg.version_number) + || !readBinaryBuffer(datagramm, datagram_length, inputstate_msg.system_counter)) + { + ROS_ERROR_STREAM("## ERROR parseBinaryLIDinputstateMsg(): error parsing version_number and system_counter (" << __FILE__ << ":" << __LINE__ << ")"); + return ExitError; + } + // Read timestamp state + datagramm += 4; + datagram_length -= 4; + if( !readBinaryBuffer(datagramm, datagram_length, inputstate_msg.time_state)) + { + ROS_ERROR_STREAM("## ERROR parseBinaryLIDinputstateMsg(): error parsing time_state (" << __FILE__ << ":" << __LINE__ << ")"); + return ExitError; + } + // Read optional timestamp + if(inputstate_msg.time_state > 0) + { + if(!readBinaryBuffer(datagramm, datagram_length, inputstate_msg.year) + || !readBinaryBuffer(datagramm, datagram_length, inputstate_msg.month) + || !readBinaryBuffer(datagramm, datagram_length, inputstate_msg.day) + || !readBinaryBuffer(datagramm, datagram_length, inputstate_msg.hour) + || !readBinaryBuffer(datagramm, datagram_length, inputstate_msg.minute) + || !readBinaryBuffer(datagramm, datagram_length, inputstate_msg.second) + || !readBinaryBuffer(datagramm, datagram_length, inputstate_msg.microsecond)) + { + ROS_ERROR_STREAM("## ERROR parseBinaryLIDinputstateMsg(): error parsing timestamp (" << __FILE__ << ":" << __LINE__ << ")"); + return ExitError; + } } - setActiveFieldset(fieldset); } else { @@ -205,6 +252,49 @@ namespace sick_scan_xd return exitCode; } + /*! + \brief Parse binary ActiveFieldSet response "sRA ActiveFieldSet" + \param datagramm: Pointer to datagram data + \param datagram_length: Number of bytes in datagram + */ + void SickScanFieldMonSingleton::parseActiveFieldSetResponse(unsigned char* datagram, int datagram_length, uint16_t* active_field_set) + { + // Example: \x02\x02\x02\x02\x00\x00\x00\x15sRA ActiveFieldSet \x00\x01 + int arg_start_idx = 27; // start after 4 byte STX + 4 byte payload length + 19 byte "sRA ActiveFieldSet " + datagram += arg_start_idx; + datagram_length -= arg_start_idx; + readBinaryBuffer(datagram, datagram_length, *active_field_set); + } + + /*! + \brief Parse binary FieldSetSelectionMethod response "sRA FieldSetSelectionMethod" + \param datagramm: Pointer to datagram data + \param datagram_length: Number of bytes in datagram + */ + void SickScanFieldMonSingleton::parseFieldSetSelectionMethodResponse(unsigned char* datagram, int datagram_length, uint8_t* field_set_selection_method) + { + int arg_start_idx = 36; // start after 4 byte STX + 4 byte payload length + 28 byte "sRA FieldSetSelectionMethod " + datagram += arg_start_idx; + datagram_length -= arg_start_idx; + readBinaryBuffer(datagram, datagram_length, *field_set_selection_method); + } + + /*! + \brief Converts a LIDinputstateMsg to a readable string + */ + std::string SickScanFieldMonSingleton::LIDinputstateMsgToString(const sick_scan_msg::LIDinputstateMsg& inputstate_msg) + { + std::stringstream state_str; + state_str << "LIDinputstateMsg = { " << "version_number: " << (uint32_t)inputstate_msg.version_number << ", system_counter: " << (uint32_t)inputstate_msg.system_counter << ", states: ("; + for(int state_cnt = 0; state_cnt < inputstate_msg.input_state.size(); state_cnt++) + state_str << (state_cnt > 0 ? ", " :"") << (uint32_t)inputstate_msg.input_state[state_cnt]; + state_str << "), active_fieldset: " << inputstate_msg.active_fieldset << ", time state: " << (uint32_t)inputstate_msg.time_state + << ", date: " << std::setfill('0') << std::setw(4) << (uint32_t)inputstate_msg.year << "-" << std::setfill('0') << std::setw(2) << (uint32_t)inputstate_msg.month << "-" << std::setfill('0') << std::setw(2) << (uint32_t)inputstate_msg.day + << ", time: " << std::setfill('0') << std::setw(2) << (uint32_t)inputstate_msg.hour << ":" << std::setfill('0') << std::setw(2) << (uint32_t)inputstate_msg.minute << ":" << std::setfill('0') << std::setw(2) << (uint32_t)inputstate_msg.second + << "." << std::setfill('0') << std::setw(6) << (uint32_t)inputstate_msg.microsecond << " }"; + return state_str.str(); + } + /*! \brief Parsing Ascii datagram \param datagram: Pointer to datagram data diff --git a/driver/src/sick_generic_laser.cpp b/driver/src/sick_generic_laser.cpp index b4b904f8..f2e28fac 100644 --- a/driver/src/sick_generic_laser.cpp +++ b/driver/src/sick_generic_laser.cpp @@ -475,7 +475,7 @@ void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNo std::string cloud_topic = "cloud"; rosDeclareParam(nhPriv, "hostname", "192.168.0.4"); - rosDeclareParam(nhPriv, "imu_enable", true); + rosDeclareParam(nhPriv, "imu_enable", false); rosDeclareParam(nhPriv, "imu_topic", "imu"); rosDeclareParam(nhPriv, "cloud_topic", cloud_topic); if (doInternalDebug) @@ -485,7 +485,7 @@ void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNo rossimu_settings(*nhPriv); // just for tiny simulations under Visual C++ #else rosSetParam(nhPriv, "hostname", "192.168.0.4"); - rosSetParam(nhPriv, "imu_enable", true); + rosSetParam(nhPriv, "imu_enable", false); rosSetParam(nhPriv, "imu_topic", "imu"); rosSetParam(nhPriv, "cloud_topic", "cloud"); #endif @@ -560,6 +560,7 @@ void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNo // Optional timestamp mode: // TICKS_TO_SYSTEM_TIMESTAMP = 0, // default: convert lidar ticks in microseconds to system timestamp by software-pll // TICKS_TO_MICROSEC_OFFSET_TIMESTAMP = 1 // optional tick-mode: convert lidar ticks in microseconds to timestamp by 1.0e-6*(curtick-firstTick)+firstSystemTimestamp; + // TICKS_TO_LIDAR_TIMESTAMP = 2 // optional tick-mode: convert lidar ticks in microseconds to lidar timestamp by sec = tick/1000000, nsec = 1000 * (tick % 1000000) int tick_to_timestamp_mode = 0; rosDeclareParam(nhPriv, "tick_to_timestamp_mode", tick_to_timestamp_mode); rosGetParam(nhPriv, "tick_to_timestamp_mode", tick_to_timestamp_mode); diff --git a/driver/src/sick_generic_parser.cpp b/driver/src/sick_generic_parser.cpp index f1e752af..437b4f01 100644 --- a/driver/src/sick_generic_parser.cpp +++ b/driver/src/sick_generic_parser.cpp @@ -1380,18 +1380,6 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported) // 9: Time since startup (eg. 13C8E59) // 10: Time of transmission (eg. 13C9CBE) // 11 + 12: Input status (0 0), active fieldset - /* - unsigned short u16_active_fieldset = 0; - if(sscanf(fields[12], "%hx", &u16_active_fieldset) == 1) - { - SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); - if(fieldMon) - { - fieldMon->setActiveFieldset(u16_active_fieldset & 0xFF); - ROS_INFO("Scandata: active_fieldset = %d", fieldMon->getActiveFieldset()); - } - } - */ // 13 + 14: Output status (8 0) // 15: Reserved Byte A (0) diff --git a/driver/src/sick_generic_radar.cpp b/driver/src/sick_generic_radar.cpp index 328781fe..351d2ca3 100644 --- a/driver/src/sick_generic_radar.cpp +++ b/driver/src/sick_generic_radar.cpp @@ -922,6 +922,7 @@ namespace sick_scan_xd } bool entriesNumOk = true; + bool allow_missing_keywords = false; int entriesNum = 0; if (keyWordPos[0] == -1) { @@ -936,27 +937,32 @@ namespace sick_scan_xd if (keyWordPos[i] == -1) { entriesNumOk = false; - ROS_WARN_STREAM("Missing keyword " << keyWordList[i] << " but first keyword found.\n"); + ROS_WARN_STREAM("parseRadarDatagram(): " << (i + 1) << ". keyword " << keyWordList[i] << " missing, but first keyword " << keyWordList[0] << " found, value set to 0.0"); entriesNumOk = false; + allow_missing_keywords = true; // P3DY1 and V3DY1 set to 0 if missing } else { int entriesNumTmp = (int)radarFieldToInt32(fields[keyWordPos[i] + 3], useBinaryProtocol); // getHexValue(fields[keyWordPos[i] + 3].data); if (entriesNumTmp != entriesNum) { - ROS_WARN_STREAM("Number of items for keyword " << keyWordList[i] << " differs from number of items for " << keyWordList[0] << "\n."); + ROS_WARN_STREAM("parseRadarDatagram(): Number of items for keyword " << keyWordList[i] << " differs from number of items for " << keyWordList[0]); entriesNumOk = false; } } } } - if (true == entriesNumOk) + if (true == entriesNumOk || true == allow_missing_keywords) { for (int i = 0; i < numKeyWords; i++) { + if (true == allow_missing_keywords && keyWordPos[i] == -1) + { + continue; // keyword is missing + } int scaleLineIdx = keyWordPos[i] + 1; int scaleOffsetLineIdx = keyWordPos[i] + 2; // std::string token = radarFieldToString(fields[scaleLineIdx], useBinaryProtocol); // fields[scaleLineIdx].data; @@ -992,6 +998,10 @@ namespace sick_scan_xd int mode = 0; for (int j = 0; j < numKeyWords; j++) { + if (true == allow_missing_keywords && keyWordPos[j] == -1) + { + continue; // keyword is missing + } int dataRowIdx = keyWordPos[j] + 4 + i; std::string token = keyWordList[j]; if (token.compare(DIST1_KEYWORD) == 0) @@ -1039,6 +1049,10 @@ namespace sick_scan_xd for (int j = 0; j < numKeyWords; j++) { + if (true == allow_missing_keywords && keyWordPos[j] == -1) + { + continue; // keyword is missing + } int dataRowIdx = keyWordPos[j] + 4 + i; std::string token = keyWordList[j]; int intVal = (int)radarFieldToInt32(fields[dataRowIdx], useBinaryProtocol); // getShortValue(fields[dataRowIdx].data); diff --git a/driver/src/sick_lmd_scandata_parser.cpp b/driver/src/sick_lmd_scandata_parser.cpp index cb681dec..ea32d40e 100644 --- a/driver/src/sick_lmd_scandata_parser.cpp +++ b/driver/src/sick_lmd_scandata_parser.cpp @@ -207,17 +207,6 @@ namespace sick_scan_xd //DataDumper::instance().pushData((double)SystemCountScan, "LASERTRANSMITDELAY", debug_duration.toSec()); #endif - /* - uint16_t u16_active_fieldset = 0; - memcpy(&u16_active_fieldset, receiveBuffer + 46, 2); // byte 46 + 47: input status (0 0), active fieldset - swap_endian((unsigned char *) &u16_active_fieldset, 2); - SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); - if(fieldMon) - { - fieldMon->setActiveFieldset(u16_active_fieldset & 0xFF); - ROS_INFO_STREAM("Binary scandata: active_fieldset = " << fieldMon->getActiveFieldset()); - } - */ // byte 48 + 49: output status (0 0) // byte 50 + 51: reserved @@ -249,6 +238,7 @@ namespace sick_scan_xd memcpy(&numOfEncoders, receiveBuffer + 60, 2); swap_endian((unsigned char *) &numOfEncoders, 2); + // ROS_DEBUG_STREAM("numOfEncoders = " << numOfEncoders); int encoderDataOffset = 6 * numOfEncoders; int32_t EncoderPosTicks[4] = {0}; int16_t EncoderSpeed[4] = {0}; diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp index 860c1d83..59701e71 100644 --- a/driver/src/sick_scan_common.cpp +++ b/driver/src/sick_scan_common.cpp @@ -115,6 +115,8 @@ #define RETURN_ERROR_ON_RESPONSE_TIMEOUT(result,reply) if(((result)!=ExitSuccess)&&((reply).empty()))return(ExitError) +static const int MAX_STR_LEN = 1024; + /*! \brief Universal swapping function \param ptr: Pointer to datablock @@ -625,15 +627,23 @@ namespace sick_scan_xd cloud_marker_ = 0; publish_lferec_ = false; + publish_lidinputstate_ = false; publish_lidoutputstate_ = false; if (parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC || parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_LMS5XX_LOGIC) { lferec_pub_ = rosAdvertise(nh, nodename + "/lferec", 100); + lidinputstate_pub_ = rosAdvertise(nh, nodename + "/lidinputstate", 100); lidoutputstate_pub_ = rosAdvertise(nh, nodename + "/lidoutputstate", 100); publish_lferec_ = true; + publish_lidinputstate_ = true; publish_lidoutputstate_ = true; cloud_marker_ = new sick_scan_xd::SickScanMarker(nh, nodename + "/marker", config_.frame_id); // "cloud"); } + else if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_RMS_XXXX_NAME) == 0) // RMS2xxx provides LIDoutputstate telegrams + { + lidoutputstate_pub_ = rosAdvertise(nh, nodename + "/lidoutputstate", 100); + publish_lidoutputstate_ = true; + } // Pointcloud2 publisher if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_SCANSEGMENT_XD_NAME) != 0 @@ -645,10 +655,16 @@ namespace sick_scan_xd imuScan_pub_ = rosAdvertise(nh, nodename + "/imu", 100); Encoder_pub = rosAdvertise(nh, nodename + "/encoder", 100); - } - // scan publisher - pub_ = rosAdvertise(nh, laserscan_topic, 1000); + // scan publisher + pub_ = rosAdvertise(nh, laserscan_topic, 1000); + } + else + { +#if defined USE_DIAGNOSTIC_UPDATER + diagnostics_ = 0; +#endif + } #if defined USE_DIAGNOSTIC_UPDATER if(diagnostics_) @@ -913,7 +929,7 @@ namespace sick_scan_xd \param requestStr command string (either as ASCII or BINARY) \return expected answer string */ - std::vector SickScanCommon::generateExpectedAnswerString(const std::vector requestStr) + std::vector SickScanCommon::generateExpectedAnswerString(const std::vector& requestStr) { std::string expectedAnswer = ""; //int i = 0; @@ -1026,6 +1042,22 @@ namespace sick_scan_xd return (expectedAnswers); } + /*! Returns a list of unexpected lidar responses like "" for request "" + * + * @param requestStr sent request string + * @return Expected answer + */ + std::vector SickScanCommon::generateUnexpectedAnswerString(const std::string& requestStr) + { + std::vector unexpected_responses; + if (requestStr.find("SetAccessMode") != std::string::npos) + { + unexpected_responses.push_back(std::string("sAN SetAccessMode 0")); // SetAccessMode failed (Cola-A) + unexpected_responses.push_back(std::string("sAN SetAccessMode \x00", 19)); // SetAccessMode failed (Cola-B) + } + return unexpected_responses; + } + /*! \brief send command and check answer \param requestStr: Sopas-Command @@ -1105,9 +1137,23 @@ namespace sick_scan_xd for(int retry_answer_cnt = 0; result != 0; retry_answer_cnt++) { std::string answerStr = replyToString(*reply); + std::string replyStrOrg = std::string((const char*)reply->data(), reply->size()); std::stringstream expectedAnswers; std::vector searchPattern = generateExpectedAnswerString(requestStr); + std::vector searchPatternNeg = generateUnexpectedAnswerString(reqStr); + for(int n = 0; n < searchPatternNeg.size(); n++) + { + ROS_DEBUG_STREAM("Compare lidar response \"" << DataDumper::binDataToAsciiString((const uint8_t*)replyStrOrg.data(), replyStrOrg.size()) << "\" to unexpected pattern \"" + << DataDumper::binDataToAsciiString((const uint8_t*)searchPatternNeg[n].data(), searchPatternNeg[n].size()) << "\" ..."); + if (replyStrOrg.find(searchPatternNeg[n]) != std::string::npos) + { + ROS_ERROR_STREAM("Unexpected response \"" << DataDumper::binDataToAsciiString((const uint8_t*)replyStrOrg.data(), replyStrOrg.size()) << "\" received from lidar, \"" + << DataDumper::binDataToAsciiString((const uint8_t*)searchPatternNeg[n].data(), searchPatternNeg[n].size()) + << "\" not expected, SOPAS command failed."); + break; + } + } for(int n = 0; result != 0 && n < searchPattern.size(); n++) { if (answerStr.find(searchPattern[n]) != std::string::npos) @@ -1137,7 +1183,11 @@ namespace sick_scan_xd diagnostics_->broadcast(getDiagnosticErrorCode(), tmpMsg); #endif result = -1; - + if (strncmp(answerStr.c_str(), "sFA", 3) == 0) // Error code received from lidar -> abort waiting for an expected answer + { + ROS_WARN_STREAM("Sopas error code " << answerStr.substr(4) << " received from lidar"); + break; + } // Problably we received some scan data message. Ignore and try again... std::vector response_keywords = { sick_scan_xd::SickScanMessages::getSopasCmdKeyword((uint8_t*)requestStr.data(), requestStr.size()) }; if(retry_answer_cnt < 100 && (rosNanosecTimestampNow() - retry_start_timestamp_nsec) / 1000000 < m_read_timeout_millisec_default) @@ -1530,6 +1580,19 @@ namespace sick_scan_xd sopasCmdVec[CMD_ACTIVATE_NTP_CLIENT] = "\x02sWN TSCRole 1\x03"; sopasCmdVec[CMD_SET_NTP_INTERFACE_ETH] = "\x02sWN TSCTCInterface 0\x03"; + /* + * Overwrite CMD_SET_ACCESS_MODE_3 by customized hash value + */ + std::string client_authorization_pw = "F4724744"; + rosDeclareParam(nh, "client_authorization_pw", client_authorization_pw); + rosGetParam(nh, "client_authorization_pw", client_authorization_pw); + if (!client_authorization_pw.empty() && client_authorization_pw != "F4724744") + { + std::string s_access_mode_3 = std::string("\x02sMN SetAccessMode 3 ") + client_authorization_pw + std::string("\x03\0"); + sopasCmdVec[CMD_SET_ACCESS_MODE_3] = s_access_mode_3; + sopasCmdVec[CMD_SET_ACCESS_MODE_3_SAFETY_SCANNER] = s_access_mode_3; + } + /* * Radar specific commands */ @@ -1642,22 +1705,23 @@ namespace sick_scan_xd sopasCmdMaskVec[CMD_APPLICATION_MODE] = "\x02sWN SetActiveApplications 1 %s %d\x03"; sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES] = "\x02sWN LMPoutputRange 1 %X %X %X\x03"; sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES_NAV3] = "\x02sWN LMPoutputRange 1 %X %X %X %X %X %X %X %X %X %X %X %X\x03"; - sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG] = "\x02sWN LMDscandatacfg %02d 00 %d %d 0 0 %02d 0 0 0 %d 1\x03"; // outputChannelFlagId, rssiFlag, rssiResolutionIs16Bit, EncoderSettings, timingflag + sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG] = "\x02sWN LMDscandatacfg %d 0 %d %d 0 %d 0 0 0 0 %d 1\x03"; // outputChannelFlagId, rssiFlag, rssiResolutionIs16Bit, EncoderSettings, timingflag // "\x02sWN LMDscandatacfg %02d 00 %d %d 0 0 %02d 0 0 0 %d 1\x03" /* - configuration - * in ASCII - * sWN LMDscandatacfg %02d 00 %d %d 0 %02d 0 0 0 1 1 - * | | | | | | | | | | +----------> Output rate -> All scans: 1--> every 1 scan - * | | | | | | | | +----------------> Time ->True (unused in Data Processing, TiM240:false) - * | | | | | | | +-------------------> Comment ->False - * | | | | | | +------------------------> Device Name ->False - * | | | | | +---------------------------> Position ->False - * | | | | +----------------------------------> Encoder ->Param set by Mask - * | | | +----------------------------------------> Unit of Remission->Always 0 - * | | +--------------------------------------------> RSSi Resolution ->0 8Bit 1 16 Bit - * | +-------------------------------------------------> Remission data ->Param set by Mask 0 False 1 True - * +--------------------------------------------------------> Data channel ->Param set by Mask -*/ + * configuration in ASCII + * sWN LMDscandatacfg %d 0 %d %d 0 %d 0 0 0 0 %d 1 + * | | | | | | | | | | | + * | | | | | | | | | | +----------> Output rate -> All scans: 1--> every 1 scan + * | | | | | | | | | +-------------> Time -> True (unused in Data Processing, TiM240:false) + * | | | | | | | | +----------------> Comment -> False + * | | | | | | | +-------------------> Device Name -> False + * | | | | | | +----------------------> Position -> False + * | | | | | +-------------------------> Encoder MSB -> Always 0 + * | | | | +----------------------------> Encoder LSB -> Param set by Mask (0 = no encoder, 1 = activate encoder), default: 0 + * | | | +--------------------------------> Unit of Remission -> Always 0 + * | | +------------------------------------> RSSi Resolution -> 0 8Bit 1 16 Bit + * | +-----------------------------------------> Remission data -> Param set by Mask 0 False 1 True + * +----------------------------------------------> Data channel -> Param set by Mask + */ sopasCmdMaskVec[CMD_GET_PARTIAL_SCANDATA_CFG] = "\x02sRA LMPscancfg %02d 00 %d %d 0 0 %02d 0 0 0 1 1\x03"; sopasCmdMaskVec[CMD_GET_SAFTY_FIELD_CFG] = "\x02sRN field%03d\x03"; sopasCmdMaskVec[CMD_SET_ECHO_FILTER] = "\x02sWN FREchoFilter %d\x03"; @@ -1761,7 +1825,6 @@ namespace sick_scan_xd // ML: Add here more useful cmd and mask entries // After definition of command, we specify the command sequence for scanner initalisation - if (parser_->getCurrentParamPtr()->getUseSafetyPasWD()) { sopasCmdChain.push_back(CMD_SET_ACCESS_MODE_3_SAFETY_SCANNER); @@ -1983,7 +2046,6 @@ namespace sick_scan_xd int SickScanCommon::init_scanner(rosNodePtr nh) { - const int MAX_STR_LEN = 1024; int maxNumberOfEchos = 1; @@ -3305,11 +3367,11 @@ namespace sick_scan_xd //normal scanconfig handling char requestLMDscandatacfg[MAX_STR_LEN]; // Uses sprintf-Mask to set bitencoded echos and rssi enable flag - // sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG] = "\x02sWN LMDscandatacfg %02d 00 %d %d 00 %d 00 0 0 0 1 %d\x03"; // outputChannelFlagId, rssiFlag, rssiResolutionIs16Bit, EncoderSettings, timingflag + // sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG] = "\x02sWN LMDscandatacfg %d 0 %d %d 0 %d 0 0 0 0 %d 1\x03"; // outputChannelFlagId, rssiFlag, rssiResolutionIs16Bit, EncoderSettings, timingflag const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG].c_str(); sprintf(requestLMDscandatacfg, pcCmdMask, outputChannelFlagId, rssi_flag, rssiResolutionIs16Bit ? 1 : 0, - EncoderSettings != -1 ? EncoderSettings : 0, + EncoderSettings > 0 ? 1 : 0, scandatacfg_timingflag); if (useBinaryCmd) { @@ -3613,6 +3675,7 @@ namespace sick_scan_xd } else if (this->parser_->getCurrentParamPtr()->getDeviceIsRadar()) { + bool activate_lidoutputstate = false; bool transmitRawTargets = true; bool transmitObjects = true; int trackingMode = 0; @@ -3629,6 +3692,10 @@ namespace sick_scan_xd rosDeclareParam(nh, "tracking_mode", trackingMode); rosGetParam(nh, "tracking_mode", trackingMode); + rosDeclareParam(nh, "activate_lidoutputstate", activate_lidoutputstate); + rosGetParam(nh, "activate_lidoutputstate", activate_lidoutputstate); + + ROS_INFO_STREAM("LIDoutputstate messages are switched [" << (activate_lidoutputstate ? "ON" : "OFF") << "]"); 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") << "]"); @@ -3638,6 +3705,12 @@ namespace sick_scan_xd startProtocolSequence.push_back(CMD_SERIAL_NUMBER); startProtocolSequence.push_back(CMD_ORDER_NUMBER); + // Optionally activate LIDoutputstate messages + if (activate_lidoutputstate) + { + startProtocolSequence.push_back(CMD_SET_LID_OUTPUTSTATE_ACTIVE); + } + /* * With "sWN TCTrackingMode 0" BASIC-Tracking activated * With "sWN TCTrackingMode 1" TRAFFIC-Tracking activated @@ -4387,11 +4460,33 @@ namespace sick_scan_xd if (true == this->parser_->getCurrentParamPtr()->getDeviceIsRadar()) { SickScanRadarSingleton *radar = SickScanRadarSingleton::getInstance(nh); - radar->setNameOfRadar(this->parser_->getCurrentParamPtr()->getScannerName(), this->parser_->getCurrentParamPtr()->getDeviceRadarType()); + std::string scanner_name = this->parser_->getCurrentParamPtr()->getScannerName(); + radar->setNameOfRadar(scanner_name, this->parser_->getCurrentParamPtr()->getDeviceRadarType()); int errorCode = ExitSuccess; - // parse radar telegram and send pointcloud2-debug messages - errorCode = radar->parseDatagram(recvTimeStamp, (unsigned char *) receiveBuffer, actual_length, - useBinaryProtocol); + if ( (useBinaryProtocol && memcmp(&receiveBuffer[8], "sSN LIDoutputstate", strlen("sSN LIDoutputstate")) == 0) + || (!useBinaryProtocol && memcmp(&receiveBuffer[1], "sSN LIDoutputstate", strlen("sSN LIDoutputstate")) == 0)) + { + // parse optional LIDoutputstate telegram + sick_scan_msg::LIDoutputstateMsg outputstate_msg; + if (sick_scan_xd::SickScanMessages::parseLIDoutputstateMsg(recvTimeStamp, receiveBuffer, actual_length, useBinaryProtocol, scanner_name, outputstate_msg)) + { + // Publish LIDoutputstate message + notifyLIDoutputstateListener(nh, &outputstate_msg); + if(publish_lidoutputstate_) + { + rosPublish(lidoutputstate_pub_, outputstate_msg); + } + } + else + { + ROS_WARN_STREAM("## ERROR SickScanCommon: parseLIDoutputstateMsg failed, received " << actual_length << " byte LIDoutputstate " << DataDumper::binDataToAsciiString(&receiveBuffer[0], actual_length)); + } + } + else + { + // parse radar telegram and send pointcloud2-debug messages + errorCode = radar->parseDatagram(recvTimeStamp, (unsigned char *) receiveBuffer, actual_length, useBinaryProtocol); + } return errorCode; // return success to continue looping } @@ -4446,11 +4541,19 @@ namespace sick_scan_xd SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); if(fieldMon && useBinaryProtocol && actual_length > 32) { - // int fieldset = (receiveBuffer[32] & 0xFF); - // fieldMon->setActiveFieldset(fieldset); - fieldMon->parseBinaryLIDinputstateMsg(receiveBuffer, actual_length); + sick_scan_msg::LIDinputstateMsg inputstate_msg; + fieldMon->parseBinaryLIDinputstateMsg(receiveBuffer, actual_length, inputstate_msg); + // notifyLIDinputstateListener(nh, &inputstate_msg); // TODO: notify LIDinputstateMsg listener (API) + if(publish_lidinputstate_) + { + rosPublish(lidinputstate_pub_, inputstate_msg); + } + if(cloud_marker_) + { + cloud_marker_->updateMarker(inputstate_msg, this->parser_->getCurrentParamPtr()->getUseEvalFields()); + } ROS_DEBUG_STREAM("SickScanCommon: received " << actual_length << " byte LIDinputstate " << DataDumper::binDataToAsciiString(&receiveBuffer[0], actual_length) - << ", active fieldset = " << fieldMon->getActiveFieldset()); + << ", active fieldset = " << fieldMon->getActiveFieldset() << ", " << fieldMon->LIDinputstateMsgToString(inputstate_msg)); } return errorCode; // return success to continue looping } @@ -5412,6 +5515,7 @@ namespace sick_scan_xd std::string KeyWord25 = "sWN NLMDLandmarkDataFormat"; std::string KeyWord26 = "sWN NAVScanDataFormat"; std::string KeyWord27 = "sMN mNLAYEraseLayout"; + std::string KeyWord28 = "sWN ActiveFieldSet"; // "\x02sWN ActiveFieldSet %02d\x03" //BBB @@ -5465,7 +5569,7 @@ namespace sick_scan_xd int scanDataStatus = 0; int keyWord3Len = keyWord3.length(); int dummyArr[12] = {0}; - //sWN LMDscandatacfg %02d 00 %d %d 0 0 %02d 0 0 0 1 1\x03" + // sWN LMDscandatacfg %d 0 %d %d 0 %d 0 0 0 0 %d 1 int sscanfresult = sscanf(requestAscii + keyWord3Len + 1, " %d %d %d %d %d %d %d %d %d %d %d %d", &dummyArr[0], // Data Channel Idx LSB &dummyArr[1], // Data Channel Idx MSB @@ -5872,6 +5976,14 @@ namespace sick_scan_xd buffer[0] = (unsigned char) (0xFF & (args[0])); bufferLen = 1; } + if (cmdAscii.find(KeyWord28) != std::string::npos && strlen(requestAscii) > KeyWord28.length() + 1) // "sWN ActiveFieldSet %02d" + { + int args[1] = { 0 }; + sscanf(requestAscii + KeyWord28.length() + 1, " %d", &(args[0])); + buffer[0] = (unsigned char) (0xFF & (args[0] >> 8)); + buffer[1] = (unsigned char) (0xFF & (args[0] >> 0)); + bufferLen = 2; + } // copy base command string to buffer bool switchDoBinaryData = false; @@ -6159,7 +6271,31 @@ namespace sick_scan_xd { //SAFTY FIELD PARSING int result = ExitSuccess; - const int MAX_STR_LEN = 1024; + if (this->parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); + int field_set_selection_method = -1; // set FieldSetSelectionMethod at startup: -1 = do not set (default), 0 = active field selection by digital inputs, 1 = active field selection by telegram (see operation manual for details about FieldSetSelectionMethod telegram) + int active_field_set = -1; // set ActiveFieldSet at startup: -1 = do not set (default), index of active field otherwise (see operation manual for details about ActiveFieldSet telegram) + rosDeclareParam(m_nh, "field_set_selection_method", field_set_selection_method); + rosGetParam(m_nh, "field_set_selection_method", field_set_selection_method); + rosDeclareParam(m_nh, "active_field_set", active_field_set); + rosGetParam(m_nh, "active_field_set", active_field_set); + std::vector sopasReply; + if (field_set_selection_method >= 0) // Write and re-read FieldSetSelectionMethod + { + result = writeFieldSetSelectionMethod(field_set_selection_method, sopasReply, useBinaryCmd); + RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, sopasReply); + result = readFieldSetSelectionMethod(field_set_selection_method, sopasReply, useBinaryCmd); + RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, sopasReply); + } + if (active_field_set >= 0) // Write and re-read ActiveFieldSet + { + result = writeActiveFieldSet(active_field_set, sopasReply, useBinaryCmd); + RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, sopasReply); + result = readActiveFieldSet(active_field_set, sopasReply, useBinaryCmd); + RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, sopasReply); + } + } if (this->parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC || this->parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_LMS5XX_LOGIC) { ROS_INFO("Reading safety fields"); @@ -6187,31 +6323,9 @@ namespace sick_scan_xd } if(cloud_marker_) { - int fieldset = 0; - // pn.getParam("fieldset", fieldset); - std::string LIDinputstateRequest = "\x02sRN LIDinputstate\x03"; - std::vector LIDinputstateResponse; - if (useBinaryCmd) - { - std::vector reqBinary; - this->convertAscii2BinaryCmd(LIDinputstateRequest.c_str(), &reqBinary); - result = sendSopasAndCheckAnswer(reqBinary, &LIDinputstateResponse); - RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, LIDinputstateResponse); // No response, non-recoverable connection error (return error and do not try other commands) - if(result == 0) - { - //fieldset = (LIDinputstateResponse[32] & 0xFF); - //fieldMon->setActiveFieldset(fieldset); - fieldMon->parseBinaryLIDinputstateMsg(LIDinputstateResponse.data(), LIDinputstateResponse.size()); - ROS_INFO_STREAM("Safety fieldset response to \"sRN LIDinputstate\": " << DataDumper::binDataToAsciiString(LIDinputstateResponse.data(), LIDinputstateResponse.size()) - << ", active fieldset = " << fieldMon->getActiveFieldset()); - } - } - else - { - result = sendSopasAndCheckAnswer(LIDinputstateRequest.c_str(), &LIDinputstateResponse); - RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, LIDinputstateResponse); // No response, non-recoverable connection error (return error and do not try other commands) - } - + if (readLIDinputstate(fieldMon, useBinaryCmd) != ExitSuccess) + return ExitError; + int fieldset = fieldMon->getActiveFieldset(); std::string scanner_name = parser_->getCurrentParamPtr()->getScannerName(); EVAL_FIELD_SUPPORT eval_field_logic = this->parser_->getCurrentParamPtr()->getUseEvalFields(); cloud_marker_->updateMarker(fieldMon->getMonFields(), fieldset, eval_field_logic); @@ -6241,6 +6355,139 @@ namespace sick_scan_xd return result; } // SickScanCommon::readParseSafetyFields() + int SickScanCommon::readLIDinputstate(SickScanFieldMonSingleton *fieldMon, bool useBinaryCmd) + { + int result = ExitError; + std::string LIDinputstateRequest = "\x02sRN LIDinputstate\x03"; + std::vector LIDinputstateResponse; + if (useBinaryCmd) + { + std::vector reqBinary; + this->convertAscii2BinaryCmd(LIDinputstateRequest.c_str(), &reqBinary); + result = sendSopasAndCheckAnswer(reqBinary, &LIDinputstateResponse, -1); + RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, LIDinputstateResponse); // No response, non-recoverable connection error (return error and do not try other commands) + if(result == 0) + { + sick_scan_msg::LIDinputstateMsg inputstate_msg; + fieldMon->parseBinaryLIDinputstateMsg(LIDinputstateResponse.data(), LIDinputstateResponse.size(), inputstate_msg); + ROS_INFO_STREAM("Safety fieldset response to \"sRN LIDinputstate\": " << DataDumper::binDataToAsciiString(LIDinputstateResponse.data(), LIDinputstateResponse.size()) + << ", active fieldset = " << fieldMon->getActiveFieldset() << ", " << fieldMon->LIDinputstateMsgToString(inputstate_msg)); + } + } + else + { + result = sendSopasAndCheckAnswer(LIDinputstateRequest.c_str(), &LIDinputstateResponse, -1); + RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, LIDinputstateResponse); // No response, non-recoverable connection error (return error and do not try other commands) + } + return 0; + } + + // Write FieldSetSelectionMethod + int SickScanCommon::writeFieldSetSelectionMethod(int field_set_selection_method, std::vector& sopasReply, bool useBinaryCmd) + { + int result = ExitSuccess; + if (field_set_selection_method >= 0 && this->parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + char reqAscii[MAX_STR_LEN]; + std::vector reqBinary; + sprintf(reqAscii, "\x02sWN FieldSetSelectionMethod %d\x03", field_set_selection_method); + if (useBinaryCmd) + { + this->convertAscii2BinaryCmd(reqAscii, &reqBinary); + result = sendSopasAndCheckAnswer(reqBinary, &sopasReply, -1); + } + else + { + result = sendSopasAndCheckAnswer(reqAscii, &sopasReply, -1); + } + RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, sopasReply); + } + return result; + } + + // Read FieldSetSelectionMethod + int SickScanCommon::readFieldSetSelectionMethod(int& field_set_selection_method, std::vector& sopasReply, bool useBinaryCmd) + { + int result = ExitSuccess; + if (this->parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + char reqAscii[MAX_STR_LEN]; + std::vector reqBinary; + sprintf(reqAscii, "\x02sRN FieldSetSelectionMethod\x03"); + if (useBinaryCmd) + { + this->convertAscii2BinaryCmd(reqAscii, &reqBinary); + result = sendSopasAndCheckAnswer(reqBinary, &sopasReply, -1); + } + else + { + result = sendSopasAndCheckAnswer(reqAscii, &sopasReply, -1); + } + RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, sopasReply); + SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); + uint8_t sopas_field_set_selection_method = (uint8_t)field_set_selection_method; + fieldMon->parseFieldSetSelectionMethodResponse(sopasReply.data(), sopasReply.size(), &sopas_field_set_selection_method); + field_set_selection_method = sopas_field_set_selection_method; + fieldMon->setFieldSelectionMethod(field_set_selection_method); + ROS_INFO_STREAM("Response to \"sRN FieldSetSelectionMethod\": \"" << DataDumper::binDataToAsciiString(sopasReply.data(), sopasReply.size()) << "\", FieldSetSelectionMethod = " << field_set_selection_method); + } + return result; + } + + // Write ActiveFieldSet + int SickScanCommon::writeActiveFieldSet(int active_field_set, std::vector& sopasReply, bool useBinaryCmd) + { + int result = ExitSuccess; + if (active_field_set >= 0 && this->parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + char reqAscii[MAX_STR_LEN]; + std::vector reqBinary; + sprintf(reqAscii, "\x02sWN ActiveFieldSet %02d\x03", active_field_set); + if (useBinaryCmd) + { + this->convertAscii2BinaryCmd(reqAscii, &reqBinary); + result = sendSopasAndCheckAnswer(reqBinary, &sopasReply, -1); + } + else + { + result = sendSopasAndCheckAnswer(reqAscii, &sopasReply, -1); + } + RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, sopasReply); + } + return result; + } + + // Read ActiveFieldSet + int SickScanCommon::readActiveFieldSet(int& active_field_set, std::vector& sopasReply, bool useBinaryCmd) + { + int result = ExitSuccess; + int eval_field_logic = this->parser_->getCurrentParamPtr()->getUseEvalFields(); + if (eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + char reqAscii[MAX_STR_LEN]; + std::vector reqBinary; + sprintf(reqAscii, "\x02sRN ActiveFieldSet\x03"); + if (useBinaryCmd) + { + this->convertAscii2BinaryCmd(reqAscii, &reqBinary); + result = sendSopasAndCheckAnswer(reqBinary, &sopasReply, -1); + } + else + { + result = sendSopasAndCheckAnswer(reqAscii, &sopasReply, -1); + } + RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, sopasReply); + SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); + uint16_t sopas_active_field_set = (uint16_t)active_field_set; + fieldMon->parseActiveFieldSetResponse(sopasReply.data(), sopasReply.size(), &sopas_active_field_set); + active_field_set = sopas_active_field_set; + fieldMon->setActiveFieldset(active_field_set); + if(cloud_marker_) + cloud_marker_->updateMarker(fieldMon->getMonFields(), active_field_set, eval_field_logic); + ROS_INFO_STREAM("Response to \"sRN ActiveFieldSet\": " << DataDumper::binDataToAsciiString(sopasReply.data(), sopasReply.size()) << "\", ActiveFieldSet = " << active_field_set); + } + return result; + } } /* namespace sick_scan_xd */ diff --git a/driver/src/sick_scan_marker.cpp b/driver/src/sick_scan_marker.cpp index 4de0e380..23cf36ef 100644 --- a/driver/src/sick_scan_marker.cpp +++ b/driver/src/sick_scan_marker.cpp @@ -133,6 +133,19 @@ void sick_scan_xd::SickScanMarker::updateMarker(const std::vector #include "sick_scan/sick_scan_messages.h" -typedef uint8_t* byte_ptr; -template static bool readBinaryBuffer(byte_ptr& buffer, int & bufferlen, T& value) +sick_scan_xd::SickScanMessages::SickScanMessages(rosNodePtr nh) { - if(sizeof(value) > bufferlen) - { - ROS_ERROR_STREAM("## ERROR SickScanMessages::readBinaryBuffer(): bufferlen=" << bufferlen << " byte, " << sizeof(value) << " byte required."); - return false; - } - memcpy(&value, buffer, sizeof(value)); - swap_endian((unsigned char *) &value, sizeof(value)); - buffer += sizeof(value); - bufferlen -= (int)sizeof(value); - return true; } -sick_scan_xd::SickScanMessages::SickScanMessages(rosNodePtr nh) +sick_scan_xd::SickScanMessages::~SickScanMessages() { } -sick_scan_xd::SickScanMessages::~SickScanMessages() +static char* readNextAsciiHexValue(char* msg_ptr, uint32_t* value) { + if (msg_ptr) + { + if(1 != sscanf(msg_ptr, "%x", value)) + return 0; // read error + msg_ptr = strchr(msg_ptr, ' '); + while (msg_ptr && isspace(*msg_ptr)) + msg_ptr++; + } + return msg_ptr; } /* @@ -103,13 +101,13 @@ sick_scan_xd::SickScanMessages::~SickScanMessages() */ bool sick_scan_xd::SickScanMessages::parseLIDoutputstateMsg(const rosTime& timeStamp, uint8_t* receiveBuffer, int receiveLength, bool useBinaryProtocol, const std::string& frame_id, sick_scan_msg::LIDoutputstateMsg& output_msg) { - if(useBinaryProtocol) + if(useBinaryProtocol) // Cola-A support in release 3.6 { // parse and convert LIDoutputstate message int dbg_telegram_length = receiveLength; std::string dbg_telegram_hex_copy = DataDumper::binDataToAsciiString(receiveBuffer, receiveLength); - int msg_start_idx = 27; // 4 byte STX + 4 byte payload_length + 19 byte "sSN LIDoutputstate " = 27 byte - int msg_parameter_length = receiveLength - msg_start_idx - 1; // start bytes + 1 byte CRC + int msg_start_idx = 27; + int msg_parameter_length = receiveLength - msg_start_idx - 1; // Cola-B: payload + 1 byte CRC // Cola-A: payload + 1 byte ETX if(msg_parameter_length < 8) // at least 6 byte (version+system) + 2 byte (timestamp status) { ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg(): received " << receiveLength << " byte, expected at least 8 byte (" << __FILE__ << ":" << __LINE__ << ")"); @@ -143,7 +141,7 @@ bool sick_scan_xd::SickScanMessages::parseLIDoutputstateMsg(const rosTime& timeS << DataDumper::binDataToAsciiString(receiveBuffer, receiveLength)); return false; } - // Read 2 byte version + 2 byte system status + // Read 2 byte version + 4 byte system counter receiveBuffer += msg_start_idx; receiveLength -= msg_start_idx; if( !readBinaryBuffer(receiveBuffer, receiveLength, output_msg.version_number) @@ -218,9 +216,67 @@ bool sick_scan_xd::SickScanMessages::parseLIDoutputstateMsg(const rosTime& timeS return true; } - else + else // i.e. RMS-2xxx { - ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg not implemented for Cola-A messages(" << __FILE__ << ":" << __LINE__ << ")"); + // ROS_ERROR_STREAM("## ERROR SickScanMessages::parseLIDoutputstateMsg not implemented for Cola-A messages(" << __FILE__ << ":" << __LINE__ << ")"); + int msg_offset = 20; // Cola-A: 1 byte STX + 19 byte "sSN LIDoutputstate " = 20 byte + char* msg_ptr = (char*)(msg_offset < receiveLength ? (receiveBuffer + msg_offset) : 0); + // Parse version_number and system_counter + uint32_t version_number = 0, system_counter = 0; + msg_ptr = readNextAsciiHexValue(msg_ptr, &version_number); + msg_ptr = readNextAsciiHexValue(msg_ptr, &system_counter); + output_msg.version_number = (uint16_t)(version_number & 0xFFFF); + output_msg.system_counter = system_counter; + for(int state_cnt = 0; state_cnt < 7 && msg_ptr != 0; state_cnt++) // Read state and count of Out1 to Out7 + { + uint32_t output_state = 2, output_count = 0; // output_state 2: not used + msg_ptr = readNextAsciiHexValue(msg_ptr, &output_state); + msg_ptr = readNextAsciiHexValue(msg_ptr, &output_count); + if (msg_ptr && (output_state == 0 || output_state == 1)) // 0: not active, 1: active, 2: not used + { + output_msg.output_state.push_back((uint8_t)(output_state & 0xFF)); + output_msg.output_count.push_back(output_count); + } + } + for(int state_cnt = 0; state_cnt < 8; state_cnt++) // Read state and count of Ext.Out1 to Ext.Out8 (not supported?) + { + } + // Read optional date and time + uint32_t time_state = 0, year = 0, month = 0, day = 0, hour = 0, minute = 0, second = 0, microsecond = 0; + msg_ptr = readNextAsciiHexValue(msg_ptr, &time_state); + if (msg_ptr && time_state > 0) + { + msg_ptr = readNextAsciiHexValue(msg_ptr, &year); + msg_ptr = readNextAsciiHexValue(msg_ptr, &month); + msg_ptr = readNextAsciiHexValue(msg_ptr, &day); + msg_ptr = readNextAsciiHexValue(msg_ptr, &hour); + msg_ptr = readNextAsciiHexValue(msg_ptr, &minute); + msg_ptr = readNextAsciiHexValue(msg_ptr, &second); + msg_ptr = readNextAsciiHexValue(msg_ptr, µsecond); + } + output_msg.time_state = (uint16_t)time_state; + output_msg.year = (uint16_t)year; + output_msg.month = (uint8_t)month; + output_msg.day = (uint8_t)day; + output_msg.hour = (uint8_t)hour; + output_msg.minute = (uint8_t)minute; + output_msg.second = (uint8_t)second; + output_msg.microsecond = (uint32_t)microsecond; + output_msg.header.stamp = timeStamp; + ROS_HEADER_SEQ(output_msg.header, 0); + output_msg.header.frame_id = frame_id; + // Debug messages + std::stringstream state_str; + for(int state_cnt = 0; state_cnt < output_msg.output_count.size(); state_cnt++) + state_str << "state[" << state_cnt << "]: " << (uint32_t)output_msg.output_state[state_cnt] << ", count=" << (uint32_t)output_msg.output_count[state_cnt] << "\n"; + ROS_DEBUG_STREAM("SickScanMessages::parseLIDoutputstateMsg():\n" + << receiveLength << " byte telegram: " << std::string((char*)receiveBuffer + 1, receiveLength - 2) << "\n" + << "version_number: " << (uint32_t)output_msg.version_number << ", system_counter: " << (uint32_t)output_msg.system_counter << "\n" + << state_str.str() + << "time state: " << (uint32_t)output_msg.time_state + << ", date: " << std::setfill('0') << std::setw(4) << (uint32_t)output_msg.year << "-" << std::setfill('0') << std::setw(2) << (uint32_t)output_msg.month << "-" << std::setfill('0') << std::setw(2) << (uint32_t)output_msg.day + << ", time: " << std::setfill('0') << std::setw(2) << (uint32_t)output_msg.hour << ":" << std::setfill('0') << std::setw(2) << (uint32_t)output_msg.minute << ":" << std::setfill('0') << std::setw(2) << (uint32_t)output_msg.second << "." << std::setfill('0') << std::setw(6) << (uint32_t)output_msg.microsecond); + return true; } return false; } diff --git a/driver/src/sick_scan_services.cpp b/driver/src/sick_scan_services.cpp index ea5d2d05..e328bd66 100644 --- a/driver/src/sick_scan_services.cpp +++ b/driver/src/sick_scan_services.cpp @@ -63,6 +63,7 @@ #include "sick_scan/sick_scan_services.h" #include "sick_scan/sick_generic_laser.h" +#include "sick_scansegment_xd/udp_poster.h" #define SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN (true) // Arguments of SOPAS commands are big endian encoded @@ -106,6 +107,8 @@ sick_scan_xd::SickScanServices::SickScanServices(rosNodePtr nh, sick_scan_xd::Si #define serviceCbSCsoftresetROS sick_scan_xd::SickScanServices::serviceCbSCsoftresetROS2 #define serviceCbSickScanExitROS sick_scan_xd::SickScanServices::serviceCbSickScanExitROS2 #define serviceCbGetContaminationResultROS sick_scan_xd::SickScanServices::serviceCbGetContaminationResultROS2 +#define serviceCbFieldSetReadROS sick_scan_xd::SickScanServices::serviceCbFieldSetReadROS2 +#define serviceCbFieldSetWriteROS sick_scan_xd::SickScanServices::serviceCbFieldSetWriteROS2 #else #define serviceCbColaMsgROS sick_scan_xd::SickScanServices::serviceCbColaMsg #define serviceCbECRChangeArrROS sick_scan_xd::SickScanServices::serviceCbECRChangeArr @@ -115,6 +118,8 @@ sick_scan_xd::SickScanServices::SickScanServices(rosNodePtr nh, sick_scan_xd::Si #define serviceCbSCsoftresetROS sick_scan_xd::SickScanServices::serviceCbSCsoftreset #define serviceCbSickScanExitROS sick_scan_xd::SickScanServices::serviceCbSickScanExit #define serviceCbGetContaminationResultROS sick_scan_xd::SickScanServices::serviceCbGetContaminationResult +#define serviceCbFieldSetReadROS sick_scan_xd::SickScanServices::serviceCbFieldSetRead +#define serviceCbFieldSetWriteROS sick_scan_xd::SickScanServices::serviceCbFieldSetWrite #endif #if __ROS_VERSION == 1 #define printServiceCreated(a,b) ROS_INFO_STREAM("SickScanServices: service \"" << a.getService() << "\" created (\"" << b.getService() << "\")"); @@ -171,6 +176,20 @@ sick_scan_xd::SickScanServices::SickScanServices(rosNodePtr nh, sick_scan_xd::Si m_srv_server_SickScanExit = rosServiceServer(srv_server_SickScanExit); printServiceCreated(srv_server_SickScanExit, m_srv_server_SickScanExit); } +#if __ROS_VERSION > 0 + if(lidar_param->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + auto srv_server_FieldSetRead = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::FieldSetReadSrv, "FieldSetRead", &serviceCbFieldSetReadROS, this); + m_srv_server_FieldSetRead = rosServiceServer(srv_server_FieldSetRead); + printServiceCreated(srv_server_FieldSetRead, m_srv_server_FieldSetRead); + } + if(lidar_param->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + auto srv_server_FieldSetWrite = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::FieldSetWriteSrv, "FieldSetWrite", &serviceCbFieldSetWriteROS, this); + m_srv_server_FieldSetWrite = rosServiceServer(srv_server_FieldSetWrite); + printServiceCreated(srv_server_FieldSetWrite, m_srv_server_FieldSetWrite); + } +#endif } } @@ -411,17 +430,31 @@ bool sick_scan_xd::SickScanServices::sendSopasCmdCheckResponse(const std::string } #if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 +static void printPicoScanImuDisabledWarning() +{ + ROS_WARN_STREAM("############################"); + ROS_WARN_STREAM("## ##"); + ROS_WARN_STREAM("## IMU will be disabled ##"); + ROS_WARN_STREAM("## ##"); + ROS_WARN_STREAM("############################"); + ROS_WARN_STREAM("## If you are using a picoScan150 w/o addons, disable the IMU by setting option \"imu_enable\" to \"False\" in your launchfile, or use sick_picoscan_single_echo.launch to avoid this error"); + ROS_WARN_STREAM("## If you are using a picoScan with IMU, check IMU settings with SOPAS Air"); +} + /*! * 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 supported are multiScan136 and picoScan150 * @param[in] scandataformat ScanDataFormat: 1 for msgpack or 2 for compact scandata, default: 2 -* @param[in] imu_enable: Imu data transfer enabled +* @param[in+out] imu_enable: Imu data transfer enabled * @param[in] imu_udp_port: UDP port of imu data (if imu_enable is true) +* @param[in] check_udp_receiver_ip: check udp_receiver_ip by sending and receiving a udp test message +* @param[in] check_udp_receiver_port: udp port to check udp_receiver_ip */ -bool sick_scan_xd::SickScanServices::sendMultiScanStartCmd(const std::string& hostname, int port, const std::string& scanner_type, int scandataformat, bool imu_enable, int imu_udp_port, int performanceprofilenumber) +bool sick_scan_xd::SickScanServices::sendMultiScanStartCmd(const std::string& hostname, int port, const std::string& scanner_type, int scandataformat, bool& imu_enable, int imu_udp_port, int performanceprofilenumber, bool check_udp_receiver_ip, int check_udp_receiver_port) { + // Check udp receiver ip address: hostname is expected to be a IPv4 address std::stringstream ip_stream(hostname); std::string ip_token; std::vector ip_tokens; @@ -431,11 +464,31 @@ bool sick_scan_xd::SickScanServices::sendMultiScanStartCmd(const std::string& ho } if (ip_tokens.size() != 4) { - 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."); + 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 udp_receiver_ip=\"" << hostname << "\" (launch file or commandline option)."); + ROS_ERROR_STREAM("## Parameter \"udp_receiver_ip\" should be the IPv4 address like 192.168.0.x of the system running sick_scan_xd."); return false; } + // Check udp receiver ip address by sending and receiving a UDP test message + std::string check_udp_receiver_msg = "sick_scan_xd udp test message verifying udp_receiver_ip"; + if (check_udp_receiver_ip && !check_udp_receiver_msg.empty()) + { + sick_scansegment_xd::UdpPoster udp_loopback_poster(hostname, check_udp_receiver_port); + std::string check_udp_receiver_response; + if (!udp_loopback_poster.Post(check_udp_receiver_msg, check_udp_receiver_response) || check_udp_receiver_msg != check_udp_receiver_response) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd() failed to send and receive a UDP test messsage to " << hostname << ":" << check_udp_receiver_port); + ROS_ERROR_STREAM("## Check configuration of parameter udp_receiver_ip=\"" << hostname << "\" (launch file or commandline option)."); + ROS_ERROR_STREAM("## Parameter \"udp_receiver_ip\" should be the IPv4 address like 192.168.0.x of the system running sick_scan_xd."); + return false; + } + else + { + ROS_DEBUG_STREAM("SickScanServices::sendMultiScanStartCmd(): UDP test message \"" << check_udp_receiver_response << "\" successfully received."); + ROS_INFO_STREAM("SickScanServices::sendMultiScanStartCmd(): UDP test message successfully send to " << hostname << ":" << check_udp_receiver_port << ", parameter udp_receiver_ip=\"" << hostname << "\" is valid."); + } + } + // Send sopas start sequence (ScanDataFormat, ScanDataEthSettings, ImuDataEthSettings, ScanDataEnable, ImuDataEnable) std::stringstream eth_settings_cmd, imu_eth_settings_cmd, scandataformat_cmd, performanceprofilenumber_cmd; scandataformat_cmd << "sWN ScanDataFormat " << scandataformat; if (performanceprofilenumber >= 0) @@ -485,7 +538,18 @@ bool sick_scan_xd::SickScanServices::sendMultiScanStartCmd(const std::string& ho } if (imu_enable && !sendSopasCmdCheckResponse(imu_eth_settings_cmd.str(), "sWA ImuDataEthSettings")) // imu data eth settings { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"" << imu_eth_settings_cmd.str() << "\") failed."); + if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) + { + // picoScan150 ships in 2 variants, with and without IMU resp. IMU licence (picoScan150 w/o addons). + // For picoScan150 w/o addons we disable the IMU, if SOPAS commands "ImuDataEthSettings" or "ImuDataEnable" failed. + ROS_WARN_STREAM("## WARNING SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"" << imu_eth_settings_cmd.str() << "\") failed."); + printPicoScanImuDisabledWarning(); + imu_enable = false; + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"" << imu_eth_settings_cmd.str() << "\") failed."); + } } if (!sendRun()) { @@ -502,7 +566,18 @@ bool sick_scan_xd::SickScanServices::sendMultiScanStartCmd(const std::string& ho } if (imu_enable && !sendSopasCmdCheckResponse("sWN ImuDataEnable 1", "sWA ImuDataEnable")) // enable imu data transfer { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ImuDataEnable 1\") failed."); + if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) + { + // picoScan150 ships in 2 variants, with and without IMU resp. IMU licence (picoScan150 w/o addons). + // For picoScan150 w/o addons we disable the IMU, if SOPAS commands "ImuDataEthSettings" or "ImuDataEnable" failed. + ROS_WARN_STREAM("## WARNING SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ImuDataEnable 1\") failed."); + printPicoScanImuDisabledWarning(); + imu_enable = false; + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ImuDataEnable 1\") failed."); + } } if (!sendRun()) { @@ -680,7 +755,7 @@ bool sick_scan_xd::SickScanServices::queryMultiScanFiltersettings(int& host_FREc bool enableFREchoFilter = true, enableLFPangleRangeFilter = true, enableLFPlayerFilter = true; if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) // LFPangleRangeFilter and LFPlayerFilter not supported by picoscan150 { - enableLFPangleRangeFilter = false; + // enableLFPangleRangeFilter = false; enableLFPlayerFilter = false; } std::vector sopasCommands; @@ -841,9 +916,9 @@ bool sick_scan_xd::SickScanServices::writeMultiScanFiltersettings(int host_FREch bool enableFREchoFilter = true, enableLFPangleRangeFilter = true, enableLFPlayerFilter = true, enableLFPintervalFilter = true; if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) // LFPangleRangeFilter, LFPlayerFilter ant LFPintervalFilter not supported by picoscan150 { - enableLFPangleRangeFilter = false; + // enableLFPangleRangeFilter = false; enableLFPlayerFilter = false; - enableLFPintervalFilter = false; + // enableLFPintervalFilter = false; } // Write FREchoFilter @@ -1055,3 +1130,42 @@ bool sick_scan_xd::SickScanServices::serviceCbSickScanExit(sick_scan_srv::SickSc service_response.success = stopScannerAndExit(false); return true; } + +#if __ROS_VERSION > 0 +bool sick_scan_xd::SickScanServices::serviceCbFieldSetRead(sick_scan_srv::FieldSetReadSrv::Request &service_request, sick_scan_srv::FieldSetReadSrv::Response &service_response) +{ + SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); + int field_set_selection_method = fieldMon->getFieldSelectionMethod(); + int active_field_set = fieldMon->getActiveFieldset(); + std::vector sopasReply; + int result1 = m_common_tcp->readFieldSetSelectionMethod(field_set_selection_method, sopasReply); + int result2 = m_common_tcp->readActiveFieldSet(active_field_set, sopasReply); + service_response.success = (result1 == ExitSuccess && result2 == ExitSuccess); + service_response.field_set_selection_method = field_set_selection_method; + service_response.active_field_set = active_field_set; + return true; +} + +bool sick_scan_xd::SickScanServices::serviceCbFieldSetWrite(sick_scan_srv::FieldSetWriteSrv::Request &service_request, sick_scan_srv::FieldSetWriteSrv::Response &service_response) +{ + int field_set_selection_method = service_request.field_set_selection_method_in; + int active_field_set = service_request.active_field_set_in; + std::vector sopasReply; + int result1 = ExitSuccess, result2 = ExitSuccess; + if (field_set_selection_method >= 0) + { + result1 = m_common_tcp->writeFieldSetSelectionMethod(field_set_selection_method, sopasReply); + } + if (active_field_set >= 0) + { + result2 = m_common_tcp->writeActiveFieldSet(active_field_set, sopasReply); + } + int result3 = m_common_tcp->readFieldSetSelectionMethod(field_set_selection_method, sopasReply); + int result4 = m_common_tcp->readActiveFieldSet(active_field_set, sopasReply); + service_response.success = (result1 == ExitSuccess && result2 == ExitSuccess && result3 == ExitSuccess && result4 == ExitSuccess); + service_response.field_set_selection_method = field_set_selection_method; + service_response.active_field_set = active_field_set; + service_response.success = true; + return true; +} +#endif diff --git a/driver/src/sick_scansegment_xd/compact_parser.cpp b/driver/src/sick_scansegment_xd/compact_parser.cpp index c196ff56..b09ca136 100644 --- a/driver/src/sick_scansegment_xd/compact_parser.cpp +++ b/driver/src/sick_scansegment_xd/compact_parser.cpp @@ -966,12 +966,11 @@ bool sick_scansegment_xd::CompactDataParser::Parse(const ScanSegmentParserConfig int64_t systemtime_nanoseconds = system_timestamp.time_since_epoch().count(); uint32_t systemtime_sec = (uint32_t)(systemtime_nanoseconds / 1000000000); // seconds part of system timestamp uint32_t systemtime_nsec = (uint32_t)(systemtime_nanoseconds % 1000000000); // nanoseconds part of system timestamp - uint32_t curtick = (uint32_t)(sensor_timeStamp & 0xFFFFFFFF); - software_pll.updatePLL(systemtime_sec, systemtime_nsec, curtick); + software_pll.updatePLL(systemtime_sec, systemtime_nsec, sensor_timeStamp); if (software_pll.IsInitialized()) { uint32_t pll_sec = 0, pll_nsec = 0; - software_pll.getCorrectedTimeStamp(pll_sec, pll_nsec, curtick); + software_pll.getCorrectedTimeStamp(pll_sec, pll_nsec, sensor_timeStamp); result.timestamp_sec = pll_sec; result.timestamp_nsec = pll_nsec; } diff --git a/driver/src/sick_scansegment_xd/config.cpp b/driver/src/sick_scansegment_xd/config.cpp index 09c691a1..58462ef7 100644 --- a/driver/src/sick_scansegment_xd/config.cpp +++ b/driver/src/sick_scansegment_xd/config.cpp @@ -160,10 +160,11 @@ sick_scansegment_xd::Config::Config() // port = 2115; // UDP port of multiScan136 to post start and stop commands // send_udp_start = false; // Send udp start string to multiScan136, default: True // send_udp_start_string = "magicalActivate"; // udp string to start multiScan136, default: "magicalActivate" - udp_timeout_ms = 60000; // Timeout for udp messages in milliseconds, default: 60*1000 + udp_timeout_ms = 10000; // Timeout for udp messages in milliseconds, default: 10*1000 + udp_timeout_ms_initial = 60000; // Initial timeout for udp messages after start in milliseconds, default: 60*1000 scandataformat = 2; // ScanDataFormat: 1 for msgpack or 2 for compact scandata, default: 2 performanceprofilenumber = -1; // Set performance profile by sending "sWN PerformanceProfileNumber" if performanceprofilenumber >= 0 (picoScan), default: -1 - imu_enable = true; // IMU enabled or disabled + imu_enable = false; // IMU enabled or disabled imu_topic = "imu"; // ROS topic for IMU messages imu_udp_port = 7503; // default udp port for multiScan imu data is 7503 imu_latency_microsec = 0; // imu latency in microseconds @@ -256,6 +257,8 @@ bool sick_scansegment_xd::Config::Init(rosNodePtr _node) 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, "check_udp_receiver_ip", check_udp_receiver_ip); + ROS_DECL_GET_PARAMETER(node, "check_udp_receiver_port", check_udp_receiver_port); 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); @@ -273,6 +276,7 @@ bool sick_scansegment_xd::Config::Init(rosNodePtr _node) // ROS_DECL_GET_PARAMETER(node, "send_udp_start", send_udp_start); // ROS_DECL_GET_PARAMETER(node, "send_udp_start_string", send_udp_start_string); ROS_DECL_GET_PARAMETER(node, "udp_timeout_ms", udp_timeout_ms); + ROS_DECL_GET_PARAMETER(node, "udp_timeout_ms_initial", udp_timeout_ms_initial); ROS_DECL_GET_PARAMETER(node, "scandataformat", scandataformat); ROS_DECL_GET_PARAMETER(node, "performanceprofilenumber", performanceprofilenumber); ROS_DECL_GET_PARAMETER(node, "imu_enable", imu_enable); @@ -291,19 +295,17 @@ bool sick_scansegment_xd::Config::Init(rosNodePtr _node) ROS_DECL_GET_PARAMETER(node, "host_set_FREchoFilter", host_set_FREchoFilter); ROS_DECL_GET_PARAMETER(node, "host_LFPangleRangeFilter", host_LFPangleRangeFilter); ROS_DECL_GET_PARAMETER(node, "host_set_LFPangleRangeFilter", host_set_LFPangleRangeFilter); + ROS_DECL_GET_PARAMETER(node, "host_LFPintervalFilter", host_LFPintervalFilter); + ROS_DECL_GET_PARAMETER(node, "host_set_LFPintervalFilter", host_set_LFPintervalFilter); if (scanner_type != SICK_SCANNER_PICOSCAN_NAME) { ROS_DECL_GET_PARAMETER(node, "host_LFPlayerFilter", host_LFPlayerFilter); ROS_DECL_GET_PARAMETER(node, "host_set_LFPlayerFilter", host_set_LFPlayerFilter); - ROS_DECL_GET_PARAMETER(node, "host_LFPintervalFilter", host_LFPintervalFilter); - ROS_DECL_GET_PARAMETER(node, "host_set_LFPintervalFilter", host_set_LFPintervalFilter); } else { host_LFPlayerFilter = ""; host_set_LFPlayerFilter = false; - host_LFPintervalFilter = ""; - host_set_LFPintervalFilter = false; } // msgpack validation settings std::string str_msgpack_validator_required_echos = "0"; @@ -336,7 +338,7 @@ bool sick_scansegment_xd::Config::Init(rosNodePtr _node) ROS_DECL_GET_PARAMETER(node, "add_transform_xyz_rpy", str_add_transform_xyz_rpy); bool add_transform_check_dynamic_updates = false; ROS_DECL_GET_PARAMETER(node, "add_transform_check_dynamic_updates", add_transform_check_dynamic_updates); - add_transform_xyz_rpy = sick_scan_xd::SickCloudTransform(node, str_add_transform_xyz_rpy, false, add_transform_check_dynamic_updates); + add_transform_xyz_rpy = sick_scan_xd::SickCloudTransform(node, str_add_transform_xyz_rpy, true, add_transform_check_dynamic_updates); // Configuration of laserscan messages (ROS only), activate/deactivate laserscan messages for each layer std::string str_laserscan_layer_filter = "0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0"; @@ -400,6 +402,8 @@ bool sick_scansegment_xd::Config::Init(int argc, char** argv) // Overwrite with commandline arguments setOptionalArgument(cli_parameter_map, "udp_sender", udp_sender); setOptionalArgument(cli_parameter_map, "udp_port", udp_port); + setOptionalArgument(cli_parameter_map, "check_udp_receiver_ip", check_udp_receiver_ip); + setOptionalArgument(cli_parameter_map, "check_udp_receiver_port", check_udp_receiver_port); 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); @@ -417,6 +421,7 @@ bool sick_scansegment_xd::Config::Init(int argc, char** argv) // setOptionalArgument(cli_parameter_map, "send_udp_start", send_udp_start);; // setOptionalArgument(cli_parameter_map, "send_udp_start_string", send_udp_start_string); setOptionalArgument(cli_parameter_map, "udp_timeout_ms", udp_timeout_ms); + setOptionalArgument(cli_parameter_map, "udp_timeout_ms_initial", udp_timeout_ms_initial); setOptionalArgument(cli_parameter_map, "scandataformat", scandataformat); setOptionalArgument(cli_parameter_map, "performanceprofilenumber", performanceprofilenumber); setOptionalArgument(cli_parameter_map, "imu_enable", imu_enable); @@ -481,6 +486,8 @@ void sick_scansegment_xd::Config::PrintConfig(void) ROS_INFO_STREAM("scanner_type: " << scanner_type); ROS_INFO_STREAM("udp_sender: " << udp_sender); ROS_INFO_STREAM("udp_port: " << udp_port); + ROS_INFO_STREAM("check_udp_receiver_ip: " << check_udp_receiver_ip); + ROS_INFO_STREAM("check_udp_receiver_port: " << check_udp_receiver_port); 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); @@ -499,6 +506,7 @@ void sick_scansegment_xd::Config::PrintConfig(void) //ROS_INFO_STREAM("send_udp_start: " << send_udp_start); //ROS_INFO_STREAM("send_udp_start_string: " << send_udp_start_string); ROS_INFO_STREAM("udp_timeout_ms: " << udp_timeout_ms); + ROS_INFO_STREAM("udp_timeout_ms_initial: " << udp_timeout_ms_initial); ROS_INFO_STREAM("scandataformat: " << scandataformat); ROS_INFO_STREAM("performanceprofilenumber: " << performanceprofilenumber); ROS_INFO_STREAM("imu_enable: " << imu_enable); diff --git a/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp b/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp index c136cdfd..30d3e1d3 100644 --- a/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp +++ b/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp @@ -532,13 +532,18 @@ class CustomizedPointXYZRAEI32f : public sick_scansegment_xd::PointXYZRAEI32f { time_offset_nanosec = (uint32_t)(1000 * (point.lidar_timestamp_microsec - lidar_timestamp_start_microsec)); time_offset_sec = (float)(1.0e-6 * (point.lidar_timestamp_microsec - lidar_timestamp_start_microsec)); + lidar_sec = (uint32_t)(point.lidar_timestamp_microsec / 1000000); + lidar_nsec = (uint32_t)(1000 * (point.lidar_timestamp_microsec % 1000000)); + // assert((uint64_t)lidar_sec * 1000000 + (uint64_t)lidar_nsec / 1000 == point.lidar_timestamp_microsec); } ring = point.layer; } // Additional fields for customized point clouds: uint32_t time_offset_nanosec = 0; // field "t": 4 byte time offset of a scan point in nano seconds relative to the timestamp in the point cloud header, used by rtabmap for deskewing - float time_offset_sec = 0; // time offset in seconds (otherwise identical to field "t") - int8_t ring = 0; // field "ring": 1 byte layer id, identical to field "layer" + float time_offset_sec = 0; // field "ts": time offset in seconds (otherwise identical to field "t") + uint32_t lidar_sec = 0; // field "lidar_sec": 4 byte seconds part of the lidar timestamp in microseconds (lidar time), lidar_sec = (uint32_t)(lidar_timestamp_microsec / 1000000) + uint32_t lidar_nsec = 0; // field "lidar_nsec": 4 byte nano seconds part of the lidar timestamp in microseconds (lidar time), lidar_nsec = (uint32_t)(1000 * (lidar_timestamp_microsec % 1000000)) + int8_t ring = 0; // field "ring": 1 byte layer id, identical to field "layer" }; /* @@ -583,6 +588,10 @@ void sick_scansegment_xd::RosMsgpackPublisher::convertPointsToCustomizedFieldsCl field_properties.push_back(PointCloudFieldProperty("t", PointField::UINT32, sizeof(uint32_t), (uint8_t*)&dummy_lidar_point.time_offset_nanosec - (uint8_t*)&dummy_lidar_point)); if (pointcloud_cfg.fieldEnabled("ts")) field_properties.push_back(PointCloudFieldProperty("ts", PointField::FLOAT32, sizeof(float), (uint8_t*)&dummy_lidar_point.time_offset_sec - (uint8_t*)&dummy_lidar_point)); + if (pointcloud_cfg.fieldEnabled("lidar_sec")) + field_properties.push_back(PointCloudFieldProperty("lidar_sec", PointField::UINT32, sizeof(uint32_t), (uint8_t*)&dummy_lidar_point.lidar_sec - (uint8_t*)&dummy_lidar_point)); + if (pointcloud_cfg.fieldEnabled("lidar_nsec")) + field_properties.push_back(PointCloudFieldProperty("lidar_nsec", PointField::UINT32, sizeof(uint32_t), (uint8_t*)&dummy_lidar_point.lidar_nsec - (uint8_t*)&dummy_lidar_point)); if (pointcloud_cfg.fieldEnabled("ring")) field_properties.push_back(PointCloudFieldProperty("ring", PointField::INT8, sizeof(int8_t), (uint8_t*)&dummy_lidar_point.ring - (uint8_t*)&dummy_lidar_point)); if (pointcloud_cfg.fieldEnabled("layer")) @@ -843,7 +852,9 @@ void sick_scansegment_xd::RosMsgpackPublisher::convertPointsToLaserscanMsg(uint3 laser_scan_msg.angle_increment = angle_diff / (float)(laser_scan_msg.ranges.size() - 1); laser_scan_msg.range_min -= 1.0e-03f; laser_scan_msg.range_max += 1.0e-03f; - laser_scan_msg.range_min = std::max(0.0f, laser_scan_msg.range_min); + laser_scan_msg.range_min = std::max(0.05f, laser_scan_msg.range_min); // min range of multiScan and picoScan: 0.05 [m] + + laser_scan_msg.header.stamp.sec = timestamp_sec; #if defined __ROS_VERSION && __ROS_VERSION > 1 laser_scan_msg.header.stamp.nanosec = timestamp_nsec; diff --git a/driver/src/sick_scansegment_xd/scansegment_threads.cpp b/driver/src/sick_scansegment_xd/scansegment_threads.cpp index 973f89c1..c5363fa4 100644 --- a/driver/src/sick_scansegment_xd/scansegment_threads.cpp +++ b/driver/src/sick_scansegment_xd/scansegment_threads.cpp @@ -325,17 +325,30 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) if (sopas_tcp->isConnected()) { sopas_service->sendAuthorization();//(m_config.client_authorization_pw); - sopas_service->sendMultiScanStartCmd(m_config.udp_receiver_ip, m_config.udp_port, m_config.scanner_type, m_config.scandataformat, m_config.imu_enable, m_config.imu_udp_port, m_config.performanceprofilenumber); + if (!sopas_service->sendMultiScanStartCmd(m_config.udp_receiver_ip, m_config.udp_port, m_config.scanner_type, m_config.scandataformat, m_config.imu_enable, m_config.imu_udp_port, + m_config.performanceprofilenumber, m_config.check_udp_receiver_ip, m_config.check_udp_receiver_port)) + { + ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: failed to send startup sequence, receiving scan data may fail."); + } } else { - ROS_WARN_STREAM("## ERROR sick_scansegment_xd: no sopas tcp connection, multiScan136 start commands not sent."); + ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: no sopas tcp connection, startup sequence not sent, receiving scan data may fail."); } } // Run event loop and monitor tcp-connection and udp messages setDiagnosticStatus(SICK_DIAGNOSTIC_STATUS::OK, ""); s_sopas_service = sopas_service; + // Wait for first udp message with initial timeout after start in milliseconds, default: 60*1000 + fifo_timestamp fifo_timestamp_start = fifo_clock::now(); + while(m_run_scansegment_thread && rosOk() && sopas_tcp->isConnected() + && udp_receiver->Fifo()->TotalMessagesPushed() <= 1 + && udp_receiver->Fifo()->Seconds(fifo_timestamp_start, fifo_clock::now()) <= 1.0e-3 * m_config.udp_timeout_ms_initial) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + // Monitor udp packets with timeout for udp messages in milliseconds, default: 10*1000 while(m_run_scansegment_thread && rosOk()) { if (!sopas_tcp->isConnected()) @@ -343,12 +356,22 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: sopas tcp connection lost, stop and reconnect..."); break; } - if (udp_receiver->Fifo()->TotalMessagesPushed() > 10 && udp_receiver->Fifo()->SecondsSinceLastPush() > 1.0e-3 * m_config.udp_timeout_ms) + if (udp_receiver->Fifo()->TotalMessagesPushed() <= 1 || udp_receiver->Fifo()->SecondsSinceLastPush() > 1.0e-3 * m_config.udp_timeout_ms) { - ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: " << (udp_receiver->Fifo()->TotalMessagesPushed()) << " udp messages received, last message " << (udp_receiver->Fifo()->SecondsSinceLastPush()) << " seconds ago."); + ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: " << (udp_receiver->Fifo()->TotalMessagesPushed()) << " udp messages received"); + if (udp_receiver->Fifo()->TotalMessagesPushed() > 0) + { + ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: last message received " << (udp_receiver->Fifo()->SecondsSinceLastPush()) << " seconds ago."); + } ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: udp receive timeout, stop and reconnect..."); break; } + // Unit test for restart after timeout error - do not activate except for error and timeout simulation + // if (udp_receiver->Fifo()->TotalMessagesPushed() > 1000) + // { + // ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: " << (udp_receiver->Fifo()->TotalMessagesPushed()) << " udp messages received, simulating timeout error, aborting to restart"); + // break; + // } setDiagnosticStatus(SICK_DIAGNOSTIC_STATUS::OK, ""); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } diff --git a/driver/src/softwarePLL.cpp b/driver/src/softwarePLL.cpp index c766a82a..ac08ff6c 100644 --- a/driver/src/softwarePLL.cpp +++ b/driver/src/softwarePLL.cpp @@ -67,7 +67,7 @@ std::istream &operator>>(std::istream &str, CSVRow &data) } -bool SoftwarePLL::pushIntoFifo(double curTimeStamp, uint32_t curtick) +bool SoftwarePLL::pushIntoFifo(double curTimeStamp, uint64_t curtick) // update tick fifo and update clock (timestamp) fifo { for (int i = 0; i < fifoSize - 1; i++) @@ -88,10 +88,9 @@ bool SoftwarePLL::pushIntoFifo(double curTimeStamp, uint32_t curtick) return (true); } -double SoftwarePLL::extraPolateRelativeTimeStamp(uint32_t tick) +double SoftwarePLL::extraPolateRelativeTimeStamp(uint64_t tick) { - int32_t tempTick = 0; - tempTick = tick - (uint32_t) (0xFFFFFFFF & FirstTick()); + uint64_t tempTick = tick - FirstTick(); double timeDiff = tempTick * this->InterpolationSlope(); return (timeDiff); @@ -123,7 +122,7 @@ int SoftwarePLL::findDiffInFifo(double diff, double tol) \param curtick micro Seconds since scanner start from SOPAS Datagram \return PLL is in valid state (true) */ -bool SoftwarePLL::updatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick) +bool SoftwarePLL::updatePLL(uint32_t sec, uint32_t nanoSec, uint64_t curtick) { if (offsetTimestampFirstLidarTick == 0) { @@ -189,9 +188,20 @@ bool SoftwarePLL::updatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick) } +bool SoftwarePLL::updatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick) +{ + return updatePLL(sec, nanoSec, (uint64_t)curtick); +} + //TODO Kommentare -bool SoftwarePLL::getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32_t curtick) +bool SoftwarePLL::getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint64_t curtick) { + if (ticksToTimestampMode == TICKS_TO_LIDAR_TIMESTAMP) // optional tick-mode: convert lidar ticks in microseconds directly into a lidar timestamp by sec = tick/1000000, nsec = 1000 * (tick % 1000000) + { + sec = (uint32_t)(curtick / 1000000); + nanoSec = (uint32_t)(1000 * (curtick % 1000000)); + return true; + } if (IsInitialized() == false) { return (false); @@ -213,9 +223,19 @@ bool SoftwarePLL::getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32 return (true); } +bool SoftwarePLL::getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32_t curtick) +{ + return getCorrectedTimeStamp(sec, nanoSec, (uint64_t)curtick); +} + // 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) +bool SoftwarePLL::convSystemtimeToLidarTimestamp(uint32_t systemtime_sec, uint32_t systemtime_nanosec, uint64_t& tick) { + if (ticksToTimestampMode == TICKS_TO_LIDAR_TIMESTAMP) // optional tick-mode: convert lidar ticks in microseconds directly into a lidar timestamp + { + tick = 1000000 * (uint64_t)systemtime_sec + (uint64_t)systemtime_nanosec / 1000; // tick in micro seconds + return true; + } if (IsInitialized() == false) { return (false); @@ -224,7 +244,7 @@ bool SoftwarePLL::convSystemtimeToLidarTimestamp(uint32_t systemtime_sec, uint32 { double relSystemTimestamp = (systemtime_sec + 1.0e-9 * systemtime_nanosec) - (offsetTimestampFirstSystemSec + 1.0e-6 * offsetTimestampFirstSystemMicroSec); double relTicks = 1.0e6 * relSystemTimestamp; - tick = (uint32_t)std::round(relTicks + offsetTimestampFirstLidarTick); + tick = (uint64_t)std::round(relTicks + offsetTimestampFirstLidarTick); } else // default: convert lidar ticks in microseconds to system timestamp by software-pll { @@ -236,11 +256,19 @@ bool SoftwarePLL::convSystemtimeToLidarTimestamp(uint32_t systemtime_sec, uint32 //=> 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); + tick = (uint64_t)std::round(relTicks + tick_offset); } return (true); } +bool SoftwarePLL::convSystemtimeToLidarTimestamp(uint32_t systemtime_sec, uint32_t systemtime_nanosec, uint32_t& tick) +{ + uint64_t lidar_ticks = 0; + bool success = convSystemtimeToLidarTimestamp(systemtime_sec, systemtime_nanosec, lidar_ticks); + tick = (uint32_t)lidar_ticks; + return success; +} + bool SoftwarePLL::nearSameTimeStamp(double relTimeStamp1, double relTimeStamp2, double& delta_time_abs) { delta_time_abs = fabs(relTimeStamp1 - relTimeStamp2); diff --git a/include/sick_scan/sick_generic_field_mon.h b/include/sick_scan/sick_generic_field_mon.h index 241afd4e..51915d08 100644 --- a/include/sick_scan/sick_generic_field_mon.h +++ b/include/sick_scan/sick_generic_field_mon.h @@ -159,7 +159,8 @@ namespace sick_scan_xd SickScanFieldMonSingleton(); std::vectormonFields; - int active_mon_fieldset; + int active_mon_fieldset = 0; + int mon_field_selection_method = 0; // FieldSetSelectionMethod: 0 = digital inputs (default), 1 = telegram "sWN ActiveFieldSet" public: /* Static access method. */ @@ -170,12 +171,20 @@ namespace sick_scan_xd void setActiveFieldset(int active_fieldset) { active_mon_fieldset = active_fieldset; } int getActiveFieldset(void) { return active_mon_fieldset; } + void setFieldSelectionMethod(int field_selection_method) { mon_field_selection_method = field_selection_method; } // FieldSetSelectionMethod: 0 = digital inputs (default), 1 = telegram "sWN ActiveFieldSet" + int getFieldSelectionMethod(void) { return mon_field_selection_method; } + int parseAsciiLIDinputstateMsg(unsigned char* datagram, int datagram_length); - int parseBinaryLIDinputstateMsg(unsigned char* datagram, int datagram_length); + int parseBinaryLIDinputstateMsg(unsigned char* datagram, int datagram_length, sick_scan_msg::LIDinputstateMsg& inputstate_msg); + + void parseActiveFieldSetResponse(unsigned char* datagram, int datagram_length, uint16_t* active_field_set); + void parseFieldSetSelectionMethodResponse(unsigned char* datagram, int datagram_length, uint8_t* field_set_selection_method); int parseBinaryDatagram(std::vector datagramm, float rectFieldAngleRefPointOffsetRad); int parseAsciiDatagram(std::vector datagramm, float rectFieldAngleRefPointOffsetRad); + + std::string LIDinputstateMsgToString(const sick_scan_msg::LIDinputstateMsg& inputstate_msg); }; diff --git a/include/sick_scan/sick_ros_wrapper.h b/include/sick_scan/sick_ros_wrapper.h index a75c4972..3640cefd 100644 --- a/include/sick_scan/sick_ros_wrapper.h +++ b/include/sick_scan/sick_ros_wrapper.h @@ -212,6 +212,7 @@ 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 @@ -228,6 +229,10 @@ inline rosDuration rosDurationFromSec(double seconds) { return rosDuration(secon #include "sick_scan_xd/SickScanExitSrv.h" // generated by srv-generator #include "sick_scan_xd/GetContaminationResultSrv.h" // generated by srv-generator #define sick_scan_srv sick_scan_xd +#if __ROS_VERSION == 1 +#include "sick_scan_xd/FieldSetReadSrv.h" // generated by srv-generator +#include "sick_scan_xd/FieldSetWriteSrv.h" // generated by srv-generator +#endif template class rosServiceClient : public ros::ServiceClient { @@ -404,6 +409,7 @@ 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 @@ -420,6 +426,8 @@ inline rosDuration rosDurationFromSec(double seconds) { return rosDuration(std:: #include "sick_scan_xd/srv/sick_scan_exit_srv.hpp" // generated by rosidl-generator #include "sick_scan_xd/srv/get_contamination_result_srv.hpp" // generated by rosidl-generator #define sick_scan_srv sick_scan_xd::srv +#include "sick_scan_xd/srv/field_set_read_srv.hpp" // generated by srv-generator +#include "sick_scan_xd/srv/field_set_write_srv.hpp" // generated by srv-generator template class rosServiceClient : public rclcpp::Client::SharedPtr { diff --git a/include/sick_scan/sick_scan_common.h b/include/sick_scan/sick_scan_common.h index caf37b7c..1c628e77 100644 --- a/include/sick_scan/sick_scan_common.h +++ b/include/sick_scan/sick_scan_common.h @@ -236,12 +236,20 @@ namespace sick_scan_xd virtual ~SickScanCommon(); int setParticleFilter(bool _active, int _particleThreshold);//actualy only 500 mm is working. + /*! Changes the Identifier of a commandstr. to its expected answer counterpart * * @param requestStr sent request string * @return Expected answer */ - std::vector generateExpectedAnswerString(const std::vector requestStr); + std::vector generateExpectedAnswerString(const std::vector& requestStr); + + /*! Returns a list of unexpected lidar responses like "" for request "" + * + * @param requestStr sent request string + * @return Expected answer + */ + std::vector generateUnexpectedAnswerString(const std::string& requestStr); /** * \brief Converts a given SOPAS command from ascii to binary (in case of binary communication), sends sopas (ascii or binary) and returns the response (if wait_for_reply:=true) @@ -356,6 +364,18 @@ namespace sick_scan_xd uint64_t getNanosecTimestampLastTcpMessageReceived(void) { return m_nw.getNanosecTimestampLastTcpMessageReceived(); } // Returns a timestamp in nanoseconds of the last received tcp message (or 0 if no message received) + // Write FieldSetSelectionMethod + int writeFieldSetSelectionMethod(int field_set_selection_method, std::vector& sopasReply, bool useBinaryCmd = true); + + // Read FieldSetSelectionMethod + int readFieldSetSelectionMethod(int& field_set_selection_method, std::vector& sopasReply, bool useBinaryCmd = true); + + // Write ActiveFieldSet + int writeActiveFieldSet(int active_field_set, std::vector& sopasReply, bool useBinaryCmd = true); + + // Read ActiveFieldSet + int readActiveFieldSet(int& active_field_set, std::vector& sopasReply, bool useBinaryCmd = true); + // move back to private /* FÃœR MRS10000 brauchen wir einen Publish und eine NAchricht */ // Should we publish laser or point cloud? @@ -434,6 +454,8 @@ namespace sick_scan_xd bool switchColaProtocol(bool useBinaryCmd); + int readLIDinputstate(SickScanFieldMonSingleton *fieldMon, bool useBinaryCmd); + #ifdef USE_DIAGNOSTIC_UPDATER std::shared_ptr diagnostics_; #endif @@ -448,7 +470,9 @@ namespace sick_scan_xd rosPublisher lferec_pub_; bool publish_lferec_; + rosPublisher lidinputstate_pub_; rosPublisher lidoutputstate_pub_; + bool publish_lidinputstate_; bool publish_lidoutputstate_; SickScanMarker* cloud_marker_; diff --git a/include/sick_scan/sick_scan_marker.h b/include/sick_scan/sick_scan_marker.h index 104b040c..bed02b92 100644 --- a/include/sick_scan/sick_scan_marker.h +++ b/include/sick_scan/sick_scan_marker.h @@ -79,6 +79,8 @@ namespace sick_scan_xd void updateMarker(const std::vector& fields, int fieldset, int eval_field_logic); + void updateMarker(sick_scan_msg::LIDinputstateMsg& msg, int eval_field_logic); + void updateMarker(sick_scan_msg::LIDoutputstateMsg& msg, int eval_field_logic); void updateMarker(sick_scan_msg::LFErecMsg& msg, int eval_field_logic); diff --git a/include/sick_scan/sick_scan_messages.h b/include/sick_scan/sick_scan_messages.h index a14cc546..213c2d51 100644 --- a/include/sick_scan/sick_scan_messages.h +++ b/include/sick_scan/sick_scan_messages.h @@ -69,6 +69,21 @@ namespace sick_scan_xd { + typedef uint8_t* uint8_ptr; + template inline bool readBinaryBuffer(uint8_ptr& buffer, int & bufferlen, T& value) + { + if((int)sizeof(value) > bufferlen) + { + ROS_ERROR_STREAM("## ERROR SickScanMessages::readBinaryBuffer(): bufferlen=" << bufferlen << " byte, " << sizeof(value) << " byte required."); + return false; + } + memcpy(&value, buffer, sizeof(value)); + swap_endian((unsigned char *) &value, sizeof(value)); + buffer += sizeof(value); + bufferlen -= (int)sizeof(value); + return true; + } + class SickScanMessages { public: diff --git a/include/sick_scan/sick_scan_services.h b/include/sick_scan/sick_scan_services.h index 341c7a52..92daec38 100644 --- a/include/sick_scan/sick_scan_services.h +++ b/include/sick_scan/sick_scan_services.h @@ -87,7 +87,7 @@ namespace sick_scan_xd bool sendAuthorization(); /*! - * Sends a multiScan136 command + * Sends a multiScan or picoScan SOPAS command */ bool sendSopasCmdCheckResponse(const std::string& sopas_request, const std::string& expected_response); @@ -149,6 +149,13 @@ namespace sick_scan_xd bool serviceCbSickScanExit(sick_scan_srv::SickScanExitSrv::Request &service_request, sick_scan_srv::SickScanExitSrv::Response &service_response); bool serviceCbSickScanExitROS2(std::shared_ptr service_request, std::shared_ptr service_response) { return serviceCbSickScanExit(*service_request, *service_response); } +#if __ROS_VERSION > 0 + bool serviceCbFieldSetRead(sick_scan_srv::FieldSetReadSrv::Request &service_request, sick_scan_srv::FieldSetReadSrv::Response &service_response); + bool serviceCbFieldSetReadROS2(std::shared_ptr service_request, std::shared_ptr service_response) { return serviceCbFieldSetRead(*service_request, *service_response); } + bool serviceCbFieldSetWrite(sick_scan_srv::FieldSetWriteSrv::Request &service_request, sick_scan_srv::FieldSetWriteSrv::Response &service_response); + bool serviceCbFieldSetWriteROS2(std::shared_ptr service_request, std::shared_ptr service_response) { return serviceCbFieldSetWrite(*service_request, *service_response); } +#endif + #if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 /*! * Sends the multiScan start commands "sWN ScanDataFormat", "sWN ScanDataPreformatting", "sWN ScanDataEthSettings", "sWN ScanDataEnable 1", "sMN LMCstartmeas", "sMN Run" @@ -158,8 +165,10 @@ namespace sick_scan_xd * @param[in] scandataformat ScanDataFormat: 1 for msgpack or 2 for compact scandata, default: 1 * @param[in] imu_enable: Imu data transfer enabled * @param[in] imu_udp_port: UDP port of imu data (if imu_enable is true) + * @param[in] check_udp_receiver_ip: check udp_receiver_ip by sending and receiving a udp test message + * @param[in] check_udp_receiver_port: udp port to check udp_receiver_ip */ - bool sendMultiScanStartCmd(const std::string& hostname, int port, const std::string& scanner_type, int scandataformat, bool imu_enable, int imu_udp_port, int performanceprofilenumber); + bool sendMultiScanStartCmd(const std::string& hostname, int port, const std::string& scanner_type, int scandataformat, bool& imu_enable, int imu_udp_port, int performanceprofilenumber, bool check_udp_receiver_ip, int check_udp_receiver_port); /*! * Sends the multiScan stop commands "sWN ScanDataEnable 0" and "sMN Run" @@ -246,6 +255,10 @@ namespace sick_scan_xd rosServiceServer m_srv_server_SCreboot; ///< service "SCreboot", &sick_scan::SickScanServices::serviceCbSCreboot rosServiceServer m_srv_server_SCsoftreset; ///< service "SCsoftreset", &sick_scan::SickScanServices::serviceCbSCsoftreset rosServiceServer m_srv_server_SickScanExit; ///< service "SickScanExitSrv", &sick_scan::SickScanServices::serviceCbSickScanExit +#if __ROS_VERSION > 0 + rosServiceServer m_srv_server_FieldSetRead ; ///< service "FieldSetReadSrv", &sick_scan::SickScanServices::serviceCbFieldSetRead + rosServiceServer m_srv_server_FieldSetWrite; ///< service "FieldSetWriteSrv", &sick_scan::SickScanServices::serviceCbFieldSetWrite +#endif }; /* class SickScanServices */ diff --git a/include/sick_scan/sick_scan_xd_version.h b/include/sick_scan/sick_scan_xd_version.h index a4497679..c44a9208 100644 --- a/include/sick_scan/sick_scan_xd_version.h +++ b/include/sick_scan/sick_scan_xd_version.h @@ -1,4 +1,4 @@ /* This file has been generated by package_version_tool.py. Do not edit this file. To provide a new version number, modify file package.xml and rebuild */ #ifndef SICK_SCAN_XD_VERSION -#define SICK_SCAN_XD_VERSION "3.5.0" +#define SICK_SCAN_XD_VERSION "3.6.0" #endif // SICK_SCAN_XD_VERSION diff --git a/include/sick_scan/softwarePLL.h b/include/sick_scan/softwarePLL.h index b4aa9803..dc2fddc1 100644 --- a/include/sick_scan/softwarePLL.h +++ b/include/sick_scan/softwarePLL.h @@ -30,12 +30,14 @@ class SoftwarePLL ~SoftwarePLL() {} - bool pushIntoFifo(double curTimeStamp, uint32_t curtick);// update tick fifo and update clock (timestamp) fifo; - double extraPolateRelativeTimeStamp(uint32_t tick); + bool pushIntoFifo(double curTimeStamp, uint64_t curtick);// update tick fifo and update clock (timestamp) fifo; + double extraPolateRelativeTimeStamp(uint64_t tick); bool getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32_t tick); + bool getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint64_t tick); bool convSystemtimeToLidarTimestamp(uint32_t systemtime_sec, uint32_t systemtime_nanosec, uint32_t& tick); + bool convSystemtimeToLidarTimestamp(uint32_t systemtime_sec, uint32_t systemtime_nanosec, uint64_t& tick); bool getDemoFileData(std::string fileName, std::vector &tickVec, std::vector &secVec, std::vector &nanoSecVec); @@ -48,7 +50,14 @@ class SoftwarePLL { return (offsetTimestampFirstLidarTick > 0); } - return isInitialized; + else if (ticksToTimestampMode == TICKS_TO_LIDAR_TIMESTAMP) + { + return true; // no initialization required + } + else + { + return isInitialized; + } } void IsInitialized(bool val) @@ -85,6 +94,7 @@ class SoftwarePLL { extrapolationDivergenceCounter = val; } bool updatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick); + bool updatePLL(uint32_t sec, uint32_t nanoSec, uint64_t curtick); int findDiffInFifo(double diff, double tol); @@ -102,17 +112,17 @@ class SoftwarePLL int numberValInFifo; static const double MaxAllowedTimeDeviation; static const uint32_t MaxExtrapolationCounter; - uint32_t tickFifo[fifoSize]; // = { 0 }; + uint64_t tickFifo[fifoSize]; // = { 0 }; double clockFifo[fifoSize]; double lastValidTimeStamp; - uint32_t lastValidTick; // = 0; + // uint64_t lastValidTick; // = 0; bool isInitialized; // = false; double dTAvgFeedback; // = 0.0; double dClockDiffFeedBack; // = 0.0; double firstTimeStamp; double allowedTimeDeviation; uint64_t firstTick; - uint32_t lastcurtick = 0; + uint64_t lastcurtick = 0; uint32_t mostRecentSec; uint32_t mostRecentNanoSec; double mostRecentTimeStamp; @@ -121,12 +131,15 @@ class SoftwarePLL enum TICKS_TO_TIMESTAMP_MODE { TICKS_TO_SYSTEM_TIMESTAMP = 0, // default: convert lidar ticks in microseconds to system timestamp by software-pll - TICKS_TO_MICROSEC_OFFSET_TIMESTAMP = 1 // optional tick-mode: convert lidar ticks in microseconds to timestamp by 1.0e-6*(curtick-firstTick)+firstSystemTimestamp + TICKS_TO_MICROSEC_OFFSET_TIMESTAMP = 1, // optional tick-mode: convert lidar ticks in microseconds to timestamp by 1.0e-6*(curtick-firstTick)+firstSystemTimestamp + TICKS_TO_LIDAR_TIMESTAMP = 2 // optional tick-mode: convert lidar ticks in microseconds directly into a lidar timestamp by sec = tick/1000000, nsec = 1000*(tick%1000000) + // Note: Using tick_to_timestamp_mode = 2, the timestamps in ROS message headers will be in lidar time, not in system time. Lidar and system time can be very different. + // Using tick_to_timestamp_mode = 2 might cause unexpected results or error messages. We recommend using tick_to_timestamp_mode = 2 for special test cases only. }; TICKS_TO_TIMESTAMP_MODE ticksToTimestampMode = TICKS_TO_SYSTEM_TIMESTAMP; uint32_t offsetTimestampFirstSystemSec = 0; uint32_t offsetTimestampFirstSystemMicroSec = 0; - uint32_t offsetTimestampFirstLidarTick = 0; + uint64_t offsetTimestampFirstLidarTick = 0; bool nearSameTimeStamp(double relTimeStamp1, double relTimeStamp2, double& delta_time_abs); diff --git a/include/sick_scansegment_xd/config.h b/include/sick_scansegment_xd/config.h index 8139d084..a1efa67e 100644 --- a/include/sick_scansegment_xd/config.h +++ b/include/sick_scansegment_xd/config.h @@ -128,6 +128,9 @@ namespace sick_scansegment_xd 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 + bool check_udp_receiver_ip = false; // check udp_receiver_ip by sending and receiving a udp test message + int check_udp_receiver_port = 2116; // udp port to check udp_receiver_ip + double all_segments_min_deg = -180; // -180 // angle range covering all segments: all segments pointcloud on topic publish_topic_all_segments is published, double all_segments_max_deg = +180; // +180 // if received segments cover angle range from all_segments_min_deg to all_segments_max_deg. -180...+180 for MultiScan136 (360 deg fullscan) @@ -146,7 +149,8 @@ namespace sick_scansegment_xd // int port; // IP port of multiScan136 to post start and stop commands // bool send_udp_start; // Send udp start string to multiScan136, default: False (obsolete) // std::string send_udp_start_string; // udp string to start multiScan136, default: "magicalActivate" - int udp_timeout_ms; // Timeout for udp messages in milliseconds, default: 60*1000 + int udp_timeout_ms; // Timeout for udp messages in milliseconds, default: 10*1000 + int udp_timeout_ms_initial; // Initial timeout for udp messages after start in milliseconds, default: 60*1000 int scandataformat; // ScanDataFormat: 1 for msgpack or 2 for compact scandata, default: 1 int performanceprofilenumber; // Set performance profile by sending "sWN PerformanceProfileNumber" if performanceprofilenumber >= 0 (picoScan), default: -1 bool imu_enable; // IMU enabled or disabled diff --git a/include/sick_scansegment_xd/ros_msgpack_publisher.h b/include/sick_scansegment_xd/ros_msgpack_publisher.h index 7b8e2257..4f3c9c88 100644 --- a/include/sick_scansegment_xd/ros_msgpack_publisher.h +++ b/include/sick_scansegment_xd/ros_msgpack_publisher.h @@ -235,18 +235,19 @@ namespace sick_scansegment_xd } void appendLidarPoints(const std::vector>& points, int32_t segment_idx, int32_t telegram_cnt) { - if (lidar_timestamp_start_microsec == 0) - lidar_timestamp_start_microsec = lidar_points[0][0].lidar_timestamp_microsec; - if (lidar_timestamp_stop_microsec == 0) - lidar_timestamp_stop_microsec = lidar_points[0][0].lidar_timestamp_microsec; for (int echoIdx = 0; echoIdx < points.size() && echoIdx < lidar_points.size(); echoIdx++) { size_t num_points = points[echoIdx].size(); if (num_points > 0) { lidar_points[echoIdx].insert(lidar_points[echoIdx].end(), points[echoIdx].begin(), points[echoIdx].end()); - lidar_timestamp_start_microsec = std::min(lidar_points[echoIdx][0].lidar_timestamp_microsec, lidar_timestamp_start_microsec); - lidar_timestamp_stop_microsec = std::max(lidar_points[echoIdx][num_points].lidar_timestamp_microsec, lidar_timestamp_stop_microsec); + assert(lidar_points.size() > echoIdx && lidar_points[echoIdx].size() > 0); + if (lidar_timestamp_start_microsec == 0) // first time initialization of lidar start timestamp + lidar_timestamp_start_microsec = lidar_points[0].front().lidar_timestamp_microsec; + if (lidar_timestamp_stop_microsec == 0) // first time initialization of lidar stop timestamp + lidar_timestamp_stop_microsec = lidar_points[0].front().lidar_timestamp_microsec; + lidar_timestamp_start_microsec = std::min(lidar_points[echoIdx].front().lidar_timestamp_microsec, lidar_timestamp_start_microsec); // update lidar start timestamp + lidar_timestamp_stop_microsec = std::max(lidar_points[echoIdx].back().lidar_timestamp_microsec, lidar_timestamp_stop_microsec); // update lidar stop timestamp for (int n = 0; n < num_points; n++) { const sick_scansegment_xd::PointXYZRAEI32f& point = points[echoIdx][n]; diff --git a/include/sick_scansegment_xd/udp_sockets.h b/include/sick_scansegment_xd/udp_sockets.h index 0bdc3328..d179b971 100644 --- a/include/sick_scansegment_xd/udp_sockets.h +++ b/include/sick_scansegment_xd/udp_sockets.h @@ -151,12 +151,13 @@ namespace sick_scansegment_xd return false; } // #if defined WIN32 || defined _MSC_VER - // char broadcast_opt = 1, reuse_addr_opt = 1; + // char broadcast_opt = 1, reuse_addr_opt = 1, reuse_port_opt = 1; // #else - // int broadcast_opt = 1, reuse_addr_opt = 1; + // int broadcast_opt = 1, reuse_addr_opt = 1, reuse_port_opt = 1; // #endif // setsockopt(m_udp_socket, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)); // setsockopt(m_udp_socket, SOL_SOCKET, SO_REUSEADDR, &reuse_addr_opt, sizeof(reuse_addr_opt)); + // setsockopt(m_udp_socket, SOL_SOCKET, SO_REUSEPORT, &reuse_port_opt, sizeof(reuse_port_opt)); struct sockaddr_in sim_servaddr = { 0 }; if(m_udp_sender.empty()) sim_servaddr.sin_addr.s_addr = htonl(INADDR_ANY); diff --git a/launch/sick_ldmrs.launch b/launch/sick_ldmrs.launch index 44b152d1..c46b680f 100644 --- a/launch/sick_ldmrs.launch +++ b/launch/sick_ldmrs.launch @@ -102,7 +102,10 @@ diff --git a/launch/sick_lms_1xx.launch b/launch/sick_lms_1xx.launch index 58720cfb..afaea658 100644 --- a/launch/sick_lms_1xx.launch +++ b/launch/sick_lms_1xx.launch @@ -101,7 +101,10 @@ diff --git a/launch/sick_lms_1xxx.launch b/launch/sick_lms_1xxx.launch index f859ab46..737401ae 100644 --- a/launch/sick_lms_1xxx.launch +++ b/launch/sick_lms_1xxx.launch @@ -110,7 +110,10 @@ diff --git a/launch/sick_lms_1xxx_v2.launch b/launch/sick_lms_1xxx_v2.launch index 3bead6a7..31bf828f 100644 --- a/launch/sick_lms_1xxx_v2.launch +++ b/launch/sick_lms_1xxx_v2.launch @@ -115,7 +115,10 @@ diff --git a/launch/sick_lms_4xxx.launch b/launch/sick_lms_4xxx.launch index c41ee8de..e510eb65 100644 --- a/launch/sick_lms_4xxx.launch +++ b/launch/sick_lms_4xxx.launch @@ -134,7 +134,10 @@ diff --git a/launch/sick_lms_5xx.launch b/launch/sick_lms_5xx.launch index e57754bc..08cf49b8 100644 --- a/launch/sick_lms_5xx.launch +++ b/launch/sick_lms_5xx.launch @@ -126,7 +126,10 @@ Check IP-address, if you scanner is not found after roslaunch. diff --git a/launch/sick_lrs_36x0.launch b/launch/sick_lrs_36x0.launch index cc1987fe..3b60cddc 100644 --- a/launch/sick_lrs_36x0.launch +++ b/launch/sick_lrs_36x0.launch @@ -109,7 +109,10 @@ diff --git a/launch/sick_lrs_36x0_upside_down.launch b/launch/sick_lrs_36x0_upside_down.launch index 5d0613fb..568a1f08 100644 --- a/launch/sick_lrs_36x0_upside_down.launch +++ b/launch/sick_lrs_36x0_upside_down.launch @@ -109,7 +109,10 @@ diff --git a/launch/sick_lrs_36x1.launch b/launch/sick_lrs_36x1.launch index 8d8797ee..cc9f4a6f 100644 --- a/launch/sick_lrs_36x1.launch +++ b/launch/sick_lrs_36x1.launch @@ -108,7 +108,10 @@ diff --git a/launch/sick_lrs_36x1_upside_down.launch b/launch/sick_lrs_36x1_upside_down.launch index aeeb856e..e9dec5fc 100644 --- a/launch/sick_lrs_36x1_upside_down.launch +++ b/launch/sick_lrs_36x1_upside_down.launch @@ -108,7 +108,10 @@ diff --git a/launch/sick_lrs_4xxx.launch b/launch/sick_lrs_4xxx.launch index 3002df53..3d01506a 100644 --- a/launch/sick_lrs_4xxx.launch +++ b/launch/sick_lrs_4xxx.launch @@ -113,7 +113,10 @@ diff --git a/launch/sick_mrs_1xxx.launch b/launch/sick_mrs_1xxx.launch index 30a338d8..76a54177 100644 --- a/launch/sick_mrs_1xxx.launch +++ b/launch/sick_mrs_1xxx.launch @@ -132,7 +132,10 @@ default max_angle for this scanner type is +137.5 degree. diff --git a/launch/sick_mrs_1xxx_cartographer.launch b/launch/sick_mrs_1xxx_cartographer.launch index 86fb4861..c7fb04dd 100644 --- a/launch/sick_mrs_1xxx_cartographer.launch +++ b/launch/sick_mrs_1xxx_cartographer.launch @@ -109,7 +109,10 @@ default max_angle for this scanner type is +137.5 degree. diff --git a/launch/sick_mrs_6xxx.launch b/launch/sick_mrs_6xxx.launch index 63a2f72b..4e401493 100644 --- a/launch/sick_mrs_6xxx.launch +++ b/launch/sick_mrs_6xxx.launch @@ -102,7 +102,10 @@ diff --git a/launch/sick_mrs_6xxx_first_echo.launch b/launch/sick_mrs_6xxx_first_echo.launch index 01127af2..77711ea3 100644 --- a/launch/sick_mrs_6xxx_first_echo.launch +++ b/launch/sick_mrs_6xxx_first_echo.launch @@ -97,7 +97,10 @@ diff --git a/launch/sick_mrs_6xxx_last_echo.launch b/launch/sick_mrs_6xxx_last_echo.launch index e731b646..55f6905c 100644 --- a/launch/sick_mrs_6xxx_last_echo.launch +++ b/launch/sick_mrs_6xxx_last_echo.launch @@ -96,7 +96,10 @@ diff --git a/launch/sick_mrs_6xxx_narrow.launch b/launch/sick_mrs_6xxx_narrow.launch index 33ad0f69..274d45fa 100644 --- a/launch/sick_mrs_6xxx_narrow.launch +++ b/launch/sick_mrs_6xxx_narrow.launch @@ -98,7 +98,10 @@ diff --git a/launch/sick_multiscan.launch b/launch/sick_multiscan.launch index 79446b25..ca65ebde 100644 --- a/launch/sick_multiscan.launch +++ b/launch/sick_multiscan.launch @@ -34,6 +34,8 @@ + + @@ -45,7 +47,8 @@ - + + @@ -57,7 +60,7 @@ - + @@ -128,7 +131,7 @@ Parameter "fields" defines the fields of the pointcloud for coordinateNotation == 3 (customized pointcloud fields), e.g. fields=x,y,z,i: cartesian pointcloud fields=range,azimuth,elevation: polar pointcloud - or any other combination of x,y,z,i,range,azimuth,elevation,t,ts,ring,layer,echo,reflector + or any other combination of x,y,z,i,range,azimuth,elevation,t,ts,lidar_sec,lidar_nsec,ring,layer,echo,reflector These fields have the following meaning: field "x": cartesian x coordinate in meter in ROS coordinates (4 byte, float32) field "y": cartesian y coordinate in meter in ROS coordinates (4 byte, float32) @@ -139,6 +142,8 @@ field "elevation": polar coordinate elevation in radians (4 byte, float32) field "t": time offset in nano seconds relative to the header timestamp in the point cloud (4 byte, uint32), used by rtabmap for deskewing field "ts": time offset in seconds relative to the header timestamp (4 byte, float32) + field "lidar_sec": 4 byte seconds part of the lidar timestamp in microseconds (lidar time), lidar_sec = (uint32_t)(lidar_timestamp_microsec / 1000000) + field "lidar_nsec": 4 byte nano seconds part of the lidar timestamp in microseconds (lidar time), lidar_nsec = (uint32_t)(1000 * (lidar_timestamp_microsec % 1000000)) field "ring": layer id (1 byte, int8), identical to field "layer" field "layer": layer (group) index (4 byte, int32), 0 <= layer < 16 for multiScan (16 layer), 0 for picoScan (1 layer) field "echo": echo index (4 byte, int32) @@ -249,11 +254,11 @@ - - + + - - + + diff --git a/launch/sick_multiscan_rtabmap.launch b/launch/sick_multiscan_rtabmap.launch index 8e8817f0..03a1484d 100644 --- a/launch/sick_multiscan_rtabmap.launch +++ b/launch/sick_multiscan_rtabmap.launch @@ -40,6 +40,8 @@ + + @@ -51,7 +53,8 @@ - + + @@ -83,7 +86,7 @@ - + diff --git a/launch/sick_nav_2xx.launch b/launch/sick_nav_2xx.launch index 901630e5..898ed14d 100644 --- a/launch/sick_nav_2xx.launch +++ b/launch/sick_nav_2xx.launch @@ -84,7 +84,10 @@ diff --git a/launch/sick_nav_31x.launch b/launch/sick_nav_31x.launch index 2d4c1ad9..71cca1a3 100644 --- a/launch/sick_nav_31x.launch +++ b/launch/sick_nav_31x.launch @@ -101,7 +101,10 @@ diff --git a/launch/sick_nav_350.launch b/launch/sick_nav_350.launch index e8ebcf8e..e08c2a5f 100644 --- a/launch/sick_nav_350.launch +++ b/launch/sick_nav_350.launch @@ -75,7 +75,10 @@ diff --git a/launch/sick_new_ip.launch b/launch/sick_new_ip.launch index 577c5132..ef974927 100644 --- a/launch/sick_new_ip.launch +++ b/launch/sick_new_ip.launch @@ -42,7 +42,10 @@ diff --git a/launch/sick_oem_15xx.launch b/launch/sick_oem_15xx.launch index 545f6be3..66b173fc 100644 --- a/launch/sick_oem_15xx.launch +++ b/launch/sick_oem_15xx.launch @@ -108,7 +108,10 @@ diff --git a/launch/sick_picoscan.launch b/launch/sick_picoscan.launch index fae3740e..2d9b068e 100644 --- a/launch/sick_picoscan.launch +++ b/launch/sick_picoscan.launch @@ -18,6 +18,10 @@ + + + + @@ -30,6 +34,8 @@ + + @@ -40,11 +46,12 @@ - + - + + @@ -58,7 +65,7 @@ - + @@ -73,6 +80,10 @@ + + + + @@ -112,7 +123,7 @@ Parameter "fields" defines the fields of the pointcloud for coordinateNotation == 3 (customized pointcloud fields), e.g. fields=x,y,z,i: cartesian pointcloud fields=range,azimuth,elevation: polar pointcloud - or any other combination of x,y,z,i,range,azimuth,elevation,t,ts,ring,layer,echo,reflector + or any other combination of x,y,z,i,range,azimuth,elevation,t,ts,lidar_sec,lidar_nsec,ring,layer,echo,reflector These fields have the following meaning: field "x": cartesian x coordinate in meter in ROS coordinates (4 byte, float32) field "y": cartesian y coordinate in meter in ROS coordinates (4 byte, float32) @@ -123,6 +134,8 @@ field "elevation": polar coordinate elevation in radians (4 byte, float32) field "t": time offset in nano seconds relative to the header timestamp in the point cloud (4 byte, uint32), used by rtabmap for deskewing field "ts": time offset in seconds relative to the header timestamp (4 byte, float32) + field "lidar_sec": 4 byte seconds part of the lidar timestamp in microseconds (lidar time), lidar_sec = (uint32_t)(lidar_timestamp_microsec / 1000000) + field "lidar_nsec": 4 byte nano seconds part of the lidar timestamp in microseconds (lidar time), lidar_nsec = (uint32_t)(1000 * (lidar_timestamp_microsec % 1000000)) field "ring": layer id (1 byte, int8), identical to field "layer" field "layer": layer (group) index (4 byte, int32), 0 <= layer < 16 for multiScan (16 layer), 0 for picoScan (1 layer) field "echo": echo index (4 byte, int32) @@ -234,10 +247,10 @@ - + - + diff --git a/launch/sick_picoscan_120.launch b/launch/sick_picoscan_120.launch index ff8fb83a..39aa566e 100644 --- a/launch/sick_picoscan_120.launch +++ b/launch/sick_picoscan_120.launch @@ -1,10 +1,10 @@ - + - - + + @@ -13,8 +13,8 @@ - - + + @@ -26,8 +26,10 @@ + + - + @@ -40,16 +42,16 @@ - + + - - + @@ -65,7 +67,7 @@ - + @@ -98,7 +100,7 @@ Parameter "fields" defines the fields of the pointcloud for coordinateNotation == 3 (customized pointcloud fields), e.g. fields=x,y,z,i: cartesian pointcloud fields=range,azimuth,elevation: polar pointcloud - or any other combination of x,y,z,i,range,azimuth,elevation,t,ts,ring,layer,echo,reflector + or any other combination of x,y,z,i,range,azimuth,elevation,t,ts,lidar_sec,lidar_nsec,ring,layer,echo,reflector These fields have the following meaning: field "x": cartesian x coordinate in meter in ROS coordinates (4 byte, float32) field "y": cartesian y coordinate in meter in ROS coordinates (4 byte, float32) @@ -109,30 +111,26 @@ field "elevation": polar coordinate elevation in radians (4 byte, float32) field "t": time offset in nano seconds relative to the header timestamp in the point cloud (4 byte, uint32), used by rtabmap for deskewing field "ts": time offset in seconds relative to the header timestamp (4 byte, float32) + field "lidar_sec": 4 byte seconds part of the lidar timestamp in microseconds (lidar time), lidar_sec = (uint32_t)(lidar_timestamp_microsec / 1000000) + field "lidar_nsec": 4 byte nano seconds part of the lidar timestamp in microseconds (lidar time), lidar_nsec = (uint32_t)(1000 * (lidar_timestamp_microsec % 1000000)) field "ring": layer id (1 byte, int8), identical to field "layer" - field "layer": layer (group) index (4 byte, int32), 0 <= layer < 16 for multiScan (16 layer), 0 for picoScan (1 layer) + field "layer": layer (group) index (4 byte, int32), 0 for picoScan (1 layer) field "echo": echo index (4 byte, int32) field "reflector": optional reflector bit (1 byte, uint8), 0 or 1, default: 0 Parameter "echos" defines which echos are included in the pointcloud, e.g. echos=0 - Note: The picoScan120 has only one echo + Note: The picoScan120 has one echo. echos=0 refers to the first echo. Parameter "layers" defines which layers are included in the pointcloud, e.g layers=1, for all layers - Note: The picoScan120 has only one layer + Note: The picoScan120 has one horizontal layer. Parameter "reflectors" filters the points by the reflector bit, i.e. reflectors=0,1 for points with reflector bit set or not set reflectors=0 for points with reflector bit not set reflectors=1 for points with reflector bit set - Parameter "infringed" defines filters the points by infringement, i.e. - infringed=0,1 for points with infringement bit set or not set - infringed=0 for points with infringement bit not set - infringed=1 for points with infringement bit set - Parameter "infringed" is currently not supported (reserved for future use) - Optional parameter "rangeFilter" configures how invalid measurements are filtered in the customized pointcloud. rangeFilter=,, The range (distance) of invalid scan points is 0 and can be filtered, i.e. removed from the pointcloud or set to 0, FLT_MAX or NAN. @@ -157,40 +155,40 @@ --> - + - + - - + + - - + + - - + + - - + + - + - + - - + + - - + + - - + + - - + + diff --git a/launch/sick_picoscan_single_echo.launch b/launch/sick_picoscan_single_echo.launch index c52ee9ac..c30a464f 100644 --- a/launch/sick_picoscan_single_echo.launch +++ b/launch/sick_picoscan_single_echo.launch @@ -16,6 +16,10 @@ + + + + @@ -29,6 +33,8 @@ + + @@ -43,7 +49,8 @@ - + + @@ -57,7 +64,7 @@ - + @@ -72,6 +79,10 @@ + + + + @@ -111,7 +122,7 @@ Parameter "fields" defines the fields of the pointcloud for coordinateNotation == 3 (customized pointcloud fields), e.g. fields=x,y,z,i: cartesian pointcloud fields=range,azimuth,elevation: polar pointcloud - or any other combination of x,y,z,i,range,azimuth,elevation,t,ts,ring,layer,echo,reflector + or any other combination of x,y,z,i,range,azimuth,elevation,t,ts,lidar_sec,lidar_nsec,ring,layer,echo,reflector These fields have the following meaning: field "x": cartesian x coordinate in meter in ROS coordinates (4 byte, float32) field "y": cartesian y coordinate in meter in ROS coordinates (4 byte, float32) @@ -122,6 +133,8 @@ field "elevation": polar coordinate elevation in radians (4 byte, float32) field "t": time offset in nano seconds relative to the header timestamp in the point cloud (4 byte, uint32), used by rtabmap for deskewing field "ts": time offset in seconds relative to the header timestamp (4 byte, float32) + field "lidar_sec": 4 byte seconds part of the lidar timestamp in microseconds (lidar time), lidar_sec = (uint32_t)(lidar_timestamp_microsec / 1000000) + field "lidar_nsec": 4 byte nano seconds part of the lidar timestamp in microseconds (lidar time), lidar_nsec = (uint32_t)(1000 * (lidar_timestamp_microsec % 1000000)) field "ring": layer id (1 byte, int8), identical to field "layer" field "layer": layer (group) index (4 byte, int32), 0 <= layer < 16 for multiScan (16 layer), 0 for picoScan (1 layer) field "echo": echo index (4 byte, int32) @@ -233,10 +246,10 @@ - + - + diff --git a/launch/sick_rms_xxxx.launch b/launch/sick_rms_xxxx.launch index c8ad7702..33af8667 100644 --- a/launch/sick_rms_xxxx.launch +++ b/launch/sick_rms_xxxx.launch @@ -9,7 +9,8 @@ - + + @@ -41,6 +42,9 @@ + + + @@ -82,7 +86,10 @@ diff --git a/launch/sick_tim_240.launch b/launch/sick_tim_240.launch index e2920abb..b25a92c2 100644 --- a/launch/sick_tim_240.launch +++ b/launch/sick_tim_240.launch @@ -105,7 +105,10 @@ diff --git a/launch/sick_tim_4xx.launch b/launch/sick_tim_4xx.launch index 549b438b..8e66ec22 100644 --- a/launch/sick_tim_4xx.launch +++ b/launch/sick_tim_4xx.launch @@ -129,7 +129,10 @@ diff --git a/launch/sick_tim_5xx.launch b/launch/sick_tim_5xx.launch index 5a049180..5d6b8896 100644 --- a/launch/sick_tim_5xx.launch +++ b/launch/sick_tim_5xx.launch @@ -106,7 +106,10 @@ diff --git a/launch/sick_tim_5xx_ASCII.launch b/launch/sick_tim_5xx_ASCII.launch index 85de64d5..5b012108 100644 --- a/launch/sick_tim_5xx_ASCII.launch +++ b/launch/sick_tim_5xx_ASCII.launch @@ -92,7 +92,10 @@ diff --git a/launch/sick_tim_7xx.launch b/launch/sick_tim_7xx.launch index 0802780c..a5e0fb5e 100644 --- a/launch/sick_tim_7xx.launch +++ b/launch/sick_tim_7xx.launch @@ -30,6 +30,7 @@ + @@ -64,6 +65,8 @@ + + @@ -107,7 +110,10 @@ diff --git a/launch/sick_tim_7xxS.launch b/launch/sick_tim_7xxS.launch index 9eb07da2..40b5ea3e 100644 --- a/launch/sick_tim_7xxS.launch +++ b/launch/sick_tim_7xxS.launch @@ -67,6 +67,8 @@ + + @@ -84,7 +86,7 @@ - + @@ -120,7 +122,10 @@ diff --git a/msg/LIDinputstateMsg.msg b/msg/LIDinputstateMsg.msg new file mode 100644 index 00000000..1ef94870 --- /dev/null +++ b/msg/LIDinputstateMsg.msg @@ -0,0 +1,22 @@ +# This message contains the "sSN LIDinputstate" message of a Sick laser scanner as an ros message. +# + +std_msgs/Header header + +# Status code +uint16 version_number # Status code version number +uint32 system_counter # Status code system counter (time in microsec since power up, max. 71 min then starting from 0 again) + +# Array of input states +uint8[] input_state # input states, each state with value 0 (not active), 1 (active) or 2 (not used) +int32 active_fieldset # active fieldset + +# Time block (sensortime from the last change of min. one of the outputs) +uint16 time_state # No time data: 0, Time data: 1 +uint16 year # f.e. 2021 +uint8 month # 1 ... 12 +uint8 day # 1 ... 31 +uint8 hour # 0 ... 23 +uint8 minute # 0 ... 59 +uint8 second # 0 ... 59 +uint32 microsecond # 0 ... 999999 diff --git a/package.xml b/package.xml index e9973a9a..9fe13536 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ sick_scan_xd - 3.5.0 + 3.6.0 ROS 1 and 2 driver for SICK scanner rostest Michael Lehning diff --git a/python/api/sick_scan_api.py b/python/api/sick_scan_api.py index 1877ad65..01296260 100644 --- a/python/api/sick_scan_api.py +++ b/python/api/sick_scan_api.py @@ -1231,6 +1231,7 @@ def SickScanApiGetStatus(sick_scan_library, api_handle, status_code, message_buf """ return sick_scan_library.SickScanApiGetStatus(api_handle, status_code, message_buffer, message_buffer_size) +# sopas_string_buffer = {} def SickScanApiSendSOPAS(sick_scan_library, api_handle, sopas_command, response_buffer_size = 1024): """ Sends a SOPAS command like "sRN SCdevicestate" or "sRN ContaminationResult" and returns the lidar response @@ -1238,6 +1239,10 @@ def SickScanApiSendSOPAS(sick_scan_library, api_handle, sopas_command, response_ response_buffer_size = max(1024, response_buffer_size) ctypes_response_buffer = ctypes.create_string_buffer(response_buffer_size + 1) ret_val = sick_scan_library.SickScanApiSendSOPAS(api_handle, ctypes.create_string_buffer(str.encode(sopas_command)), ctypes_response_buffer, response_buffer_size) + # global sopas_string_buffer + # if sopas_command not in sopas_string_buffer: + # sopas_string_buffer[sopas_command] = ctypes.create_string_buffer(str.encode(sopas_command)) + # ret_val = sick_scan_library.SickScanApiSendSOPAS(api_handle, sopas_string_buffer[sopas_command], ctypes_response_buffer, response_buffer_size) return ctypes_response_buffer.value.decode() def SickScanApiSetVerboseLevel(sick_scan_library, api_handle, verbose_level): diff --git a/roswrap/src/include/boost_wrap/shared_count.hpp b/roswrap/src/include/boost_wrap/shared_count.hpp index 907a854b..f0ea6baa 100644 --- a/roswrap/src/include/boost_wrap/shared_count.hpp +++ b/roswrap/src/include/boost_wrap/shared_count.hpp @@ -69,7 +69,7 @@ template< class T > class sp_reference_wrapper { public: - explicit sp_reference_wrapper( T & t): t_(__builtin_addressof(t)) // t_( boost::addressof( t ) ) + explicit sp_reference_wrapper( T & t): t_(std::addressof(t)) // t_( boost::addressof( t ) ) { } diff --git a/roswrap/src/include/boost_wrap/sp_counted_impl.hpp b/roswrap/src/include/boost_wrap/sp_counted_impl.hpp index 1db27602..05399eae 100644 --- a/roswrap/src/include/boost_wrap/sp_counted_impl.hpp +++ b/roswrap/src/include/boost_wrap/sp_counted_impl.hpp @@ -173,7 +173,7 @@ template class sp_counted_impl_pd: public sp_counted_base virtual void * get_local_deleter( sp_typeinfo const & ti ) { - return ti == BOOST_SP_TYPEID(D)? boost::detail::get_local_deleter( __builtin_addressof( del ) ): 0; + return ti == BOOST_SP_TYPEID(D)? boost::detail::get_local_deleter( std::addressof( del ) ): 0; } virtual void * get_untyped_deleter() @@ -266,7 +266,7 @@ template class sp_counted_impl_pda: public sp_counted virtual void * get_local_deleter( sp_typeinfo const & ti ) { - return ti == BOOST_SP_TYPEID(D)? boost::detail::get_local_deleter( __builtin_addressof( d_ ) ): 0; + return ti == BOOST_SP_TYPEID(D)? boost::detail::get_local_deleter( std::addressof( d_ ) ): 0; } virtual void * get_untyped_deleter() diff --git a/roswrap/src/include/sick_scan_xd/LIDinputstateMsg.h b/roswrap/src/include/sick_scan_xd/LIDinputstateMsg.h new file mode 100644 index 00000000..69c596db --- /dev/null +++ b/roswrap/src/include/sick_scan_xd/LIDinputstateMsg.h @@ -0,0 +1,347 @@ +#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_xd/LIDinputstateMsg.msg +// DO NOT EDIT! + + +#ifndef SICK_SCAN_XD_MESSAGE_LIDINPUTSTATEMSG_H +#define SICK_SCAN_XD_MESSAGE_LIDINPUTSTATEMSG_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sick_scan_xd +{ +template +struct LIDinputstateMsg_ +{ + typedef LIDinputstateMsg_ Type; + + LIDinputstateMsg_() + : header() + , version_number(0) + , system_counter(0) + , input_state() + , active_fieldset(0) + , time_state(0) + , year(0) + , month(0) + , day(0) + , hour(0) + , minute(0) + , second(0) + , microsecond(0) { + } + LIDinputstateMsg_(const ContainerAllocator& _alloc) + : header(_alloc) + , version_number(0) + , system_counter(0) + , input_state(_alloc) + , active_fieldset(0) + , time_state(0) + , year(0) + , month(0) + , day(0) + , hour(0) + , minute(0) + , second(0) + , microsecond(0) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint16_t _version_number_type; + _version_number_type version_number; + + typedef uint32_t _system_counter_type; + _system_counter_type system_counter; + + typedef std::vector::template rebind_alloc> _input_state_type; + _input_state_type input_state; + + typedef int32_t _active_fieldset_type; + _active_fieldset_type active_fieldset; + + typedef uint16_t _time_state_type; + _time_state_type time_state; + + typedef uint16_t _year_type; + _year_type year; + + typedef uint8_t _month_type; + _month_type month; + + typedef uint8_t _day_type; + _day_type day; + + typedef uint8_t _hour_type; + _hour_type hour; + + typedef uint8_t _minute_type; + _minute_type minute; + + typedef uint8_t _second_type; + _second_type second; + + typedef uint32_t _microsecond_type; + _microsecond_type microsecond; + + + + + + typedef std::shared_ptr< ::sick_scan_xd::LIDinputstateMsg_ > Ptr; + typedef std::shared_ptr< ::sick_scan_xd::LIDinputstateMsg_ const> ConstPtr; + +}; // struct LIDinputstateMsg_ + +typedef ::sick_scan_xd::LIDinputstateMsg_ > LIDinputstateMsg; + +typedef std::shared_ptr< ::sick_scan_xd::LIDinputstateMsg > LIDinputstateMsgPtr; +typedef std::shared_ptr< ::sick_scan_xd::LIDinputstateMsg const> LIDinputstateMsgConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sick_scan_xd::LIDinputstateMsg_ & v) +{ +ros::message_operations::Printer< ::sick_scan_xd::LIDinputstateMsg_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sick_scan_xd::LIDinputstateMsg_ & lhs, const ::sick_scan_xd::LIDinputstateMsg_ & rhs) +{ + return lhs.header == rhs.header && + lhs.version_number == rhs.version_number && + lhs.system_counter == rhs.system_counter && + lhs.input_state == rhs.input_state && + lhs.active_fieldset == rhs.active_fieldset && + lhs.time_state == rhs.time_state && + lhs.year == rhs.year && + lhs.month == rhs.month && + lhs.day == rhs.day && + lhs.hour == rhs.hour && + lhs.minute == rhs.minute && + lhs.second == rhs.second && + lhs.microsecond == rhs.microsecond; +} + +template +bool operator!=(const ::sick_scan_xd::LIDinputstateMsg_ & lhs, const ::sick_scan_xd::LIDinputstateMsg_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sick_scan_xd + +namespace roswrap +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sick_scan_xd::LIDinputstateMsg_ > + : TrueType + { }; + +template +struct IsMessage< ::sick_scan_xd::LIDinputstateMsg_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sick_scan_xd::LIDinputstateMsg_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sick_scan_xd::LIDinputstateMsg_ const> + : FalseType + { }; + +template +struct HasHeader< ::sick_scan_xd::LIDinputstateMsg_ > + : TrueType + { }; + +template +struct HasHeader< ::sick_scan_xd::LIDinputstateMsg_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sick_scan_xd::LIDinputstateMsg_ > +{ + static const char* value() + { + return "9733baf8db424fa686b779e20fb5e171"; + } + + static const char* value(const ::sick_scan_xd::LIDinputstateMsg_&) { return value(); } + static const uint64_t static_value1 = 0x9733baf8db424fa6ULL; + static const uint64_t static_value2 = 0x86b779e20fb5e171ULL; +}; + +template +struct DataType< ::sick_scan_xd::LIDinputstateMsg_ > +{ + static const char* value() + { + return "sick_scan_xd/LIDinputstateMsg"; + } + + static const char* value(const ::sick_scan_xd::LIDinputstateMsg_&) { return value(); } +}; + +template +struct Definition< ::sick_scan_xd::LIDinputstateMsg_ > +{ + static const char* value() + { + return "# This message contains the \"sSN LIDinputstate\" message of a Sick laser scanner as an ros message.\n" +"#\n" +"\n" +"std_msgs/Header header\n" +"\n" +"# Status code\n" +"uint16 version_number # Status code version number\n" +"uint32 system_counter # Status code system counter (time in microsec since power up, max. 71 min then starting from 0 again)\n" +"\n" +"# Array of input states\n" +"uint8[] input_state # input states, each state with value 0 (not active), 1 (active) or 2 (not used)\n" +"int32 active_fieldset # active fieldset\n" +"\n" +"# Time block (sensortime from the last change of min. one of the outputs)\n" +"uint16 time_state # No time data: 0, Time data: 1\n" +"uint16 year # f.e. 2021\n" +"uint8 month # 1 ... 12\n" +"uint8 day # 1 ... 31\n" +"uint8 hour # 0 ... 23\n" +"uint8 minute # 0 ... 59\n" +"uint8 second # 0 ... 59\n" +"uint32 microsecond # 0 ... 999999\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_xd::LIDinputstateMsg_&) { return value(); } +}; + +} // namespace message_traits +} // namespace roswrap + +namespace roswrap +{ +namespace serialization +{ + + template struct Serializer< ::sick_scan_xd::LIDinputstateMsg_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.version_number); + stream.next(m.system_counter); + stream.next(m.input_state); + stream.next(m.active_fieldset); + stream.next(m.time_state); + stream.next(m.year); + stream.next(m.month); + stream.next(m.day); + stream.next(m.hour); + stream.next(m.minute); + stream.next(m.second); + stream.next(m.microsecond); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct LIDinputstateMsg_ + +} // namespace serialization +} // namespace roswrap + +namespace roswrap +{ +namespace message_operations +{ + +template +struct Printer< ::sick_scan_xd::LIDinputstateMsg_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sick_scan_xd::LIDinputstateMsg_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "version_number: "; + Printer::stream(s, indent + " ", v.version_number); + s << indent << "system_counter: "; + Printer::stream(s, indent + " ", v.system_counter); + s << indent << "input_state[]" << std::endl; + for (size_t i = 0; i < v.input_state.size(); ++i) + { + s << indent << " input_state[" << i << "]: "; + Printer::stream(s, indent + " ", v.input_state[i]); + } + s << indent << "active_fieldset: "; + Printer::stream(s, indent + " ", v.active_fieldset); + s << indent << "time_state: "; + Printer::stream(s, indent + " ", v.time_state); + s << indent << "year: "; + Printer::stream(s, indent + " ", v.year); + s << indent << "month: "; + Printer::stream(s, indent + " ", v.month); + s << indent << "day: "; + Printer::stream(s, indent + " ", v.day); + s << indent << "hour: "; + Printer::stream(s, indent + " ", v.hour); + s << indent << "minute: "; + Printer::stream(s, indent + " ", v.minute); + s << indent << "second: "; + Printer::stream(s, indent + " ", v.second); + s << indent << "microsecond: "; + Printer::stream(s, indent + " ", v.microsecond); + } +}; + +} // namespace message_operations +} // namespace roswrap + +#endif // SICK_SCAN_XD_MESSAGE_LIDINPUTSTATEMSG_H diff --git a/srv/FieldSetReadSrv.srv b/srv/FieldSetReadSrv.srv new file mode 100644 index 00000000..726c7bc7 --- /dev/null +++ b/srv/FieldSetReadSrv.srv @@ -0,0 +1,26 @@ +# Definition of ROS service FieldSetRead for TiM7xx and TiM7xxS devices: +# Read FieldSetSelectionMethod and ActiveFieldSet +# +# Example call (ROS1): +# rosservice call /sick_tim_7xx/FieldSetRead "{}" +# +# Example call (ROS2): +# ros2 service call /FieldSetRead sick_scan_xd/srv/FieldSetReadSrv "{}" +# +# field_set_selection_method < 0: do not set (default), field_set_selection_method = 0: active field selection by digital inputs, field_set_selection_method = 1: active field selection by telegram (see operation manual for details about FieldSetSelectionMethod telegram) +# active_field_set < 0: do not set (default), active_field_set > 0: index of active field otherwise (see operation manual for details about ActiveFieldSet telegram) +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +int32 field_set_selection_method # value of field_set_selection_method after read from lidar +int32 active_field_set # value of active_field_set after read from lidar +bool success # true: success response received from lidar, false: service failed (timeout or error status from controller) diff --git a/srv/FieldSetWriteSrv.srv b/srv/FieldSetWriteSrv.srv new file mode 100644 index 00000000..2d80e65a --- /dev/null +++ b/srv/FieldSetWriteSrv.srv @@ -0,0 +1,30 @@ +# Definition of ROS service FieldSetWrite for TiM7xx and TiM7xxS devices: +# Write FieldSetSelectionMethod and ActiveFieldSet +# +# Example call (ROS1): +# rosservice call /sick_tim_7xx/FieldSetWrite "{field_set_selection_method_in: -1, active_field_set_in: 1}" +# +# Example call (ROS2): +# ros2 service call /FieldSetWrite sick_scan_xd/srv/FieldSetWriteSrv "{field_set_selection_method_in: -1, active_field_set_in: 1}" +# +# Parameter field_set_selection_method < 0: do not set (default), field_set_selection_method = 0: active field selection by digital inputs, field_set_selection_method = 1: active field selection by telegram (see operation manual for details about FieldSetSelectionMethod telegram) +# Note that FieldSetSelectionMethod (parameter field_set_selection_method) requires a higher authorization level and should be configured in the launch file. It is recommended to set field_set_selection_method_in: -1 when using ros service FieldSetWrite. +# Parameter active_field_set < 0: do not set (default), active_field_set > 0: index of active field otherwise (see operation manual for details about ActiveFieldSet telegram) +# + +# +# Request (input) +# + +int32 field_set_selection_method_in # value of field_set_selection_method to write to lidar +int32 active_field_set_in # value of active_field_set to write to lidar + +--- + +# +# Response (output) +# + +int32 field_set_selection_method # value of field_set_selection_method after write to and re-read from lidar +int32 active_field_set # value of active_field_set after write to and re-read from lidar +bool success # true: success response received from lidar, false: service failed (timeout or error status from controller) diff --git a/test/emulator/config/rviz_emulator_cfg_lms4xxx.rviz b/test/emulator/config/rviz_emulator_cfg_lms4xxx.rviz new file mode 100644 index 00000000..9269483a --- /dev/null +++ b/test/emulator/config/rviz_emulator_cfg_lms4xxx.rviz @@ -0,0 +1,186 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /PointCloud21 + - /LaserScan1 + 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: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0.5 + 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.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 + - 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: /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: 5.085607528686523 + 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 + 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: + Yaw: 3.131500244140625 + 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: 545 + Y: 27 diff --git a/test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz b/test/emulator/config/rviz_emulator_cfg_rms2xxx.rviz index 7f50648c..5007f440 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: "" + SyncSource: PointCloud2 Preferences: PromptSaveOnExit: true Toolbars: @@ -88,7 +88,7 @@ Visualization Manager: Min Value: -10 Value: true Axis: Z - Channel Name: objId + Channel Name: x Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: Intensity @@ -183,4 +183,4 @@ Window Geometry: collapsed: true Width: 1359 X: 718 - Y: 78 + Y: 41 diff --git a/test/emulator/launch/emulator_tim781_lidinputstate.launch b/test/emulator/launch/emulator_tim781_lidinputstate.launch new file mode 100644 index 00000000..c0bcb6c4 --- /dev/null +++ b/test/emulator/launch/emulator_tim781_lidinputstate.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/test/pcap_json_converter/pcap_json_converter.cmd b/test/pcap_json_converter/pcap_json_converter.cmd index 38f839a7..eaffaac2 100644 --- a/test/pcap_json_converter/pcap_json_converter.cmd +++ b/test/pcap_json_converter/pcap_json_converter.cmd @@ -13,6 +13,12 @@ echo PYTHON_DIR=%PYTHON_DIR% python --version @echo. +python pcap_json_converter.py --pcap_filename=../emulator/scandata/20241022_lms4000_encoder.pcapng +python pcap_json_converter.py --pcap_filename=../emulator/scandata/20241021_lms4000_encoder.pcapng +python pcap_json_converter.py --pcap_filename=../emulator/scandata/20241010-tim781-lidinputstate-toggle-1-9.pcapng +python pcap_json_converter.py --pcap_filename=../emulator/scandata/20241008_tim781_lidinputstate_telegram.pcapng +python pcap_json_converter.py --pcap_filename=../emulator/scandata/20240909-rms2xxx-field-evaluation.pcapng +python pcap_json_converter.py --pcap_filename=../emulator/scandata/20240909-rms2xxx-field-evaluation-with-rotating-fan.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20240527-LRS36x0.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20240527-OEM15xx.pcapng python pcap_json_converter.py --pcap_filename=../emulator/scandata/20240307-MRS1xxx-default-settings-rssiflag3-angres0.2500-scanfreq50.0.pcapng diff --git a/test/python/multiscan_sopas_test_server.py b/test/python/multiscan_sopas_test_server.py index b560ae72..abd162bf 100644 --- a/test/python/multiscan_sopas_test_server.py +++ b/test/python/multiscan_sopas_test_server.py @@ -70,6 +70,9 @@ def __init__(self, cola_binary = 0, val_FREchoFilter = 0): "sEN InertialMeasurementUnit": "\x02sEA InertialMeasurementUnit\x03", # "sEN InertialMeasurementUnit" -> "sEA InertialMeasurementUnit" "sWN ImuDataEnable": "\x02sWA ImuDataEnable 1\x03", # "sWN ImuDataEnable" -> "sWA ImuDataEnable" "sWN ImuDataEthSettings": "\x02sWA ImuDataEthSettings\x03", # "sWN ImuDataEthSettings" -> "sWA ImuDataEthSettings" + # Simulate picoScan150 w/o addons (no IMU available): "sWN ImuDataEthSettings" -> "sFA 3" (unknown sopas index, no IMU or IMU license error) + # "sWN ImuDataEnable": "\x02sFA 3\x03", # "sWN ImuDataEnable" -> "sFA 3" + # "sWN ImuDataEthSettings": "\x02sFA 3\x03", # "sWN ImuDataEthSettings" -> "sFA 3" } # Search for a mapped response given a cola request and returns key and response as strings diff --git a/test/python/sopas_json_test_server.py b/test/python/sopas_json_test_server.py index c89c4420..e5b70741 100644 --- a/test/python/sopas_json_test_server.py +++ b/test/python/sopas_json_test_server.py @@ -178,10 +178,14 @@ def stop(self): tcp_port = cli_args.tcp_port json_file = cli_args.json_file if len(cli_args.scandata_id) > 0: - scandata_ids = [ cli_args.scandata_id ] + # scandata_ids = [ cli_args.scandata_id ] + scandata_ids = [ ] + for scandata_id in cli_args.scandata_id.split(","): + scandata_ids.append(scandata_id) for n, scandata_id in enumerate(scandata_ids): scandata_ids[n] = scandata_ids[n].replace("?", " ") - scandata_ids[n] = scandata_ids[n].replace("\"", "") + scandata_ids[n] = scandata_ids[n].replace("\"", "") + print("sopas_json_test_server: scandata_ids = {}".format(scandata_ids)) verbosity = cli_args.verbosity send_rate = cli_args.send_rate send_scandata_after = cli_args.scandata_after diff --git a/test/scripts/run_linux_api_test_py_lms4xxx.bash b/test/scripts/run_linux_api_test_py_lms4xxx.bash new file mode 100644 index 00000000..8365125a --- /dev/null +++ b/test/scripts/run_linux_api_test_py_lms4xxx.bash @@ -0,0 +1,27 @@ +#!/bin/bash + +printf "\033c" + +# +# Run sick_scan_xd_api_test.py with sick_lms_4xxx.launch (Linux native with test server) +# + +pushd ../.. +export LD_LIBRARY_PATH=.:./build_linux:$LD_LIBRARY_PATH +export PYTHONPATH=.:./python/api:$PYTHONPATH + +# Start emulator +python3 ./test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./test/emulator/scandata/20240522-LMS4xxx.pcapng.json --verbosity=0 & +sleep 1 + +# Start sick_scan_xd api example +python3 ./test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./launch/sick_lms_4xxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & +popd + +# Run simulation for 30 seconds and exit +sleep 30 +echo -e "\nFinishing simulation after 30 sec, killing sick_scan_xd_api_test...\n" +pkill -f sick_scan_xd_api_test.py +pkill -f sopas_json_test_server.py +echo -e "Finished simulation.\n" + diff --git a/test/scripts/run_linux_ros1_simu_lms4xxx.bash b/test/scripts/run_linux_ros1_simu_lms4xxx.bash new file mode 100644 index 00000000..f93095d5 --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_lms4xxx.bash @@ -0,0 +1,40 @@ +#!/bin/bash +printf "\033c" +pushd ../../../.. +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 + +# 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_xd 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/20240522-LMS4xxx.pcapng.json --verbosity=0 & +sleep 1 + +# Start rviz +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_lms4xxx.rviz --opengl 210 & +sleep 1 + +# Start sick_scan_xd driver +roslaunch sick_scan_xd sick_lms_4xxx.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 "sick_scan_xd 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 sick_scan_xd emulation, shutdown ros nodes\n" +rosnode kill -a ; sleep 1 +killall sick_generic_caller ; sleep 1 +pkill -f sopas_json_test_server.py +popd + diff --git a/test/scripts/run_linux_ros1_simu_multiscan_std.bash b/test/scripts/run_linux_ros1_simu_multiscan_std.bash new file mode 100644 index 00000000..83543a81 --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_multiscan_std.bash @@ -0,0 +1,81 @@ +#!/bin/bash + +# killall and cleanup after exit +function killall_cleanup() +{ + rosnode kill multiScan + sleep 3 + killall sick_generic_caller + killall rviz + killall static_transform_publisher + pkill -f multiscan_sopas_test_server.py + pkill -f multiscan_pcap_player.py + pkill -f polar_to_cartesian_pointcloud_ros1.py + pkill -f sick_scan_xd_simu.py + pkill -f sopas_json_test_server.py + killall -9 sick_generic_caller + sleep 3 +} + +# start static transforms for laserscan messages (all laserscan frame ids "world_1", "world_2", "world_3", ... "world_16" for all layers are mapped to "world_6") +function run_laserscan_frame_transformers() +{ + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_1 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_2 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_3 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_4 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_5 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_7 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_8 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_9 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_10 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_11 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_12 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_13 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_14 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_15 100 & + rosrun tf static_transform_publisher 0 0 0 0 0 0 world_6 world_16 100 & +} + +# +# Run multiscan simulation with default configuration on ROS1-Linux +# + +pushd ../../../.. +printf "\033c" +source /opt/ros/noetic/setup.bash +source ./devel_isolated/setup.bash +killall_cleanup +sleep 1 + +# Run multiscan 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.rviz & +sleep 1 +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_cfg_multiscan_laserscan_360.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 multiscan in compact format +echo -e "run_multiscan.bash: sick_scan_xd sick_multiscan.launch ..." +roslaunch sick_scan_xd 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 + +# Map all laserscan massages to frame id "world_6" +run_laserscan_frame_transformers + +# Start polar to cartesian pointcloud converter +# polar pointcloud on topic "/cloud_polar_unstructured_fullframe" with fields "i", "range", "azimuth", "elevation" (input) +# cartesian pointcloud on topic "/cloud_polar_to_cartesian" with fields "x", "y", "z", "intensity" (output) +python3 ./src/sick_scan_xd/test/python/polar_to_cartesian_pointcloud_ros1.py --polar_topic="/cloud_polar_unstructured_fullframe" --cartesian_topic="/cloud_polar_to_cartesian" & + +# Play compact pcapng-files to emulate multiscan output +echo -e "\nPlaying pcapng-files to emulate multiscan ...\n" +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20231009-multiscan-compact-imu-01.pcapng --udp_port=-1 --repeat=1 --verbose=0 --filter=pcap_filter_multiscan_hildesheim --max_seconds=60 + +# Shutdown +echo -e "run_multiscan.bash finished, killing all processes ..." +killall_cleanup +popd + diff --git a/test/scripts/run_linux_ros1_simu_picoscan120.bash b/test/scripts/run_linux_ros1_simu_picoscan120.bash new file mode 100644 index 00000000..b428abdb --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_picoscan120.bash @@ -0,0 +1,45 @@ +#!/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_pcap_player.py +} + +# +# 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 sopas test server (emulate picoscan) +python3 ./src/sick_scan_xd/test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 --FREchoFilter=1 & +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_cfg_picoscan_emu.rviz & +sleep 1 +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_cfg_picoscan_emu_360.rviz & +sleep 1 + +# Start sick_generic_caller with sick_picoscan with compact format +echo -e "run_lidar3d.bash: sick_scan_xd sick_picoscan.launch ..." +roslaunch sick_scan_xd sick_picoscan_120.launch hostname:="127.0.0.1" udp_receiver_ip:="127.0.0.1" & +sleep 3 + +# Play picoscan pcapng-file with picoscan compact-data +python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20230911-picoscan-compact.pcapng --udp_port=2115 --repeat=1 +sleep 3 + +# Shutdown +echo -e "run_linux_ros1_simu_picoScan.bash finished, killing all processes ..." +killall_cleanup +popd diff --git a/test/scripts/run_linux_ros1_simu_picoscan_compact.bash b/test/scripts/run_linux_ros1_simu_picoscan_compact.bash index 5384dd17..eea4c4f8 100644 --- a/test/scripts/run_linux_ros1_simu_picoscan_compact.bash +++ b/test/scripts/run_linux_ros1_simu_picoscan_compact.bash @@ -3,10 +3,13 @@ # killall and cleanup after exit function killall_cleanup() { - sleep 1 ; rosnode kill -a - sleep 1 ; killall sick_generic_caller - sleep 1 ; pkill -f multiscan_sopas_test_server.py - sleep 1 ; pkill -f multiscan_pcap_player.py + rosnode kill -a + sleep 1 + killall sick_generic_caller + sleep 1 + pkill -f multiscan_sopas_test_server.py + pkill -f multiscan_pcap_player.py + killall -9 sick_generic_caller } # @@ -38,7 +41,7 @@ roslaunch sick_scan_xd sick_picoscan.launch hostname:="127.0.0.1" udp_receiver_i sleep 3 # read -p "Press ENTER to continue..." # Play picoscan pcapng-file with picoscan compact-data -python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20230911-picoscan-compact.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/20230911-picoscan-compact.pcapng --udp_port=2115 --repeat=5 # Old pcapng files, require old configuration (all_segments_min_deg=-134 und all_segments_max_deg=+135), i.e. # roslaunch sick_scan_xd sick_picoscan.launch hostname:="127.0.0.1" udp_receiver_ip:="127.0.0.1" scandataformat:=2 all_segments_min_deg:=-134 all_segments_max_deg:=135 # echo -e "\nPlaying pcapng-files to emulate picoscan with compact data format\n" @@ -68,7 +71,7 @@ python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename= # #python3 ./src/sick_scan_xd/test/python/multiscan_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20230509_picoscan_compact_profile8_echofilter_lastecho_fogfilter_off.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/20230509_picoscan_compact_profile1_echofilter_lastecho_fogfilter_1.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/20230509_picoscan_compact_profile8_echofilter_lastecho_fogfilter_1.pcapng --udp_port=2115 --repeat=1 -sleep 3 +# sleep 3 # Shutdown echo -e "run_linux_ros1_simu_timtwo.bash finished, killing all processes ..." diff --git a/test/scripts/run_linux_ros1_simu_rmsxxxx.bash b/test/scripts/run_linux_ros1_simu_rmsxxxx.bash index 3a8ba4bb..d4692e6d 100644 --- a/test/scripts/run_linux_ros1_simu_rmsxxxx.bash +++ b/test/scripts/run_linux_ros1_simu_rmsxxxx.bash @@ -18,9 +18,10 @@ 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 + # rosnode kill -a ; sleep 3 + killall rviz + killall sick_generic_caller ; sleep 3 + killall -9 sick_generic_caller } printf "\033c" @@ -32,24 +33,35 @@ if [ -f ./devel_isolated/setup.bash ] ; then source ./devel_isolated/setup.b 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 +# 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_xd 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_xd 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_xd sick_rms_xxxx.launch (ascii) with LMDscandata and LIDoutputstate telegrams +# Settings for 20221018_rms_1xxx_ascii_rms2_objects.pcapng.json: --scandata_id="sSN LMDradardata", activate_lidoutputstate:=false +# 20240909-rms2xxx-field-evaluation.pcapng.json: --scandata_id="sSN LMDradardata,sSN LIDoutputstate" +# 20240909-rms2xxx-field-evaluation-with-rotating-fan.pcapng.json -# Run rms2xxx emulator, rviz and launch sick_scan_xd 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 & +python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20240909-rms2xxx-field-evaluation.pcapng.json --scandata_id="sSN LMDradardata,sSN LIDoutputstate" --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_xd sick_rms_xxxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & -waitUntilRvizClosed 15 +waitUntilRvizClosed 60 kill_simu +# Run rms1xxx emulator, rviz and launch sick_scan_xd 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_xd sick_rms_xxxx.launch hostname:=127.0.0.1 activate_lidoutputstate:=false sw_pll_only_publish:=False & +# waitUntilRvizClosed 60 +# kill_simu + +# Run rms2xxx emulator, rviz and launch sick_scan_xd 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_xd sick_rms_xxxx.launch hostname:=127.0.0.1 activate_lidoutputstate:=false sw_pll_only_publish:=False & +# waitUntilRvizClosed 60 +# kill_simu + popd diff --git a/test/scripts/run_linux_ros1_simu_tim781_lidinputstate.bash b/test/scripts/run_linux_ros1_simu_tim781_lidinputstate.bash new file mode 100644 index 00000000..3c6a434f --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_tim781_lidinputstate.bash @@ -0,0 +1,44 @@ +#!/bin/bash +printf "\033c" +pushd ../../../.. +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 + +# 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 rviz +#rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg.rviz --opengl 210 & +sleep 1 + +# Start sick_scan_xd emulator +roslaunch --wait sick_scan_xd emulator_tim781_lidinputstate.launch & +sleep 1 + +# Start sick_scan_xd driver for TiM7xx +echo -e "Launching sick_scan_xd sick_tim_7xx.launch\n" +# rosrun --prefix 'gdb -ex run --args' sick_scan_xd sick_generic_caller _sick_scan_args:="./src/sick_scan_xd/launch/sick_tim_7xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False" +roslaunch --wait sick_scan_xd sick_tim_7xx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & + +# Wait for 'q' or 'Q' to exit or until rviz is closed +for i in {1..60}; do # while true ; do + # echo -e "TiM7xx 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 TiM7xx emulation, shutdown ros nodes\n" +rosnode kill -a ; sleep 1 +killall -9 sick_generic_caller ; sleep 1 +killall -9 sick_scan_emulator ; sleep 1 +echo -e "run_linux_ros1_simu_tim781_lidinputstate.bash finished.\n\n" + +popd + diff --git a/test/scripts/run_linux_ros2_simu_lms4xxx.bash b/test/scripts/run_linux_ros2_simu_lms4xxx.bash new file mode 100644 index 00000000..2ef632d1 --- /dev/null +++ b/test/scripts/run_linux_ros2_simu_lms4xxx.bash @@ -0,0 +1,47 @@ +#!/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 ; pkill -f sopas_json_test_server.py ; pkill -f "ros2 topic echo" + 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/jazzy/setup.bash ] ; then source /opt/ros/jazzy/setup.bash ; export QT_QPA_PLATFORM=xcb +elif [ -f /opt/ros/humble/setup.bash ] ; then source /opt/ros/humble/setup.bash +elif [ -f /opt/ros/foxy/setup.bash ] ; then source /opt/ros/foxy/setup.bash +elif [ -f /opt/ros/eloquent/setup.bash ] ; then source /opt/ros/eloquent/setup.bash +fi +source ./install/setup.bash + +# Start sick_scan_xd 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/20240522-LMS4xxx.pcapng.json --verbosity=0 & +# python3 ./src/sick_scan_xd/test/python/sopas_json_test_server.py --tcp_port=2112 --json_file=./src/sick_scan_xd/test/emulator/scandata/20241021_lms4000_encoder_test.pcapng.json --verbosity=0 & +sleep 3 # Wait until pcapng.json file is loaded and parsed + +# Start sick_scan_xd driver +ros2 launch sick_scan_xd sick_lms_4xxx.launch.py encoder_mode:=2 hostname:=127.0.0.1 sw_pll_only_publish:=False & +sleep 1 + +# Start rviz +ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2.rviz & +(ros2 topic echo /sick_lms_4xxx/encoder | tr '\n' ' ') & # ros2 topic echo /sick_lms_4xxx/encoder +sleep 1 + +# Run for 30 seconds +sleep 30 +simu_killall +popd + diff --git a/test/scripts/run_linux_ros2_simu_multiscan_compact.bash b/test/scripts/run_linux_ros2_simu_multiscan_compact.bash index 1f58bb42..428fb84d 100644 --- a/test/scripts/run_linux_ros2_simu_multiscan_compact.bash +++ b/test/scripts/run_linux_ros2_simu_multiscan_compact.bash @@ -38,6 +38,7 @@ sleep 1 # Start sick_generic_caller with sick_scansegment_xd echo -e "run_lidar3d.bash: sick_scan_xd sick_multiscan.launch.py ..." +# ros2 run --prefix 'gdb -ex run --args' sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" scandataformat:=2 ros2 launch sick_scan_xd sick_multiscan.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" scandataformat:=2 & sleep 3 diff --git a/test/scripts/run_linux_ros2_simu_picoscan_compact.bash b/test/scripts/run_linux_ros2_simu_picoscan_compact.bash index adb5a8f2..c79d2255 100644 --- a/test/scripts/run_linux_ros2_simu_picoscan_compact.bash +++ b/test/scripts/run_linux_ros2_simu_picoscan_compact.bash @@ -38,6 +38,7 @@ sleep 1 # Start sick_generic_caller with sick_picoscan with compact format echo -e "run_lidar3d.bash: sick_scan_xd sick_picoscan.launch.py ..." # ros2 launch sick_scan_xd sick_picoscan.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" scandataformat:=2 all_segments_min_deg:=-134 all_segments_max_deg:=135 & +# ros2 run --prefix 'gdb -ex run --args' sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/sick_picoscan.launch hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" scandataformat:=2 ros2 launch sick_scan_xd sick_picoscan.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" scandataformat:=2 & sleep 3 # read -p "Press ENTER to continue..." diff --git a/test/scripts/run_linux_ros2_simu_tim781_lidinputstate.bash b/test/scripts/run_linux_ros2_simu_tim781_lidinputstate.bash new file mode 100644 index 00000000..b6ce771c --- /dev/null +++ b/test/scripts/run_linux_ros2_simu_tim781_lidinputstate.bash @@ -0,0 +1,54 @@ +#!/bin/bash + +# +# Set environment +# + +function simu_killall() +{ + sleep 1 ; pkill -f "ros2 topic echo" + 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 +} + +function run_simu() +{ + emulator_launch_cfg=$1 + sick_scan_launch_file=$2 + sleep 1 ; ros2 run sick_scan_xd sick_scan_emulator ./src/sick_scan_xd/test/emulator/launch/$emulator_launch_cfg scanner_type:=sick_tim_7xx& + # sleep 1 ; ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/$sick_scan_launch_file hostname:=127.0.0.1 port:=2111 sw_pll_only_publish:=False & + sleep 1 ; ros2 run sick_scan_xd sick_generic_caller ./src/sick_scan_xd/launch/$sick_scan_launch_file hostname:=127.0.0.1 port:=2111 & + sleep 1 ; ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2.rviz & + sleep 1 ; ros2 topic echo /sick_tim_7xx/lidinputstate & + sleep 30 ; simu_killall +} + +simu_killall +printf "\033c" +pushd ../../../.. +if [ -f /opt/ros/jazzy/setup.bash ] ; then source /opt/ros/jazzy/setup.bash ; export QT_QPA_PLATFORM=xcb +elif [ -f /opt/ros/humble/setup.bash ] ; then source /opt/ros/humble/setup.bash +elif [ -f /opt/ros/foxy/setup.bash ] ; then source /opt/ros/foxy/setup.bash +elif [ -f /opt/ros/eloquent/setup.bash ] ; then source /opt/ros/eloquent/setup.bash +fi +source ./install/setup.bash + +# +# Run simulation: +# 1. Start sick_scan_emulator +# 2. Start sick_scan_xd driver sick_generic_caller +# 3. Run rviz +# 4. Stop simulation after 30 seconds +# + +echo -e "run_linux_ros2_simu_tim7xx_tim7xxS.bash: starting TiM7xx/TiM7xxS emulation with $sick_scan_launch_file\n" +cp -f ./src/sick_scan_xd/test/emulator/scandata/20241010-tim781-lidinputstate-toggle-1-9.pcapng.json /tmp/lmd_scandata.pcapng.json +run_simu emulator_tim781_lidinputstate.launch sick_tim_7xx.launch + +popd + diff --git a/test/scripts/run_win64_simu_picoscan.cmd b/test/scripts/run_win64_simu_picoscan.cmd index ae89dfb6..61470323 100644 --- a/test/scripts/run_win64_simu_picoscan.cmd +++ b/test/scripts/run_win64_simu_picoscan.cmd @@ -18,7 +18,8 @@ REM REM Start sick_generic_caller REM -start "sick_generic_caller" cmd /k .\Debug\sick_generic_caller.exe ../launch/sick_picoscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 scandataformat:=1 all_segments_min_deg:=-134 all_segments_max_deg:=135 +rem start "sick_generic_caller" cmd /k .\Debug\sick_generic_caller.exe ../launch/sick_picoscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 scandataformat:=1 all_segments_min_deg:=-134 all_segments_max_deg:=135 +start "sick_generic_caller" cmd /k .\Debug\sick_generic_caller.exe ../launch/sick_picoscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 host_FREchoFilter:=0 @timeout /t 3 REM @@ -28,8 +29,9 @@ REM @echo. @echo Playing pcapng-files to emulate picoscan. Note: Start of UDP msgpacks in 20221010_timtwo.pcapng takes a while... @echo. -python ../test/python/multiscan_pcap_player.py --pcap_filename=../test/emulator/scandata/20230315-picoscan.pcapng --udp_port=2115 -python ../test/python/multiscan_pcap_player.py --pcap_filename=../test/emulator/scandata/20221010_timtwo.pcapng --udp_port=2115 +python ../test/python/multiscan_pcap_player.py --pcap_filename=../test/emulator/scandata/20230911-picoscan-compact.pcapng --udp_port=2115 --repeat=5 +rem python ../test/python/multiscan_pcap_player.py --pcap_filename=../test/emulator/scandata/20230315-picoscan.pcapng --udp_port=2115 +rem python ../test/python/multiscan_pcap_player.py --pcap_filename=../test/emulator/scandata/20221010_timtwo.pcapng --udp_port=2115 popd @pause diff --git a/test/scripts/vs_code.bash b/test/scripts/vs_code.bash index 1c59b5d7..aea1a168 100644 --- a/test/scripts/vs_code.bash +++ b/test/scripts/vs_code.bash @@ -3,8 +3,8 @@ if [ -f /opt/ros/noetic/setup.bash ] ; then source /opt/ros/noetic/setup.bash -elif [ -f /opt/ros/melodic/setup.bash ] ; then - source /opt/ros/melodic/setup.bash +elif [ -f /opt/ros/humble/setup.bash ] ; then + source /opt/ros/humble/setup.bash elif [ -f /opt/ros/foxy/setup.bash ] ; then source /opt/ros/foxy/setup.bash elif [ -f /opt/ros/eloquent/setup.bash ] ; then @@ -12,7 +12,11 @@ elif [ -f /opt/ros/eloquent/setup.bash ] ; then fi pushd ../../../.. -source ./devel_isolated/setup.bash +if [ -f /devel_isolated/setup.bash ] ; then + source ./devel_isolated/setup.bash +elif [ -f ./install/setup.bash ] ; then + source ./install/setup.bash +fi code ./sick_scan_xd_vscode.code-workspace popd 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 2914af55..61a6786e 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 @@ -221,7 +221,7 @@ static void apiTestDiagnosticMsgCallback(SickScanApiHandle apiHandle, const Sick { if (msg->status_code == 1) // status_code defined in SICK_DIAGNOSTIC_STATUS: WARN=1 printf("[WARN]: apiTestDiagnosticMsgCallback(apiHandle:%p): status_code = %d (WARNING), status_message = \"%s\"\n", apiHandle, msg->status_code, msg->status_message); - else if (msg->status_code == 1) // status_code defined in SICK_DIAGNOSTIC_STATUS: ERROR=2 + else if (msg->status_code == 2) // status_code defined in SICK_DIAGNOSTIC_STATUS: ERROR=2 printf("[ERROR]: apiTestDiagnosticMsgCallback(apiHandle:%p): status_code = %d (ERROR), status_message = \"%s\"\n", apiHandle, msg->status_code, msg->status_message); else printf("[Info]: apiTestDiagnosticMsgCallback(apiHandle:%p): status_code = %d, status_message = \"%s\"\n", apiHandle, msg->status_code, msg->status_message);