v5.5.2: Support for message type 7, improved source id and documentation

* Support for message type 7
* Configuration and support for message specific source ID
* Improved transform messages
* Improved documentation
* Renaming and cleanup

Author: rostest

.gitignore

@@ -52,6 +52,8 @@ doc/20210726_rest_api_test.pptx
 doc/*_lidar_localization2.pptx
 
 test/scripts/wireshark.cmd
+test/rest_server/python/scripts_LLS1-OdometryUDPSender.py
+test/rest_server/python/scripts_LLS2-OdometryUDPSender.py
 
 test/data/wireshark/20210727_*.*
 test/data/wireshark/20210921-*.*

CMakeLists.txt

@@ -116,24 +116,24 @@ add_executable(gen_service_call src/gen_service_call.cpp)
 target_link_libraries(gen_service_call sick_localization_lib ${CURL_LIBRARIES} jsoncpp_lib ${catkin_LIBRARIES})
 
 if(ROS_VERSION GREATER 0)
-    add_executable(pointcloud_converter src/pointcloud_converter.cpp src/pointcloud_converter_thread.cpp)
-    target_link_libraries(pointcloud_converter sick_localization_lib ${CURL_LIBRARIES} jsoncpp_lib ${catkin_LIBRARIES})
+    add_executable(lls_transform src/lls_transform.cpp src/lls_transform_thread.cpp)
+    target_link_libraries(lls_transform sick_localization_lib ${CURL_LIBRARIES} jsoncpp_lib ${catkin_LIBRARIES})
 endif()     
 
 if(ROS_VERSION EQUAL 1)
     add_dependencies(sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
     add_dependencies(sick_lidar_localization_main sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-    add_dependencies(pointcloud_converter sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+    add_dependencies(lls_transform sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 elseif(ROS_VERSION EQUAL 2)
     ament_target_dependencies(sick_localization_lib rclcpp geometry_msgs nav_msgs sensor_msgs std_msgs tf2_ros visualization_msgs)
-    ament_target_dependencies(pointcloud_converter rclcpp geometry_msgs nav_msgs sensor_msgs std_msgs tf2_ros visualization_msgs)
+    ament_target_dependencies(lls_transform rclcpp geometry_msgs nav_msgs sensor_msgs std_msgs tf2_ros visualization_msgs)
     if(ROS2_HUMBLE) # rosidl_typesupport for ROS2 humble or later
         rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
         target_link_libraries(sick_localization_lib "${cpp_typesupport_target}")
-        target_link_libraries(pointcloud_converter "${cpp_typesupport_target}")
+        target_link_libraries(lls_transform "${cpp_typesupport_target}")
     else()
         rosidl_target_interfaces(sick_localization_lib ${PROJECT_NAME} "rosidl_typesupport_cpp")
-        rosidl_target_interfaces(pointcloud_converter ${PROJECT_NAME} "rosidl_typesupport_cpp")
+        rosidl_target_interfaces(lls_transform ${PROJECT_NAME} "rosidl_typesupport_cpp")
     endif()
 else()
     add_dependencies(sick_lidar_localization_main sick_localization_lib)
@@ -144,16 +144,16 @@ endif()
 ############# 
 
 if(ROS_VERSION EQUAL 1)
-    install(TARGETS sick_localization_lib sick_lidar_localization_main gen_service_call pointcloud_converter ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+    install(TARGETS sick_localization_lib sick_lidar_localization_main gen_service_call lls_transform ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
     install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h*")
     install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 elseif(ROS_VERSION EQUAL 2)
     ament_export_dependencies(sick_localization_lib rclcpp geometry_msgs nav_msgs sensor_msgs std_msgs tf2_ros)
     ament_export_include_directories(include/${PROJECT_NAME}/)
     ament_export_libraries(sick_localization_lib sick_lidar_localization_main)
     ament_package()
-    # install(TARGETS sick_localization_lib sick_lidar_localization_main gen_service_call pointcloud_converter DESTINATION lib)
-    install(TARGETS sick_localization_lib sick_lidar_localization_main gen_service_call pointcloud_converter DESTINATION lib/${PROJECT_NAME})
+    # install(TARGETS sick_localization_lib sick_lidar_localization_main gen_service_call lls_transform DESTINATION lib)
+    install(TARGETS sick_localization_lib sick_lidar_localization_main gen_service_call lls_transform DESTINATION lib/${PROJECT_NAME})
     install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 else() 
     install(DIRECTORY launch DESTINATION ${CMAKE_BINARY_DIR})

README.md

@@ -15,7 +15,7 @@ The drivers support the following ROS versions in the same source:
 ## Specification
 
 The customer requirements and the REST API specification for the implementation of the project:
--[specification](doc/specifications/README.md)
+[specification](doc/specifications/README.md)
 
 ## Build on native Linux
 
@@ -210,8 +210,8 @@ Common parameters are:
 |--------------------|-------------------|--------------------|-------------|-----------------|
 | hostname | 192.168.0.1 | string | hostname:=192.168.0.1 | IP address of the SIM localization controller |
 | verbose  | 0           | int    | verbose:=1            | Print informational messages (verbose>0, otherwise error messages only) |
-| udp_ip_sim_output | "" | string | udp_ip_sim_output:=192.168.0.100 | IP address of your local machine (i.e. the receiver of UDP stream messages) |
-| udp_ip_sim_input | 192.168.0.1 | string | udp_ip_sim_input:=192.168.0.1 | IP address of host to send input UDP messages to, should be identical to hostname (except for unittests) |
+| udp_ip_lls_output | "" | string | udp_ip_lls_output:=192.168.0.100 | IP address of your local machine (i.e. the receiver of UDP stream messages) |
+| udp_ip_lls_input | 192.168.0.1 | string | udp_ip_lls_input:=192.168.0.1 | IP address of host to send input UDP messages to, should be identical to hostname (except for unittests) |
 
 ## REST API services
 
@@ -234,32 +234,36 @@ UDP stream output messages are:
 * [Localization result messages type 5 version 2](msg/LocalizationControllerResultMessage0502.msg)
 
 UDP stream input messages are:
-* [Odometry messages type 1 version 1](msg/OdometryMessage0101.msg)
 * [Odometry messages type 1 version 4](msg/OdometryMessage0104.msg)
 * [Odometry messages type 1 version 5](msg/OdometryMessage0105.msg)
 * [Encoder measurement messages type 2 version 2](msg/EncoderMeasurementMessage0202.msg)
 * [Code measurement messages type 3 version 3](msg/CodeMeasurementMessage0303.msg)
+* [Code measurement messages type 7 version 1](msg/CodeMeasurementMessage0701.msg)
 * [Line measurement messages type 4 version 3](msg/LineMeasurementMessage0403.msg)
 * [Line measurement messages type 4 version 4](msg/LineMeasurementMessage0404.msg)
 
-See [UDP stream messages](doc/sim_messages.md) for details and examples.
+See [UDP stream messages](doc/lls_messages.md) for details and examples.
 
 ## Timestamps and time synchronization
 
 The localization timestamps in UDP output messages are converted to system time using a Software-PLL. See [Time synchronization](doc/timing.md) and [Software-PLL](doc/software_pll.md) for details.
 
+## System setup and source Ids
+
+See [System setup and source Ids](doc/source_id.md) for further information about source Ids and their configuration.
+
 ## Tools and unittests
 
 ### Visualization of localization results
 
-Localization results (i.e. the sensor position and orientation) can be visualized on ROS using pointcloud_convert. The tool converts Localization result messages and publishes the sensor pose and transform. Run pointcloud_converter and rviz, then add topic `/cloud/PointCloud2` and display type `TF`.
+Localization results (i.e. the sensor position and orientation) can be visualized on ROS using lls_transform. The tool converts localization result messages and publishes the sensor pose and transform. Run lls_transform and rviz, then add display type `TF`.
 
 ROS-1 usage example:
 
 ```
 source ./install/setup.bash 
 roslaunch sick_lidar_localization sick_lidar_localization.launch &
-sleep 3 ; roslaunch sick_lidar_localization pointcloud_converter.launch &
+sleep 3 ; roslaunch sick_lidar_localization lls_transform.launch &
 sleep 3 ; rosrun rviz rviz -d ./src/sick_lidar_localization/test/config/rviz_sick_lidar_localization_pointcloud.rviz
 ```
 
@@ -268,13 +272,29 @@ ROS-2 usage example:
 ```
 source ./install/setup.bash 
 ros2 run sick_lidar_localization sick_lidar_localization ./src/sick_lidar_localization/launch/sick_lidar_localization.launch &
-sleep 3 ; ros2 run sick_lidar_localization pointcloud_converter ./src/sick_lidar_localization/launch/pointcloud_converter.launch &
+sleep 3 ; ros2 run sick_lidar_localization lls_transform ./src/sick_lidar_localization/launch/lls_transform.launch &
 sleep 3 ; rviz2 -d ./src/sick_lidar_localization//test/config/rviz2_sick_lidar_localization_pointcloud.rviz2
 ```
 
-The following screenshot shows an example of the sensor pose with the sensor transform and 4 points in "cloud" coordinates:
+The following screenshot shows an example of the sensor pose:
+
+![rviz_pointcloud_converter_screenshot](doc/screenshots/rviz_pointcloud_converter_screenshot2.png)
+
+Poses are published by ROS-transform-messages (TF) of tType geometry_msgs::TransformStamped. Each TF message has a parent and a child coordinate system identified by tf_parent_frame_id resp. tf_child_frame_id. These frame ids are configured in the launchfile, by default:
+* tf_parent_frame_id: "map" 
+* tf_child_frame_id: "lls"
+
+The default frame ids for transformations should only be used as an example. In a real application, the reference to the robot is usually specified here, as it is, for example, in the ROS Nagivation stack. Here one would then use the reference
+* tf_parent_frame_id: "base_link"
+* tf_child_frame_id: "lls"
 
-![rviz_pointcloud_converter_screenshot](doc/screenshots/rviz_pointcloud_converter_screenshot1.png)
+Frame ids are normally customized according to the application resp. setup
+* tf_parent_frame_id: "map" is used by rviz by default
+* tf_parent_frame_id: "base_link" is often used in examples
+
+Other frame ids can be e.g. "robot", "vehicle" or "lidar". Choose parameter tf_parent_frame_id and tf_child_frame_id to match the target setup!
+
+See https://roboticsknowledgebase.com/wiki/state-estimation/ros-navigation/ for further examples.
 
 ### Unittests
 
@@ -319,6 +339,12 @@ Open sick_lidar_localization.sln in build folder and rebuild (debug version)
 See [Quickstart-Setup-SOPASair.md](doc/Quickstart-Setup-SOPASair.md) for a quickstart. 
 Find detailed information in the operation manuals published on https://supportportal.sick.com/products/localization/lidar-localization/lidar-loc/ .  
 
+### Source Ids
+
+:question: What are source Ids and how should I configure them?
+
+:white_check_mark: See [System setup and source Ids](doc/source_id.md) for further information about source Ids and their configuration.
+
 ### Test and diagnosis
 
 :question: How can I activate informational messages for tests and diagnosis?
@@ -335,7 +361,7 @@ Find detailed information in the operation manuals published on https://supportp
 
 :white_check_mark: sick_rest_server.py requires python3 and flask. Install flask with `pip install flask` (on Linux, use `pip3 install flask`)
 
-:question: `ModuleNotFoundError: No module named pcapng` when running sim_pcapng_player.py
+:question: `ModuleNotFoundError: No module named pcapng` when running lls_pcapng_player.py
 
 :white_check_mark: sick_rest_server.py requires python3 with modules scapy, pypcapfile and python-pcapng. Install with `pip` (resp `pip3` on Linux):
 ```

doc/cpp_api.md

@@ -32,41 +32,56 @@ int main(int argc, char** argv)
     sick_lidar_localization::UDPMessage::OdometryPayload0105 odometry0105;
     sick_lidar_localization::UDPMessage::EncoderMeasurementPayload0202 encoder_measurement0202;
     sick_lidar_localization::UDPMessage::CodeMeasurementPayload0303 code_measurement0303;
+    sick_lidar_localization::UDPMessage::CodeMeasurementPayload0701 code_measurement0701;
     sick_lidar_localization::UDPMessage::LineMeasurementPayload0403 line_measurement0403;
     sick_lidar_localization::UDPMessage::LineMeasurementPayload0404 line_measurement0404;
     odometry0104.telegram_count = 1000001;
     odometry0104.timestamp = 123456789;
+    odometry0104.source_id = 0;
     odometry0104.x_velocity = -1234;
     odometry0104.y_velocity = -1234;
     odometry0104.angular_velocity = 1234;
     odometry0105.telegram_count = 1000002;
     odometry0105.timestamp = 123456780;
+    odometry0105.source_id = 0;
     odometry0105.x_position = -1234;
     odometry0105.y_position = -1234;
     odometry0105.heading = 1234;
     encoder_measurement0202.telegram_count = 1000003;
     encoder_measurement0202.timestamp = 123456781;
+    encoder_measurement0202.source_id = 0;
     encoder_measurement0202.encoder_value = 123456789;
     code_measurement0303.telegram_count = 1000004;
     code_measurement0303.timestamp = 123456782;
+    code_measurement0303.source_id = 0;
     code_measurement0303.code = 1234;
+    code_measurement0701.telegram_count = 1000004;
+    code_measurement0701.timestamp = 123456782;
+    code_measurement0701.source_id = 0;
+    code_measurement0701.code = "1234";
+    code_measurement0701.x_position = -1234;
+    code_measurement0701.y_position = -1234;
+    code_measurement0701.heading = 1234;
     line_measurement0403.telegram_count = 1000005;
     line_measurement0403.timestamp = 123456783;
+    line_measurement0403.source_id = 0;
     line_measurement0403.num_lanes = 1;
     line_measurement0403.lanes.push_back(1234);
     line_measurement0404.telegram_count = 1000006;
     line_measurement0404.timestamp = 123456784;
+    line_measurement0404.source_id = 0;
     line_measurement0404.lcp1 = 12;
     line_measurement0404.lcp2 = 34;
     line_measurement0404.lcp3 = 56;
     line_measurement0404.cnt_lpc = 78;
     line_measurement0404.reserved = 0;
-    if (!lidar_loc_api.sendUDPMessage(odometry0104, false, false, 1)
-        || !lidar_loc_api.sendUDPMessage(odometry0105, false, false, 1)
-        || !lidar_loc_api.sendUDPMessage(encoder_measurement0202, false, false, 1)
-        || !lidar_loc_api.sendUDPMessage(code_measurement0303, false, false, 1)
-        || !lidar_loc_api.sendUDPMessage(line_measurement0403, false, false, 1)
-        || !lidar_loc_api.sendUDPMessage(line_measurement0404, false, false, 1))
+    if (!lidar_loc_api.sendUDPMessage(odometry0104, false, false)
+        || !lidar_loc_api.sendUDPMessage(odometry0105, false, false)
+        || !lidar_loc_api.sendUDPMessage(encoder_measurement0202, false, false)
+        || !lidar_loc_api.sendUDPMessage(code_measurement0303, false, false)
+        || !lidar_loc_api.sendUDPMessage(code_measurement0701, false, false)
+        || !lidar_loc_api.sendUDPMessage(line_measurement0403, false, false)
+        || !lidar_loc_api.sendUDPMessage(line_measurement0404, false, false))
     {
         ROS_ERROR_STREAM("## ERROR sick_lidar_localization: UDPSender::sendUDPPayload() failed");
     }