From 6c106e509c3148f89833019ed6df29f03c55b6b4 Mon Sep 17 00:00:00 2001 From: rostest Date: Mon, 25 Mar 2024 15:31:38 +0100 Subject: [PATCH] Release 3.3.0 --- CHANGELOG.md | 9 +- CHANGELOG.rst | 35 ++- CMakeLists.txt | 13 +- INSTALL-ROS1.md | 2 +- INSTALL-ROS2.md | 15 ++ README.md | 22 ++ USAGE.md | 12 +- doc/bloom.md | 12 +- doc/sick_scan_api/sick_scan_api.md | 10 + driver/src/ldmrs/sick_ldmrs_driver.cpp | 5 +- driver/src/sick_generic_callback.cpp | 6 +- driver/src/sick_generic_laser.cpp | 17 +- driver/src/sick_generic_radar.cpp | 2 +- driver/src/sick_lmd_scandata_parser.cpp | 73 ++++-- driver/src/sick_scan_common.cpp | 41 +++- driver/src/sick_scan_services.cpp | 2 +- driver/src/sick_scan_xd_api/api_impl.cpp | 50 +++- .../ros_msgpack_publisher.cpp | 47 +++- include/sick_scan/ldmrs/sick_ldmrs_driver.hpp | 1 + include/sick_scan/sick_generic_callback.h | 15 +- include/sick_scan/sick_lmd_scandata_parser.h | 2 +- include/sick_scan/sick_scan_common.h | 1 + include/sick_scan/sick_scan_logging.h | 21 +- include/sick_scan_xd_api/sick_scan_api.h | 9 + .../ros_msgpack_publisher.h | 4 +- launch/sick_mrs_1xxx.launch | 1 + launch/sick_picoscan.launch | 5 +- launch/sick_picoscan_single_echo.launch | 227 ++++++++++++++++++ package.xml | 2 +- python/api/sick_scan_api.py | 25 +- .../geometry_msgs/PoseWithCovariance.h | 1 + test/docker/cleanup_docker.bash | 19 ++ .../data/picoscan_compact_test01_cfg.json | 4 +- .../rviz_cfg_picoscan_laserscan_360.rviz | 4 +- ...fg_picoscan_single_echo_laserscan_360.rviz | 184 ++++++++++++++ .../config/rviz_emulator_cfg_mrs1104.rviz | 2 +- .../rviz_emulator_cfg_ros2_mrs1104.rviz | 12 +- .../sick_scan_xd_api/sick_scan_xd_api_test.py | 172 +++++++------ test/scripts/make_ros2.cmd | 2 +- .../run_linux_api_test_py_multiscan.bash | 42 ++++ test/scripts/run_linux_ros1_simu_mrs1xxx.bash | 74 ++++++ .../run_linux_ros1_simu_picoscan_compact.bash | 2 +- ..._linux_ros1_simu_picoscan_single_echo.bash | 48 ++++ test/scripts/run_linux_ros2_simu_mrs1xxx.bash | 73 ++++++ .../sick_scan_xd_api_wrapper.c | 28 +++ 45 files changed, 1185 insertions(+), 168 deletions(-) create mode 100644 launch/sick_picoscan_single_echo.launch create mode 100644 test/docker/cleanup_docker.bash create mode 100644 test/emulator/config/rviz_cfg_picoscan_single_echo_laserscan_360.rviz create mode 100644 test/scripts/run_linux_api_test_py_multiscan.bash create mode 100644 test/scripts/run_linux_ros1_simu_mrs1xxx.bash create mode 100644 test/scripts/run_linux_ros1_simu_picoscan_single_echo.bash create mode 100644 test/scripts/run_linux_ros2_simu_mrs1xxx.bash diff --git a/CHANGELOG.md b/CHANGELOG.md index 8dfd3bf5..4cbff4b1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,9 +4,16 @@ Possible types are **Added** for new features. **Changed** for changes to the ex features that will be removed in future versions **Removed** for deprecated features that were removed in this release. **Fixed** for all bug fixes. **Security** to prompt users to update in case of closed vulnerabilities. - ## Released ## +### Release v3.3.0 + + - **add** Option for MRS1xxx azimuth correction table + - **add** Support for picoScan100 (single echo w/o addons) + - **add** API logging functions and verbosity (#270) + - **add** API documentation (multiple lidars not supported, #281) + - **changed** API extended (added topic in PointCloud messages, #271) + ### Release v3.2.0 - **add** IMU support for multiScan and picoScan diff --git a/CHANGELOG.rst b/CHANGELOG.rst index fa3bf5b4..150dc8e4 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -4,19 +4,28 @@ Changelog for package sick_scan_xd Forthcoming ----------- -* add: IMU support for multiScan and picoScan -* add: support bloom releases for ROS-2 humble -* add: docker tests for ROS1 noetic incl. testcases for multiScan, picoScan, MRS1xxx -* add: PR #255, support picoScan performance profiles -* change: configuration time flag LMDscandatacfg switched off for the TiM240 -* fix: #218 (API reinit) -* fix: #220 (Fullframe Laserscan messages multi- and picoScan) -* fix: #221 (No scandata while activated scan range filter) -* fix: #222 (rviz visualization of polar pointclouds) -* fix: #247 (Launchfile option for laserscan topic) -* fix: #256 (Clean API-exit picoScan and multiScan) -* fix: #260 (Provide API-functions to query lidar status, error codes and error messages) -* Contributors: Manuel Aiple +* Release v3.3.0 + * add: Option for MRS1xxx azimuth correction table + * add: Support for picoScan100 (single echo w/o addons) + * add: API logging functions and verbosity (#270) + * add: API documentation (multiple lidars not supported, #281) + * changed: API extended (added topic in PointCloud messages, #271) + +3.2.0 (2024-02-12) +------------------ +* Release v3.1.0 + * add: IMU support for multiScan and picoScan + * add: support bloom releases for ROS-2 humble + * add: docker tests for ROS1 noetic incl. testcases for multiScan, picoScan, MRS1xxx + * add: PR #255, support picoScan performance profiles + * changed: configuration time flag LMDscandatacfg switched off for the TiM240 + * fix: #218 (API reinit) + * fix: #220 (Fullframe Laserscan messages multi- and picoScan) + * fix: #221 (No scandata while activated scan range filter) + * fix: #222 (rviz visualization of polar pointclouds) + * fix: #247 (Launchfile option for laserscan topic) + * fix: #256 (Clean API-exit picoScan and multiScan) + * fix: #260 (Provide API-functions to query lidar status, error codes and error messages) 3.1.0 (2023-11-21) ------------------ diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f6b9743..469ccef9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,9 +95,11 @@ elseif(DEFINED SICK_TARGET_ENDIANESS) endif() # ROS Version -message(STATUS "ROS_VERSION precheck: ENV{ROS_VERSION}=$ENV{ROS_VERSION} ROS_VERSION=${ROS_VERSION} CMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}") +message(STATUS "ROS_VERSION precheck: ENV{ROS_VERSION}=$ENV{ROS_VERSION} ENV{ROS_DISTRO}=$ENV{ROS_DISTRO} ROS_VERSION=${ROS_VERSION} CMAKE_PREFIX_PATH=${CMAKE_PREFIX_PATH}") if(DEFINED CMAKE_PREFIX_PATH AND "${CMAKE_PREFIX_PATH}" STREQUAL "/opt/ros/humble") - set(ROS_VERSION 2) + set(ROS_VERSION 2) # required for ROS2 humble bloom releases with jenkins buildfarm +elseif(NOT DEFINED ROS_VERSION AND EXISTS "/opt/ros/humble/setup.sh") + set(ROS_VERSION 2) # required for ROS2 humble bloom releases with jenkins buildfarm endif() if(NOT DEFINED ROS_VERSION AND DEFINED ENV{ROS_VERSION}) set(ROS_VERSION $ENV{ROS_VERSION}) @@ -656,7 +658,12 @@ else() # i.e. (ROS_VERSION EQUAL 0 OR ROS_VERSION EQUAL 2) target_link_libraries(${PROJECT_NAME}_shared_lib ${SICK_LDMRS_LIBRARIES}) endif() add_executable(sick_generic_caller driver/src/sick_generic_caller.cpp) - target_link_libraries(sick_generic_caller ${PROJECT_NAME}_lib ${SICK_LDMRS_LIBRARIES}) + if(ROS_VERSION EQUAL 0 AND NOT WIN32) + target_link_libraries(sick_generic_caller ${PROJECT_NAME}_lib ${SICK_LDMRS_LIBRARIES} "pthread") # pthread required for std::thread + target_link_options(sick_generic_caller PUBLIC "LINKER:--no-as-needed") # fixes exception "Enable multithreading to use std::thread: Operation not permitted" + else() + target_link_libraries(sick_generic_caller ${PROJECT_NAME}_lib ${SICK_LDMRS_LIBRARIES}) + endif() endif() diff --git a/INSTALL-ROS1.md b/INSTALL-ROS1.md index 6252345f..3b16a281 100644 --- a/INSTALL-ROS1.md +++ b/INSTALL-ROS1.md @@ -4,7 +4,7 @@ To build resp. install sick_scan_xd on Linux with ROS-1, you can build sick_scan ## Install prebuilt binaries -Run the following steps to install sick_scan_xd on Linux with ROS 1: +Run the following steps to install sick_scan_xd on Linux with ROS 1 noetic: ``` sudo apt update diff --git a/INSTALL-ROS2.md b/INSTALL-ROS2.md index 2bb59451..d52adfdd 100644 --- a/INSTALL-ROS2.md +++ b/INSTALL-ROS2.md @@ -1,5 +1,20 @@ ## 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`. + +## 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): diff --git a/README.md b/README.md index 9f487b63..a5670c2e 100644 --- a/README.md +++ b/README.md @@ -41,6 +41,28 @@ This project provides a driver for the SICK LiDARs and Radar sensors mentioned [ * 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. + +Example: + +Checking out the latest revision (usually older than the develop version, but officially released): +``` +git clone -b master https://github.com/SICKAG/sick_scan_xd.git +``` + +Checking out the latest development status: +``` +git clone https://github.com/SICKAG/sick_scan_xd.git +``` + ## Build targets sick_scan_xd can be build on Linux and Windows, with and without ROS, with and without LDMRS. The following table shows the allowed combinations and how to build. diff --git a/USAGE.md b/USAGE.md index 57462186..d25f97c9 100644 --- a/USAGE.md +++ b/USAGE.md @@ -274,22 +274,20 @@ The use of the parameters can be looked up in the launch files. This is also rec 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 +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 +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 -- `timelimit` - Timelimit in [sec] for max. wait time of incoming sensor reply +- **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. -- `sw_pll_only_publish` - If true, the internal Software PLL is fored to sync the scan generation time stamp to a system timestamp +- **Angle compensation**: For highest angle accuracy the NAV-Lidar series supports an [angle compensation mechanism](./doc/angular_compensation.md). -- Angle compensation: For highest angle accuracy the NAV-Lidar series supports an [angle compensation mechanism](./doc/angular_compensation.md). +- **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). diff --git a/doc/bloom.md b/doc/bloom.md index 72a9e2f7..6997d44c 100644 --- a/doc/bloom.md +++ b/doc/bloom.md @@ -158,6 +158,10 @@ * 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). +* Troubleshooting, FAQ: + * bloom builds an old sick_scan_xd version: + * 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. + ## 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: @@ -191,12 +195,18 @@ For ROS-2 follow the instructions on https://docs.ros.org/en/humble/How-To-Guide sudo rosdep init rosdep update ``` +* Troubleshooting, FAQ: + * bloom builds an old sick_scan_xd version: + * 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. ## 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 -* Jenkins: https://build.ros.org/search/?q=sick_scan_xd +* 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 ## Useful links and tutorials diff --git a/doc/sick_scan_api/sick_scan_api.md b/doc/sick_scan_api/sick_scan_api.md index d14c9c06..569d105d 100644 --- a/doc/sick_scan_api/sick_scan_api.md +++ b/doc/sick_scan_api/sick_scan_api.md @@ -15,6 +15,7 @@ The generic sick_scan_xd API ships with the API-header, the library (binary or s ![apiComponentsDiagram1.png](apiComponentsDiagram1.png) **Note: Running multiple lidars simultaneously in one 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. + ## 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. @@ -167,6 +168,13 @@ The sick_scan_xd API can be used on Linux or Windows in any language with suppor * [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 @@ -335,6 +343,8 @@ The API provides the following functions for diagnostics: * 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. +* 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 diff --git a/driver/src/ldmrs/sick_ldmrs_driver.cpp b/driver/src/ldmrs/sick_ldmrs_driver.cpp index f952774c..2640f260 100644 --- a/driver/src/ldmrs/sick_ldmrs_driver.cpp +++ b/driver/src/ldmrs/sick_ldmrs_driver.cpp @@ -75,7 +75,6 @@ SickLDMRS::SickLDMRS(rosNodePtr nh, Manager *manager, std::shared_ptr -static sick_scan_xd::SickCallbackHandler s_cartesian_poincloud_callback_handler; -static sick_scan_xd::SickCallbackHandler s_polar_poincloud_callback_handler; +static sick_scan_xd::SickCallbackHandler s_cartesian_poincloud_callback_handler; +static sick_scan_xd::SickCallbackHandler s_polar_poincloud_callback_handler; static sick_scan_xd::SickCallbackHandler s_imu_callback_handler; static sick_scan_xd::SickCallbackHandler s_lidoutputstate_callback_handler; static sick_scan_xd::SickCallbackHandler s_lferec_callback_handler; static sick_scan_xd::SickCallbackHandler s_ldmrsobjectarray_callback_handler; static sick_scan_xd::SickCallbackHandler s_radarscan_callback_handler; static sick_scan_xd::SickCallbackHandler s_visualizationmarker_callback_handler; -static sick_scan_xd::SickCallbackHandler s_navposelandmark_callback_handler; +static sick_scan_xd::SickCallbackHandler s_navposelandmark_callback_handler; namespace sick_scan_xd { diff --git a/driver/src/sick_generic_laser.cpp b/driver/src/sick_generic_laser.cpp index 3a64a053..d48124c8 100644 --- a/driver/src/sick_generic_laser.cpp +++ b/driver/src/sick_generic_laser.cpp @@ -92,7 +92,7 @@ #include #define SICK_GENERIC_MAJOR_VER "3" -#define SICK_GENERIC_MINOR_VER "2" +#define SICK_GENERIC_MINOR_VER "3" #define SICK_GENERIC_PATCH_LEVEL "0" #define DELETE_PTR(p) if(p){delete(p);p=0;} @@ -147,6 +147,7 @@ static GenericLaserCallable* s_generic_laser_thread = 0; static NodeRunState runState = scanner_init; SICK_DIAGNOSTIC_STATUS s_status_code = SICK_DIAGNOSTIC_STATUS::INIT; std::string s_status_message = ""; +int32_t s_verbose_level = 1; // verbose level: 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. print informational, warnings and error messages. /*! \brief splitting expressions like := into and @@ -255,6 +256,20 @@ void getDiagnosticStatus(SICK_DIAGNOSTIC_STATUS& status_code, std::string& statu status_message = s_status_message; } +// Set verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET (equivalent to ros::console::levels), +// i.e. print messages on console above the given verbose level. +// Default verbose level is 1 (INFO), i.e. print informational, warnings and error messages. +void setVerboseLevel(int32_t verbose_level) +{ + s_verbose_level = verbose_level; +} + +// Returns the current verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET. Default verbose level is 1 (INFO) +int32_t getVerboseLevel() +{ + return s_verbose_level; +} + inline bool ends_with(std::string const &value, std::string const &ending) { if (ending.size() > value.size()) diff --git a/driver/src/sick_generic_radar.cpp b/driver/src/sick_generic_radar.cpp index 0de47b61..328781fe 100644 --- a/driver/src/sick_generic_radar.cpp +++ b/driver/src/sick_generic_radar.cpp @@ -1668,7 +1668,7 @@ namespace sick_scan_xd if (numFilteredTargets < numTargets) m_range_filter.resizePointCloud(numFilteredTargets, cloud_); // targets dropped by range filter, resize pointcloud #ifndef ROSSIMU - sick_scan_xd::PointCloud2withEcho sick_cloud_msg(&cloud_, 1, 0); + sick_scan_xd::PointCloud2withEcho sick_cloud_msg(&cloud_, 1, 0, "radar"); switch (iLoop) { case RADAR_PROC_RAW_TARGET: diff --git a/driver/src/sick_lmd_scandata_parser.cpp b/driver/src/sick_lmd_scandata_parser.cpp index d14b5ebe..4948b4f3 100644 --- a/driver/src/sick_lmd_scandata_parser.cpp +++ b/driver/src/sick_lmd_scandata_parser.cpp @@ -130,7 +130,7 @@ namespace sick_scan_xd /** Parse common result telegrams, i.e. parse telegrams of type LMDscandata received from the lidar */ bool parseCommonBinaryResultTelegram(const uint8_t* receiveBuffer, int receiveBufferLength, short& elevAngleX200, double elevAngleTelegramValToDeg, double& elevationAngleInRad, rosTime& recvTimeStamp, bool config_sw_pll_only_publish, bool use_generation_timestamp, SickGenericParser* parser_, bool& FireEncoder, sick_scan_msg::Encoder& EncoderMsg, int& numEchos, - std::vector& vang_vec, ros_sensor_msgs::LaserScan & msg) + std::vector& vang_vec, std::vector& azimuth_vec, ros_sensor_msgs::LaserScan & msg) { // bool lms1000_debug = true; // LMS-1000 diagnosis elevAngleX200 = 0; // signed short (F5 B2 -> Layer 24 @@ -300,11 +300,13 @@ namespace sick_scan_xd { process_dist, process_vang, + process_azimuth, process_rssi, process_idle }; int rssiCnt = 0; int vangleCnt = 0; + int azimuthCnt = 0; int distChannelCnt = 0; for (int processLoop = 0; processLoop < 2; processLoop++) @@ -324,6 +326,8 @@ namespace sick_scan_xd distChannelCnt = 0; rssiCnt = 0; vangleCnt = 0; + azimuthCnt = 0; + azimuth_vec.clear(); } if (processLoop == 1) @@ -346,12 +350,17 @@ namespace sick_scan_xd { vang_vec.clear(); } + if (azimuthCnt > 0) + { + azimuth_vec.resize(numberOfItems * azimuthCnt); + } // echoMask = (1 << numEchos) - 1; // reset count. We will use the counter for index calculation now. distChannelCnt = 0; rssiCnt = 0; vangleCnt = 0; + azimuthCnt = 0; } @@ -368,13 +377,11 @@ namespace sick_scan_xd #if 1 // prepared for multiecho parsing bCont = true; - bool doVangVecProc = false; // try to get number of DIST and RSSI from binary data task = process_idle; do { task = process_idle; - doVangVecProc = false; int processDataLenValuesInBytes = 2; if (totalChannelCnt == numberOf16BitChannels) @@ -407,9 +414,8 @@ namespace sick_scan_xd numberOfItems = 0; memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2); swap_endian((unsigned char *) &numberOfItems, 2); - } - if (strstr(szChannel, "VANG") == szChannel) + if (strstr(szChannel, "VANG") == szChannel) // MRS6000 transmits elevation angles on channel "VANG" { vangleCnt++; task = process_vang; @@ -417,9 +423,17 @@ namespace sick_scan_xd numberOfItems = 0; memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2); swap_endian((unsigned char *) &numberOfItems, 2); - vang_vec.resize(numberOfItems); - + } + if (strstr(szChannel, "ANGL") == szChannel) // MRS1xxx transmits azimuth angles on channel "ANGL" + { + azimuthCnt++; + task = process_azimuth; + bCont = true; + numberOfItems = 0; + memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2); + swap_endian((unsigned char *) &numberOfItems, 2); + azimuth_vec.resize(numberOfItems); } if (strstr(szChannel, "RSSI") == szChannel) { @@ -430,7 +444,6 @@ namespace sick_scan_xd // copy two byte value (unsigned short to numberOfItems memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2); swap_endian((unsigned char *) &numberOfItems, 2); // swap - } if (bCont) { @@ -527,7 +540,7 @@ namespace sick_scan_xd msg.angle_max *= -1.0; } - ROS_DEBUG_STREAM("msg.angle_min=" << (msg.angle_min*180/M_PI) << ", msg.angle_max=" << (msg.angle_max*180/M_PI) << ", msg.angle_increment=" << (msg.angle_increment*180/M_PI)); + ROS_DEBUG_STREAM("process_dist: msg.angle_min=" << (msg.angle_min*180/M_PI) << ", msg.angle_max=" << (msg.angle_max*180/M_PI) << ", msg.angle_increment=" << (msg.angle_increment*180/M_PI) << ", scaleFactor=" << scaleFactor << ", scaleFactorOffset=" << scaleFactorOffset); // Avoid 2*PI wrap around, if (msg.angle_max - msg.angle_min - 2*PI) is slightly above 0.0 due to floating point arithmetics bool wrap_avoid = false; @@ -548,7 +561,7 @@ namespace sick_scan_xd { msg.angle_increment = (msg.angle_max - msg.angle_min) / (numberOfItems - 1); } - ROS_DEBUG_STREAM("msg.angle_min=" << (msg.angle_min*180/M_PI) << ", msg.angle_max=" << (msg.angle_max*180/M_PI) << ", msg.angle_increment=" << (msg.angle_increment*180/M_PI)); + ROS_DEBUG_STREAM("process_dist: msg.angle_min=" << (msg.angle_min*180/M_PI) << ", msg.angle_max=" << (msg.angle_max*180/M_PI) << ", msg.angle_increment=" << (msg.angle_increment*180/M_PI) << ", scaleFactor=" << scaleFactor << ", scaleFactorOffset=" << scaleFactorOffset << ", " << (8*processDataLenValuesInBytes) << "-bit channel"); float *rangePtr = NULL; @@ -606,15 +619,45 @@ namespace sick_scan_xd } break; - case process_vang: - float *vangPtr = NULL; + case process_vang: // elevation angles MRS6000 if (numberOfItems > 0) { - vangPtr = &vang_vec[0]; // much faster, with vang_vec[i] each time the size will be checked + float *vangPtr = &vang_vec[0]; // much faster, with vang_vec[i] each time the size will be checked + for (int i = 0; i < numberOfItems; i++) + { + vangPtr[i] = (float) data[i] * scaleFactor + scaleFactorOffset; + } } - for (int i = 0; i < numberOfItems; i++) + break; + + case process_azimuth: // azimuth angles MRS1xxx + if (numberOfItems > 0) { - vangPtr[i] = (float) data[i] * scaleFactor + scaleFactorOffset; + float angle_min_rad = (1.0e-4f * startAngleDiv10000) * M_PI / 180.0 + parser_->getCurrentParamPtr()->getScanAngleShift(); + float angle_inc_rad = (1.0e-4f * sizeOfSingleAngularStepDiv10000) * M_PI / 180.0; + float angle_max_rad = angle_min_rad + (numberOfItems - 1) * angle_inc_rad; + ROS_DEBUG_STREAM("process_dist: msg.angle_min=" << (msg.angle_min*180/M_PI) << ", msg.angle_max=" << (msg.angle_max*180/M_PI) << ", msg.angle_increment=" << (msg.angle_increment*180/M_PI)); + ROS_DEBUG_STREAM("process_azimuth: angle_min=" << (angle_min_rad*180/M_PI) << ", angle_max=" << (angle_max_rad*180/M_PI) << ", angle_inc=" << (angle_inc_rad*180/M_PI) + << ", scaleFactor=" << scaleFactor << ", scaleFactorOffset=" << scaleFactorOffset << ", " << (8*processDataLenValuesInBytes) << "-bit channel"); + // angular correction according to "LMS4000---Angular-correction.pptx" for scalefactor=1: + // azimuth = angle_min + (i * angle_inc) + (offset + data[i]) with 0 < i < numberOfItems + float *azimuthPtr = &azimuth_vec[0]; + if (processDataLenValuesInBytes == 1) + { + uint8_t* data_ptr = (uint8_t*) data; + for (int i = 0; i < numberOfItems; i++) + azimuthPtr[i] = (angle_min_rad + i * angle_inc_rad + (float)(1.0e-4 * M_PI / 180.0) * ((float)data_ptr[i] * scaleFactor + scaleFactorOffset)); + } + else if (processDataLenValuesInBytes == 2) + { + uint16_t* data_ptr = (uint16_t*) data; + for (int i = 0; i < numberOfItems; i++) + azimuthPtr[i] = (angle_min_rad + i * angle_inc_rad + (float)(1.0e-4 * M_PI / 180.0) * ((float)data_ptr[i] * scaleFactor + scaleFactorOffset)); + } + // std::stringstream dbg_stream; + // for (int i = 0; i < numberOfItems; i++) + // dbg_stream << " " << std::fixed << std::setprecision(3) << (azimuth_vec[i]*180/M_PI); + // ROS_DEBUG_STREAM("azimuth table: " << dbg_stream.str()); } break; } diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp index 1da1e442..a81c037b 100644 --- a/driver/src/sick_scan_common.cpp +++ b/driver/src/sick_scan_common.cpp @@ -517,7 +517,6 @@ namespace sick_scan_xd { datagram_pub_ = rosAdvertise(nh, nodename + "/datagram", 1000); } - std::string cloud_topic_val = "cloud"; rosDeclareParam(nh, "cloud_topic", cloud_topic_val); rosGetParam(nh, "cloud_topic", cloud_topic_val); @@ -3324,13 +3323,23 @@ namespace sick_scan_xd else scandatacfg_timingflag = 1; // default: Timing flag LMDscandatacfg on } + int rssi_flag = rssiFlag ? 1 : 0; + if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_MRS_1XXX_NAME) == 0) + { + int scandatacfg_azimuth_table = 0; + rosDeclareParam(nh, "scandatacfg_azimuth_table", scandatacfg_azimuth_table); + rosGetParam(nh, "scandatacfg_azimuth_table", scandatacfg_azimuth_table); + if (scandatacfg_azimuth_table > 0) + rssi_flag |= 0x2; // set (enable) "transmit angle flag" for MRS-1xxx + ROS_INFO_STREAM("MRS1xxx scandatacfg_azimuth_table=" << scandatacfg_azimuth_table << ", rssi_flag=" << rssi_flag << ", azimuth table " << ((rssi_flag & 0x02) != 0 ? "": "not ") << "activated"); + } //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 const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_PARTIAL_SCANDATA_CFG].c_str(); - sprintf(requestLMDscandatacfg, pcCmdMask, outputChannelFlagId, rssiFlag ? 1 : 0, + sprintf(requestLMDscandatacfg, pcCmdMask, outputChannelFlagId, rssi_flag, rssiResolutionIs16Bit ? 1 : 0, EncoderSettings != -1 ? EncoderSettings : 0, scandatacfg_timingflag); @@ -4534,7 +4543,7 @@ namespace sick_scan_xd char *dstart, *dend; bool dumpDbg = false; bool dataToProcess = true; - std::vector vang_vec; + std::vector vang_vec, azimuth_vec; vang_vec.clear(); dstart = NULL; dend = NULL; @@ -4613,7 +4622,7 @@ namespace sick_scan_xd else { success = parseCommonBinaryResultTelegram(receiveBuffer, actual_length, elevAngleX200, elevAngleTelegramValToDeg, elevationAngleInRad, recvTimeStamp, - config_.sw_pll_only_publish, config_.use_generation_timestamp, parser_, FireEncoder, EncoderMsg, numEchos, vang_vec, msg); + config_.sw_pll_only_publish, config_.use_generation_timestamp, parser_, FireEncoder, EncoderMsg, numEchos, vang_vec, azimuth_vec, msg); if (!success) ROS_ERROR_STREAM("## ERROR SickScanCommon::loopOnce(): parseCommonBinaryResultTelegram() failed"); } @@ -4716,7 +4725,6 @@ namespace sick_scan_xd // Copy to pointcloud int layer = 0; int baseLayer = 0; - bool useGivenElevationAngle = false; switch (numOfLayers) { @@ -4748,10 +4756,6 @@ namespace sick_scan_xd #endif elevationPreCalculated = true; - if (vang_vec.size() > 0) - { - useGivenElevationAngle = true; - } break; default: assert(0); @@ -5008,11 +5012,16 @@ namespace sick_scan_xd float *sinAlphaTablePtr = &sinAlphaTable[0]; float *vangPtr = NULL; + float *azimuthPtr = NULL; float *rangeTmpPtr = &rangeTmp[0]; if (vang_vec.size() > 0) { vangPtr = &vang_vec[0]; } + if (azimuth_vec.size() > 0) + { + azimuthPtr = &azimuth_vec[0]; + } size_t rangeNumPointcloudCurEcho = 0; for (size_t rangeIdxScan = 0; rangeIdxScan < rangeNum; rangeIdxScan++) @@ -5037,7 +5046,7 @@ namespace sick_scan_xd float phi = angle; // azimuth angle float alpha = 0.0; // elevation angle - if (useGivenElevationAngle) // FOR MRS6124 + if (vangPtr) // use elevation table for MRS6124 { alpha = -vangPtr[rangeIdxScan] * deg2rad_const; } @@ -5079,6 +5088,12 @@ namespace sick_scan_xd { phi_used = angleCompensator->compensateAngleInRadFromRos(phi_used); } + if (azimuthPtr) + { + // ROS_DEBUG_STREAM("azimuth[" << rangeIdxScan << "] = " << std::fixed << std::setprecision(3) << (azimuthPtr[rangeIdxScan] * 180 / M_PI) << ", angle diff = " << ((azimuthPtr[rangeIdxScan] - phi_used) * 180 / M_PI)); + phi_used = azimuthPtr[rangeIdxScan]; // use azimuth table for MRS1xxx + } + // Cartesian pointcloud float phi2_used = phi_used + m_add_transform_xyz_rpy.azimuthOffset(); fptr[idx_x] = rangeCos * (float)cos(phi2_used) * mirror_factor; // copy x value in pointcloud @@ -5155,8 +5170,8 @@ namespace sick_scan_xd range_filter.resizePointCloud(rangeNumPointcloudAllEchos, cloud_polar_); } - sick_scan_xd::PointCloud2withEcho cloud_msg(&cloud_, numValidEchos, 0); - sick_scan_xd::PointCloud2withEcho cloud_msg_polar(&cloud_polar_, numValidEchos, 0); + sick_scan_xd::PointCloud2withEcho cloud_msg(&cloud_, numValidEchos, 0, cloud_topic_val); + sick_scan_xd::PointCloud2withEcho cloud_msg_polar(&cloud_polar_, numValidEchos, 0, cloud_topic_val); #ifdef ROSSIMU notifyPolarPointcloudListener(nh, &cloud_msg_polar); notifyCartesianPointcloudListener(nh, &cloud_msg); @@ -5256,7 +5271,7 @@ namespace sick_scan_xd assert(partialCloud.data.size() == partialCloud.width * partialCloud.point_step); - sick_scan_xd::PointCloud2withEcho partial_cloud_msg(&partialCloud, numValidEchos, 0); + sick_scan_xd::PointCloud2withEcho partial_cloud_msg(&partialCloud, numValidEchos, 0, cloud_topic_val); notifyCartesianPointcloudListener(nh, &partial_cloud_msg); rosPublish(cloud_pub_, partialCloud); //memcpy(&(partialCloud.data[0]), &(cloud_.data[0]) + i * cloud_.point_step, cloud_.point_step * numPartialShots); diff --git a/driver/src/sick_scan_services.cpp b/driver/src/sick_scan_services.cpp index 24ca4f9e..b8e33e21 100644 --- a/driver/src/sick_scan_services.cpp +++ b/driver/src/sick_scan_services.cpp @@ -416,7 +416,7 @@ bool sick_scan_xd::SickScanServices::sendSopasCmdCheckResponse(const std::string * @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: 1 +* @param[in] scandataformat ScanDataFormat: 1 for msgpack or 2 for compact scandata, default: 2 * @param[in] imu_enable: Imu data transfer enabled * @param[in] imu_udp_port: UDP port of imu data (if imu_enable is true) */ diff --git a/driver/src/sick_scan_xd_api/api_impl.cpp b/driver/src/sick_scan_xd_api/api_impl.cpp index 2679d2bd..5023450d 100644 --- a/driver/src/sick_scan_xd_api/api_impl.cpp +++ b/driver/src/sick_scan_xd_api/api_impl.cpp @@ -63,6 +63,7 @@ static SickScanPointCloudMsg convertPointCloudMsg(const sick_scan_xd::PointCloud export_msg.header.timestamp_sec = sec(msg.header.stamp); // msg.header.stamp.sec; export_msg.header.timestamp_nsec = nsec(msg.header.stamp); // msg.header.stamp.nsec; strncpy(export_msg.header.frame_id, msg.header.frame_id.c_str(), sizeof(export_msg.header.frame_id) - 2); + strncpy(export_msg.topic, msg_with_echo.topic.c_str(), sizeof(export_msg.topic) - 2); export_msg.width = msg.width; export_msg.height = msg.height; export_msg.is_bigendian = msg.is_bigendian; @@ -254,7 +255,7 @@ static SickScanRadarScan convertRadarScanMsg(const sick_scan_msg::RadarScan& src dst_msg.radarpreheader.iencoderspeed[n] = src_msg.radarpreheader.radarpreheaderarrayencoderblock[n].iencoderspeed; } // Copy radar target pointcloud data - sick_scan_xd::PointCloud2withEcho targets_with_echo(&src_msg.targets, 1, 0); + sick_scan_xd::PointCloud2withEcho targets_with_echo(&src_msg.targets, 1, 0, "radar"); dst_msg.targets = convertPointCloudMsg(targets_with_echo); // Copy radar object data dst_msg.objects.size = src_msg.objects.size(); @@ -1364,6 +1365,53 @@ int32_t SickScanApiGetStatus(SickScanApiHandle apiHandle, int32_t* status_code, return SICK_SCAN_API_ERROR; } +// Set verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET (equivalent to ros::console::levels), +// i.e. print messages on console above the given verbose level. +// Default verbose level is 1 (INFO), i.e. print informational, warnings and error messages. +int32_t SickScanApiSetVerboseLevel(SickScanApiHandle apiHandle, int32_t verbose_level) +{ + try + { + if (apiHandle == 0) + { + ROS_ERROR_STREAM("## ERROR SickScanApiSetVerboseLevel(): invalid apiHandle"); + return SICK_SCAN_API_NOT_INITIALIZED; + } + setVerboseLevel(verbose_level); + return SICK_SCAN_API_SUCCESS; + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("## ERROR SickScanApiSetVerboseLevel(): exception " << e.what()); + } + catch(...) + { + ROS_ERROR_STREAM("## ERROR SickScanApiSetVerboseLevel(): unknown exception "); + } + return SICK_SCAN_API_ERROR; +} + +// Returns the current verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET. Default verbose level is 1 (INFO) +int32_t SickScanApiGetVerboseLevel(SickScanApiHandle apiHandle) +{ + int32_t verbose_level = 1; + try + { + if (apiHandle == 0) + ROS_ERROR_STREAM("## ERROR getVerboseLevel(): invalid apiHandle"); + verbose_level = getVerboseLevel(); + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("## ERROR getVerboseLevel(): exception " << e.what()); + } + catch(...) + { + ROS_ERROR_STREAM("## ERROR getVerboseLevel(): unknown exception "); + } + return verbose_level; +} + // Notifies all registered log message listener, i.e. all registered listener callbacks are called for all messages of type INFO, WARN, ERROR or FATAL void notifyLogMessageListener(int msg_level, const std::string& message) { diff --git a/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp b/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp index 342085eb..52e68ecd 100644 --- a/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp +++ b/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp @@ -273,6 +273,8 @@ sick_scansegment_xd::RosMsgpackPublisher::RosMsgpackPublisher(const std::string& // m_segment_count = config.segment_count; m_all_segments_azimuth_min_deg = (float)config.all_segments_min_deg; m_all_segments_azimuth_max_deg = (float)config.all_segments_max_deg; + m_host_FREchoFilter = config.host_FREchoFilter; + m_host_set_FREchoFilter = config.host_set_FREchoFilter; if (config.host_set_LFPangleRangeFilter) { initLFPangleRangeFilterSettings(config.host_LFPangleRangeFilter); @@ -461,18 +463,18 @@ std::string sick_scansegment_xd::RosMsgpackPublisher::printCoverageTable(const s } /** Shortcut to publish a PointCloud2Msg */ -void sick_scansegment_xd::RosMsgpackPublisher::publishPointCloud2Msg(rosNodePtr node, PointCloud2MsgPublisher& publisher, PointCloud2Msg& pointcloud_msg, int32_t num_echos, int32_t segment_idx, int coordinate_notation) +void sick_scansegment_xd::RosMsgpackPublisher::publishPointCloud2Msg(rosNodePtr node, PointCloud2MsgPublisher& publisher, PointCloud2Msg& pointcloud_msg, int32_t num_echos, int32_t segment_idx, int coordinate_notation, const std::string& topic) { if (coordinate_notation == 0) // coordinateNotation=0: cartesian (default, pointcloud has fields x,y,z,i) => notify cartesian pointcloud listener { - sick_scan_xd::PointCloud2withEcho cloud_msg_with_echo(&pointcloud_msg, num_echos, segment_idx); + sick_scan_xd::PointCloud2withEcho cloud_msg_with_echo(&pointcloud_msg, num_echos, segment_idx, topic); notifyCartesianPointcloudListener(node, &cloud_msg_with_echo); } #if defined RASPBERRY && RASPBERRY > 0 // polar pointcloud deactivated on Raspberry for performance reasons #else if (coordinate_notation == 1) // coordinateNotation=1: polar (pointcloud has fields azimuth,elevation,r,i) => notify polar pointcloud listener { - sick_scan_xd::PointCloud2withEcho cloud_msg_with_echo(&pointcloud_msg, num_echos, segment_idx); + sick_scan_xd::PointCloud2withEcho cloud_msg_with_echo(&pointcloud_msg, num_echos, segment_idx, topic); notifyPolarPointcloudListener(node, &cloud_msg_with_echo); } #endif @@ -530,7 +532,7 @@ void sick_scansegment_xd::RosMsgpackPublisher::convertPointsToCustomizedFieldsCl pointcloud_msg.header.stamp.sec = timestamp_sec; #if defined __ROS_VERSION && __ROS_VERSION > 1 pointcloud_msg.header.stamp.nanosec = timestamp_nsec; -#elif defined __ROS_VERSION && __ROS_VERSION > 0 +#else pointcloud_msg.header.stamp.nsec = timestamp_nsec; #endif pointcloud_msg.header.frame_id = pointcloud_cfg.frameid(); @@ -779,9 +781,22 @@ void sick_scansegment_xd::RosMsgpackPublisher::convertPointsToLaserscanMsg(uint3 // Create laserscan messages for all echos and layers int num_echos = (int)lidar_points.size(); + int num_echos_publish = num_echos; for(LaserScanMsgMap::iterator laser_scan_echo_iter = laser_scan_msg_map.begin(); laser_scan_echo_iter != laser_scan_msg_map.end(); laser_scan_echo_iter++) { int echo_idx = laser_scan_echo_iter->first; + bool echo_enabled = true; + // If only one echo is activated by FREchoFilter, but 3 echos are received, we apply the FREchoFilter for laserscan messages: + if (m_host_set_FREchoFilter && num_echos > 1 && (m_host_FREchoFilter == 0 || m_host_FREchoFilter == 2)) // m_host_FREchoFilter == 0: FIRST_ECHO only (EchoCount=1), m_host_FREchoFilter == 1: ALL_ECHOS, m_host_FREchoFilter == 2: LAST_ECHO (EchoCount=1) + { + num_echos_publish = 1; + if (m_host_FREchoFilter == 0 && echo_idx > 0) + echo_enabled = false; // m_host_FREchoFilter == 0: FIRST_ECHO only (EchoCount=1) + else if (m_host_FREchoFilter == 2 && echo_idx < num_echos - 1) + echo_enabled = false; // m_host_FREchoFilter == 2: LAST_ECHO only (EchoCount=1) + } + if (!echo_enabled) + continue; std::map& laser_scan_layer_map = laser_scan_echo_iter->second; for(std::map::iterator laser_scan_msg_iter = laser_scan_layer_map.begin(); laser_scan_msg_iter != laser_scan_layer_map.end(); laser_scan_msg_iter++) { @@ -805,7 +820,7 @@ void sick_scansegment_xd::RosMsgpackPublisher::convertPointsToLaserscanMsg(uint3 laser_scan_msg.header.stamp.nsec = timestamp_nsec; #endif laser_scan_msg.header.frame_id = frame_id + "_" + std::to_string(layer_idx + 1); - if (num_echos > 1) + if (num_echos_publish > 1) laser_scan_msg.header.frame_id = laser_scan_msg.header.frame_id + "_" + std::to_string(echo_idx); // scan_time = 1 / scan_frequency = time for a full 360-degree rotation of the sensor laser_scan_msg.scan_time = m_scan_time; @@ -820,6 +835,24 @@ void sick_scansegment_xd::RosMsgpackPublisher::convertPointsToLaserscanMsg(uint3 } } } + // Remove filtered echos + for(LaserScanMsgMap::iterator laser_scan_echo_iter = laser_scan_msg_map.begin(); laser_scan_echo_iter != laser_scan_msg_map.end(); ) + { + int echo_idx = laser_scan_echo_iter->first; + bool echo_found = false; + std::map& laser_scan_layer_map = laser_scan_echo_iter->second; + for(std::map::iterator laser_scan_msg_iter = laser_scan_layer_map.begin(); !echo_found && laser_scan_msg_iter != laser_scan_layer_map.end(); laser_scan_msg_iter++) + { + int layer_idx = laser_scan_msg_iter->first; + ros_sensor_msgs::LaserScan& laser_scan_msg = laser_scan_msg_iter->second; + if (!laser_scan_msg.header.frame_id.empty() && !laser_scan_msg.ranges.empty()) + echo_found = true; + } + if(!echo_found) + laser_scan_echo_iter = laser_scan_msg_map.erase(laser_scan_echo_iter); + else + laser_scan_echo_iter++; + } #endif // !RASPBERRY } @@ -951,7 +984,7 @@ void sick_scansegment_xd::RosMsgpackPublisher::HandleMsgPackData(const sick_scan { PointCloud2Msg pointcloud_msg_custom_fields; convertPointsToCustomizedFieldsCloud(m_points_collector.timestamp_sec, m_points_collector.timestamp_nsec, m_points_collector.lidar_points, custom_pointcloud_cfg, pointcloud_msg_custom_fields); - publishPointCloud2Msg(m_node, custom_pointcloud_cfg.publisher(), pointcloud_msg_custom_fields, std::max(1, (int)echo_count), -1, custom_pointcloud_cfg.coordinateNotation()); + publishPointCloud2Msg(m_node, custom_pointcloud_cfg.publisher(), pointcloud_msg_custom_fields, std::max(1, (int)echo_count), -1, custom_pointcloud_cfg.coordinateNotation(), custom_pointcloud_cfg.topic()); // ROS_INFO_STREAM("RosMsgpackPublisher::HandleMsgPackData(): published " << pointcloud_msg_custom_fields.width << "x" << pointcloud_msg_custom_fields.height << " pointcloud, " << pointcloud_msg_custom_fields.fields.size() << " fields/point, " << pointcloud_msg_custom_fields.data.size() << " bytes"); } } @@ -1027,7 +1060,7 @@ void sick_scansegment_xd::RosMsgpackPublisher::HandleMsgPackData(const sick_scan { PointCloud2Msg pointcloud_msg_custom_fields; convertPointsToCustomizedFieldsCloud(msgpack_data.timestamp_sec, msgpack_data.timestamp_nsec, lidar_points, custom_pointcloud_cfg, pointcloud_msg_custom_fields); - publishPointCloud2Msg(m_node, custom_pointcloud_cfg.publisher(), pointcloud_msg_custom_fields, std::max(1, (int)echo_count), segment_idx, custom_pointcloud_cfg.coordinateNotation()); + publishPointCloud2Msg(m_node, custom_pointcloud_cfg.publisher(), pointcloud_msg_custom_fields, std::max(1, (int)echo_count), segment_idx, custom_pointcloud_cfg.coordinateNotation(), custom_pointcloud_cfg.topic()); ROS_DEBUG_STREAM("publishPointCloud2Msg: " << pointcloud_msg_custom_fields.width << "x" << pointcloud_msg_custom_fields.height << " pointcloud, " << pointcloud_msg_custom_fields.fields.size() << " fields/point, " << pointcloud_msg_custom_fields.data.size() << " bytes"); } } diff --git a/include/sick_scan/ldmrs/sick_ldmrs_driver.hpp b/include/sick_scan/ldmrs/sick_ldmrs_driver.hpp index 32d78e3d..d701af5f 100644 --- a/include/sick_scan/ldmrs/sick_ldmrs_driver.hpp +++ b/include/sick_scan/ldmrs/sick_ldmrs_driver.hpp @@ -128,6 +128,7 @@ class SickLDMRS : public application::BasicApplication // ROS rosNodePtr nh_; + std::string cloud_topic_val = "cloud"; rosPublisher pub_; rosPublisher object_pub_; // Diagnostics diff --git a/include/sick_scan/sick_generic_callback.h b/include/sick_scan/sick_generic_callback.h index 3a5aed1e..f22c2742 100644 --- a/include/sick_scan/sick_generic_callback.h +++ b/include/sick_scan/sick_generic_callback.h @@ -75,13 +75,16 @@ namespace sick_scan_xd { struct PointCloud2withEcho { - PointCloud2withEcho(const ros_sensor_msgs::PointCloud2* msg = 0, int32_t _num_echos = 0, int32_t _segment_idx = 0) : num_echos(_num_echos), segment_idx(_segment_idx) + PointCloud2withEcho() {} + PointCloud2withEcho(const ros_sensor_msgs::PointCloud2* msg, int32_t _num_echos, int32_t _segment_idx, const std::string& _topic) + : num_echos(_num_echos), segment_idx(_segment_idx), topic(_topic) { pointcloud = ((msg) ? (*msg) : (ros_sensor_msgs::PointCloud2())); } ros_sensor_msgs::PointCloud2 pointcloud; // ROS PointCloud2 - int32_t num_echos; // number of echos - int32_t segment_idx; // segment index (or -1 if pointcloud contains data from multiple segments) + int32_t num_echos = 0; // number of echos + int32_t segment_idx = 0; // segment index (or -1 if pointcloud contains data from multiple segments) + std::string topic; // ros topic this pointcloud is published }; typedef void(* PointCloud2Callback)(rosNodePtr handle, const PointCloud2withEcho* msg); @@ -314,15 +317,15 @@ namespace sick_scan_xd static std::mutex s_wait_for_message_handler_mutex; // mutex to protect access to s_wait_for_message_handler_list }; // class SickWaitForMessageHandler - typedef SickWaitForMessageHandler WaitForCartesianPointCloudMessageHandler; - typedef SickWaitForMessageHandler WaitForPolarPointCloudMessageHandler; + typedef SickWaitForMessageHandler WaitForCartesianPointCloudMessageHandler; + typedef SickWaitForMessageHandler WaitForPolarPointCloudMessageHandler; typedef SickWaitForMessageHandler WaitForImuMessageHandler; typedef SickWaitForMessageHandler WaitForLFErecMessageHandler; typedef SickWaitForMessageHandler WaitForLIDoutputstateMessageHandler; typedef SickWaitForMessageHandler WaitForRadarScanMessageHandler; typedef SickWaitForMessageHandler WaitForLdmrsObjectArrayMessageHandler; typedef SickWaitForMessageHandler WaitForVisualizationMarkerMessageHandler; - typedef SickWaitForMessageHandler WaitForNAVPOSDataMessageHandler; + typedef SickWaitForMessageHandler WaitForNAVPOSDataMessageHandler; } // namespace sick_scan_xd #endif // __SICK_GENERIC_CALLBACK_H_INCLUDED diff --git a/include/sick_scan/sick_lmd_scandata_parser.h b/include/sick_scan/sick_lmd_scandata_parser.h index eb52d475..d8f16267 100644 --- a/include/sick_scan/sick_lmd_scandata_parser.h +++ b/include/sick_scan/sick_lmd_scandata_parser.h @@ -72,7 +72,7 @@ namespace sick_scan_xd /** Parse common result telegrams, i.e. parse telegrams of type LMDscandata received from the lidar */ bool parseCommonBinaryResultTelegram(const uint8_t* receiveBuffer, int receiveBufferLength, short& elevAngleX200, double elevAngleTelegramValToDeg, double& elevationAngleInRad, rosTime& recvTimeStamp, bool config_sw_pll_only_publish, bool use_generation_timestamp, SickGenericParser* parser_, bool& FireEncoder, sick_scan_msg::Encoder& EncoderMsg, int& numEchos, - std::vector& vang_vec, ros_sensor_msgs::LaserScan & msg); + std::vector& vang_vec, std::vector& azimuth_vec, ros_sensor_msgs::LaserScan & msg); /** Increments the number of packets received in the SoftwarePLL */ void incSoftwarePLLPacketReceived(); diff --git a/include/sick_scan/sick_scan_common.h b/include/sick_scan/sick_scan_common.h index 05d33ed2..dae5c272 100644 --- a/include/sick_scan/sick_scan_common.h +++ b/include/sick_scan/sick_scan_common.h @@ -440,6 +440,7 @@ namespace sick_scan_xd private: SopasProtocol m_protocolId; + std::string cloud_topic_val = "cloud"; // ROS rosPublisher pub_; rosPublisher datagram_pub_; diff --git a/include/sick_scan/sick_scan_logging.h b/include/sick_scan/sick_scan_logging.h index 42e76e73..7fdfd5e1 100644 --- a/include/sick_scan/sick_scan_logging.h +++ b/include/sick_scan/sick_scan_logging.h @@ -92,12 +92,25 @@ void notifyLogMessageListener(int msg_level, const std::string& message); // Notifies all registered listener about a new diagnostic status void notifyDiagnosticListener(SICK_DIAGNOSTIC_STATUS status_code, const std::string& status_message); +// Set verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET (equivalent to ros::console::levels), +// i.e. print messages on console above the given verbose level. +// Default verbose level is 1 (INFO), i.e. print informational, warnings and error messages. +void setVerboseLevel(int32_t verbose_level); + +// Returns the current verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET. Default verbose level is 1 (INFO) +int32_t getVerboseLevel(); + #if __ROS_VERSION <= 1 // i.e. native Linux or Windows or ROS-1 -#define SICK_INFO_LOG(ros_level,...) do{ std::string _msg=vargs_to_string(__VA_ARGS__); ROS_LOG(ros_level,ROSCONSOLE_DEFAULT_NAME,__VA_ARGS__); notifyLogMessageListener(ros_level,_msg); }while(0) -#define SICK_INFO_LOG_STREAM(ros_level,args) do{ std::stringstream _msg; _msg<=getVerboseLevel()){ROS_LOG(ros_level,ROSCONSOLE_DEFAULT_NAME,__VA_ARGS__);} notifyLogMessageListener(ros_level,_msg); }while(0) +#define SICK_INFO_LOG_STREAM(ros_level,args) do{ std::stringstream _msg; _msg<=getVerboseLevel()){ROS_LOG_STREAM(ros_level,ROSCONSOLE_DEFAULT_NAME,args);} notifyLogMessageListener(ros_level,_msg.str()); }while(0) +#define SICK_ERROR_LOG(ros_level,diag_status,...) do{ std::string _msg=vargs_to_string(__VA_ARGS__); setDiagnosticStatus(diag_status,_msg); if(ros_level>=getVerboseLevel()){ROS_LOG(ros_level,ROSCONSOLE_DEFAULT_NAME,__VA_ARGS__);} notifyLogMessageListener(ros_level,_msg); }while(0) +#define SICK_ERROR_LOG_STREAM(ros_level,diag_status,args) do{ std::stringstream _msg; _msg<=getVerboseLevel()){ROS_LOG_STREAM(ros_level,ROSCONSOLE_DEFAULT_NAME,args);} notifyLogMessageListener(ros_level,_msg.str()); }while(0) + +#undef ROS_DEBUG +#undef ROS_DEBUG_STREAM +#define ROS_DEBUG(...) SICK_INFO_LOG(::ros::console::levels::Debug,__VA_ARGS__) +#define ROS_DEBUG_STREAM(args) SICK_INFO_LOG_STREAM(::ros::console::levels::Debug,args) #undef ROS_INFO #undef ROS_INFO_STREAM diff --git a/include/sick_scan_xd_api/sick_scan_api.h b/include/sick_scan_xd_api/sick_scan_api.h index 92c57ec3..48da24ea 100644 --- a/include/sick_scan_xd_api/sick_scan_api.h +++ b/include/sick_scan_xd_api/sick_scan_api.h @@ -147,6 +147,7 @@ typedef struct SickScanPointCloudMsgType // equivalent to ros::std_msgs::PointCl uint8_t is_dense; // True if there are no invalid points int32_t num_echos; // number of echos int32_t segment_idx; // segment index (or -1 if pointcloud contains data from multiple segments) + char topic[256]; // ros topic this pointcloud is published } SickScanPointCloudMsg; typedef struct SickScanVector3MsgType // equivalent to geometry_msgs/Vector3 @@ -544,6 +545,14 @@ SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiDeregisterLogMsg(SickScanApiHan // Query current status and status message SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiGetStatus(SickScanApiHandle apiHandle, int32_t* status_code, char* message_buffer, int32_t message_buffer_size); +// Set verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET (equivalent to ros::console::levels), +// i.e. print messages on console above the given verbose level. +// Default verbose level is 1 (INFO), i.e. print informational, warnings and error messages. +SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiSetVerboseLevel(SickScanApiHandle apiHandle, int32_t verbose_level); + +// Returns the current verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET. Default verbose level is 1 (INFO) +SICK_SCAN_API_DECLSPEC_EXPORT int32_t SickScanApiGetVerboseLevel(SickScanApiHandle apiHandle); + /* * Polling functions */ diff --git a/include/sick_scansegment_xd/ros_msgpack_publisher.h b/include/sick_scansegment_xd/ros_msgpack_publisher.h index 1d6b4217..ebbf5361 100644 --- a/include/sick_scansegment_xd/ros_msgpack_publisher.h +++ b/include/sick_scansegment_xd/ros_msgpack_publisher.h @@ -355,7 +355,7 @@ namespace sick_scansegment_xd void convertPointsToLaserscanMsg(uint32_t timestamp_sec, uint32_t timestamp_nsec, const std::vector>& lidar_points, size_t total_point_count, LaserScanMsgMap& laser_scan_msg_map, const std::string& frame_id, bool is_fullframe); /** Shortcut to publish a PointCloud2Msg */ - void publishPointCloud2Msg(rosNodePtr node, PointCloud2MsgPublisher& publisher, PointCloud2Msg& pointcloud_msg, int32_t num_echos, int32_t segment_idx, int coordinate_notation); + void publishPointCloud2Msg(rosNodePtr node, PointCloud2MsgPublisher& publisher, PointCloud2Msg& pointcloud_msg, int32_t num_echos, int32_t segment_idx, int coordinate_notation, const std::string& topic); /** Shortcut to publish Laserscan messages */ void publishLaserScanMsg(rosNodePtr node, LaserscanMsgPublisher& laserscan_publisher, LaserScanMsgMap& laser_scan_msg_map, int32_t num_echos, int32_t segment_idx); @@ -377,6 +377,8 @@ namespace sick_scansegment_xd float m_all_segments_azimuth_max_deg = +180; // if received segments cover azimuth angle range from m_all_segments_azimuth_min_deg to m_all_segments_azimuth_max_deg. -180...+180 for multiScan136 (360 deg fullscan) float m_all_segments_elevation_min_deg = 0; // angle range covering all segments: all segments pointcloud on topic publish_topic_all_segments is published, float m_all_segments_elevation_max_deg = 0; // if received segments cover elevation angle range from m_all_segments_elevation_min_deg to m_all_segments_elevation_max_deg. + int m_host_FREchoFilter; // configuration from launchfile: Optionally set FREchoFilter with 0 for FIRST_ECHO (EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1) + bool m_host_set_FREchoFilter; // configuration from launchfile: If true, FREchoFilter is set at startup (default: false) SegmentPointsCollector m_points_collector; // collects all points of 12 segments (12 segments * 30 deg = 360 deg) LaserscanMsgPublisher m_publisher_laserscan_360; // ros publisher to publish LaserScan messages of all segments (360 degree) LaserscanMsgPublisher m_publisher_laserscan_segment; // ros publisher to publish LaserScan messages of the current segment diff --git a/launch/sick_mrs_1xxx.launch b/launch/sick_mrs_1xxx.launch index a19e3025..f8eaa56b 100644 --- a/launch/sick_mrs_1xxx.launch +++ b/launch/sick_mrs_1xxx.launch @@ -52,6 +52,7 @@ default max_angle for this scanner type is +137.5 degree. + diff --git a/launch/sick_picoscan.launch b/launch/sick_picoscan.launch index 69cbc83d..fe1296c0 100644 --- a/launch/sick_picoscan.launch +++ b/launch/sick_picoscan.launch @@ -14,6 +14,9 @@ + + + @@ -66,7 +69,7 @@ - + diff --git a/launch/sick_picoscan_single_echo.launch b/launch/sick_picoscan_single_echo.launch new file mode 100644 index 00000000..b415f1e1 --- /dev/null +++ b/launch/sick_picoscan_single_echo.launch @@ -0,0 +1,227 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 57fb3e33..23b9dfca 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ sick_scan_xd - 3.2.0 + 3.3.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 cf63f7b0..bea549c6 100644 --- a/python/api/sick_scan_api.py +++ b/python/api/sick_scan_api.py @@ -168,8 +168,9 @@ class SickScanPointCloudMsg(ctypes.Structure): ("data", SickScanUint8Array), # Actual point data, size is (row_step*height) ("is_dense", ctypes.c_uint8), # True if there are no invalid points ("num_echos", ctypes.c_int32), # number of echos - ("segment_idx", ctypes.c_int32) # segment index (or -1 if pointcloud contains data from multiple segments) - ] + ("segment_idx", ctypes.c_int32), # segment index (or -1 if pointcloud contains data from multiple segments) + ("topic", ctypes.c_char * 256) # ros topic this pointcloud is published + ] class SickScanVector3Msg(ctypes.Structure): """ @@ -977,6 +978,12 @@ def SickScanApiLoadLibrary(paths, lib_filname): # sick_scan_api.h: int32_t SickScanApiGetStatus(SickScanApiHandle apiHandle, int32_t* status_code, char* message_buffer, int32_t message_buffer_size); sick_scan_library.SickScanApiGetStatus.argtypes = [ctypes.c_void_p, ctypes.POINTER(ctypes.c_int32), ctypes.c_char_p, ctypes.c_int32] sick_scan_library.SickScanApiGetStatus.restype = ctypes.c_int + # sick_scan_api.h: int32_t SickScanApiSetVerboseLevel(SickScanApiHandle apiHandle, int32_t verbose_level); + sick_scan_library.SickScanApiSetVerboseLevel.argtypes = [ctypes.c_void_p, ctypes.c_int32] + sick_scan_library.SickScanApiSetVerboseLevel.restype = ctypes.c_int + # sick_scan_api.h: int32_t SickScanApiGetVerboseLevel(SickScanApiHandle apiHandle); + sick_scan_library.SickScanApiGetVerboseLevel.argtypes = [ctypes.c_void_p] + sick_scan_library.SickScanApiGetVerboseLevel.restype = ctypes.c_int # sick_scan_api.h: int32_t SickScanApiWaitNextCartesianPointCloudMsg(SickScanApiHandle apiHandle, SickScanPointCloudMsg* msg, double timeout_sec); sick_scan_library.SickScanApiWaitNextCartesianPointCloudMsg.argtypes = [ctypes.c_void_p, ctypes.POINTER(SickScanPointCloudMsg), ctypes.c_double] sick_scan_library.SickScanApiWaitNextCartesianPointCloudMsg.restype = ctypes.c_int @@ -1221,6 +1228,20 @@ 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) +def SickScanApiSetVerboseLevel(sick_scan_library, api_handle, verbose_level): + """ + Set verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET (equivalent to ros::console::levels), + i.e. print messages on console above the given verbose level. + Default verbose level is 1 (INFO), i.e. print informational, warnings and error messages. + """ + return sick_scan_library.SickScanApiSetVerboseLevel(api_handle, verbose_level) + +def SickScanApiGetVerboseLevel(sick_scan_library, api_handle): + """ + Returns the current verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET. Default verbose level is 1 (INFO) + """ + return sick_scan_library.SickScanApiGetVerboseLevel(api_handle) + """ Polling functions """ diff --git a/roswrap/src/rossimu/kinetic/include/geometry_msgs/PoseWithCovariance.h b/roswrap/src/rossimu/kinetic/include/geometry_msgs/PoseWithCovariance.h index ba6cf10a..13e11207 100644 --- a/roswrap/src/rossimu/kinetic/include/geometry_msgs/PoseWithCovariance.h +++ b/roswrap/src/rossimu/kinetic/include/geometry_msgs/PoseWithCovariance.h @@ -7,6 +7,7 @@ #define GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCE_H +#include #include #include #include diff --git a/test/docker/cleanup_docker.bash b/test/docker/cleanup_docker.bash new file mode 100644 index 00000000..db8157fe --- /dev/null +++ b/test/docker/cleanup_docker.bash @@ -0,0 +1,19 @@ +#!/bin/bash + + +# +# Remove all docker images and container +# +pushd ../../../.. +docker ps -a -q # list all docker container +docker_running=`(docker ps -a -q | wc -l)` +if [ $docker_running -gt 0 ] ; then + docker stop $(docker ps -a -q) + docker rm $(docker ps -a -q) +fi +docker system prune -a -f +docker volume prune -f +docker images -a # list all docker images +# docker rmi -f $(docker images -a) # remove all docker images + + diff --git a/test/docker/data/picoscan_compact_test01_cfg.json b/test/docker/data/picoscan_compact_test01_cfg.json index ff8845e8..15f0e34b 100644 --- a/test/docker/data/picoscan_compact_test01_cfg.json +++ b/test/docker/data/picoscan_compact_test01_cfg.json @@ -35,9 +35,9 @@ "info.sick_scan_xd_imu_topics": "# Optional list of imu topics to subscribe. Messages received on these topics are verified against the reference given by reference_messages_jsonfile.", "sick_scan_xd_imu_topics": [], - "info.args_sick_scan_xd_launch": "# Launch parameter for sick_scan_xd", + "info.args_sick_scan_xd_launch": "# Launch parameter for sick_scan_xd, optional host_FREchoFilter:=1 for all echos (reference scan contains 3 echos)", "args_sick_scan_xd_launch": [ - "sick_scan_xd sick_picoscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1" + "sick_scan_xd sick_picoscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 host_FREchoFilter:=1" ], "info.args_udp_scandata_sender": "# Launch parameter for udp scandata sender (multiScan or picoScan)", diff --git a/test/emulator/config/rviz_cfg_picoscan_laserscan_360.rviz b/test/emulator/config/rviz_cfg_picoscan_laserscan_360.rviz index a4961c8f..f27c8004 100644 --- a/test/emulator/config/rviz_cfg_picoscan_laserscan_360.rviz +++ b/test/emulator/config/rviz_cfg_picoscan_laserscan_360.rviz @@ -120,7 +120,7 @@ Visualization Manager: Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: world_1_0 + Fixed Frame: world_1 Frame Rate: 30 Name: root Tools: @@ -181,4 +181,4 @@ Window Geometry: collapsed: true Width: 1343 X: 1183 - Y: 743 + Y: 706 diff --git a/test/emulator/config/rviz_cfg_picoscan_single_echo_laserscan_360.rviz b/test/emulator/config/rviz_cfg_picoscan_single_echo_laserscan_360.rviz new file mode 100644 index 00000000..f27c8004 --- /dev/null +++ b/test/emulator/config/rviz_cfg_picoscan_single_echo_laserscan_360.rviz @@ -0,0 +1,184 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /LaserScan1 + Splitter Ratio: 0.5 + Tree Height: 177 + - 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 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: LaserScan +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: i + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_unstructured_fullframe + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: + Show Trail: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /sick_picoscan/scan_fullframe + 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: world_1 + 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: 8.525259971618652 + 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.138563632965088 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 474 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001560000013cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000013c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002e3000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000053f0000003efc0100000002fb0000000800540069006d006501000000000000053f000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e30000013c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1343 + X: 1183 + Y: 706 diff --git a/test/emulator/config/rviz_emulator_cfg_mrs1104.rviz b/test/emulator/config/rviz_emulator_cfg_mrs1104.rviz index 54b8798b..fafff2e0 100644 --- a/test/emulator/config/rviz_emulator_cfg_mrs1104.rviz +++ b/test/emulator/config/rviz_emulator_cfg_mrs1104.rviz @@ -27,7 +27,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: diff --git a/test/emulator/config/rviz_emulator_cfg_ros2_mrs1104.rviz b/test/emulator/config/rviz_emulator_cfg_ros2_mrs1104.rviz index 06e17492..65776e09 100644 --- a/test/emulator/config/rviz_emulator_cfg_ros2_mrs1104.rviz +++ b/test/emulator/config/rviz_emulator_cfg_ros2_mrs1104.rviz @@ -7,6 +7,7 @@ Panels: - /Global Options1 - /PointCloud21 - /PointCloud21/Topic1 + - /Axes1 Splitter Ratio: 0.5 Tree Height: 721 - Class: rviz_common/Selection @@ -58,9 +59,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0.17797201871871948 + Max Intensity: 0.1678037792444229 Min Color: 0; 0; 0 - Min Intensity: -0.07127407938241959 + Min Intensity: -0.07663926482200623 Name: PointCloud2 Position Transformer: XYZ Selectable: true @@ -76,6 +77,13 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py b/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py index 0c8b8391..20be4181 100644 --- a/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py +++ b/test/python/sick_scan_xd_api/sick_scan_xd_api_test.py @@ -50,7 +50,18 @@ def __init__(self): self.plot_points_y = [] self.plot_points_z = [] self.polling = False - + self.polling_functions = {} + self.polling_functions["SickScanApiWaitNextCartesianPointCloudMsg"] = True # default if polling is True: poll cartesian pointcloud message by SickScanApiWaitNextCartesianPointCloudMsg + self.polling_functions["SickScanApiWaitNextPolarPointCloudMsg"] = False + self.polling_functions["SickScanApiWaitNextImuMsg"] = False + self.polling_functions["SickScanApiWaitNextLFErecMsg"] = False + self.polling_functions["SickScanApiWaitNextLIDoutputstateMsg"] = False + self.polling_functions["SickScanApiWaitNextRadarScanMsg"] = False + self.polling_functions["SickScanApiWaitNextLdmrsObjectArrayMsg"] = False + self.polling_functions["SickScanApiWaitNextVisualizationMarkerMsg"] = False + self.polling_functions["SickScanApiWaitNextNavPoseLandmarkMsg"] = False + self.verbose_level = 2 # Set verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET (equivalent to ros::console::levels) + # Convert a SickScanCartesianPointCloudMsg to 3D arrays def pySickScanCartesianPointCloudMsgToXYZ(pointcloud_msg): # get pointcloud fields @@ -95,8 +106,9 @@ def pySickScanCartesianPointCloudMsgToXYZ(pointcloud_msg): # Callback for cartesian pointcloud messages def pySickScanCartesianPointCloudMsgCallback(api_handle, pointcloud_msg): pointcloud_msg = pointcloud_msg.contents # dereference msg pointer (pointcloud_msg = pointcloud_msg[0]) - print("pySickScanCartesianPointCloudMsgCallback (ROS-{}): api_handle={}, {}x{} pointcloud, {} echo(s), segment {}".format( - __ROS_VERSION, api_handle, pointcloud_msg.width, pointcloud_msg.height, pointcloud_msg.num_echos , pointcloud_msg.segment_idx)) + print("pySickScanCartesianPointCloudMsgCallback (ROS-{}): api_handle={}, {}x{} pointcloud, {} echo(s), segment {}, {} fields, frame_id \"{}\", topic {}, timestamp {}.{:06d}".format( + __ROS_VERSION, api_handle, pointcloud_msg.width, pointcloud_msg.height, pointcloud_msg.num_echos , pointcloud_msg.segment_idx, pointcloud_msg.fields.size, + pointcloud_msg.header.frame_id, pointcloud_msg.topic, pointcloud_msg.header.timestamp_sec, pointcloud_msg.header.timestamp_nsec)) global api_test_settings # Note: Pointcloud conversion and visualization consumes cpu time, therefore we convert and publish the cartesian pointcloud with low frequency. cur_timestamp = datetime.datetime.now() @@ -112,8 +124,9 @@ def pySickScanCartesianPointCloudMsgCallback(api_handle, pointcloud_msg): # Callback for polar pointcloud messages def pySickScanPolarPointCloudMsgCallback(api_handle, pointcloud_msg): pointcloud_msg = pointcloud_msg.contents # dereference msg pointer (pointcloud_msg = pointcloud_msg[0]) - print("pySickScanPolarPointCloudMsgCallback (ROS-{}): api_handle={}, {}x{} pointcloud, {} echo(s), segment {}".format( - __ROS_VERSION, api_handle, pointcloud_msg.width, pointcloud_msg.height, pointcloud_msg.num_echos , pointcloud_msg.segment_idx)) + print("pySickScanPolarPointCloudMsgCallback (ROS-{}): api_handle={}, {}x{} pointcloud, {} echo(s), segment {}, {} fields, frame_id \"{}\", topic {}, timestamp {}.{:06d}".format( + __ROS_VERSION, api_handle, pointcloud_msg.width, pointcloud_msg.height, pointcloud_msg.num_echos , pointcloud_msg.segment_idx, pointcloud_msg.fields.size, + pointcloud_msg.header.frame_id, pointcloud_msg.topic, pointcloud_msg.header.timestamp_sec, pointcloud_msg.header.timestamp_nsec)) if __ROS_VERSION == 1: # Convert polar pointcloud_msg to cartesian ros pointcloud and publish. # Note: Pointcloud conversion from polar to cartesian is too cpu-intensive to process all segments from a Multiscan136. @@ -242,72 +255,76 @@ def pyrunSickScanApiTestWaitNext(sick_scan_library, api_handle): navodom_msg.coordbase = 0 global api_test_settings while api_test_settings.polling: - # Get/poll the next cartesian PointCloud message - ret = SickScanApiWaitNextCartesianPointCloudMsg(sick_scan_library, api_handle, ctypes.pointer(pointcloud_msg), wait_next_message_timeout) - if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): - pySickScanCartesianPointCloudMsgCallback(api_handle, ctypes.pointer(pointcloud_msg)) - elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): - print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextCartesianPointCloudMsg failed, error code {} ({})".format(ret, int(ret))) - SickScanApiFreePointCloudMsg(sick_scan_library, api_handle, ctypes.pointer(pointcloud_msg)) - # Get/poll the next polar PointCloud message - ret = SickScanApiWaitNextPolarPointCloudMsg(sick_scan_library, api_handle, ctypes.pointer(pointcloud_msg), wait_next_message_timeout) - if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): - pySickScanPolarPointCloudMsgCallback(api_handle, ctypes.pointer(pointcloud_msg)) - elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): - print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextPolarPointCloudMsg failed, error code {} ({})".format(ret, int(ret))) - SickScanApiFreePointCloudMsg(sick_scan_library, api_handle, ctypes.pointer(pointcloud_msg)) - # Get/poll the next Imu message - ret = SickScanApiWaitNextImuMsg(sick_scan_library, api_handle, ctypes.pointer(imu_msg), wait_next_message_timeout) - if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): - pySickScanImuMsgCallback(api_handle, ctypes.pointer(imu_msg)) - elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): - print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextImuMsg failed, error code {} ({})".format(ret, int(ret))) - SickScanApiFreeImuMsg(sick_scan_library, api_handle, ctypes.pointer(imu_msg)) - # Get/poll the next LFErec message - ret = SickScanApiWaitNextLFErecMsg(sick_scan_library, api_handle, ctypes.pointer(lferec_msg), wait_next_message_timeout) - if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): - pySickScanLFErecMsgCallback(api_handle, ctypes.pointer(lferec_msg)) - elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): - print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextLFErecMsg failed, error code {} ({})".format(ret, int(ret))) - SickScanApiFreeLFErecMsg(sick_scan_library, api_handle, ctypes.pointer(lferec_msg)) - # Get/poll the next LIDoutputstate message - ret = SickScanApiWaitNextLIDoutputstateMsg(sick_scan_library, api_handle, ctypes.pointer(lidoutputstate_msg), wait_next_message_timeout) - if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): - pySickScanLIDoutputstateMsgCallback(api_handle, ctypes.pointer(lidoutputstate_msg)) - elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): - print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextLIDoutputstateMsg failed, error code {} ({})".format(ret, int(ret))) - SickScanApiFreeLIDoutputstateMsg(sick_scan_library, api_handle, ctypes.pointer(lidoutputstate_msg)) - # Get/poll the next RadarScan message - ret = SickScanApiWaitNextRadarScanMsg(sick_scan_library, api_handle, ctypes.pointer(radarscan_msg), wait_next_message_timeout) - if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): - pySickScanRadarScanCallback(api_handle, ctypes.pointer(radarscan_msg)) - elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): - print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextRadarScanMsg failed, error code {} ({})".format(ret, int(ret))) - SickScanApiFreeRadarScanMsg(sick_scan_library, api_handle, ctypes.pointer(radarscan_msg)) - # Get/poll the next LdmrsObjectArray message - ret = SickScanApiWaitNextLdmrsObjectArrayMsg(sick_scan_library, api_handle, ctypes.pointer(ldmrsobjectarray_msg), wait_next_message_timeout) - if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): - pySickScanLdmrsObjectArrayCallback(api_handle, ctypes.pointer(ldmrsobjectarray_msg)) - elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): - print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextLdmrsObjectArrayMsg failed, error code {} ({})".format(ret, int(ret))) - SickScanApiFreeLdmrsObjectArrayMsg(sick_scan_library, api_handle, ctypes.pointer(ldmrsobjectarray_msg)) - # Get/poll the next VisualizationMarker message - ret = SickScanApiWaitNextVisualizationMarkerMsg(sick_scan_library, api_handle, ctypes.pointer(visualizationmarker_msg), wait_next_message_timeout) - if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): - pySickScanVisualizationMarkerCallback(api_handle, ctypes.pointer(visualizationmarker_msg)) - elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): - print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextVisualizationMarkerMsg failed, error code {} ({})".format(ret, int(ret))) - SickScanApiFreeVisualizationMarkerMsg(sick_scan_library, api_handle, ctypes.pointer(visualizationmarker_msg)) - # Get/poll the next SickScanNavPoseLandmarkMsg message - ret = SickScanApiWaitNextNavPoseLandmarkMsg(sick_scan_library, api_handle, ctypes.pointer(navposelandmark_msg), wait_next_message_timeout) - if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): - pySickScanNavPoseLandmarkCallback(api_handle, ctypes.pointer(navposelandmark_msg)) - elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): - print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextNavPoseLandmarkMsg failed, error code {} ({})".format(ret, int(ret))) - SickScanApiFreeNavPoseLandmarkMsg(sick_scan_library, api_handle, ctypes.pointer(navposelandmark_msg)) - # Send NAV350 odom message example - # ret = SickScanApiNavOdomVelocityMsg(sick_scan_library, api_handle, ctypes.pointer(navodom_msg)) - # ret = SickScanApiOdomVelocityMsg(sick_scan_library, api_handle, ctypes.pointer(odom_msg)) + if api_test_settings.polling_functions["SickScanApiWaitNextCartesianPointCloudMsg"]: # Get/poll the next cartesian PointCloud message + ret = SickScanApiWaitNextCartesianPointCloudMsg(sick_scan_library, api_handle, ctypes.pointer(pointcloud_msg), wait_next_message_timeout) + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): + pySickScanCartesianPointCloudMsgCallback(api_handle, ctypes.pointer(pointcloud_msg)) + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): + print("pyrunSickScanApiTestWaitNext: success") + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): + print("## ERROR pyrunSickScanApiTestWaitNext: SICK_SCAN_API_TIMEOUT, SickScanApiWaitNextCartesianPointCloudMsg failed, error code {} ({})".format(ret, int(ret))) + elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): + print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextCartesianPointCloudMsg failed, error code {} ({})".format(ret, int(ret))) + SickScanApiFreePointCloudMsg(sick_scan_library, api_handle, ctypes.pointer(pointcloud_msg)) + if api_test_settings.polling_functions["SickScanApiWaitNextPolarPointCloudMsg"]: # Get/poll the next polar PointCloud message + ret = SickScanApiWaitNextPolarPointCloudMsg(sick_scan_library, api_handle, ctypes.pointer(pointcloud_msg), wait_next_message_timeout) + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): + pySickScanPolarPointCloudMsgCallback(api_handle, ctypes.pointer(pointcloud_msg)) + elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): + print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextPolarPointCloudMsg failed, error code {} ({})".format(ret, int(ret))) + SickScanApiFreePointCloudMsg(sick_scan_library, api_handle, ctypes.pointer(pointcloud_msg)) + if api_test_settings.polling_functions["SickScanApiWaitNextImuMsg"]: # Get/poll the next Imu message + ret = SickScanApiWaitNextImuMsg(sick_scan_library, api_handle, ctypes.pointer(imu_msg), wait_next_message_timeout) + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): + pySickScanImuMsgCallback(api_handle, ctypes.pointer(imu_msg)) + elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): + print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextImuMsg failed, error code {} ({})".format(ret, int(ret))) + SickScanApiFreeImuMsg(sick_scan_library, api_handle, ctypes.pointer(imu_msg)) + if api_test_settings.polling_functions["SickScanApiWaitNextLFErecMsg"]: # Get/poll the next LFErec message + ret = SickScanApiWaitNextLFErecMsg(sick_scan_library, api_handle, ctypes.pointer(lferec_msg), wait_next_message_timeout) + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): + pySickScanLFErecMsgCallback(api_handle, ctypes.pointer(lferec_msg)) + elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): + print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextLFErecMsg failed, error code {} ({})".format(ret, int(ret))) + SickScanApiFreeLFErecMsg(sick_scan_library, api_handle, ctypes.pointer(lferec_msg)) + if api_test_settings.polling_functions["SickScanApiWaitNextLIDoutputstateMsg"]: # Get/poll the next LIDoutputstate message + ret = SickScanApiWaitNextLIDoutputstateMsg(sick_scan_library, api_handle, ctypes.pointer(lidoutputstate_msg), wait_next_message_timeout) + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): + pySickScanLIDoutputstateMsgCallback(api_handle, ctypes.pointer(lidoutputstate_msg)) + elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): + print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextLIDoutputstateMsg failed, error code {} ({})".format(ret, int(ret))) + SickScanApiFreeLIDoutputstateMsg(sick_scan_library, api_handle, ctypes.pointer(lidoutputstate_msg)) + if api_test_settings.polling_functions["SickScanApiWaitNextRadarScanMsg"]: # Get/poll the next RadarScan message + ret = SickScanApiWaitNextRadarScanMsg(sick_scan_library, api_handle, ctypes.pointer(radarscan_msg), wait_next_message_timeout) + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): + pySickScanRadarScanCallback(api_handle, ctypes.pointer(radarscan_msg)) + elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): + print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextRadarScanMsg failed, error code {} ({})".format(ret, int(ret))) + SickScanApiFreeRadarScanMsg(sick_scan_library, api_handle, ctypes.pointer(radarscan_msg)) + if api_test_settings.polling_functions["SickScanApiWaitNextLdmrsObjectArrayMsg"]: # Get/poll the next LdmrsObjectArray message + ret = SickScanApiWaitNextLdmrsObjectArrayMsg(sick_scan_library, api_handle, ctypes.pointer(ldmrsobjectarray_msg), wait_next_message_timeout) + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): + pySickScanLdmrsObjectArrayCallback(api_handle, ctypes.pointer(ldmrsobjectarray_msg)) + elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): + print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextLdmrsObjectArrayMsg failed, error code {} ({})".format(ret, int(ret))) + SickScanApiFreeLdmrsObjectArrayMsg(sick_scan_library, api_handle, ctypes.pointer(ldmrsobjectarray_msg)) + if api_test_settings.polling_functions["SickScanApiWaitNextVisualizationMarkerMsg"]: # Get/poll the next VisualizationMarker message + ret = SickScanApiWaitNextVisualizationMarkerMsg(sick_scan_library, api_handle, ctypes.pointer(visualizationmarker_msg), wait_next_message_timeout) + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): + pySickScanVisualizationMarkerCallback(api_handle, ctypes.pointer(visualizationmarker_msg)) + elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): + print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextVisualizationMarkerMsg failed, error code {} ({})".format(ret, int(ret))) + SickScanApiFreeVisualizationMarkerMsg(sick_scan_library, api_handle, ctypes.pointer(visualizationmarker_msg)) + if api_test_settings.polling_functions["SickScanApiWaitNextNavPoseLandmarkMsg"]: # Get/poll the next SickScanNavPoseLandmarkMsg message + ret = SickScanApiWaitNextNavPoseLandmarkMsg(sick_scan_library, api_handle, ctypes.pointer(navposelandmark_msg), wait_next_message_timeout) + if ret == int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS): + pySickScanNavPoseLandmarkCallback(api_handle, ctypes.pointer(navposelandmark_msg)) + elif ret != int(SickScanApiErrorCodes.SICK_SCAN_API_SUCCESS) and ret != int(SickScanApiErrorCodes.SICK_SCAN_API_TIMEOUT): + print("## ERROR pyrunSickScanApiTestWaitNext: SickScanApiWaitNextNavPoseLandmarkMsg failed, error code {} ({})".format(ret, int(ret))) + SickScanApiFreeNavPoseLandmarkMsg(sick_scan_library, api_handle, ctypes.pointer(navposelandmark_msg)) + # Send NAV350 odom message example + # ret = SickScanApiNavOdomVelocityMsg(sick_scan_library, api_handle, ctypes.pointer(navodom_msg)) + # ret = SickScanApiOdomVelocityMsg(sick_scan_library, api_handle, ctypes.pointer(odom_msg)) # # Python usage example for sick_scan_api @@ -320,9 +337,12 @@ def pyrunSickScanApiTestWaitNext(sick_scan_library, api_handle): cli_arg_start_idx = 1 for n, cli_arg in enumerate(sys.argv): if cli_arg.startswith("_polling:="): - cli_arg_start_idx = n + 1 + cli_arg_start_idx = max(n + 1, cli_arg_start_idx) if int(cli_arg[10:]) > 0: api_test_settings.polling = True + if cli_arg.startswith("_verbose:="): + cli_arg_start_idx = max(n + 1, cli_arg_start_idx) + api_test_settings.verbose_level = int(cli_arg[10:]) cli_args = " ".join(sys.argv[cli_arg_start_idx:]) if __ROS_VERSION == 0 and plt_enabled: api_test_settings.plot_figure = plt.figure() @@ -399,6 +419,14 @@ def pyrunSickScanApiTestWaitNext(sick_scan_library, api_handle): log_msg_callback = SickScanLogMsgCallback(pySickScanLogMsgCallback) SickScanApiRegisterLogMsg(sick_scan_library, api_handle, log_msg_callback) + # Set verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET (equivalent to ros::console::levels) + SickScanApiSetVerboseLevel(sick_scan_library, api_handle, api_test_settings.verbose_level) + verbose_level = SickScanApiGetVerboseLevel(sick_scan_library, api_handle) + if api_test_settings.verbose_level != verbose_level: + print(f"## ERROR sick_scan_xd_api_test: SickScanApiSetVerboseLevel(verbose_level={api_test_settings.verbose_level}) failed, running with verbose_level={verbose_level}") + else: + print(f"sick_scan_xd_api_test running with verbose_level={verbose_level}") + # Run main loop if __ROS_VERSION == 0: while True: diff --git a/test/scripts/make_ros2.cmd b/test/scripts/make_ros2.cmd index 016f97ea..172f1929 100644 --- a/test/scripts/make_ros2.cmd +++ b/test/scripts/make_ros2.cmd @@ -35,7 +35,7 @@ REM REM Build msgpack11 and sick_scan_xd on Windows with colcon for ROS2 REM -# colcon build --packages-select msgpack11 --cmake-args " -DMSGPACK11_BUILD_TESTS=0" --event-handlers console_direct+ +rem colcon build --packages-select msgpack11 --cmake-args " -DMSGPACK11_BUILD_TESTS=0" --event-handlers console_direct+ colcon build --packages-select sick_scan_xd --cmake-args " -DROS_VERSION=2" " -DCMAKE_ENABLE_EMULATOR=1" --event-handlers "console_direct+" call .\install\setup.bat start "sick_scan_xd.sln" .\build\sick_scan_xd\sick_scan_xd.sln diff --git a/test/scripts/run_linux_api_test_py_multiscan.bash b/test/scripts/run_linux_api_test_py_multiscan.bash new file mode 100644 index 00000000..65c1ea2f --- /dev/null +++ b/test/scripts/run_linux_api_test_py_multiscan.bash @@ -0,0 +1,42 @@ +#!/bin/bash + +# killall and cleanup after exit +function killall_cleanup() +{ + killall sick_scan_xd_api_test + pkill -f sick_scan_xd_api_test.py + pkill -f multiscan_sopas_test_server.py + pkill -f multiscan_pcap_player.py +} + +# Play pcapng-files to emulate MRS100 output +function multiscan_pcap_player() +{ + sleep 3 + python3 ./test/python/multiscan_perftest_player.py --udp_port=2115 --repeat=50 --send_rate=100 --verbose=0 --prompt=0 + killall_cleanup +} + +# +# Run sick_scan_xd_api_test.py with sick_multiscan.launch +# + +printf "\033c" +pushd ../.. +export LD_LIBRARY_PATH=.:./build_linux:$LD_LIBRARY_PATH +export PYTHONPATH=.:./python/api:$PYTHONPATH + +# Start multiscan emulator (sopas test server) +python3 ./test/python/multiscan_sopas_test_server.py --tcp_port=2111 --cola_binary=0 & +sleep 1 + +# Start sick_scan_xd api example +python3 ./test/python/sick_scan_xd_api/sick_scan_xd_api_test.py ./launch/sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 scandataformat:=1 & +sleep 3 + +# Play pcapng-files to emulate MRS100 output +multiscan_pcap_player + +popd + +killall_cleanup diff --git a/test/scripts/run_linux_ros1_simu_mrs1xxx.bash b/test/scripts/run_linux_ros1_simu_mrs1xxx.bash new file mode 100644 index 00000000..0b6cd5bb --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_mrs1xxx.bash @@ -0,0 +1,74 @@ +#!/bin/bash + +function simu_killall() +{ + echo -e "Finishing mrs1xxx emulation, shutdown ros nodes\n" + rosnode kill -a ; sleep 1 + killall sick_generic_caller ; sleep 1 + killall sick_scan_emulator ; sleep 1 +} + +# Wait for max 30 seconds, or until 'q' or 'Q' pressed, or until rviz is closed +function waitUntilRvizClosed() +{ + duration_sec=$1 + sleep 1 + for ((cnt=1;cnt<=$duration_sec;cnt++)) ; do + echo -e "sick_scan_xd running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key + if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi + rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)` + if [ $rviz_running -lt 1 ] ; then break ; fi + done +} + +function run_mrs1xxx_simu() +{ + # Start sick_scan_xd emulator + pcapng_json_file=$1 + duration_sec=$2 + roslaunch sick_scan_xd emulator_mrs1xxx_imu.launch scandatafiles:=$pcapng_json_file & + sleep 1 + + # Start rviz + rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_mrs1104.rviz --opengl 210 & + sleep 1 + + # Start sick_scan_xd driver for mrs1104 + echo -e "Launching sick_scan_xd sick_mrs_1xxx.launch\n" + roslaunch sick_scan_xd sick_mrs_1xxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & + sleep 1 + + # Wait for 'q' or 'Q' to exit or until rviz is closed + rostopic echo -p /sick_mrs_1xxx/imu & + waitUntilRvizClosed $duration_sec + + # Shutdown + simu_killall +} + +simu_killall +printf "\033c" +pushd ../../../.. +if [ -f /opt/ros/melodic/setup.bash ] ; then source /opt/ros/melodic/setup.bash ; fi +if [ -f /opt/ros/noetic/setup.bash ] ; then source /opt/ros/noetic/setup.bash ; fi +if [ -f ./devel_isolated/setup.bash ] ; then source ./devel_isolated/setup.bash ; fi + +echo -e "run_simu_mrs1xxx.bash: starting mrs1xxx emulation\n" + +# Start roscore if not yet running +roscore_running=`(ps -elf | grep roscore | grep -v grep | wc -l)` +if [ $roscore_running -lt 1 ] ; then + roscore & + sleep 3 +fi + +# Run sick_scan_xd with MRS1xxx emulator +pcapng_folder=`(pwd)`/src/sick_scan_xd/test/emulator/scandata +run_mrs1xxx_simu $pcapng_folder/20240304-MRS1xxx-default-settings-rssiflag-3.pcapng.json 10 +run_mrs1xxx_simu $pcapng_folder/20240307-MRS1xxx-default-settings-rssiflag3-angres0.2500-scanfreq50.0.pcapng.json 10 +run_mrs1xxx_simu $pcapng_folder/20240307-MRS1xxx-default-settings-rssiflag3-angres0.1250-scanfreq25.0.pcapng.json 10 +run_mrs1xxx_simu $pcapng_folder/20240307-MRS1xxx-default-settings-rssiflag3-angres0.0625-scanfreq12.5.pcapng.json 10 +run_mrs1xxx_simu $pcapng_folder/20240304-MRS1xxx-default-settings-rssiflag-1.pcapng.json 10 + +popd + diff --git a/test/scripts/run_linux_ros1_simu_picoscan_compact.bash b/test/scripts/run_linux_ros1_simu_picoscan_compact.bash index 8d30df9c..b2b2094f 100644 --- a/test/scripts/run_linux_ros1_simu_picoscan_compact.bash +++ b/test/scripts/run_linux_ros1_simu_picoscan_compact.bash @@ -34,7 +34,7 @@ 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.launch hostname:="127.0.0.1" udp_receiver_ip:="127.0.0.1" scandataformat:=2 & +roslaunch sick_scan_xd sick_picoscan.launch hostname:="127.0.0.1" udp_receiver_ip:="127.0.0.1" scandataformat:=2 host_FREchoFilter:=0 & sleep 3 # read -p "Press ENTER to continue..." # Play picoscan pcapng-file with picoscan compact-data diff --git a/test/scripts/run_linux_ros1_simu_picoscan_single_echo.bash b/test/scripts/run_linux_ros1_simu_picoscan_single_echo.bash new file mode 100644 index 00000000..7147df7b --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_picoscan_single_echo.bash @@ -0,0 +1,48 @@ +#!/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_single_echo_laserscan_360.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_single_echo.launch hostname:="127.0.0.1" udp_receiver_ip:="127.0.0.1" & +sleep 3 # read -p "Press ENTER to continue..." +(rostopic echo /sick_picoscan/scan_fullframe | grep frame_id) & + +# 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_timtwo.bash finished, killing all processes ..." +killall_cleanup +popd diff --git a/test/scripts/run_linux_ros2_simu_mrs1xxx.bash b/test/scripts/run_linux_ros2_simu_mrs1xxx.bash new file mode 100644 index 00000000..da52ae1b --- /dev/null +++ b/test/scripts/run_linux_ros2_simu_mrs1xxx.bash @@ -0,0 +1,73 @@ +#!/bin/bash + +# +# Set environment +# + +function simu_killall() +{ + sleep 1 ; killall -SIGINT rviz2 + sleep 1 ; killall -SIGINT sick_generic_caller + sleep 1 ; killall -SIGINT sick_scan_emulator + sleep 1 ; killall -9 rviz2 + sleep 1 ; killall -9 sick_generic_caller + sleep 1 ; killall -9 sick_scan_emulator + sleep 1 +} + +# Wait for max 30 seconds, or until 'q' or 'Q' pressed, or until rviz is closed +function waitUntilRvizClosed() +{ + duration_sec=$1 + sleep 1 + for ((cnt=1;cnt<=$duration_sec;cnt++)) ; do + echo -e "sick_scan_xd running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key + if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi + rviz_running=`(ps -elf | grep rviz2 | grep -v grep | wc -l)` + if [ $rviz_running -lt 1 ] ; then break ; fi + done +} + +function run_mrs1xxx_simu() +{ + # Start sick_scan_xd emulator + pcapng_json_file=$1 + duration_sec=$2 + cp -f $pcapng_json_file /tmp/lmd_scandata.pcapng.json + ros2 run sick_scan_xd sick_scan_emulator ./src/sick_scan_xd/test/emulator/launch/emulator_mrs1xxx_imu.launch & + sleep 1 + + # Start sick_scan_xd driver for mrs1104 + echo -e "Launching sick_scan_xd sick_mrs_1xxx.launch\n" + ros2 launch sick_scan_xd sick_mrs_1xxx.launch.py hostname:=127.0.0.1 port:=2111 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_mrs1104.rviz & + sleep 1 + + # Wait for 'q' or 'Q' to exit or until rviz is closed + ros2 topic echo --csv /sick_mrs_1xxx/imu & + waitUntilRvizClosed $duration_sec + + # Shutdown + simu_killall +} + +simu_killall +printf "\033c" +pushd ../../../.. +if [ -f /opt/ros/eloquent/setup.bash ] ; then source /opt/ros/eloquent/setup.bash ; fi +if [ -f /opt/ros/foxy/setup.bash ] ; then source /opt/ros/foxy/setup.bash ; fi +source ./install/setup.bash + +# Run sick_scan_xd with MRS1xxx emulator +pcapng_folder=`(pwd)`/src/sick_scan_xd/test/emulator/scandata +run_mrs1xxx_simu $pcapng_folder/20240304-MRS1xxx-default-settings-rssiflag-3.pcapng.json 10 +run_mrs1xxx_simu $pcapng_folder/20240307-MRS1xxx-default-settings-rssiflag3-angres0.2500-scanfreq50.0.pcapng.json 10 +run_mrs1xxx_simu $pcapng_folder/20240307-MRS1xxx-default-settings-rssiflag3-angres0.1250-scanfreq25.0.pcapng.json 10 +run_mrs1xxx_simu $pcapng_folder/20240307-MRS1xxx-default-settings-rssiflag3-angres0.0625-scanfreq12.5.pcapng.json 10 +run_mrs1xxx_simu $pcapng_folder/20240304-MRS1xxx-default-settings-rssiflag-1.pcapng.json 10 + +popd + diff --git a/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c b/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c index 7ddb3b97..ccff86ad 100644 --- a/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c +++ b/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c @@ -119,6 +119,12 @@ static SickScanApiDeregisterLogMsg_PROCTYPE ptSickScanApiDeregisterLogMsg = 0; typedef int32_t (*SickScanApiGetStatus_PROCTYPE)(SickScanApiHandle apiHandle, int32_t* status_code, char* message_buffer, int32_t message_buffer_size); static SickScanApiGetStatus_PROCTYPE ptSickScanApiGetStatus = 0; +typedef int32_t (*SickScanApiSetVerboseLevel_PROCTYPE)(SickScanApiHandle apiHandle, int32_t verbose_level); +static SickScanApiSetVerboseLevel_PROCTYPE ptSickScanApiSetVerboseLevel = 0; + +typedef int32_t (*SickScanApiGetVerboseLevel_PROCTYPE)(SickScanApiHandle apiHandle); +static SickScanApiGetVerboseLevel_PROCTYPE ptSickScanApiGetVerboseLevel = 0; + typedef int32_t(*SickScanApiWaitNextCartesianPointCloudMsg_PROCTYPE)(SickScanApiHandle apiHandle, SickScanPointCloudMsg* msg, double timeout_sec); static SickScanApiWaitNextCartesianPointCloudMsg_PROCTYPE ptSickScanApiWaitNextCartesianPointCloudMsg = 0; @@ -260,6 +266,8 @@ int32_t SickScanApiUnloadLibrary() ptSickScanApiRegisterLogMsg = 0; ptSickScanApiDeregisterLogMsg = 0; ptSickScanApiGetStatus = 0; + ptSickScanApiSetVerboseLevel = 0; + ptSickScanApiGetVerboseLevel = 0; ptSickScanApiWaitNextCartesianPointCloudMsg = 0; ptSickScanApiWaitNextPolarPointCloudMsg = 0; ptSickScanApiFreePointCloudMsg = 0; @@ -551,6 +559,26 @@ int32_t SickScanApiGetStatus(SickScanApiHandle apiHandle, int32_t* status_code, return ret; } +// Set verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET (equivalent to ros::console::levels), +// i.e. print messages on console above the given verbose level. +// Default verbose level is 1 (INFO), i.e. print informational, warnings and error messages. +int32_t SickScanApiSetVerboseLevel(SickScanApiHandle apiHandle, int32_t verbose_level) +{ + CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiSetVerboseLevel, "SickScanApiSetVerboseLevel", SickScanApiSetVerboseLevel_PROCTYPE); + int32_t ret = (ptSickScanApiSetVerboseLevel ? (ptSickScanApiSetVerboseLevel(apiHandle, verbose_level)) : SICK_SCAN_API_NOT_INITIALIZED); + if (ret != SICK_SCAN_API_SUCCESS) + printf("## ERROR SickScanApiSetVerboseLevel: library call SickScanApiSetVerboseLevel() failed, error code %d\n", ret); + return ret; +} + +// Returns the current verbose level 0=DEBUG, 1=INFO, 2=WARN, 3=ERROR, 4=FATAL or 5=QUIET. Default verbose level is 1 (INFO) +int32_t SickScanApiGetVerboseLevel(SickScanApiHandle apiHandle) +{ + CACHE_FUNCTION_PTR(apiHandle, ptSickScanApiGetVerboseLevel, "SickScanApiGetVerboseLevel", SickScanApiGetVerboseLevel_PROCTYPE); + int32_t verbose_level = (ptSickScanApiGetVerboseLevel ? (ptSickScanApiGetVerboseLevel(apiHandle)) : SICK_SCAN_API_NOT_INITIALIZED); + return verbose_level; +} + /* * Polling functions */