diff --git a/.gitignore b/.gitignore index f705d6a..e313ad9 100644 --- a/.gitignore +++ b/.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-*.* diff --git a/CMakeLists.txt b/CMakeLists.txt index e4019a8..6cb969f 100644 --- a/CMakeLists.txt +++ b/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,7 +144,7 @@ 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) @@ -152,8 +152,8 @@ elseif(ROS_VERSION EQUAL 2) 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}) diff --git a/README.md b/README.md index 4a5699e..8d351e3 100644 --- a/README.md +++ b/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): ``` diff --git a/doc/cpp_api.md b/doc/cpp_api.md index d5b6c0c..6673ba4 100644 --- a/doc/cpp_api.md +++ b/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"); } diff --git a/doc/sim_messages.md b/doc/lls_messages.md similarity index 77% rename from doc/sim_messages.md rename to doc/lls_messages.md index 75b8499..bd96316 100644 --- a/doc/sim_messages.md +++ b/doc/lls_messages.md @@ -1,8 +1,8 @@ # UDP stream messages -LiDAR-LOC receives and sends messages from resp. to the localization controller using UDP. [UDP output messages](#sim_output_messages) are UDP messages sent from localization server to the local PC. [UDP input messages](#sim_input_messages) are UDP messages sent from the local PC to the localization server. On ROS-1 and ROS-2, these UDP-messages are converted from resp. to ROS messages. On native Linux and Windows systems, these UDP-messages can be processed using the [C++ API](../README.md#cpp_api). +LiDAR-LOC receives and sends messages from resp. to the localization controller using UDP. [UDP output messages](#lls_output_messages) are UDP messages sent from localization server to the local PC. [UDP input messages](#lls_input_messages) are UDP messages sent from the local PC to the localization server. On ROS-1 and ROS-2, these UDP-messages are converted from resp. to ROS messages. On native Linux and Windows systems, these UDP-messages can be processed using the [C++ API](../README.md#cpp_api). -## UDP output messages +## UDP output messages UDP output messages are: @@ -128,19 +128,19 @@ Parameters can be configured in the launch file [sick_lidar_localization.launch] | **parameter name** | **default value** | **parameter type** | **description** | |--------------------|-------------------|--------------------|-----------------| -| udp_ip_sim_output | "" | string | IP address of your local machine (i.e. the receiver of UDP stream messages) | -| udp_port_sim_output | 5010 | int | UDP port of output messages | -| udp_sim_output_logfile | "" | string | Optional logfile for human readable UDP output messages, or "" to disable logfile | +| udp_ip_lls_output | "" | string | IP address of your local machine (i.e. the receiver of UDP stream messages) | +| udp_port_lls_output | 5010 | int | UDP port of output messages | +| udp_lls_output_logfile | "" | string | Optional logfile for human readable UDP output messages, or "" to disable logfile | -## UDP input messages +## UDP input messages UDP 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) * [ROS odometry messages](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) @@ -152,12 +152,13 @@ ROS-1 examples to send UDP input messages (odometry, encoder measurement, code m ``` source ./install/setup.bash roslaunch sick_lidar_localization sick_lidar_localization.launch verbose:=1 & -rostopic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 '{telegram_count: 1000001, timestamp: 123456789, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' -rostopic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 '{telegram_count: 1000002, timestamp: 123456780, x_position: -1234, y_position: -1234, heading: 1234}' -rostopic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 '{telegram_count: 1000003, timestamp: 123456781, encoder_value: 123456789}' -rostopic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 '{telegram_count: 1000004, timestamp: 123456782, code: 1234}' -rostopic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 '{telegram_count: 1000005, timestamp: 123456783, num_lanes: 1, lanes: [1234]}' -rostopic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 '{telegram_count: 1000006, timestamp: 123456784, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}' +rostopic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 '{telegram_count: 1000001, timestamp: 123456789, source_id: 0, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' +rostopic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 '{telegram_count: 1000002, timestamp: 123456780, source_id: 0, x_position: -1234, y_position: -1234, heading: 1234}' +rostopic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 '{telegram_count: 1000003, timestamp: 123456781, source_id: 0, encoder_value: 123456789}' +rostopic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 '{telegram_count: 1000004, timestamp: 123456782, source_id: 0, code: 1234}' +rostopic pub --once /localizationcontroller/in/code_measurement_message_0701 sick_lidar_localization/CodeMeasurementMessage0701 '{telegram_count: 1000004, timestamp: 123456782, source_id: 0, code: "1234", x_position: -1234, y_position: -2345, heading: -3456}' +rostopic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 '{telegram_count: 1000005, timestamp: 123456783, source_id: 0, num_lanes: 1, lanes: [1234]}' +rostopic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 '{telegram_count: 1000006, timestamp: 123456784, source_id: 0, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}' ``` Example output from `sick_lidar_localization`: @@ -174,12 +175,13 @@ Example output from `sick_lidar_localization`: Use `ros2 topic pub` on ROS-2. Examples to to send UDP input messages (odometry, encoder measurement, code measurement and line measurement message): ``` -ros2 topic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 '{telegram_count: 1000001, timestamp: 123456789, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' -ros2 topic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 '{telegram_count: 1000002, timestamp: 123456780, x_position: -1234, y_position: -1234, heading: 1234}' -ros2 topic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 '{telegram_count: 1000003, timestamp: 123456781, encoder_value: 123456789}' -ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 '{telegram_count: 1000004, timestamp: 123456782, code: 1234}' -ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 '{telegram_count: 1000005, timestamp: 123456783, num_lanes: 1, lanes: [1234]}' -ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 '{telegram_count: 1000006, timestamp: 123456784, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}' +ros2 topic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 '{telegram_count: 1000001, timestamp: 123456789, source_id: 0, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' +ros2 topic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 '{telegram_count: 1000002, timestamp: 123456780, source_id: 0, x_position: -1234, y_position: -1234, heading: 1234}' +ros2 topic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 '{telegram_count: 1000003, timestamp: 123456781, source_id: 0, encoder_value: 123456789}' +ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 '{telegram_count: 1000004, timestamp: 123456782, source_id: 0, code: 1234}' +ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0701 sick_lidar_localization/CodeMeasurementMessage0701 '{telegram_count: 1000004, timestamp: 123456782, source_id: 0, code: "1234", x_position: -1234, y_position: -2345, heading: -3456}' +ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 '{telegram_count: 1000005, timestamp: 123456783, source_id: 0, num_lanes: 1, lanes: [1234]}' +ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 '{telegram_count: 1000006, timestamp: 123456784, source_id: 0, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}' ``` ### Configuration of UDP input messages @@ -188,16 +190,27 @@ Parameters can be configured in the launch file [sick_lidar_localization.launch] | **parameter name** | **default value** | **parameter type** | **description** | |--------------------|-------------------|--------------------|-----------------| -| udp_ip_sim_input | "192.168.0.1" | string | IP address for input UDP messages, i.e. IP adress of the localization controller or "" for UDP broadcasts | -| udp_port_sim_input | 5009 | int | UDP port of input messages | +| udp_ip_lls_input | "192.168.0.1" | string | IP address for input UDP messages, i.e. IP address of the localization controller or "" for UDP broadcasts | +| udp_port_lls_input | 5009 | int | UDP port of input messages | | odom_topic | "/odom" | string | Topic of ros odom messages or "" to deactivate | -| udp_sim_input_source_id | 21 | int | Source_id of UDP input messages, see notes below | -| ros_odom_to_udp_msg | 3 | int | Convert ros odom message to upd, see notes below | - -Parameter `udp_sim_input_source_id` is an identifier of the odometry sender. This ID has to match the ID in the SIM configuration file. You can get SIM configuration file using Sopas Air: Log in as user service, download the configuration file and get parameter odometer/external/interface/sourceId. Use this sourceId for the parameter `udp_sim_input_source_id`. - -Parameter `ros_odom_to_udp_msg` converts ros odom message to upd: -* ros_odom_to_udp_msg = 0: map velocity to OdometryPayload0101 (Type 1, Version 1, LidarLoc 1), +| udp_lls_input_source_id | 21 | int | Source_id of UDP input messages, see notes below | +| ros_odom_to_udp_msg | 3 | int | Convert ros odom message to udp, see notes below | + +Parameter `udp_lls_input_source_id` is a default identifier of all UDP input messages if source_id is not otherwise specified in the sender. This ID has to match the ID in the LLS configuration file (lidarloc_config.yml). You can get configuration file using SOPASair: Log in as user service, download the configuration file and get parameter odometer/external/interface/sourceId. Use this sourceId for the parameter `udp_lls_input_source_id`. +Note: In sick_lidar_localization version 5.5 or newer, the ID can be set in 3 different ways: +* The ID can be specified in each UDP input message by its parameter `source_id`. If the source_id in the message is greater 0, it will be send to the localizer server. We recommend this setting. +* If the source_id in the message is equal 0, a default ID from configuration file (launch-file) is used. +* This default source_id can be configured for different message types and message versions in [sick_lidar_localization.launch](../launch/sick_lidar_localization.launch) by + ``` + + ``` + e.g. + ``` + + ``` +* If the parameter `udp_lls_input_source_id__` is not configured, the default source id is given by parameter `udp_lls_input_source_id`. + +Parameter `ros_odom_to_udp_msg` converts ros odom message to udp: * ros_odom_to_udp_msg = 1: map velocity to OdometryPayload0104 (Type 1, Version 4, LidarLoc 2), * ros_odom_to_udp_msg = 2: map position to OdometryPayload0105 (Type 1, Version 5, LidarLoc 2), * ros_odom_to_udp_msg = 3: map velocity to OdometryPayload0104 and position to OdometryPayload0105 diff --git a/doc/screenshots/rviz_pointcloud_converter_screenshot2.png b/doc/screenshots/rviz_pointcloud_converter_screenshot2.png new file mode 100644 index 0000000..8441ef3 Binary files /dev/null and b/doc/screenshots/rviz_pointcloud_converter_screenshot2.png differ diff --git a/doc/sequenceDiagramROSUDPMessages.png b/doc/sequenceDiagramROSUDPMessages.png new file mode 100644 index 0000000..eabcde9 Binary files /dev/null and b/doc/sequenceDiagramROSUDPMessages.png differ diff --git a/doc/sequenceDiagramROSUDPMessages.txt b/doc/sequenceDiagramROSUDPMessages.txt new file mode 100644 index 0000000..0712bbb --- /dev/null +++ b/doc/sequenceDiagramROSUDPMessages.txt @@ -0,0 +1,20 @@ +@startuml +... +"Lidar" -> "SIM1000FXA": scan data +"SIM1000FXA" -> "sick_lidar_localization2": UDP output message \ntype 5 version 2 \n(sourceID:160, \nx, y, heading, \nstatus, timestamp) +"sick_lidar_localization2" -> "ROS": ROS message \nLocalizationControllerResultMessage0502 \n(sourceID:160, \nx, y, heading, status, timestamp) +"sick_lidar_localization2" -> "ROS": ROS TF message \n(geometry_msgs/Transform, \nparent "base_link", child "lls", \nVector3D translation, Quaternion rotation) +"ROS" -> "Application": ROS TF message \n(geometry_msgs/Transform \nused e.g. for robot navigation) +... +"Left wheel encoder ID:150" -> "Application": Encoder datagram +"Application" -> "Application": Encoder datagram conversion\nto ROS message \nEncoderMeasurementMessage0202 \n(sourceID:150, timestamp, encoderTics) +"Application" -> "sick_lidar_localization2": ROS message \nEncoderMeasurementMessage0202 \n(sourceID:150, timestamp, encoderTics) +"sick_lidar_localization2" -> "SIM1000FXA": UDP input message \ntype 2 version 2 \n(sourceID:150, \ntimestamp, encoderTics) +... +... +"Right wheel encoder ID:151" -> "Application": Encoder datagram +"Application" -> "Application": Encoder datagram conversion\nto ROS message \nEncoderMeasurementMessage0202 \n(sourceID:151, timestamp, encoderTics) +"Application" -> "sick_lidar_localization2": ROS message \nEncoderMeasurementMessage0202 \n(sourceID:151, timestamp, encoderTics) +"sick_lidar_localization2" -> "SIM1000FXA": UDP input message \ntype 2 version 2 \n(sourceID:151, \ntimestamp, encoderTics) +... +@enduml diff --git a/doc/sequenceDiagramResultTelegrams.txt b/doc/sequenceDiagramResultTelegrams.txt new file mode 100644 index 0000000..43995ce --- /dev/null +++ b/doc/sequenceDiagramResultTelegrams.txt @@ -0,0 +1,13 @@ +@startuml +... +"SIM1000FXA" -> "ROS sim_loc_driver": Time t: TCP result port telegram +"ROS sim_loc_driver" -> "ROS pointcloud_converter": ROS message SickLocResultPortTelegramMsg, topic "/sick_lidar_localization/driver/result_telegrams" +"ROS pointcloud_converter" -> "ROS rviz": PointCloud2 message +"ROS pointcloud_converter" -> "ROS rviz": TF message +... +"SIM1000FXA" -> "ROS sim_loc_driver": Time t + 0.05: TCP result port telegram +"ROS sim_loc_driver" -> "ROS pointcloud_converter": ROS message SickLocResultPortTelegramMsg, topic "/sick_lidar_localization/driver/result_telegrams" +"ROS pointcloud_converter" -> "ROS rviz": PointCloud2 message +"ROS pointcloud_converter" -> "ROS rviz": TF message +... +@enduml diff --git a/doc/sequenceDiagramSourceIdExample1.png b/doc/sequenceDiagramSourceIdExample1.png new file mode 100644 index 0000000..8ce69a7 Binary files /dev/null and b/doc/sequenceDiagramSourceIdExample1.png differ diff --git a/doc/sequenceDiagramSourceIdExample1.txt b/doc/sequenceDiagramSourceIdExample1.txt new file mode 100644 index 0000000..23f317f --- /dev/null +++ b/doc/sequenceDiagramSourceIdExample1.txt @@ -0,0 +1,15 @@ +@startuml +... +... +"Left wheel encoder ID:150" -> "Application": Encoder datagram +"Application" -> "Application": Encoder datagram conversion\nto ROS message \nEncoderMeasurementMessage0202 \n(sourceID:150, timestamp, encoderTics) +"Application" -> "sick_lidar_localization2": ROS message \nEncoderMeasurementMessage0202 \n(sourceID:150, timestamp, encoderTics) +"sick_lidar_localization2" -> "SIM1000FXA": UDP input message \ntype 2 version 2 \n(sourceID:150, \ntimestamp, encoderTics) +... +... +"Right wheel encoder ID:151" -> "Application": Encoder datagram +"Application" -> "Application": Encoder datagram conversion\nto ROS message \nEncoderMeasurementMessage0202 \n(sourceID:151, timestamp, encoderTics) +"Application" -> "sick_lidar_localization2": ROS message \nEncoderMeasurementMessage0202 \n(sourceID:151, timestamp, encoderTics) +"sick_lidar_localization2" -> "SIM1000FXA": UDP input message \ntype 2 version 2 \n(sourceID:151, \ntimestamp, encoderTics) +... +@enduml diff --git a/doc/sequenceDiagramSourceIdExample2.png b/doc/sequenceDiagramSourceIdExample2.png new file mode 100644 index 0000000..452e6e1 Binary files /dev/null and b/doc/sequenceDiagramSourceIdExample2.png differ diff --git a/doc/sequenceDiagramSourceIdExample2.txt b/doc/sequenceDiagramSourceIdExample2.txt new file mode 100644 index 0000000..a9fd940 --- /dev/null +++ b/doc/sequenceDiagramSourceIdExample2.txt @@ -0,0 +1,7 @@ +@startuml +... +... +"Odometry measurement device" -> "sick_lidar_localization2": ROS message OdometryMessage0104 \n(sourceID:155, timestamp, linear and angular velocity) +"sick_lidar_localization2" -> "SIM1000FXA": UDP input message type 1 version 4 \n(sourceID:155, timestamp, linear and angular velocity) +... +@enduml diff --git a/doc/sequenceDiagramSourceIdExample3.png b/doc/sequenceDiagramSourceIdExample3.png new file mode 100644 index 0000000..ada7fb7 Binary files /dev/null and b/doc/sequenceDiagramSourceIdExample3.png differ diff --git a/doc/sequenceDiagramSourceIdExample3.txt b/doc/sequenceDiagramSourceIdExample3.txt new file mode 100644 index 0000000..0eb001a --- /dev/null +++ b/doc/sequenceDiagramSourceIdExample3.txt @@ -0,0 +1,7 @@ +@startuml +... +... +"Odometry measurement device" -> "sick_lidar_localization2": ROS message OdometryMessage0104 \n(sourceID:0, timestamp, linear and angular velocity) +"sick_lidar_localization2" -> "SIM1000FXA": UDP input message type 1 version 4 \n(sourceID:155, timestamp, linear and angular velocity) +... +@enduml diff --git a/doc/sequenceDiagramSourceIdExample4.png b/doc/sequenceDiagramSourceIdExample4.png new file mode 100644 index 0000000..054810e Binary files /dev/null and b/doc/sequenceDiagramSourceIdExample4.png differ diff --git a/doc/sequenceDiagramSourceIdExample4.txt b/doc/sequenceDiagramSourceIdExample4.txt new file mode 100644 index 0000000..112cd51 --- /dev/null +++ b/doc/sequenceDiagramSourceIdExample4.txt @@ -0,0 +1,7 @@ +@startuml +... +... +"Odometry measurement device" -> "sick_lidar_localization2": ROS odometry message type nav_msgs/Odometry\n(timestamp, linear and angular velocity) +"sick_lidar_localization2" -> "SIM1000FXA": UDP input message type 1 version 4 \n(sourceID:155, timestamp, linear and angular velocity) +... +@enduml diff --git a/doc/sick_localization_services.md b/doc/sick_localization_services.md index d00db54..f017067 100644 --- a/doc/sick_localization_services.md +++ b/doc/sick_localization_services.md @@ -12,7 +12,7 @@ gen_service_call [options] ``` with the following commandline options: ``` -: name of command (service request), f.e. LocIsSystemReady or LocStartLocalizing +: name of command (service request), f.e. LocIsSystemReady or LocStart : GET or POST : parameter as json string, f.e. {} --hostname=: ip address of the localization server, default: 192.168.0.1 @@ -44,7 +44,7 @@ The following table lists supported commands using `gen_service_call` (supported | LocSetOdometryActive | gen_service_call
LocSetOdometryActive POST
"\{\\"data\\": \{\\"active\\":true\}\}" | Enable or disable the support of odometry to enhance the robustness of the contour localization algorithm | | LocSetRecordingActive | gen_service_call
LocSetRecordingActive POST
"\{\\"data\\": \{\\"active\\":true\}\}" | Starts or stops the recording of sensor data | | LocSetRingBufferRecordingActive | gen_service_call
LocSetRingBufferRecordingActive POST
"\{\\"data\\": \{\\"active\\":true\}\}" | Enable or disable the continuous recording of data for improved support in critical situations | -| LocStartLocalizing | gen_service_call
LocStartLocalizing POST "\{\}" | Start the localization | +| LocStart | gen_service_call
LocStart POST "\{\}" | Start the localization | | LocStop | gen_service_call
LocStop POST "\{\}" | Stop the localization or the demo mapping and return to IDLE state | | LocSwitchMap | gen_service_call
LocSwitchMap POST
"\{\\"data\\": \{\\"subMapName\\": \\"test.vmap\\"\}\}" | Transits to the given sub map if close enough to a transition point between the current and the given sub map | | LocGetLocalizationStatus | gen_service_call
LocGetLocalizationStatus POST "\{\}" | Returns the localization status | @@ -78,7 +78,7 @@ The following table lists supported commands in their shortest form: | LocSetOdometryActive | gen_service_call
LocSetOdometryActive
"\{active: true\}" | Enable or disable the support of odometry to enhance the robustness of the contour localization algorithm | | LocSetRecordingActive | gen_service_call
LocSetRecordingActive
"\{active: true\}" | Starts or stops the recording of sensor data | | LocSetRingBufferRecordingActive | gen_service_call
LocSetRingBufferRecordingActive
"\{active: true\}" | Enable or disable the continuous recording of data for improved support in critical situations | -| LocStartLocalizing | gen_service_call
LocStartLocalizing | Start the localization | +| LocStart | gen_service_call
LocStart | Start the localization | | LocStop | gen_service_call
LocStop | Stop the localization or the demo mapping and return to IDLE state | | LocSwitchMap | gen_service_call
LocSwitchMap
"\{subMapName: \\"test.vmap\\"\}\}" | Transits to the given sub map if close enough to a transition point between the current and the given sub map | | LocGetLocalizationStatus | gen_service_call
LocGetLocalizationStatus | Returns the localization status | @@ -98,7 +98,7 @@ gen_service_call examples: ./build/gen_service_call LocSetMappingActive POST "{\"data\":{\"active\": true}}" -d=2 ./build/gen_service_call LocSetOdometryActive POST "{\"data\":{\"active\": true}}" -d=2 ./build/gen_service_call LocStop POST "{}" -d=2 -./build/gen_service_call LocStartLocalizing POST "{}" -d=2 +./build/gen_service_call LocStart POST "{}" -d=2 # short form ./build/gen_service_call LocIsSystemReady ./build/gen_service_call LocGetMap @@ -106,7 +106,7 @@ gen_service_call examples: ./build/gen_service_call LocSetMappingActive "{active: true}" ./build/gen_service_call LocSetOdometryActive "{active: true}" ./build/gen_service_call LocStop -./build/gen_service_call LocStartLocalizing +./build/gen_service_call LocStart ``` Output examples (test against SIM1000FX): @@ -153,12 +153,12 @@ gen_service_call request: LocStop, response: {"header":{"status":0,"message":"Ok curl command: curl -i -H "Content-Type: application/json" -X POST -d "{}" http://192.168.0.1/api/LocStop -./build/gen_service_call LocStartLocalizing POST "{}" -d=2 -gen_service_call LocStartLocalizing POST {} --hostname=192.168.0.1 --verbose=1 -INFO : curl send http POST request "LocStartLocalizing", response: "{"header":{"status":0,"message":"Ok"},"data":{"success":true}}" -gen_service_call request: LocStartLocalizing, response: {"header":{"status":0,"message":"Ok"},"data":{"success":true}} +./build/gen_service_call LocStart POST "{}" -d=2 +gen_service_call LocStart POST {} --hostname=192.168.0.1 --verbose=1 +INFO : curl send http POST request "LocStart", response: "{"header":{"status":0,"message":"Ok"},"data":{"success":true}}" +gen_service_call request: LocStart, response: {"header":{"status":0,"message":"Ok"},"data":{"success":true}} curl command: -curl -i -H "Content-Type: application/json" -X POST -d "{}" http://192.168.0.1/api/LocStartLocalizing +curl -i -H "Content-Type: application/json" -X POST -d "{}" http://192.168.0.1/api/LocStart ``` Note: A REST-API can be served with tool curl, too. `gen_service_call` with option `-d=2` prints the corresponding curl command, f.e.: @@ -169,7 +169,7 @@ curl -i -H "Content-Type: application/json" -X POST -d "{}" http://192.168.0.1/a curl -i -H "Content-Type: application/json" -X POST -d "{\"data\":{\"active\":true}}" http://192.168.0.1/api/LocSetMappingActive curl -i -H "Content-Type: application/json" -X POST -d "{\"data\":{\"active\":true}}" http://192.168.0.1/api/LocSetOdometryActive curl -i -H "Content-Type: application/json" -X POST -d "{}" http://192.168.0.1/api/LocStop -curl -i -H "Content-Type: application/json" -X POST -d "{}" http://192.168.0.1/api/LocStartLocalizing +curl -i -H "Content-Type: application/json" -X POST -d "{}" http://192.168.0.1/api/LocStart ``` ## ROS-1 services @@ -196,7 +196,7 @@ The following table lists the same commands using ROS1 services: | LocSetOdometryActive | rosservice call LocSetOdometryActive
"\{active: true\}" | | LocSetRecordingActive | rosservice call LocSetRecordingActive
"\{active: true\}" | | LocSetRingBufferRecordingActive | rosservice call LocSetRingBufferRecordingActive
"\{active: true\}" | -| LocStartLocalizing | rosservice call LocStartLocalizing "\{\}" | +| LocStart | rosservice call LocStart "\{\}" | | LocStop | rosservice call LocStop "\{\}" | | LocSwitchMap | rosservice call LocSwitchMap
"\{submapname: \\"test.vmap\\"\}" | | LocGetLocalizationStatus | rosservice call LocGetLocalizationStatus "\{\}" | @@ -214,7 +214,7 @@ rosservice call LocSetMappingActive "{active: true}" rosservice call LocSetOdometryActive "{active: true}" rosservice call LocGetSystemState "{}" rosservice call LocStop "{}" -rosservice call LocStartLocalizing "{}" +rosservice call LocStart "{}" ``` Output examples (test examples against SIM1000FX): @@ -246,8 +246,8 @@ success: True rosservice call LocStop "{}" [ INFO] [1626875300.376469527]: SickServices::serviceCb("LocStop", "POST", "{}"): success=1 success: True -rosservice call LocStartLocalizing "{}" -[ INFO] [1626875302.384810233]: SickServices::serviceCb("LocStartLocalizing", "POST", "{}"): success=1 +rosservice call LocStart "{}" +[ INFO] [1626875302.384810233]: SickServices::serviceCb("LocStart", "POST", "{}"): success=1 success: True ``` @@ -275,7 +275,7 @@ The following table lists the same commands using ROS2 services: | LocSetOdometryActive | ros2 service call LocSetOdometryActive sick_lidar_localization/srv/LocSetOdometryActiveSrv
"\{active: true\}" | | LocSetRecordingActive | ros2 service call LocSetRecordingActive sick_lidar_localization/srv/LocSetRecordingActiveSrv
"\{active: true\}" | | LocSetRingBufferRecordingActive | ros2 service call LocSetRingBufferRecordingActive sick_lidar_localization/srv/LocSetRingBufferRecordingActiveSrv
"\{active: true\}" | -| LocStartLocalizing | ros2 service call LocStartLocalizing sick_lidar_localization/srv/LocStartLocalizingSrv "\{\}" | +| LocStart | ros2 service call LocStart sick_lidar_localization/srv/LocStartSrv "\{\}" | | LocStop | ros2 service call LocStop sick_lidar_localization/srv/LocStopSrv "\{\}" | | LocSwitchMap | ros2 service call LocSwitchMap sick_lidar_localization/srv/LocSwitchMapSrv
"\{submapname: \\"test.vmap\\"\}" | | LocGetLocalizationStatus | ros2 service call LocGetLocalizationStatus sick_lidar_localization/srv/LocGetLocalizationStatusSrv "\{\}" | @@ -292,7 +292,7 @@ ros2 service call LocGetSystemState sick_lidar_localization/srv/LocGetSystemStat ros2 service call LocSetMappingActive sick_lidar_localization/srv/LocSetMappingActiveSrv "{active: true}" ros2 service call LocSetOdometryActive sick_lidar_localization/srv/LocSetOdometryActiveSrv "{active: true}" ros2 service call LocStop sick_lidar_localization/srv/LocStopSrv "{}" -ros2 service call LocStartLocalizing sick_lidar_localization/srv/LocStartLocalizingSrv "{}" +ros2 service call LocStart sick_lidar_localization/srv/LocStartSrv "{}" ``` Output examples (test examples against SIM1000FX): @@ -319,7 +319,7 @@ response: sick_lidar_localization.srv.LocSetOdometryActiveSrv_Response(success=T ros2 service call LocStop sick_lidar_localization/srv/LocStopSrv "{}" [INFO] [1626874812.001814744] [sick_lidar_localization]: SickServices::serviceCb("LocStop", "POST", "{}"): success=1 response: sick_lidar_localization.srv.LocStopSrv_Response(success=True) -ros2 service call LocStartLocalizing sick_lidar_localization/srv/LocStartLocalizingSrv "{}" -[INFO] [1626874813.582692773] [sick_lidar_localization]: SickServices::serviceCb("LocStartLocalizing", "POST", "{}"): success=1 -response: sick_lidar_localization.srv.LocStartLocalizingSrv_Response(success=True) +ros2 service call LocStart sick_lidar_localization/srv/LocStartSrv "{}" +[INFO] [1626874813.582692773] [sick_lidar_localization]: SickServices::serviceCb("LocStart", "POST", "{}"): success=1 +response: sick_lidar_localization.srv.LocStartSrv_Response(success=True) ``` diff --git a/doc/source_id.md b/doc/source_id.md new file mode 100644 index 0000000..1ca4638 --- /dev/null +++ b/doc/source_id.md @@ -0,0 +1,91 @@ +# System setup and source Ids + +This chapter describes the system setup, messages and source ids. + +## Background: System setup, messages and source ids + +System example: A unit (e.g. robot) has multiple sensors (e.g. a lidar and 2 wheel encoders). The localization server, for example a SIM1000FX, is used for localization. The sensors communicate with the server using sick_lidar_localization2 on a target platform (e.g. ROS1 on Linux). + +Communication: +* Sensors communicate with ROS messages and services with sick_lidar_localization2 +* sick_lidar_localization2 communicates with SIM1000FX using UDP +* UDP input messages are sent from sick_lidar_localization2 to SIM1000FX +* UDP output messages are sent from SIM1000FX to sick_lidar_localization2 +* sick_lidar_localization2 converts UDP to ROS messages and vice versa + +Example: +![source_id_01.png](source_id_01.png) + +Source Ids: +* Each sensor has a source_id (e.g. left wheel encoder: sourceId=150, right wheel encoder: sourceId=151) +* All ROS and UDP messages contain this sourceId +* All sourceIds must be configured in the SIM1000FX localization server configuration file (yml) +* SOPASair (web client) is used to down- and upload the LiDAR-LOC configuration file (yml). Modify this file to change or add sourceIds of your sensors. + +Example sequence diagram: +![sequenceDiagramROSUDPMessages.png](sequenceDiagramROSUDPMessages.png) + +## Configuration of source ids + +The Source ID can either be +* set individually for each UDP input message, or +* configured in launch-file for each message type and version (this works only if there is only 1 source per message type. E.g. for 2 Encoders this is not suitable). + +![source_id_02.png](source_id_02.png) + +If no source ID is set (sourceID: 0), the default source ID configured in the launch-file will be used as fallback. + +### Examples + +Example 1: Customized source ID configuration for 2 wheel encoders: +* Source ID of left wheel encoder: 150 (configured by customer) +* Source ID of right wheel encoder: 151 (configured by customer) +* Source ID is set individually for each wheel encoder in the ROS message +* ROS message EncoderMeasurementMessage0202 of left wheel encoder: source_id=150 + * sick_lidar_localization2 generates UDP input message type 2 version 2 with source_id=150 +* ROS message EncoderMeasurementMessage0202 of right wheel encoder: source_id=151 + * sick_lidar_localization2 generates UDP input message type 2 version 2 with source_id=151 + +![sequenceDiagramSourceIdExample1.png](sequenceDiagramSourceIdExample1.png) + +Example 2: Customized source ID configuration for 1 odometry measurement device: +* Source ID of odometry device: 155 (configured by customer) +* Source ID is set individually in the ROS message OdometryMessage0104 +* ROS message OdometryMessage0104: source_id=155 + * sick_lidar_localization2 generates UDP input message type 1 version 4 with source_id=155 + +![sequenceDiagramSourceIdExample2.png](sequenceDiagramSourceIdExample2.png) + +Example 3: Source ID configuration by launchfile for 1 odometry measurement device: +* Source ID of odometry device: 155 (configured by launchfile): + `` +* Source ID = 0 (not set) in the ROS message OdometryMessage0104 +* Source ID = 155 set by sick_lidar_localization2 in the UDP input message +* ROS message OdometryMessage0104: source_id=0 + * sick_lidar_localization2 generates UDP input message type 1 version 4 with source_id=155 + +![sequenceDiagramSourceIdExample3.png](sequenceDiagramSourceIdExample3.png) + +Example 4: A robot uses standard ROS odometry messages of type [nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) to publish odometry data. Standard ROS odometry messages of type [nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) do not contain a source id. In this case, the source ID must be configured in the launchfile: +* Source ID of odometry device: 155 (configured by launchfile): + `` +* A robot publishes its velocity using [ROS odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) messages +* sick_lidar_localization2 receives the [ROS odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) messages and generates UDP input message type 1 version 4 with source_id=155 + +![sequenceDiagramSourceIdExample4.png](sequenceDiagramSourceIdExample4.png) + +Note for source id configuration by launchfile: + * A target can be used with different localization servers with different configurations. This can simplify maintenance when source ids must be changed or adapted to different localization servers. + * Standard ROS odometry messages of type [nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) do not contain a source id. ROS [nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) messages are supported by sick_lidar_localization2; their source id is configured in the launchfile. Depending on launchfile parameter `ros_odom_to_udp_msg`, ROS [nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) messages are converted to UDP input message type 1 version 4 or 5: + * ros_odom_to_udp_msg = 1: [nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) messages are converted to UDP input message type 1 version 4 (source id by launchfile, linear and angular velocity by ROS message [nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) + * ros_odom_to_udp_msg = 2: [nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) messages are converted to UDP input message type 1 version 5 (source id by launchfile, position and heading by ROS message [nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) + * ros_odom_to_udp_msg = 3: [nav_msgs/Odometry](http://docs.ros.org/en/noetic/api/nav_msgs/html/msg/Odometry.html) messages are converted to both UDP input message type 1 version 4 and 5 + * Default value for ros_odom_to_udp_msg is 3 (convert to both udp message type 1, version 4 and 5): + ``` + + + + + + ``` + * A customer does not need to know details about source ids - except: "The source id used in the SIM1000FX configuration and in the launchfile must be identical" diff --git a/doc/source_id_01.png b/doc/source_id_01.png new file mode 100644 index 0000000..651e6fa Binary files /dev/null and b/doc/source_id_01.png differ diff --git a/doc/source_id_02.png b/doc/source_id_02.png new file mode 100644 index 0000000..843eb85 Binary files /dev/null and b/doc/source_id_02.png differ diff --git a/include/sick_lidar_localization/fifo_buffer.h b/include/sick_lidar_localization/fifo_buffer.h index 23e7bdf..cf1e82a 100644 --- a/include/sick_lidar_localization/fifo_buffer.h +++ b/include/sick_lidar_localization/fifo_buffer.h @@ -1,5 +1,5 @@ /* - * @brief sim_loc_fifo implements a threadsafe fifo-buffer ("first in, first out"). + * @brief lls_loc_fifo implements a threadsafe fifo-buffer ("first in, first out"). * * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim * Copyright (C) 2019 SICK AG, Waldkirch diff --git a/include/sick_lidar_localization/pointcloud_converter.h b/include/sick_lidar_localization/lls_transform.h similarity index 79% rename from include/sick_lidar_localization/pointcloud_converter.h rename to include/sick_lidar_localization/lls_transform.h index d9dd18d..1ee75ed 100644 --- a/include/sick_lidar_localization/pointcloud_converter.h +++ b/include/sick_lidar_localization/lls_transform.h @@ -1,5 +1,5 @@ /* - * @brief sim_loc_pointcloud_converts sim_loc_driver messages (type sick_lidar_localization::LocResultPortTelegramMsg), + * @brief lls_loc_pointcloud_converts lls_loc_driver messages (type sick_lidar_localization::LocResultPortTelegramMsg), * to PointCloud2 messages and publishes PointCloud2 messages on topic "/cloud". * * The vehicle poses (PoseX, PoseY, PoseYaw of result port telegrams) are transformed and published @@ -78,11 +78,11 @@ namespace sick_lidar_localization { using namespace msg; } namespace sick_lidar_localization { /*! - * class PointCloudConverter implements a thread to converts lidar_loc_driver messages + * class LLSTransformer implements a thread to converts lidar_loc_driver messages * (type sick_lidar_localization::LocResultPortTelegramMsg) to PointCloud2 messages * and publishes them on topic "/cloud". */ - class PointCloudConverter + class LLSTransformer { public: @@ -90,12 +90,12 @@ namespace sick_lidar_localization * Constructor * @param[in] nh ros node handle */ - PointCloudConverter(rosNodePtr nh = 0); + LLSTransformer(rosNodePtr nh = 0); /*! * Destructor */ - virtual ~PointCloudConverter(); + virtual ~LLSTransformer(); /*! * Starts the converter thread, converts telegrams and publishes PointCloud2 messages. @@ -131,7 +131,7 @@ namespace sick_lidar_localization * @param[in] triangle_height heigt of the virtual triangle in meter * @return list of 3D points */ - std::vector poseToDemoPoints(double posx, double posy, double yaw, double triangle_height); + // std::vector poseToDemoPoints(double posx, double posy, double yaw, double triangle_height); /*! * Converts the vehicle position from a result port telegram to PointCloud2 message with 4 points @@ -139,8 +139,15 @@ namespace sick_lidar_localization * @param[in] msg result telegram message (LocResultPortTelegramMsg) * @return PointCloud2 message */ - ros_sensor_msgs::PointCloud2 convertToPointCloud(const sick_lidar_localization::LocalizationControllerResultMessage0502 & msg); + // ros_sensor_msgs::PointCloud2 convertToPointCloud(const sick_lidar_localization::LocalizationControllerResultMessage0502 & msg); + /*! + * Converts the vehicle pose from a result port telegram to a marker array. + * @param[in] msg result telegram message (LocResultPortTelegramMsg) + * @return marker array + */ + // ros_visualization_msgs::MarkerArray convertToMarker(const sick_lidar_localization::LocalizationControllerResultMessage0502 & msg); + /*! * Converts the vehicle pose from a result port telegram to a tf transform. * @param[in] msg result telegram message (LocResultPortTelegramMsg) @@ -153,7 +160,7 @@ namespace sick_lidar_localization * converts the telegrams from type LocResultPortTelegramMsg to PointCloud2 and publishes * PointCloud2 messages. The vehicle pose is converted to a tf transform and broadcasted. */ - virtual void runPointCloudConverterThreadCb(void); + virtual void runLLSTransformerThreadCb(void); /* * member data @@ -161,14 +168,22 @@ namespace sick_lidar_localization rosNodePtr m_nh; ///< ROS node handle sick_lidar_localization::FifoBuffer m_result_port_telegram_fifo; ///< fifo buffer for result port telegrams from lidar_loc_driver - std::string m_point_cloud_frame_id; ///< ros frame id of PointCloud2 messages, default: "sick_lidar_localization" std::string m_tf_parent_frame_id; ///< parent frame of tf messages of of vehicles pose (typically frame of the loaded map) std::string m_tf_child_frame_id; ///< child frame of tf messages of of vehicles pose - rosPublisher m_point_cloud_publisher; ///< ros publisher for PointCloud2 messages + // std::string m_marker_frame_id; ///< ros frame id marker arrays + // double m_marker_arrow_length; ///< arrow length of lls marker messages --> + // double m_marker_arrow_width; ///< arrow width of lls marker messages --> + // double m_marker_arrow_height; ///< arrow length of lls marker messages --> + // double m_marker_color_r; ///< color of lls marker messages (RGBA) --> + // double m_marker_color_g; ///< color of lls marker messages (RGBA) --> + // double m_marker_color_b; ///< color of lls marker messages (RGBA) --> + // double m_marker_color_a; ///< color of lls marker messages (RGBA) --> + // rosPublisher m_marker_publisher; ///< ros publisher for marker arrays + // rosPublisher m_point_cloud_publisher; ///< ros publisher for PointCloud2 messages bool m_converter_thread_running; ///< true: m_verification_thread is running, otherwise false std::thread* m_converter_thread; ///< thread to verify lidar_loc_driver - }; // class PointCloudConverter + }; // class LLSTransformer } // namespace sick_lidar_localization #endif // __LIDAR_LOC_POINT_CLOUD_CONVERTER_H_INCLUDED diff --git a/include/sick_lidar_localization/sick_lidar_localization.h b/include/sick_lidar_localization/sick_lidar_localization.h index 2e1bcbc..0fba84a 100644 --- a/include/sick_lidar_localization/sick_lidar_localization.h +++ b/include/sick_lidar_localization/sick_lidar_localization.h @@ -69,18 +69,19 @@ namespace sick_lidar_localization class Config { public: - std::string hostname = "192.168.0.1"; // IP adress of the localization controller + std::string hostname = "192.168.0.1"; // IP address of the localization controller std::string serverpath = "api"; // Relative path to the rest api, i.e. url of rest requests is "http://// int verbose = 0; // If verbose>0: print informational messages, otherwise silent except for error messages - std::string udp_ip_sim_output = ""; // IP address for output UDP messages, or "" for broadcast (INADDR_ANY), default: "", use IP address of your local machine - std::string udp_ip_sim_input = "192.168.0.1"; // IP address for input UDP messages, or "" for broadcast, default: "192.168.0.1", (IP adress of the localization controller) - int udp_port_sim_input = 5009; // UDP port of input messages - int udp_sim_input_source_id = 1; // Source_id of input messages - int udp_port_sim_output = 5010; // UDP port of output messages - std::string udp_sim_output_logfile = ""; // Optional logfile for human readable UDP output messages, default: "" (no outputlogfile) + std::string udp_ip_lls_output = ""; // IP address for output UDP messages, or "" for broadcast (INADDR_ANY), default: "", use IP address of your local machine + std::string udp_ip_lls_input = "192.168.0.1"; // IP address for input UDP messages, or "" for broadcast, default: "192.168.0.1", (IP address of the localization controller) + int udp_port_lls_input = 5009; // UDP port of input messages + int udp_lls_input_source_id = 1; // Default source_id of UDP input messages (used if source_id not set otherwise) + std::map> msgtype_version_sourceid_map; // msgtype_version_sourceid_map[msgtype][msgversion] := default source_id for UDP input messages of type and version + int udp_port_lls_output = 5010; // UDP port of output messages + std::string udp_lls_output_logfile = ""; // Optional logfile for human readable UDP output messages, default: "" (no outputlogfile) int software_pll_fifo_length = 7; // Length of fifo in SoftwarePLL std::string odom_topic = "/odom"; // Topic of ros odom messages - int ros_odom_to_udp_msg = 0; // Convert ros odom message to upd: 0 = map velocity to OdometryPayload0101 (Type 1, Version 1, LidarLoc 1), + int ros_odom_to_udp_msg = 0; // Convert ros odom message to udp: // 1 = map velocity to OdometryPayload0104 (Type 1, Version 4, LidarLoc 2), // 2 = map position to OdometryPayload0105 (Type 1, Version 5, LidarLoc 2), // 3 = map velocity to OdometryPayload0104 and position to OdometryPayload0105 @@ -121,7 +122,7 @@ namespace sick_lidar_localization void close(); /* - ** @brief Register a listener for upd messages. The callback functions of the listener will be called after receiving a new udp message. + ** @brief Register a listener for udp messages. The callback functions of the listener will be called after receiving a new udp message. ** Overwrite the functions defined in sick_lidar_localization::UDPMessage::Listener with customized code to handle udp messages. */ bool registerListener(sick_lidar_localization::UDPMessage::Listener* listener); @@ -134,12 +135,13 @@ namespace sick_lidar_localization /* ** @brief Send udp messages to the localization controller */ - bool sendUDPMessage(const sick_lidar_localization::UDPMessage::OdometryPayload0104& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id); - bool sendUDPMessage(const sick_lidar_localization::UDPMessage::OdometryPayload0105& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id); - bool sendUDPMessage(const sick_lidar_localization::UDPMessage::EncoderMeasurementPayload0202& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id); - bool sendUDPMessage(const sick_lidar_localization::UDPMessage::CodeMeasurementPayload0303& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id); - bool sendUDPMessage(const sick_lidar_localization::UDPMessage::LineMeasurementPayload0403& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id); - bool sendUDPMessage(const sick_lidar_localization::UDPMessage::LineMeasurementPayload0404& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id); + bool sendUDPMessage(const sick_lidar_localization::UDPMessage::OdometryPayload0104& payload, bool encode_header_big_endian, bool encode_payload_big_endian); + bool sendUDPMessage(const sick_lidar_localization::UDPMessage::OdometryPayload0105& payload, bool encode_header_big_endian, bool encode_payload_big_endian); + bool sendUDPMessage(const sick_lidar_localization::UDPMessage::EncoderMeasurementPayload0202& payload, bool encode_header_big_endian, bool encode_payload_big_endian); + bool sendUDPMessage(const sick_lidar_localization::UDPMessage::CodeMeasurementPayload0303& payload, bool encode_header_big_endian, bool encode_payload_big_endian); + bool sendUDPMessage(const sick_lidar_localization::UDPMessage::CodeMeasurementPayload0701& payload, bool encode_header_big_endian, bool encode_payload_big_endian); + bool sendUDPMessage(const sick_lidar_localization::UDPMessage::LineMeasurementPayload0403& payload, bool encode_header_big_endian, bool encode_payload_big_endian); + bool sendUDPMessage(const sick_lidar_localization::UDPMessage::LineMeasurementPayload0404& payload, bool encode_header_big_endian, bool encode_payload_big_endian); /* ** @brief Parses commandline arguments, reads a launchfile and sets parameters for sick_lidar_localization. diff --git a/include/sick_lidar_localization/sick_ros_wrapper.h b/include/sick_lidar_localization/sick_ros_wrapper.h index 3eeefd2..b90150c 100644 --- a/include/sick_lidar_localization/sick_ros_wrapper.h +++ b/include/sick_lidar_localization/sick_ros_wrapper.h @@ -140,8 +140,8 @@ inline bool rosOk(void) { return true; } #include #include #include -#include -#include +// #include +// #include typedef ros::NodeHandle* rosNodePtr; @@ -234,8 +234,8 @@ template class rosServiceServer : public ros::ServiceServer #include #include #include -#include -#include +// #include +// #include typedef rclcpp::Node::SharedPtr rosNodePtr; diff --git a/include/sick_lidar_localization/sick_services.h b/include/sick_lidar_localization/sick_services.h index 3974859..de21a0d 100644 --- a/include/sick_lidar_localization/sick_services.h +++ b/include/sick_lidar_localization/sick_services.h @@ -83,7 +83,7 @@ #include "sick_lidar_localization/LocSetOdometryActiveSrv.h" #include "sick_lidar_localization/LocSetRecordingActiveSrv.h" #include "sick_lidar_localization/LocSetRingBufferRecordingActiveSrv.h" -#include "sick_lidar_localization/LocStartLocalizingSrv.h" +#include "sick_lidar_localization/LocStartSrv.h" #include "sick_lidar_localization/LocStopSrv.h" #include "sick_lidar_localization/LocSwitchMapSrv.h" #include "sick_lidar_localization/LocGetLocalizationStatusSrv.h" @@ -109,7 +109,7 @@ #include "sick_lidar_localization/srv/loc_set_odometry_active_srv.hpp" #include "sick_lidar_localization/srv/loc_set_recording_active_srv.hpp" #include "sick_lidar_localization/srv/loc_set_ring_buffer_recording_active_srv.hpp" -#include "sick_lidar_localization/srv/loc_start_localizing_srv.hpp" +#include "sick_lidar_localization/srv/loc_start_srv.hpp" #include "sick_lidar_localization/srv/loc_stop_srv.hpp" #include "sick_lidar_localization/srv/loc_switch_map_srv.hpp" #include "sick_lidar_localization/srv/loc_get_localization_status_srv.hpp" @@ -282,9 +282,9 @@ namespace sick_lidar_localization bool serviceCbLocSetRingBufferRecordingActiveSrvROS2(std::shared_ptr service_request, std::shared_ptr service_response); rosServiceServer m_srv_server_LocSetRingBufferRecordingActiveSrv; - bool serviceCbLocStartLocalizingSrvROS1(sick_lidar_localization::LocStartLocalizingSrv::Request &service_request, sick_lidar_localization::LocStartLocalizingSrv::Response &service_response); - bool serviceCbLocStartLocalizingSrvROS2(std::shared_ptr service_request, std::shared_ptr service_response); - rosServiceServer m_srv_server_LocStartLocalizingSrv; + bool serviceCbLocStartSrvROS1(sick_lidar_localization::LocStartSrv::Request &service_request, sick_lidar_localization::LocStartSrv::Response &service_response); + bool serviceCbLocStartSrvROS2(std::shared_ptr service_request, std::shared_ptr service_response); + rosServiceServer m_srv_server_LocStartSrv; bool serviceCbLocStopSrvROS1(sick_lidar_localization::LocStopSrv::Request &service_request, sick_lidar_localization::LocStopSrv::Response &service_response); bool serviceCbLocStopSrvROS2(std::shared_ptr service_request, std::shared_ptr service_response); diff --git a/include/sick_lidar_localization/udp_message_parser.h b/include/sick_lidar_localization/udp_message_parser.h index 0481481..d626777 100644 --- a/include/sick_lidar_localization/udp_message_parser.h +++ b/include/sick_lidar_localization/udp_message_parser.h @@ -82,18 +82,6 @@ namespace sick_lidar_localization uint16_t sourceid; // 2 byte source id }; - /* - ** @brief Container for the odometry payload message type 1 version 1 (UDP input) - */ - struct OdometryPayload0101 - { - uint64_t telegram_count; // 8 byte TelegramCount uint64 - uint64_t timestamp; // 8 byte Timestamp uint64 [microseconds] - int32_t x_velocity; // 4 byte X - velocity int16 [mm/s] - int32_t y_velocity; // 4 byte Y - velocity int16 [mm/s] - int64_t angular_velocity; // 8 byte angular velocity int32 [mdeg/s] - }; - /* ** @brief Container for the odometry payload message type 1 version 4 (24 byte payload, UDP input and output) */ @@ -101,6 +89,7 @@ namespace sick_lidar_localization { uint64_t telegram_count; // 8 byte TelegramCount uint64 uint64_t timestamp; // 8 byte Timestamp uint64 [microseconds] + int32_t source_id; // 4 byte source id int16_t x_velocity; // 2 byte X - velocity int16 [mm/s] int16_t y_velocity; // 2 byte Y - velocity int16 [mm/s] int32_t angular_velocity; // 4 byte angular velocity int32 [mdeg/s] @@ -116,6 +105,7 @@ namespace sick_lidar_localization { uint64_t telegram_count; // 8 byte TelegramCount uint64 uint64_t timestamp; // 8 byte Timestamp uint64 [microseconds] + int32_t source_id; // 4 byte source id int64_t x_position; // 8 byte X-position int64 [mm] int64_t y_position; // 8 byte Y-position int64 [mm] int64_t heading; // 8 byte heading int32 [mdeg] @@ -131,6 +121,7 @@ namespace sick_lidar_localization { uint64_t telegram_count; // 8 byte TelegramCount uint64 uint64_t timestamp; // 8 byte Timestamp uint64 [microseconds] + int32_t source_id; // 4 byte source id int64_t encoder_value; // 8 byte EncoderValue int64 in tics }; @@ -141,9 +132,24 @@ namespace sick_lidar_localization { uint64_t telegram_count; // 8 byte TelegramCount uint64 uint64_t timestamp; // 8 byte Timestamp uint64 [microseconds] + int32_t source_id; // 4 byte source id int64_t code; // 8 byte Code int32 }; + /* + ** @brief Container for the code measurement payload message type 7 version 1 (variable length payload, UDP input message) + */ + struct CodeMeasurementPayload0701 + { + uint64_t telegram_count; // 8 byte TelegramCount uint64 + uint64_t timestamp; // 8 byte Timestamp uint64 [microseconds] + int32_t source_id; // 4 byte source id + std::string code; // variable length string => udp message: 2 byte codelength (uint16) + byte string + int32_t x_position; // 4 byte relative pose along the x-axis of the sensor frame [mm] + int32_t y_position; // 4 byte relative pose along the y-axis of the sensor frame [mm] + int32_t heading; // 4 byte relative orientation of the code in the sensor frame [mdeg] + }; + /* ** @brief Container for the code measurement payload message type 3 version 4 (24 byte payload, UDP output message) */ @@ -151,6 +157,7 @@ namespace sick_lidar_localization { uint64_t telegram_count; // 8 byte TelegramCount uint64 uint64_t timestamp; // 8 byte Timestamp uint64 [microseconds] + int32_t source_id; // 4 byte source id int32_t code; // 4 byte Code int32 int32_t distance; // 4 byte Distance int32 [mm] uint32_t sync_timestamp_sec; // seconds part of synchronized timestamp in system time calculated by Software-PLL @@ -165,6 +172,7 @@ namespace sick_lidar_localization { uint64_t telegram_count; // 8 byte TelegramCount uint64 uint64_t timestamp; // 8 byte Timestamp uint64 [microseconds] + int32_t source_id; // 4 byte source id uint8_t num_lanes; // 1 byte NumOfLanes uint8 std::vector lanes; // Lanes: num_lanes * 2 byte, each lane encoded by int16 uint32_t sync_timestamp_sec; // seconds part of synchronized timestamp in system time calculated by Software-PLL @@ -179,6 +187,7 @@ namespace sick_lidar_localization { uint64_t telegram_count; // 8 byte TelegramCount uint64 uint64_t timestamp; // 8 byte Timestamp uint64 [microseconds] + int32_t source_id; // 4 byte source id int16_t lcp1; // 2 byte lcp1 int16 [mm] int16_t lcp2; // 2 byte lcp2 int16 [mm] int16_t lcp3; // 2 byte lcp3 int16 [mm] @@ -196,6 +205,7 @@ namespace sick_lidar_localization { uint64_t telegram_count; // 8 byte TelegramCount uint64 uint64_t timestamp; // 8 byte Timestamp uint64 [microseconds] + int32_t source_id; // 4 byte source id int64_t x; // 8 byte X int64 [mm] int64_t y; // 8 byte Y int64 [mm] int32_t heading; // 4 byte Heading int32 [mdeg] @@ -294,6 +304,10 @@ namespace sick_lidar_localization ** @brief Sets the synchronized timestamp calculated by software pll */ virtual void setSyncTimestamp(uint32_t sec, uint32_t nsec) = 0; + /* + ** @brief Get/set the source id + */ + virtual int32_t& sourceID(void) = 0; }; /* @@ -347,6 +361,13 @@ namespace sick_lidar_localization m_payload_data.sync_timestamp_nsec = nsec; m_payload_data.sync_timestamp_valid = 1; } + /* + ** @brief Get/set the source id + */ + virtual int32_t& sourceID(void) + { + return m_payload_data.source_id; + } protected: /* @@ -389,7 +410,7 @@ namespace sick_lidar_localization /* ** @brief Encodes the payload of a udp message ** @param[in] message_payload payload of a udp message, can be OdometryPayload0104, OdometryPayload0105, EncoderMeasurementPayload0202, - ** CodeMeasurementPayload0303, LineMeasurementPayload0403 or LineMeasurementPayload0404 + ** CodeMeasurementPayload0303, CodeMeasurementPayload0701, LineMeasurementPayload0403 or LineMeasurementPayload0404 ** @param[in] encode_big_endian encode in big or little endiang ** @param[out] encoded vector of bytes */ @@ -398,7 +419,7 @@ namespace sick_lidar_localization /* ** @brief Encodes a UDP input message ** @param[in] message_payload payload of a udp message, can be OdometryPayload0104, OdometryPayload0105, EncoderMeasurementPayload0202, - ** CodeMeasurementPayload0303, LineMeasurementPayload0403 or LineMeasurementPayload0404 + ** CodeMeasurementPayload0303, CodeMeasurementPayload0701, LineMeasurementPayload0403 or LineMeasurementPayload0404 ** @param[in] encode_big_endian encode in big or little endiang ** @param[out] encoded vector of bytes */ diff --git a/include/sick_lidar_localization/udp_message_publisher.h b/include/sick_lidar_localization/udp_message_publisher.h index bf7ba4a..74f5e55 100644 --- a/include/sick_lidar_localization/udp_message_publisher.h +++ b/include/sick_lidar_localization/udp_message_publisher.h @@ -110,6 +110,7 @@ namespace sick_lidar_localization ros_msg.header.stamp = rosTimeNow(); ros_msg.telegram_count = message.telegram_count; ros_msg.timestamp = message.timestamp; + ros_msg.source_id = message.source_id; ros_msg.x_velocity = message.x_velocity; ros_msg.y_velocity = message.y_velocity; ros_msg.angular_velocity = message.angular_velocity; @@ -127,6 +128,7 @@ namespace sick_lidar_localization ros_msg.header.stamp = rosTimeNow(); ros_msg.telegram_count = message.telegram_count; ros_msg.timestamp = message.timestamp; + ros_msg.source_id = message.source_id; ros_msg.x_position = message.x_position; ros_msg.y_position = message.y_position; ros_msg.heading = message.heading; @@ -144,6 +146,7 @@ namespace sick_lidar_localization ros_msg.header.stamp = rosTimeNow(); ros_msg.telegram_count = message.telegram_count; ros_msg.timestamp = message.timestamp; + ros_msg.source_id = message.source_id; ros_msg.code = message.code; ros_msg.distance = message.distance; ros_msg.sync_timestamp_sec = message.sync_timestamp_sec; @@ -160,6 +163,7 @@ namespace sick_lidar_localization ros_msg.header.stamp = rosTimeNow(); ros_msg.telegram_count = message.telegram_count; ros_msg.timestamp = message.timestamp; + ros_msg.source_id = message.source_id; ros_msg.num_lanes = message.num_lanes; ros_msg.lanes = message.lanes; ros_msg.sync_timestamp_sec = message.sync_timestamp_sec; @@ -176,6 +180,7 @@ namespace sick_lidar_localization ros_msg.header.stamp = rosTimeNow(); ros_msg.telegram_count = message.telegram_count; ros_msg.timestamp = message.timestamp; + ros_msg.source_id = message.source_id; ros_msg.lcp1 = message.lcp1; ros_msg.lcp2 = message.lcp2; ros_msg.lcp3 = message.lcp3; @@ -194,6 +199,7 @@ namespace sick_lidar_localization ros_msg.header.stamp = rosTimeNow(); ros_msg.telegram_count = message.telegram_count; ros_msg.timestamp = message.timestamp; + ros_msg.source_id = message.source_id; ros_msg.x = message.x; ros_msg.y = message.y; ros_msg.heading = message.heading; diff --git a/include/sick_lidar_localization/udp_receiver_thread.h b/include/sick_lidar_localization/udp_receiver_thread.h index 391b0a9..c1ad762 100644 --- a/include/sick_lidar_localization/udp_receiver_thread.h +++ b/include/sick_lidar_localization/udp_receiver_thread.h @@ -73,11 +73,11 @@ namespace sick_lidar_localization /* ** @brief Default constructor ** @param[in] services time sync services - ** @param[in] udp_ip_sim_output IP address for output UDP messages, or "" for broadcast (INADDR_ANY), default: "", use IP address of your local machine - ** @param[in] udp_port_sim_output UDP port of output messages, default: 5010 - ** @param[in] udp_sim_output_logfile Optional logfile for human readable UDP output messages, default: "" (no outputlogfile) + ** @param[in] udp_ip_lls_output IP address for output UDP messages, or "" for broadcast (INADDR_ANY), default: "", use IP address of your local machine + ** @param[in] udp_port_lls_output UDP port of output messages, default: 5010 + ** @param[in] udp_lls_output_logfile Optional logfile for human readable UDP output messages, default: "" (no outputlogfile) */ - UDPReceiverThread(sick_lidar_localization::SickServices* services = 0, const std::string& udp_ip_sim_output = "", int udp_port_sim_output = 5010, const std::string& udp_sim_output_logfile = ""); + UDPReceiverThread(sick_lidar_localization::SickServices* services = 0, const std::string& udp_ip_lls_output = "", int udp_port_lls_output = 5010, const std::string& udp_lls_output_logfile = ""); /* ** @brief Default destructor, exits running threads @@ -99,7 +99,7 @@ namespace sick_lidar_localization void stop(); /* - ** @brief Register a listener for upd messages. The callback functions of the listener will be called after receiving a new udp message. + ** @brief Register a listener for udp messages. The callback functions of the listener will be called after receiving a new udp message. ** Overwrite the functions defined in sick_lidar_localization::UDPMessage::Listener with customized code to handle udp messages. */ void registerListener(sick_lidar_localization::UDPMessage::Listener* listener); @@ -127,9 +127,9 @@ namespace sick_lidar_localization bool runReceiver(void); sick_lidar_localization::SickServices* m_services; // time sync services - std::string m_udp_ip_sim_output; // IP address for output UDP messages, or "" for broadcast (INADDR_ANY), default: "", use IP address of your local machine - int m_udp_port_sim_output; // UDP port of UDP output messages, default: 5010 - std::string m_udp_sim_output_logfile; // Optional logfile for human readable UDP output messages, default: "" (no outputlogfile) + std::string m_udp_ip_lls_output; // IP address for output UDP messages, or "" for broadcast (INADDR_ANY), default: "", use IP address of your local machine + int m_udp_port_lls_output; // UDP port of UDP output messages, default: 5010 + std::string m_udp_lls_output_logfile; // Optional logfile for human readable UDP output messages, default: "" (no outputlogfile) bool m_run_receiver_thread; // Flag to start and stop m_receiver_thread std::thread* m_receiver_thread; // Background thread to receive udp data, parsing, conversion and publishing std::list m_udp_message_listener; diff --git a/include/sick_lidar_localization/udp_sender.h b/include/sick_lidar_localization/udp_sender.h index 8137bbb..e23ffcd 100644 --- a/include/sick_lidar_localization/udp_sender.h +++ b/include/sick_lidar_localization/udp_sender.h @@ -59,19 +59,19 @@ #include "sick_lidar_localization/udp_message_parser.h" #if __ROS_VERSION == 1 -#include "sick_lidar_localization/OdometryMessage0101.h" #include "sick_lidar_localization/OdometryMessage0104.h" #include "sick_lidar_localization/OdometryMessage0105.h" #include "sick_lidar_localization/EncoderMeasurementMessage0202.h" #include "sick_lidar_localization/CodeMeasurementMessage0303.h" +#include "sick_lidar_localization/CodeMeasurementMessage0701.h" #include "sick_lidar_localization/LineMeasurementMessage0403.h" #include "sick_lidar_localization/LineMeasurementMessage0404.h" #elif __ROS_VERSION == 2 -#include "sick_lidar_localization/msg/odometry_message0101.hpp" #include "sick_lidar_localization/msg/odometry_message0104.hpp" #include "sick_lidar_localization/msg/odometry_message0105.hpp" #include "sick_lidar_localization/msg/encoder_measurement_message0202.hpp" #include "sick_lidar_localization/msg/code_measurement_message0303.hpp" +#include "sick_lidar_localization/msg/code_measurement_message0701.hpp" #include "sick_lidar_localization/msg/line_measurement_message0403.hpp" #include "sick_lidar_localization/msg/line_measurement_message0404.hpp" namespace sick_lidar_localization { using namespace msg; } @@ -84,6 +84,27 @@ namespace sick_lidar_localization */ class UDPSenderImpl; + /* + ** @brief class UDPDefaultInputSourceId provides default source ids for UDP input messages. + ** If the source id of a UDP input message is not set (source_id = 0, e.g. in case of ROS odometry messages), + ** the source id is taken from configuration file (launch file). + */ + class UDPDefaultInputSourceId + { + public: + /** default constructor */ + UDPDefaultInputSourceId(int default_source_id = 1, const std::map>& sourceid_map = std::map>()) + : m_default_source_id(default_source_id), m_msgtype_version_sourceid_map(sourceid_map) {} + + /** returns the default source id from configuration file */ + virtual int getSourceId(int msgtype, int msgversion); + + protected: + + int m_default_source_id; // default source id, if not otherwise set in a message or by configuration + std::map> m_msgtype_version_sourceid_map; // msgtype_version_sourceid_map[msgtype][msgversion] := default source_id for UDP input messages of type and version + }; + /* ** @brief class UDPSender implements a UDP sender for UDP input messages. */ @@ -94,17 +115,18 @@ namespace sick_lidar_localization /* ** @brief Default constructor ** @param[in] nh ros node handle (always 0 for native linus or windows) - ** @param[in] sim_ip_address UDP IP address, or "" for broadcast - ** @param[in] udp_port_sim_input UDP port of UDP input messages, default: 5009 - ** @param[in] udp_sim_input_source_id source_id of UDP input messages, default: 1 + ** @param[in] lls_ip_address UDP IP address, or "" for broadcast + ** @param[in] udp_port_lls_input UDP port of UDP input messages, default: 5009 + ** @param[in] source_id_cfg source_id map of UDP input messages, default source id: 1 ** @param[in] verbose print informational messages if verbose > 0, otherwise silent mode (error messages only) - ** @param[in] ros_odom_to_udp_msg Convert ros odom message to upd: 0 = map velocity to OdometryPayload0101 (Type 1, Version 1, LidarLoc 1), + ** @param[in] ros_odom_to_udp_msg Convert ros odom message to udp: ** 1 = map velocity to OdometryPayload0104 (Type 1, Version 4, LidarLoc 2), ** 2 = map position to OdometryPayload0105 (Type 1, Version 5, LidarLoc 2), ** 3 = map velocity to OdometryPayload0104 and position to OdometryPayload0105 */ - UDPSender(rosNodePtr nh = 0, const std::string& sim_ip_address = "192.168.0.1", int udp_port_sim_input = 5009, int udp_sim_input_source_id = 1, int verbose = 0, - const std::string& odom_topic = "/odom", int ros_odom_to_udp_msg = 3); + UDPSender(rosNodePtr nh = 0, const std::string& lls_ip_address = "192.168.0.1", int udp_port_lls_input = 5009, + const sick_lidar_localization::UDPDefaultInputSourceId& source_id_cfg = UDPDefaultInputSourceId(1), + int verbose = 0, const std::string& odom_topic = "/odom", int ros_odom_to_udp_msg = 3); /* ** @brief Default destructor, exits running threads @@ -114,18 +136,18 @@ namespace sick_lidar_localization /* ** @brief Sends a UDP input message. ** payload can be OdometryPayload0104, OdometryPayload0105, EncoderMeasurementPayload0202, - ** CodeMeasurementPayload0303, LineMeasurementPayload0403 or LineMeasurementPayload0404 + ** CodeMeasurementPayload0303, CodeMeasurementPayload0701, LineMeasurementPayload0403 or LineMeasurementPayload0404 ** @param[in] payload UDP message payload data ** @return true on success or false on error */ - template bool sendUDPPayload(const T& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id) + template bool sendUDPPayload(const T& payload, bool encode_header_big_endian, bool encode_payload_big_endian) { - std::vector message = sick_lidar_localization::UDPMessage::encodeMessage(payload, encode_header_big_endian, encode_payload_big_endian, source_id); + std::vector message = sick_lidar_localization::UDPMessage::encodeMessage(payload, encode_header_big_endian, encode_payload_big_endian, (uint16_t)(payload.source_id & 0xFFFF)); bool ok = sendData(message); if (!ok) - ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::sendUDPPayload(" << m_sim_ip_address << ":" << m_udp_port_sim_input << ", payload=" << sick_lidar_localization::UDPMessage::printPayload(payload) << ", header_big_endian=" << encode_header_big_endian << ", payload_big_endian=" << encode_payload_big_endian << ", source_id=" << source_id << ") failed"); + ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::sendUDPPayload(" << m_lls_ip_address << ":" << m_udp_port_lls_input << ", payload=" << sick_lidar_localization::UDPMessage::printPayload(payload) << ", header_big_endian=" << encode_header_big_endian << ", payload_big_endian=" << encode_payload_big_endian << ", source_id=" << payload.source_id << ") failed"); else if (ok && m_verbose) - ROS_INFO_STREAM("sick_lidar_localization::UDPSender: udp payload " << sick_lidar_localization::UDPMessage::printPayload(payload, false) << " sent to " << m_sim_ip_address << ":" << m_udp_port_sim_input); + ROS_INFO_STREAM("sick_lidar_localization::UDPSender: udp payload " << sick_lidar_localization::UDPMessage::printPayload(payload, false) << " sent to " << m_lls_ip_address << ":" << m_udp_port_lls_input); return ok; } @@ -149,11 +171,11 @@ namespace sick_lidar_localization */ bool sendData(const std::vector& udp_data); - std::string m_sim_ip_address; // UDP IP address, or "" for broadcast - int m_udp_port_sim_input; // UDP port of UDP input messages, default: 5009 - int m_source_id; // source_id of UDP input messages, default: 1 + std::string m_lls_ip_address; // UDP IP address, or "" for broadcast + int m_udp_port_lls_input; // UDP port of UDP input messages, default: 5009 + sick_lidar_localization::UDPDefaultInputSourceId m_source_id_cfg; // source_id map of UDP input messages, default source id: 1 int m_verbose; // Print informational messages if verbose > 0, otherwise silent mode (error messages only) - int m_ros_odom_to_udp_msg; // Convert ros odom message to upd + int m_ros_odom_to_udp_msg; // Convert ros odom message to udp UDPSenderImpl* m_udp_sender_impl; // UDP sender implementation hiding the udp socket details #if __ROS_VERSION > 0 @@ -161,9 +183,6 @@ namespace sick_lidar_localization void messageCbOdomROS(const ros_nav_msgs::Odometry& msg); void messageCbOdomROS2(const std::shared_ptr msg) { messageCbOdomROS(*msg); } - void messageCbOdometryMessage0101(const sick_lidar_localization::OdometryMessage0101 & msg); - void messageCbOdometryMessage0101ROS2(const std::shared_ptr msg) { messageCbOdometryMessage0101(*msg); } - void messageCbOdometryMessage0104(const sick_lidar_localization::OdometryMessage0104 & msg); void messageCbOdometryMessage0104ROS2(const std::shared_ptr msg) { messageCbOdometryMessage0104(*msg); } @@ -176,6 +195,9 @@ namespace sick_lidar_localization void messageCbCodeMeasurementMessage0303(const sick_lidar_localization::CodeMeasurementMessage0303 & msg); void messageCbCodeMeasurementMessage0303ROS2(const std::shared_ptr msg) { messageCbCodeMeasurementMessage0303(*msg); } + void messageCbCodeMeasurementMessage0701(const sick_lidar_localization::CodeMeasurementMessage0701 & msg); + void messageCbCodeMeasurementMessage0701ROS2(const std::shared_ptr msg) { messageCbCodeMeasurementMessage0701(*msg); } + void messageCbLineMeasurementMessage0403(const sick_lidar_localization::LineMeasurementMessage0403 & msg); void messageCbLineMeasurementMessage0403ROS2(const std::shared_ptr msg) { messageCbLineMeasurementMessage0403(*msg); } @@ -184,11 +206,11 @@ namespace sick_lidar_localization /** subscriber for ros messages, converted to udp messages and send to localization server */ rosSubscriber m_subOdomROS; - rosSubscriber m_subOdometryMessage0101; rosSubscriber m_subOdometryMessage0104; rosSubscriber m_subOdometryMessage0105; rosSubscriber m_subEncoderMeasurementMessage0202; rosSubscriber m_subCodeMeasurementMessage0303; + rosSubscriber m_subCodeMeasurementMessage0701; rosSubscriber m_subLineMeasurementMessage0403; rosSubscriber m_subLineMeasurementMessage0404; #endif diff --git a/launch/lls_transform.launch b/launch/lls_transform.launch new file mode 100644 index 0000000..c4c0f2a --- /dev/null +++ b/launch/lls_transform.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + \ No newline at end of file diff --git a/launch/pointcloud_converter.launch b/launch/pointcloud_converter.launch deleted file mode 100644 index 500b156..0000000 --- a/launch/pointcloud_converter.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/launch/sick_lidar_localization.launch b/launch/sick_lidar_localization.launch index 0cc3546..f81cd59 100644 --- a/launch/sick_lidar_localization.launch +++ b/launch/sick_lidar_localization.launch @@ -2,35 +2,44 @@ - + - - - - - - + + + + + + - - - - + + + + + - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + diff --git a/msg/CodeMeasurementMessage0303.msg b/msg/CodeMeasurementMessage0303.msg index 2f25ebb..fb702e7 100644 --- a/msg/CodeMeasurementMessage0303.msg +++ b/msg/CodeMeasurementMessage0303.msg @@ -2,4 +2,5 @@ std_msgs/Header header uint64 telegram_count # 8 byte TelegramCount uint64 uint64 timestamp # 8 byte Timestamp uint64 [microseconds] +int32 source_id # 4 byte source id int64 code # 8 byte Code int32 diff --git a/msg/CodeMeasurementMessage0304.msg b/msg/CodeMeasurementMessage0304.msg index 70fb92c..ef2d19f 100644 --- a/msg/CodeMeasurementMessage0304.msg +++ b/msg/CodeMeasurementMessage0304.msg @@ -2,6 +2,7 @@ std_msgs/Header header uint64 telegram_count # 8 byte TelegramCount uint64 uint64 timestamp # 8 byte Timestamp uint64 [microseconds] +int32 source_id # 4 byte source id int32 code # 4 byte Code int32 int32 distance # 4 byte Distance int32 [mm] diff --git a/msg/CodeMeasurementMessage0701.msg b/msg/CodeMeasurementMessage0701.msg new file mode 100644 index 0000000..b20f7e1 --- /dev/null +++ b/msg/CodeMeasurementMessage0701.msg @@ -0,0 +1,9 @@ +# Code measurement message type 7 version 1 (localization controller input message for 2D codes) +std_msgs/Header header +uint64 telegram_count # 8 byte TelegramCount uint64 +uint64 timestamp # 8 byte Timestamp uint64 [microseconds] +int32 source_id # 4 byte source id +string code # variable length string # udp message: 2 byte codelength (uint16) + byte string +int32 x_position # 4 byte relative pose along the x-axis of the sensor frame [mm] +int32 y_position # 4 byte relative pose along the y-axis of the sensor frame [mm] +int32 heading # 4 byte relative orientation of the code in the sensor frame [mdeg] diff --git a/msg/EncoderMeasurementMessage0202.msg b/msg/EncoderMeasurementMessage0202.msg index 8f025dc..48a84fc 100644 --- a/msg/EncoderMeasurementMessage0202.msg +++ b/msg/EncoderMeasurementMessage0202.msg @@ -2,4 +2,5 @@ std_msgs/Header header uint64 telegram_count # 8 byte TelegramCount uint64 uint64 timestamp # 8 byte Timestamp uint64 [microseconds] +int32 source_id # 4 byte source id int64 encoder_value # 8 byte EncoderValue int64 in tics diff --git a/msg/LineMeasurementMessage0403.msg b/msg/LineMeasurementMessage0403.msg index 59b8c3c..472220b 100644 --- a/msg/LineMeasurementMessage0403.msg +++ b/msg/LineMeasurementMessage0403.msg @@ -2,6 +2,7 @@ std_msgs/Header header uint64 telegram_count # 8 byte TelegramCount uint64 uint64 timestamp # 8 byte Timestamp uint64 [microseconds] +int32 source_id # 4 byte source id uint8 num_lanes # 1 byte NumOfLanes uint8 int16[] lanes # Lanes: num_lanes * 2 byte, each lane encoded by int16 diff --git a/msg/LineMeasurementMessage0404.msg b/msg/LineMeasurementMessage0404.msg index bfb6d54..f06c6dc 100644 --- a/msg/LineMeasurementMessage0404.msg +++ b/msg/LineMeasurementMessage0404.msg @@ -2,6 +2,7 @@ std_msgs/Header header uint64 telegram_count # 8 byte TelegramCount uint64 uint64 timestamp # 8 byte Timestamp uint64 [microseconds] +int32 source_id # 4 byte source id int16 lcp1 # 2 byte lcp1 int16 [mm] int16 lcp2 # 2 byte lcp2 int16 [mm] int16 lcp3 # 2 byte lcp3 int16 [mm] diff --git a/msg/LocalizationControllerResultMessage0502.msg b/msg/LocalizationControllerResultMessage0502.msg index cb408da..6d2eae8 100644 --- a/msg/LocalizationControllerResultMessage0502.msg +++ b/msg/LocalizationControllerResultMessage0502.msg @@ -2,6 +2,7 @@ std_msgs/Header header uint64 telegram_count # 8 byte TelegramCount uint64 uint64 timestamp # 8 byte Timestamp uint64 [microseconds] +int32 source_id # 4 byte source id int64 x # 8 byte X int64 [mm] int64 y # 8 byte Y int64 [mm] int32 heading # 4 byte Heading int32 [mdeg] diff --git a/msg/OdometryMessage0101.msg b/msg/OdometryMessage0101.msg deleted file mode 100644 index 23a660e..0000000 --- a/msg/OdometryMessage0101.msg +++ /dev/null @@ -1,7 +0,0 @@ -# Odometry message type 1 version 1 (localization controller input message) -std_msgs/Header header -uint64 telegram_count # 8 byte TelegramCount uint64 -uint64 timestamp # 8 byte Timestamp uint64 [milliseconds] -int32 x_velocity # 4 byte X - velocity int16 [mm/s] -int32 y_velocity # 4 byte Y - velocity int16 [mm/s] -int64 angular_velocity # 8 byte angular velocity int32 [mdeg/s] diff --git a/msg/OdometryMessage0104.msg b/msg/OdometryMessage0104.msg index 04ddff9..45210fb 100644 --- a/msg/OdometryMessage0104.msg +++ b/msg/OdometryMessage0104.msg @@ -2,6 +2,7 @@ std_msgs/Header header uint64 telegram_count # 8 byte TelegramCount uint64 uint64 timestamp # 8 byte Timestamp uint64 [microseconds] +int32 source_id # 4 byte source id int16 x_velocity # 2 byte X - velocity int16 [mm/s] int16 y_velocity # 2 byte Y - velocity int16 [mm/s] int32 angular_velocity # 4 byte angular velocity int32 [mdeg/s] diff --git a/msg/OdometryMessage0105.msg b/msg/OdometryMessage0105.msg index f7042a3..7d4f4b5 100644 --- a/msg/OdometryMessage0105.msg +++ b/msg/OdometryMessage0105.msg @@ -2,6 +2,7 @@ std_msgs/Header header uint64 telegram_count # 8 byte TelegramCount uint64 uint64 timestamp # 8 byte Timestamp uint64 [microseconds] +int32 source_id # 4 byte source id int64 x_position # 8 byte X-position int64 [mm] int64 y_position # 8 byte Y-position int64 [mm] int64 heading # 8 byte heading int32 [mdeg] diff --git a/src/gen_service_call.cpp b/src/gen_service_call.cpp index cd77966..3a657e0 100644 --- a/src/gen_service_call.cpp +++ b/src/gen_service_call.cpp @@ -76,7 +76,7 @@ static void printUsage(void) { std::cout << "gen_service_call sends requests to the localization server via http REST API and returns its response." << std::endl; std::cout << "Usage: gen_service_call [--hostname=] [--verbose=<0|1>] [--output=] [--append=] [-d=<0|1|2>" << std::endl; - std::cout << " : name of command (service request), f.e. LocIsSystemReady or LocStartLocalizing" << std::endl; + std::cout << " : name of command (service request), f.e. LocIsSystemReady or LocStart" << std::endl; std::cout << " : GET or POST" << std::endl; std::cout << " : parameter as json string, f.e. {}" << std::endl; std::cout << " --hostname=: ip address of the localization server, default: 192.168.0.1" << std::endl; @@ -225,14 +225,14 @@ int main(int argc, char** argv) response = sick_curl.send("LocSaveRingBufferRecording", "POST", "{\"data\":{\"reason\":\"YYYY-MM-DD_HH-MM-SS pose quality low\"}}"); // LocSaveRingBufferRecordingSrv.srv // "sMN LocSaveRingBufferRecording" // response: { "header": std_header_ok, "data": { "success": True } } response = sick_curl.send("LocSetKinematicVehicleModelActive", "POST", "{\"data\":{\"active\":true}}"); // LocSetKinematicVehicleModelActiveSrv.srv // "sMN LocSetKinematicVehicleModelActive" // response: { "header": std_header_ok, "data": { "success": True } } response = sick_curl.send("LocSetLinesForSupportActive", "POST", "{\"data\":{\"active\":true}}"); // LocSetLinesForSupportActiveSrv.srv // "sMN LocSetLinesForSupportActive" // response: { "header": std_header_ok, "data": { "success": True } } - response = sick_curl.send("LocSetMap", "POST", "{\"data\":{\"mappath\":\"test.vmap\"}}"); // LocSetMapSrv.srv // "sMN LocSetMap" // response: { "header": std_header_ok, "data": { "success": True } } + response = sick_curl.send("LocSetMap", "POST", "{\"data\":{\"mappath\":\"test.vmap\"}}"); // LocSetMapSrv.srv // "sMN LocSetMap" // response: { "header": std_header_ok, "data": { "success": True } } response = sick_curl.send("LocSetMappingActive", "POST", "{\"data\":{\"active\":true}}"); // LocSetMappingActiveSrv.srv // "sMN LocSetMappingActive" // response: { "header": std_header_ok, "data": { "success": True } } response = sick_curl.send("LocSetOdometryActive", "POST", "{\"data\":{\"active\":true}}"); // LocSetOdometryActiveSrv.srv // "sMN LocSetOdometryActive" // response: { "header": std_header_ok, "data": { "success": True } } response = sick_curl.send("LocSetRecordingActive", "POST", "{\"data\":{\"active\":true}}"); // LocSetRecordingActiveSrv.srv // "sMN LocSetRecordingActive" // response: { "header": std_header_ok, "data": { "success": True } } response = sick_curl.send("LocSetRingBufferRecordingActive", "POST", "{\"data\":{\"active\":true}}"); // LocSetRingBufferRecordingActiveSrv.srv // "sMN LocSetRingBufferRecordingActive" // response: { "header": std_header_ok, "data": { "success": True } } - response = sick_curl.send("LocStartLocalizing", "POST", "{}"); // LocStartLocalizingSrv.srv // "sMN LocStartLocalizing" // json response: { "header": { "status": 0, "message": "Ok" }, "data": { "success": True } } + response = sick_curl.send("LocStart", "POST", "{}"); // LocStartSrv.srv // "sMN LocStart" // json response: { "header": { "status": 0, "message": "Ok" }, "data": { "success": True } } response = sick_curl.send("LocStop", "POST", "{}"); // LocStopSrv.srv // "sMN LocStop" // json response: { "header": { "status": 0, "message": "Ok" }, "data": { "success": True } } - response = sick_curl.send("LocSwitchMap", "POST", "{\"data\":{\"submapname\":\"test.vmap\"}}"); // LocSwitchMapSrv.srv // "sMN LocSwitchMap" // response: { "header": std_header_ok, "data": { "success": True } } + response = sick_curl.send("LocSwitchMap", "POST", "{\"data\":{\"submapname\":\"test.vmap\"}}"); // LocSwitchMapSrv.srv // "sMN LocSwitchMap" // response: { "header": std_header_ok, "data": { "success": True } } */ return EXIT_SUCCESS; diff --git a/src/pointcloud_converter.cpp b/src/lls_transform.cpp similarity index 75% rename from src/pointcloud_converter.cpp rename to src/lls_transform.cpp index efd417e..b2f74a3 100644 --- a/src/pointcloud_converter.cpp +++ b/src/lls_transform.cpp @@ -1,5 +1,5 @@ /* - * @brief pointcloud_converter subscribes to sim_loc_driver messages (type sick_lidar_localization::LocResultPortTelegramMsg), + * @brief lls_transform subscribes to lls_loc_driver messages (type sick_lidar_localization::LocResultPortTelegramMsg), * converts them to PointCloud2 and publishes PointCloud2 messages on topic "/cloud". * * It also serves as an usage example for sick_lidar_localization and shows how to subscribe and use the @@ -57,14 +57,16 @@ * */ #include "sick_lidar_localization/sick_common.h" - -#include "sick_lidar_localization/pointcloud_converter.h" +#include "sick_lidar_localization/lls_transform.h" +#if __ROS_VERSION == 2 +#include "launchparser/launchparser.h" +#endif int main(int argc, char** argv) { // Ros configuration and initialization #if __ROS_VERSION == 1 - ros::init(argc, argv, "pointcloud_converter", ros::init_options::NoSigintHandler); + ros::init(argc, argv, "lls_transform", ros::init_options::NoSigintHandler); ros::NodeHandle node("~"); rosNodePtr nh = &node; #elif __ROS_VERSION == 2 @@ -72,44 +74,48 @@ int main(int argc, char** argv) rclcpp::NodeOptions node_options; node_options.allow_undeclared_parameters(true); //node_options.automatically_declare_initial_parameters(true); - rosNodePtr nh = rclcpp::Node::make_shared("pointcloud_converter", "", node_options); - //parseLaunchfileSetParameter(nh, argc, argv); + rosNodePtr nh = rclcpp::Node::make_shared("lls_transform", "", node_options); + if (!LaunchParser::parseLaunchfileSetParameter(nh, argc, argv)) + { + ROS_ERROR_STREAM("## ERROR sick_lidar_localization: parseLaunchfileSetParameter() failed, aborting... "); + return false; + } #else #error __ROS_VERSION not defined, define __ROS_VERSION 1 or 2 #endif - ROS_INFO_STREAM("pointcloud_converter started."); + ROS_INFO_STREAM("lls_transform started."); std::string result_telegrams_topic = "/localizationcontroller/out/localizationcontroller_result_message_0502"; // default topic to publish result port telegram messages (type LocResultPortTelegramMsg) //rosGetParam(nh, "result_telegrams_topic", result_telegrams_topic); - // Init verifier to compare and check sim_loc_driver and sim_loc_test_server messages - sick_lidar_localization::PointCloudConverter pointcloud_converter(nh); + // Init verifier to compare and check lls_loc_driver and lls_loc_test_server messages + sick_lidar_localization::LLSTransformer lls_transform(nh); - // Subscribe to sim_loc_driver messages + // Subscribe to lls_loc_driver messages #if __ROS_VERSION == 1 rosSubscriber result_telegram_subscriber = rosSubscribe(nh, - result_telegrams_topic, &sick_lidar_localization::PointCloudConverter::messageCbResultPortTelegrams, &pointcloud_converter); + result_telegrams_topic, &sick_lidar_localization::LLSTransformer::messageCbResultPortTelegrams, &lls_transform); #elif __ROS_VERSION == 2 # ifdef _MSC_VER - auto subscriber = nh->create_subscription(result_telegrams_topic, 10, std::bind(&sick_lidar_localization::PointCloudConverter::messageCbResultPortTelegramsROS2, &pointcloud_converter, std::placeholders::_1)); + auto subscriber = nh->create_subscription(result_telegrams_topic, 10, std::bind(&sick_lidar_localization::LLSTransformer::messageCbResultPortTelegramsROS2, &lls_transform, std::placeholders::_1)); rosSubscriber result_telegram_subscriber = rosSubscriber(subscriber); # else rosSubscriber result_telegram_subscriber = rosSubscribe(nh, - result_telegrams_topic, &sick_lidar_localization::PointCloudConverter::messageCbResultPortTelegramsROS2, &pointcloud_converter); + result_telegrams_topic, &sick_lidar_localization::LLSTransformer::messageCbResultPortTelegramsROS2, &lls_transform); #endif #endif // Start pointcloud converter thread - pointcloud_converter.start(); + lls_transform.start(); // Run ros event loop rosSpin(nh); - std::cout << "pointcloud_converter finished." << std::endl; - ROS_INFO_STREAM("pointcloud_converter finished."); - pointcloud_converter.stop(); - std::cout << "pointcloud_converter exits." << std::endl; - ROS_INFO_STREAM("pointcloud_converter exits."); + std::cout << "lls_transform finished." << std::endl; + ROS_INFO_STREAM("lls_transform finished."); + lls_transform.stop(); + std::cout << "lls_transform exits." << std::endl; + ROS_INFO_STREAM("lls_transform exits."); return 0; } diff --git a/src/lls_transform_thread.cpp b/src/lls_transform_thread.cpp new file mode 100644 index 0000000..c2f9dc0 --- /dev/null +++ b/src/lls_transform_thread.cpp @@ -0,0 +1,354 @@ +/* + * @brief lls_loc_pointcloud_converts lls_loc_driver messages (type sick_lidar_localization::LocResultPortTelegramMsg), + * to PointCloud2 messages and publishes PointCloud2 messages on topic "/cloud". + * + * The vehicle poses (PoseX, PoseY, PoseYaw of result port telegrams) are transformed and published + * by tf-messages with configurable parent and child frame id. + * + * It also serves as an usage example for sick_lidar_localization and shows how to use sick_lidar_localization + * in a custumized application. + * + * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2019 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 + * + * Copyright 2019 SICK AG + * Copyright 2019 Ing.-Buero Dr. Michael Lehning + * + */ +#include + +#include "sick_lidar_localization/sick_common.h" +#include "sick_lidar_localization/lls_transform.h" + +/*! + * Constructor + * @param[in] nh ros node handle + */ +sick_lidar_localization::LLSTransformer::LLSTransformer(rosNodePtr nh) +: m_nh(nh), m_converter_thread_running(false), m_converter_thread(0) +{ + if(nh) + { + m_tf_parent_frame_id = "map"; + m_tf_child_frame_id = "lls"; + rosGetParam(nh, "tf_parent_frame_id", m_tf_parent_frame_id); + rosGetParam(nh, "tf_child_frame_id", m_tf_child_frame_id); + // m_marker_frame_id = "map"; + // std::string marker_topic = "lls/marker"; + // rosGetParam(nh, "marker_topic", marker_topic); + // rosGetParam(nh, "marker_frame_id", m_marker_frame_id); + // m_marker_arrow_length = 1.0; + // m_marker_arrow_width = 0.1; + // m_marker_arrow_height = 0.1; + // m_marker_color_r = 1.0; + // m_marker_color_g = 1.0; + // m_marker_color_b = 1.0; + // m_marker_color_a = 0.5; + // rosGetParam(nh, "marker_arrow_length", m_marker_arrow_length); + // rosGetParam(nh, "marker_arrow_width", m_marker_arrow_width); + // rosGetParam(nh, "marker_arrow_height", m_marker_arrow_height); + // rosGetParam(nh, "marker_color_r", m_marker_color_r); + // rosGetParam(nh, "marker_color_g", m_marker_color_g); + // rosGetParam(nh, "marker_color_b", m_marker_color_b); + // rosGetParam(nh, "marker_color_a", m_marker_color_a); + // m_marker_publisher = rosAdvertise(nh, marker_topic); + // m_point_cloud_publisher = rosAdvertise(nh, "/map"); + } +} + +/*! + * Destructor + */ +sick_lidar_localization::LLSTransformer::~LLSTransformer() +{ + stop(); +} + +/*! + * Starts the converter thread, converts telegrams and publishes PointCloud2 messages. + * @return true on success, false on failure. + */ +bool sick_lidar_localization::LLSTransformer::start(void) +{ + m_converter_thread_running = true; + m_converter_thread = new std::thread(&sick_lidar_localization::LLSTransformer::runLLSTransformerThreadCb, this); + return true; +} + +/*! + * Stops the converter thread. + * @return true on success, false on failure. + */ +bool sick_lidar_localization::LLSTransformer::stop(void) +{ + m_converter_thread_running = false; + if(m_converter_thread) + { + sick_lidar_localization::LocalizationControllerResultMessage0502 empty_msg; + m_result_port_telegram_fifo.push(empty_msg); // push empty telegram to interrupt converter thread waiting for notification + m_converter_thread->join(); + delete(m_converter_thread); + m_converter_thread = 0; + } + return true; +} + +/*! + * Callback for result telegram messages (LocResultPortTelegramMsg) from lls_loc_driver. + * The received message is buffered by fifo m_result_port_telegram_fifo. + * Message handling and evaluation is done in the converter thread. + * @param[in] msg result telegram message (LocResultPortTelegramMsg) + */ +void sick_lidar_localization::LLSTransformer::messageCbResultPortTelegrams(const sick_lidar_localization::LocalizationControllerResultMessage0502 & msg) +{ + m_result_port_telegram_fifo.push(msg); +} + +/*! + * Creates and returns a list of 4 points from position and orientaion of a sensor: + * the center point (PoseX,PoseY) and 3 corner points of a triangle showing the orientation (PoseYaw) + * Note: This is just an example to show the position and orientation of a sensor by a list of points. + * @param[in] posx x-position of sensor in meter + * @param[in] posy y-position of sensor in meter + * @param[in] yaw orientation (yaw) of sensor in radians + * @param[in] triangle_height heigt of the virtual triangle in meter + * @return list of 3D points + */ +// std::vector sick_lidar_localization::LLSTransformer::poseToDemoPoints(double posx, double posy, double yaw, double triangle_height) +// { +// // Center point at (posx, posy) +// std::vector points; +// ros_geometry_msgs::Point centerpoint; +// centerpoint.x = posx; +// centerpoint.y = posy; +// centerpoint.z = 0; +// points.push_back(centerpoint); +// ros_geometry_msgs::Point cornerpoint = centerpoint; +// // Add triangle corner points at (posx + dx, posy + dy) +// cornerpoint.x = centerpoint.x + triangle_height * std::cos(yaw); +// cornerpoint.y = centerpoint.y + triangle_height * std::sin(yaw); +// points.push_back(cornerpoint); +// cornerpoint.x = centerpoint.x + 0.5 * triangle_height * std::cos(yaw + M_PI_2); +// cornerpoint.y = centerpoint.y + 0.5 * triangle_height * std::sin(yaw + M_PI_2); +// points.push_back(cornerpoint); +// cornerpoint.x = centerpoint.x + 0.5 * triangle_height * std::cos(yaw - M_PI_2); +// cornerpoint.y = centerpoint.y + 0.5 * triangle_height * std::sin(yaw - M_PI_2); +// points.push_back(cornerpoint); +// return points; +// } + +/*! + * Converts the vehicle position from a result port telegram to PointCloud2 message with 4 points + * (centerpoint plus 3 demo corner points showing a triangle). + * @param[in] msg result telegram message (LocResultPortTelegramMsg) + * @return PointCloud2 message + */ +// ros_sensor_msgs::PointCloud2 sick_lidar_localization::LLSTransformer::convertToPointCloud(const sick_lidar_localization::LocalizationControllerResultMessage0502 & msg) +// { +// ros_sensor_msgs::PointCloud2 pointcloud_msg; +// // Create center point (PoseX,PoseY) and 3 corner points of a triangle showing the orientation (PoseYaw) +// // Note: This is just an example to demonstrate use of sick_lidar_localization and PointCloud2 messages! +// std::vector points = poseToDemoPoints( +// 1.0e-3 * msg.x, // x-position in meter +// 1.0e-3 * msg.y, // y-position in meter +// 1.0e-3 * msg.heading * M_PI / 180.0, // yaw angle in radians +// 0.6); // triangle_height in meter (demo only) +// // set pointcloud header +// pointcloud_msg.header.stamp = msg.header.stamp; // telegram timestamp +// if(msg.sync_timestamp_valid) // software pll initialized: use system time of vehicle pose calculated from lidar ticks by software pll +// { +// pointcloud_msg.header.stamp.sec = msg.sync_timestamp_sec; +// pointcloud_msg.header.stamp.NSEC = msg.sync_timestamp_nsec; +// } +// pointcloud_msg.header.frame_id = "map"; +// // pointcloud_msg.header.seq = 0; // no seq field in ROS2 +// // clear cloud data +// pointcloud_msg.height = 0; +// pointcloud_msg.width = 0; +// pointcloud_msg.data.clear(); +// // set pointcloud field properties +// int numChannels = 3; // "x", "y", "z" +// std::string channelId[] = { "x", "y", "z" }; +// pointcloud_msg.height = 1; +// pointcloud_msg.width = points.size(); // normally we have 4 points (center point and 3 corner points) +// 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 = ros_sensor_msgs::PointField::FLOAT32; +// } +// // set pointcloud data values +// pointcloud_msg.data.resize(pointcloud_msg.row_step * pointcloud_msg.height); +// float* pfdata = reinterpret_cast(&pointcloud_msg.data[0]); +// for(size_t data_cnt = 0, point_cnt = 0; point_cnt < points.size(); point_cnt++) +// { +// pfdata[data_cnt++] = static_cast(points[point_cnt].x); +// pfdata[data_cnt++] = static_cast(points[point_cnt].y); +// pfdata[data_cnt++] = static_cast(points[point_cnt].z); +// } +// return pointcloud_msg; +// } + +/*! + * Converts the vehicle pose from a result port telegram to a marker array. + * @param[in] msg result telegram message (LocResultPortTelegramMsg) + * @return marker array + */ +// ros_visualization_msgs::MarkerArray sick_lidar_localization::LLSTransformer::convertToMarker(const sick_lidar_localization::LocalizationControllerResultMessage0502 & msg) +// { +// double posx = 1.0e-3 * msg.x; // x-position in meter +// double posy = 1.0e-3 * msg.y; // y-position in meter +// double yaw = 1.0e-3 * msg.heading * M_PI / 180.0; // yaw angle in radians +// ros_visualization_msgs::Marker marker_msg; +// marker_msg.header.stamp = msg.header.stamp; // telegram timestamp +// if(msg.sync_timestamp_valid) // software pll initialized: use system time of vehicle pose calculated from lidar ticks by software pll +// { +// marker_msg.header.stamp.sec = msg.sync_timestamp_sec; +// marker_msg.header.stamp.NSEC = msg.sync_timestamp_nsec; +// } +// marker_msg.header.frame_id = m_marker_frame_id; +// marker_msg.ns = "lls"; +// marker_msg.id = 1; +// marker_msg.type = ros_visualization_msgs::Marker::ARROW; +// marker_msg.scale.x = m_marker_arrow_length; // arrow length +// marker_msg.scale.y = m_marker_arrow_width; // arrow width +// marker_msg.scale.z = m_marker_arrow_height; // arrow height +// marker_msg.pose.position.x = posx; +// marker_msg.pose.position.y = posy; +// marker_msg.pose.position.z = 0.0; +// tf2::Quaternion q; +// q.setRPY(0, 0, yaw); +// marker_msg.pose.orientation.x = q.x(); +// marker_msg.pose.orientation.y = q.y(); +// marker_msg.pose.orientation.z = q.z(); +// marker_msg.pose.orientation.w = q.w(); +// marker_msg.action = ros_visualization_msgs::Marker::ADD; // note: ADD == MODIFY +// marker_msg.color.r = m_marker_color_r; +// marker_msg.color.g = m_marker_color_g; +// marker_msg.color.b = m_marker_color_b; +// marker_msg.color.a = m_marker_color_a; +// marker_msg.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever +// ros_visualization_msgs::MarkerArray marker_array; +// marker_array.markers.push_back(marker_msg); +// return marker_array; +// } + +/*! + * Converts the vehicle pose from a result port telegram to a tf transform. + * @param[in] msg result telegram message (LocResultPortTelegramMsg) + * @return tf transform + */ +ros_geometry_msgs::TransformStamped sick_lidar_localization::LLSTransformer::convertToTransform(const sick_lidar_localization::LocalizationControllerResultMessage0502 & msg) +{ + double posx = 1.0e-3 * msg.x; // x-position in meter + double posy = 1.0e-3 * msg.y; // y-position in meter + double yaw = 1.0e-3 * msg.heading * M_PI / 180.0; // yaw angle in radians + ros_geometry_msgs::TransformStamped vehicle_transform; + vehicle_transform.header.stamp = msg.header.stamp; // telegram timestamp + if(msg.sync_timestamp_valid) // software pll initialized: use system time of vehicle pose calculated from lidar ticks by software pll + { + vehicle_transform.header.stamp.sec = msg.sync_timestamp_sec; + vehicle_transform.header.stamp.NSEC = msg.sync_timestamp_nsec; + } + vehicle_transform.header.frame_id = m_tf_parent_frame_id; + vehicle_transform.child_frame_id = m_tf_child_frame_id; + vehicle_transform.transform.translation.x = posx; + vehicle_transform.transform.translation.y = posy; + vehicle_transform.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + vehicle_transform.transform.rotation.x = q.x(); + vehicle_transform.transform.rotation.y = q.y(); + vehicle_transform.transform.rotation.z = q.z(); + vehicle_transform.transform.rotation.w = q.w(); + return vehicle_transform; +} + +/*! + * Thread callback, pops received telegrams from the fifo buffer m_result_port_telegram_fifo, + * converts the telegrams from type LocResultPortTelegramMsg to PointCloud2 and publishes + * PointCloud2 messages. The vehicle pose is converted to a tf transform and broadcasted. + */ +void sick_lidar_localization::LLSTransformer::runLLSTransformerThreadCb(void) +{ + ROS_INFO_STREAM("LLSTransformer: converter thread for lls_loc_driver messages started"); +#if defined __ROS_VERSION && __ROS_VERSION == 1 + tf2_ros::TransformBroadcaster tf_broadcaster; +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + tf2_ros::TransformBroadcaster tf_broadcaster(m_nh); +#endif + + while(rosOk() && m_converter_thread_running) + { + // Wait for next telegram + while(rosOk() && m_converter_thread_running && m_result_port_telegram_fifo.empty()) + { + rosSleep(0.0001); + m_result_port_telegram_fifo.waitForElement(); + } + if(rosOk() && m_converter_thread_running && !m_result_port_telegram_fifo.empty()) + { + sick_lidar_localization::LocalizationControllerResultMessage0502 telegram = m_result_port_telegram_fifo.pop(); + // Convert vehicle position from result telegram to PointCloud2 + // ros_sensor_msgs::PointCloud2 pointcloud_msg = convertToPointCloud(telegram); + // rosPublish(m_point_cloud_publisher, pointcloud_msg); + // Convert vehicle position from result telegram to marker + // ros_visualization_msgs::MarkerArray marker_msg = convertToMarker(telegram); + // rosPublish(m_marker_publisher, marker_msg); + // Convert vehicle pose from result telegram to tf transform + ros_geometry_msgs::TransformStamped tf2_vehicle_transform = convertToTransform(telegram); + tf_broadcaster.sendTransform(tf2_vehicle_transform); + } + } + ROS_INFO_STREAM("LLSTransformer: converter thread for lls_loc_driver messages finished"); +} diff --git a/src/pointcloud_converter_thread.cpp b/src/pointcloud_converter_thread.cpp deleted file mode 100644 index 2170525..0000000 --- a/src/pointcloud_converter_thread.cpp +++ /dev/null @@ -1,292 +0,0 @@ -/* - * @brief sim_loc_pointcloud_converts sim_loc_driver messages (type sick_lidar_localization::LocResultPortTelegramMsg), - * to PointCloud2 messages and publishes PointCloud2 messages on topic "/cloud". - * - * The vehicle poses (PoseX, PoseY, PoseYaw of result port telegrams) are transformed and published - * by tf-messages with configurable parent and child frame id. - * - * It also serves as an usage example for sick_lidar_localization and shows how to use sick_lidar_localization - * in a custumized application. - * - * Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim - * Copyright (C) 2019 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 - * - * Copyright 2019 SICK AG - * Copyright 2019 Ing.-Buero Dr. Michael Lehning - * - */ -#include - -#include "sick_lidar_localization/sick_common.h" -#include "sick_lidar_localization/pointcloud_converter.h" - -/*! - * Constructor - * @param[in] nh ros node handle - */ -sick_lidar_localization::PointCloudConverter::PointCloudConverter(rosNodePtr nh) -: m_nh(nh), m_point_cloud_frame_id("sick_lidar_localization"), m_converter_thread_running(false), m_converter_thread(0) -{ - if(nh) - { - std::string point_cloud_topic = "/cloud"; // default topic to publish result port telegram messages (type LocResultPortTelegramMsg) - m_point_cloud_frame_id = "cloud"; - m_tf_parent_frame_id = "cloud"; - m_tf_child_frame_id = "sick_lidar_localization"; - //rosGetParam(nh, "point_cloud_topic", point_cloud_topic); - //rosGetParam(nh, "point_cloud_frame_id", m_point_cloud_frame_id); - //rosGetParam(nh, "tf_parent_frame_id", m_tf_parent_frame_id); - //rosGetParam(nh, "tf_child_frame_id", m_tf_child_frame_id); - m_point_cloud_publisher = rosAdvertise(nh, point_cloud_topic); - } -} - -/*! - * Destructor - */ -sick_lidar_localization::PointCloudConverter::~PointCloudConverter() -{ - stop(); -} - -/*! - * Starts the converter thread, converts telegrams and publishes PointCloud2 messages. - * @return true on success, false on failure. - */ -bool sick_lidar_localization::PointCloudConverter::start(void) -{ - m_converter_thread_running = true; - m_converter_thread = new std::thread(&sick_lidar_localization::PointCloudConverter::runPointCloudConverterThreadCb, this); - return true; -} - -/*! - * Stops the converter thread. - * @return true on success, false on failure. - */ -bool sick_lidar_localization::PointCloudConverter::stop(void) -{ - m_converter_thread_running = false; - if(m_converter_thread) - { - sick_lidar_localization::LocalizationControllerResultMessage0502 empty_msg; - m_result_port_telegram_fifo.push(empty_msg); // push empty telegram to interrupt converter thread waiting for notification - m_converter_thread->join(); - delete(m_converter_thread); - m_converter_thread = 0; - } - return true; -} - -/*! - * Callback for result telegram messages (LocResultPortTelegramMsg) from sim_loc_driver. - * The received message is buffered by fifo m_result_port_telegram_fifo. - * Message handling and evaluation is done in the converter thread. - * @param[in] msg result telegram message (LocResultPortTelegramMsg) - */ -void sick_lidar_localization::PointCloudConverter::messageCbResultPortTelegrams(const sick_lidar_localization::LocalizationControllerResultMessage0502 & msg) -{ - m_result_port_telegram_fifo.push(msg); -} - -/*! - * Creates and returns a list of 4 points from position and orientaion of a sensor: - * the center point (PoseX,PoseY) and 3 corner points of a triangle showing the orientation (PoseYaw) - * Note: This is just an example to show the position and orientation of a sensor by a list of points. - * @param[in] posx x-position of sensor in meter - * @param[in] posy y-position of sensor in meter - * @param[in] yaw orientation (yaw) of sensor in radians - * @param[in] triangle_height heigt of the virtual triangle in meter - * @return list of 3D points - */ -std::vector sick_lidar_localization::PointCloudConverter::poseToDemoPoints(double posx, double posy, double yaw, double triangle_height) -{ - // Center point at (posx, posy) - std::vector points; - ros_geometry_msgs::Point centerpoint; - centerpoint.x = posx; - centerpoint.y = posy; - centerpoint.z = 0; - points.push_back(centerpoint); - ros_geometry_msgs::Point cornerpoint = centerpoint; - // Add triangle corner points at (posx + dx, posy + dy) - cornerpoint.x = centerpoint.x + triangle_height * std::cos(yaw); - cornerpoint.y = centerpoint.y + triangle_height * std::sin(yaw); - points.push_back(cornerpoint); - cornerpoint.x = centerpoint.x + 0.5 * triangle_height * std::cos(yaw + M_PI_2); - cornerpoint.y = centerpoint.y + 0.5 * triangle_height * std::sin(yaw + M_PI_2); - points.push_back(cornerpoint); - cornerpoint.x = centerpoint.x + 0.5 * triangle_height * std::cos(yaw - M_PI_2); - cornerpoint.y = centerpoint.y + 0.5 * triangle_height * std::sin(yaw - M_PI_2); - points.push_back(cornerpoint); - return points; -} - -/*! - * Converts the vehicle position from a result port telegram to PointCloud2 message with 4 points - * (centerpoint plus 3 demo corner points showing a triangle). - * @param[in] msg result telegram message (LocResultPortTelegramMsg) - * @return PointCloud2 message - */ -ros_sensor_msgs::PointCloud2 sick_lidar_localization::PointCloudConverter::convertToPointCloud(const sick_lidar_localization::LocalizationControllerResultMessage0502 & msg) -{ - ros_sensor_msgs::PointCloud2 pointcloud_msg; - // Create center point (PoseX,PoseY) and 3 corner points of a triangle showing the orientation (PoseYaw) - // Note: This is just an example to demonstrate use of sick_lidar_localization and PointCloud2 messages! - std::vector points = poseToDemoPoints( - 1.0e-3 * msg.x, // x-position in meter - 1.0e-3 * msg.y, // y-position in meter - 1.0e-3 * msg.heading * M_PI / 180.0, // yaw angle in radians - 0.6); // triangle_height in meter (demo only) - // set pointcloud header - pointcloud_msg.header.stamp = msg.header.stamp; // telegram timestamp - if(msg.sync_timestamp_valid) // software pll initialized: use system time of vehicle pose calculated from lidar ticks by software pll - { - pointcloud_msg.header.stamp.sec = msg.sync_timestamp_sec; - pointcloud_msg.header.stamp.NSEC = msg.sync_timestamp_nsec; - } - pointcloud_msg.header.frame_id = m_point_cloud_frame_id; - // pointcloud_msg.header.seq = 0; // no seq field in ROS2 - // clear cloud data - pointcloud_msg.height = 0; - pointcloud_msg.width = 0; - pointcloud_msg.data.clear(); - // set pointcloud field properties - int numChannels = 3; // "x", "y", "z" - std::string channelId[] = { "x", "y", "z" }; - pointcloud_msg.height = 1; - pointcloud_msg.width = points.size(); // normally we have 4 points (center point and 3 corner points) - 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 = ros_sensor_msgs::PointField::FLOAT32; - } - // set pointcloud data values - pointcloud_msg.data.resize(pointcloud_msg.row_step * pointcloud_msg.height); - float* pfdata = reinterpret_cast(&pointcloud_msg.data[0]); - for(size_t data_cnt = 0, point_cnt = 0; point_cnt < points.size(); point_cnt++) - { - pfdata[data_cnt++] = static_cast(points[point_cnt].x); - pfdata[data_cnt++] = static_cast(points[point_cnt].y); - pfdata[data_cnt++] = static_cast(points[point_cnt].z); - } - return pointcloud_msg; -} - -/*! - * Converts the vehicle pose from a result port telegram to a tf transform. - * @param[in] msg result telegram message (LocResultPortTelegramMsg) - * @return tf transform - */ -ros_geometry_msgs::TransformStamped sick_lidar_localization::PointCloudConverter::convertToTransform(const sick_lidar_localization::LocalizationControllerResultMessage0502 & msg) -{ - double posx = 1.0e-3 * msg.x; // x-position in meter - double posy = 1.0e-3 * msg.y; // y-position in meter - double yaw = 1.0e-3 * msg.heading * M_PI / 180.0; // yaw angle in radians - ros_geometry_msgs::TransformStamped vehicle_transform; - vehicle_transform.header.stamp = msg.header.stamp; // telegram timestamp - if(msg.sync_timestamp_valid) // software pll initialized: use system time of vehicle pose calculated from lidar ticks by software pll - { - vehicle_transform.header.stamp.sec = msg.sync_timestamp_sec; - vehicle_transform.header.stamp.NSEC = msg.sync_timestamp_nsec; - } - vehicle_transform.header.frame_id = m_tf_parent_frame_id; - vehicle_transform.child_frame_id = m_tf_child_frame_id; - vehicle_transform.transform.translation.x = posx; - vehicle_transform.transform.translation.y = posy; - vehicle_transform.transform.translation.z = 0.0; - tf2::Quaternion q; - q.setRPY(0, 0, yaw); - vehicle_transform.transform.rotation.x = q.x(); - vehicle_transform.transform.rotation.y = q.y(); - vehicle_transform.transform.rotation.z = q.z(); - vehicle_transform.transform.rotation.w = q.w(); - return vehicle_transform; -} - -/*! - * Thread callback, pops received telegrams from the fifo buffer m_result_port_telegram_fifo, - * converts the telegrams from type LocResultPortTelegramMsg to PointCloud2 and publishes - * PointCloud2 messages. The vehicle pose is converted to a tf transform and broadcasted. - */ -void sick_lidar_localization::PointCloudConverter::runPointCloudConverterThreadCb(void) -{ - ROS_INFO_STREAM("PointCloudConverter: converter thread for sim_loc_driver messages started"); -#if defined __ROS_VERSION && __ROS_VERSION == 1 - tf2_ros::TransformBroadcaster tf_broadcaster; -#elif defined __ROS_VERSION && __ROS_VERSION == 2 - tf2_ros::TransformBroadcaster tf_broadcaster(m_nh); -#endif - - while(rosOk() && m_converter_thread_running) - { - // Wait for next telegram - while(rosOk() && m_converter_thread_running && m_result_port_telegram_fifo.empty()) - { - rosSleep(0.0001); - m_result_port_telegram_fifo.waitForElement(); - } - if(rosOk() && m_converter_thread_running && !m_result_port_telegram_fifo.empty()) - { - sick_lidar_localization::LocalizationControllerResultMessage0502 telegram = m_result_port_telegram_fifo.pop(); - // Convert vehicle position from result telegram to PointCloud2 - ros_sensor_msgs::PointCloud2 pointcloud_msg = convertToPointCloud(telegram); - rosPublish(m_point_cloud_publisher, pointcloud_msg); - // Convert vehicle pose from result telegram to tf transform - ros_geometry_msgs::TransformStamped tf2_vehicle_transform = convertToTransform(telegram); - tf_broadcaster.sendTransform(tf2_vehicle_transform); - } - } - ROS_INFO_STREAM("PointCloudConverter: converter thread for sim_loc_driver messages finished"); -} diff --git a/src/sick_lidar_localization.cpp b/src/sick_lidar_localization.cpp index 524521b..1d0c4b4 100644 --- a/src/sick_lidar_localization.cpp +++ b/src/sick_lidar_localization.cpp @@ -74,6 +74,15 @@ template static bool getRequiredParam(rosNodePtr nh, const std::str return true; } +template static bool getOptionalParam(rosNodePtr nh, const std::string& param_name, T& param_value, const T& default_value) +{ + bool ret_val = rosGetParam(nh, param_name, param_value); + if (!ret_val) + param_value = default_value; + return ret_val; +} + + /* ** @brief Parses commandline arguments, reads a launchfile and sets parameters for sick_lidar_localization */ @@ -96,12 +105,12 @@ bool sick_lidar_localization::API::getParams(rosNodePtr node, sick_lidar_localiz okay = getRequiredParam(node, "hostname", config.hostname) && okay; okay = getRequiredParam(node, "serverpath", config.serverpath) && okay; okay = getRequiredParam(node, "verbose", config.verbose) && okay; - okay = getRequiredParam(node, "udp_ip_sim_output", config.udp_ip_sim_output) && okay; - okay = getRequiredParam(node, "udp_ip_sim_input", config.udp_ip_sim_input) && okay; - okay = getRequiredParam(node, "udp_port_sim_input", config.udp_port_sim_input) && okay; - okay = getRequiredParam(node, "udp_sim_input_source_id", config.udp_sim_input_source_id) && okay; - okay = getRequiredParam(node, "udp_port_sim_output", config.udp_port_sim_output) && okay; - okay = getRequiredParam(node, "udp_sim_output_logfile", config.udp_sim_output_logfile) && okay; + okay = getRequiredParam(node, "udp_ip_lls_output", config.udp_ip_lls_output) && okay; + okay = getRequiredParam(node, "udp_ip_lls_input", config.udp_ip_lls_input) && okay; + okay = getRequiredParam(node, "udp_port_lls_input", config.udp_port_lls_input) && okay; + okay = getRequiredParam(node, "udp_lls_input_source_id", config.udp_lls_input_source_id) && okay; + okay = getRequiredParam(node, "udp_port_lls_output", config.udp_port_lls_output) && okay; + okay = getRequiredParam(node, "udp_lls_output_logfile", config.udp_lls_output_logfile) && okay; okay = getRequiredParam(node, "software_pll_fifo_length", config.software_pll_fifo_length) && okay; okay = getRequiredParam(node, "odom_topic", config.odom_topic) && okay; okay = getRequiredParam(node, "ros_odom_to_udp_msg", config.ros_odom_to_udp_msg) && okay; @@ -109,6 +118,32 @@ bool sick_lidar_localization::API::getParams(rosNodePtr node, sick_lidar_localiz { ROS_ERROR_STREAM("## ERROR sick_lidar_localization: getRequiredParam failed"); } + getOptionalParam(node, "udp_lls_input_source_id_1_1", config.msgtype_version_sourceid_map[1][1], config.udp_lls_input_source_id); + getOptionalParam(node, "udp_lls_input_source_id_1_4", config.msgtype_version_sourceid_map[1][4], config.udp_lls_input_source_id); + getOptionalParam(node, "udp_lls_input_source_id_1_5", config.msgtype_version_sourceid_map[1][5], config.udp_lls_input_source_id); + getOptionalParam(node, "udp_lls_input_source_id_3_3", config.msgtype_version_sourceid_map[3][3], config.udp_lls_input_source_id); + getOptionalParam(node, "udp_lls_input_source_id_4_3", config.msgtype_version_sourceid_map[4][3], config.udp_lls_input_source_id); + getOptionalParam(node, "udp_lls_input_source_id_4_4", config.msgtype_version_sourceid_map[4][4], config.udp_lls_input_source_id); + getOptionalParam(node, "udp_lls_input_source_id_7_1", config.msgtype_version_sourceid_map[7][1], config.udp_lls_input_source_id); + ROS_INFO_STREAM("sick_lidar_localization config: hostname = " << config.hostname); + ROS_INFO_STREAM("sick_lidar_localization config: serverpath = " << config.serverpath); + ROS_INFO_STREAM("sick_lidar_localization config: verbose = " << config.verbose); + ROS_INFO_STREAM("sick_lidar_localization config: udp_ip_lls_output = " << config.udp_ip_lls_output); + ROS_INFO_STREAM("sick_lidar_localization config: udp_ip_lls_input = " << config.udp_ip_lls_input); + ROS_INFO_STREAM("sick_lidar_localization config: udp_port_lls_input = " << config.udp_port_lls_input); + ROS_INFO_STREAM("sick_lidar_localization config: udp_lls_input_source_id = " << config.udp_lls_input_source_id); + ROS_INFO_STREAM("sick_lidar_localization config: msgtype_version_sourceid_map[1][1] = " << config.msgtype_version_sourceid_map[1][1]); + ROS_INFO_STREAM("sick_lidar_localization config: msgtype_version_sourceid_map[1][4] = " << config.msgtype_version_sourceid_map[1][4]); + ROS_INFO_STREAM("sick_lidar_localization config: msgtype_version_sourceid_map[1][5] = " << config.msgtype_version_sourceid_map[1][5]); + ROS_INFO_STREAM("sick_lidar_localization config: msgtype_version_sourceid_map[3][3] = " << config.msgtype_version_sourceid_map[3][3]); + ROS_INFO_STREAM("sick_lidar_localization config: msgtype_version_sourceid_map[4][3] = " << config.msgtype_version_sourceid_map[4][3]); + ROS_INFO_STREAM("sick_lidar_localization config: msgtype_version_sourceid_map[4][4] = " << config.msgtype_version_sourceid_map[4][4]); + ROS_INFO_STREAM("sick_lidar_localization config: msgtype_version_sourceid_map[7][1] = " << config.msgtype_version_sourceid_map[7][1]); + ROS_INFO_STREAM("sick_lidar_localization config: udp_port_lls_output = " << config.udp_port_lls_output); + ROS_INFO_STREAM("sick_lidar_localization config: udp_lls_output_logfile = " << config.udp_lls_output_logfile); + ROS_INFO_STREAM("sick_lidar_localization config: software_pll_fifo_length = " << config.software_pll_fifo_length); + ROS_INFO_STREAM("sick_lidar_localization config: odom_topic = " << config.odom_topic); + ROS_INFO_STREAM("sick_lidar_localization config: ros_odom_to_udp_msg = " << config.ros_odom_to_udp_msg); return okay; } @@ -148,7 +183,7 @@ bool sick_lidar_localization::API::init(rosNodePtr node, sick_lidar_localization m_services = new sick_lidar_localization::SickServices(node, m_config.hostname, m_config.serverpath, m_config.software_pll_fifo_length, m_config.verbose); // Start UDP receiver thread for UDP output messages - m_udp_receiver_thread = new sick_lidar_localization::UDPReceiverThread(m_services, m_config.udp_ip_sim_output, m_config.udp_port_sim_output, m_config.udp_sim_output_logfile); + m_udp_receiver_thread = new sick_lidar_localization::UDPReceiverThread(m_services, m_config.udp_ip_lls_output, m_config.udp_port_lls_output, m_config.udp_lls_output_logfile); m_udp_receiver_listener = new sick_lidar_localization::UDPMessage::InfoListener(); if (m_config.verbose) m_udp_receiver_thread->registerListener(m_udp_receiver_listener); @@ -166,8 +201,9 @@ bool sick_lidar_localization::API::init(rosNodePtr node, sick_lidar_localization } // Initialize UDP sender for UDP input messages - m_udp_sender = new sick_lidar_localization::UDPSender(node, m_config.udp_ip_sim_input, m_config.udp_port_sim_input, m_config.udp_sim_input_source_id, m_config.verbose, - m_config.odom_topic, m_config.ros_odom_to_udp_msg); + m_udp_sender = new sick_lidar_localization::UDPSender(node, m_config.udp_ip_lls_input, m_config.udp_port_lls_input, + sick_lidar_localization::UDPDefaultInputSourceId(m_config.udp_lls_input_source_id, m_config.msgtype_version_sourceid_map), + m_config.verbose, m_config.odom_topic, m_config.ros_odom_to_udp_msg); return true; } @@ -195,7 +231,7 @@ void sick_lidar_localization::API::close() } /* -** @brief Register a listener for upd messages. The callback functions of the listener will be called after receiving a new udp message. +** @brief Register a listener for udp messages. The callback functions of the listener will be called after receiving a new udp message. ** Overwrite the functions defined in sick_lidar_localization::UDPMessage::Listener with customized code to handle udp messages. */ bool sick_lidar_localization::API::registerListener(sick_lidar_localization::UDPMessage::Listener* listener) @@ -233,16 +269,16 @@ bool sick_lidar_localization::API::unregisterListener(sick_lidar_localization::U /* ** @brief Sends a UDP input message. ** payload can be OdometryPayload0104, OdometryPayload0105, EncoderMeasurementPayload0202, -** CodeMeasurementPayload0303, LineMeasurementPayload0403 or LineMeasurementPayload0404 +** CodeMeasurementPayload0303, odeMeasurementPayload0701, LineMeasurementPayload0403 or LineMeasurementPayload0404 ** @param[in] payload UDP message payload data ** @return true on success or false on error */ -template bool sendUDPPayload(sick_lidar_localization::UDPSender* udp_sender, const T& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id) +template bool sendUDPPayload(sick_lidar_localization::UDPSender* udp_sender, const T& payload, bool encode_header_big_endian, bool encode_payload_big_endian) { if (udp_sender) { - return udp_sender->sendUDPPayload(payload, encode_header_big_endian, encode_payload_big_endian, source_id); + return udp_sender->sendUDPPayload(payload, encode_header_big_endian, encode_payload_big_endian); } else { @@ -254,27 +290,31 @@ template bool sendUDPPayload(sick_lidar_localization::UDPSender* udp /* ** @brief Send udp messages to the localization controller */ -bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::OdometryPayload0104& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id) +bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::OdometryPayload0104& payload, bool encode_header_big_endian, bool encode_payload_big_endian) +{ + return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian); +} +bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::OdometryPayload0105& payload, bool encode_header_big_endian, bool encode_payload_big_endian) { - return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian, source_id); + return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian); } -bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::OdometryPayload0105& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id) +bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::EncoderMeasurementPayload0202& payload, bool encode_header_big_endian, bool encode_payload_big_endian) { - return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian, source_id); + return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian); } -bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::EncoderMeasurementPayload0202& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id) +bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::CodeMeasurementPayload0303& payload, bool encode_header_big_endian, bool encode_payload_big_endian) { - return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian, source_id); + return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian); } -bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::CodeMeasurementPayload0303& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id) +bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::CodeMeasurementPayload0701& payload, bool encode_header_big_endian, bool encode_payload_big_endian) { - return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian, source_id); + return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian); } -bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::LineMeasurementPayload0403& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id) +bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::LineMeasurementPayload0403& payload, bool encode_header_big_endian, bool encode_payload_big_endian) { - return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian, source_id); + return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian); } -bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::LineMeasurementPayload0404& payload, bool encode_header_big_endian, bool encode_payload_big_endian, uint16_t source_id) +bool sick_lidar_localization::API::sendUDPMessage(const sick_lidar_localization::UDPMessage::LineMeasurementPayload0404& payload, bool encode_header_big_endian, bool encode_payload_big_endian) { - return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian, source_id); + return sendUDPPayload(m_udp_sender, payload, encode_header_big_endian, encode_payload_big_endian); } diff --git a/src/sick_lidar_localization_main.cpp b/src/sick_lidar_localization_main.cpp index 696324b..f827925 100644 --- a/src/sick_lidar_localization_main.cpp +++ b/src/sick_lidar_localization_main.cpp @@ -65,41 +65,56 @@ static void sendUDPMessageExamples(sick_lidar_localization::API& lidar_loc_api) 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 = 1; odometry0104.x_velocity = -1234; odometry0104.y_velocity = -1234; odometry0104.angular_velocity = 1234; odometry0105.telegram_count = 1000002; odometry0105.timestamp = 123456780; + odometry0105.source_id = 1; odometry0105.x_position = -1234; odometry0105.y_position = -1234; odometry0105.heading = 1234; encoder_measurement0202.telegram_count = 1000003; encoder_measurement0202.timestamp = 123456781; + encoder_measurement0202.source_id = 1; encoder_measurement0202.encoder_value = 123456789; code_measurement0303.telegram_count = 1000004; code_measurement0303.timestamp = 123456782; + code_measurement0303.source_id = 1; code_measurement0303.code = 1234; + code_measurement0701.telegram_count = 1000004; + code_measurement0701.timestamp = 123456782; + code_measurement0701.source_id = 1; + 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 = 1; line_measurement0403.num_lanes = 1; line_measurement0403.lanes.push_back(1234); line_measurement0404.telegram_count = 1000006; line_measurement0404.timestamp = 123456784; + line_measurement0404.source_id = 1; 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"); } diff --git a/src/sick_services.cpp b/src/sick_services.cpp index a233281..28106ac 100644 --- a/src/sick_services.cpp +++ b/src/sick_services.cpp @@ -79,7 +79,7 @@ #define serviceCbLocSetOdometryActiveSrv sick_lidar_localization::SickServices::serviceCbLocSetOdometryActiveSrvROS1 #define serviceCbLocSetRecordingActiveSrv sick_lidar_localization::SickServices::serviceCbLocSetRecordingActiveSrvROS1 #define serviceCbLocSetRingBufferRecordingActiveSrv sick_lidar_localization::SickServices::serviceCbLocSetRingBufferRecordingActiveSrvROS1 -#define serviceCbLocStartLocalizingSrv sick_lidar_localization::SickServices::serviceCbLocStartLocalizingSrvROS1 +#define serviceCbLocStartSrv sick_lidar_localization::SickServices::serviceCbLocStartSrvROS1 #define serviceCbLocStateSrv sick_lidar_localization::SickServices::serviceCbLocStateSrvROS1 #define serviceCbLocStopSrv sick_lidar_localization::SickServices::serviceCbLocStopSrvROS1 #define serviceCbLocSwitchMapSrv sick_lidar_localization::SickServices::serviceCbLocSwitchMapSrvROS1 @@ -107,7 +107,7 @@ #define serviceCbLocSetOdometryActiveSrv sick_lidar_localization::SickServices::serviceCbLocSetOdometryActiveSrvROS2 #define serviceCbLocSetRecordingActiveSrv sick_lidar_localization::SickServices::serviceCbLocSetRecordingActiveSrvROS2 #define serviceCbLocSetRingBufferRecordingActiveSrv sick_lidar_localization::SickServices::serviceCbLocSetRingBufferRecordingActiveSrvROS2 -#define serviceCbLocStartLocalizingSrv sick_lidar_localization::SickServices::serviceCbLocStartLocalizingSrvROS2 +#define serviceCbLocStartSrv sick_lidar_localization::SickServices::serviceCbLocStartSrvROS2 #define serviceCbLocStateSrv sick_lidar_localization::SickServices::serviceCbLocStateSrvROS2 #define serviceCbLocStopSrv sick_lidar_localization::SickServices::serviceCbLocStopSrvROS2 #define serviceCbLocSwitchMapSrv sick_lidar_localization::SickServices::serviceCbLocSwitchMapSrvROS2 @@ -171,8 +171,8 @@ sick_lidar_localization::SickServices::SickServices(rosNodePtr nh, const std::st m_srv_server_LocSetRecordingActiveSrv = rosServiceServer(srv_LocSetRecordingActiveSrv); auto srv_LocSetRingBufferRecordingActiveSrv = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::LocSetRingBufferRecordingActiveSrv, "/LocSetRingBufferRecordingActive", &serviceCbLocSetRingBufferRecordingActiveSrv, this); m_srv_server_LocSetRingBufferRecordingActiveSrv = rosServiceServer(srv_LocSetRingBufferRecordingActiveSrv); - auto srv_LocStartLocalizingSrv = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::LocStartLocalizingSrv, "/LocStartLocalizing", &serviceCbLocStartLocalizingSrv, this); - m_srv_server_LocStartLocalizingSrv = rosServiceServer(srv_LocStartLocalizingSrv); + auto srv_LocStartSrv = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::LocStartSrv, "/LocStart", &serviceCbLocStartSrv, this); + m_srv_server_LocStartSrv = rosServiceServer(srv_LocStartSrv); auto srv_LocStopSrv = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::LocStopSrv, "/LocStop", &serviceCbLocStopSrv, this); m_srv_server_LocStopSrv = rosServiceServer(srv_LocStopSrv); auto srv_LocSwitchMapSrv = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::LocSwitchMapSrv, "/LocSwitchMap", &serviceCbLocSwitchMapSrv, this); @@ -220,7 +220,7 @@ void sick_lidar_localization::SickServices::getDefaultCommand(const std::string& command_map["LocSetOdometryActive"] = {"LocSetOdometryActive", "POST"}; command_map["LocSetRecordingActive"] = {"LocSetRecordingActive", "POST"}; command_map["LocSetRingBufferRecordingActive"] = {"LocSetRingBufferRecordingActive", "POST"}; - command_map["LocStartLocalizing"] = {"LocStartLocalizing", "POST", "{}"}; + command_map["LocStart"] = {"LocStart", "POST", "{}"}; command_map["LocStop"] = {"LocStop", "POST", "{}"}; command_map["LocSwitchMap"] = {"LocSwitchMap", "POST"}; command_map["LocGetLocalizationStatus"] = {"LocGetLocalizationStatus", "POST"}; @@ -634,17 +634,17 @@ bool sick_lidar_localization::SickServices::serviceCbLocSetRingBufferRecordingAc return serviceCbLocSetRingBufferRecordingActiveSrvROS1(*service_request, *service_response); } -bool sick_lidar_localization::SickServices::serviceCbLocStartLocalizingSrvROS1(sick_lidar_localization::LocStartLocalizingSrv::Request &service_request, sick_lidar_localization::LocStartLocalizingSrv::Response &service_response) +bool sick_lidar_localization::SickServices::serviceCbLocStartSrvROS1(sick_lidar_localization::LocStartSrv::Request &service_request, sick_lidar_localization::LocStartSrv::Response &service_response) { - std::string command = "LocStartLocalizing", method = "POST", json_data = "{}"; + std::string command = "LocStart", method = "POST", json_data = "{}"; std::map response_data = sendJsonRequestGetResponse(command, method, json_data); service_response.success = response_data["/data/success"].toBool(); ROS_INFO_STREAM("SickServices::serviceCb(\"" << command << "\", \"" << method << "\", \"" << json_data << "\"): success=" << std::to_string(service_response.success)); return true; } -bool sick_lidar_localization::SickServices::serviceCbLocStartLocalizingSrvROS2(std::shared_ptr service_request, std::shared_ptr service_response) +bool sick_lidar_localization::SickServices::serviceCbLocStartSrvROS2(std::shared_ptr service_request, std::shared_ptr service_response) { - return serviceCbLocStartLocalizingSrvROS1(*service_request, *service_response); + return serviceCbLocStartSrvROS1(*service_request, *service_response); } bool sick_lidar_localization::SickServices::serviceCbLocStopSrvROS1(sick_lidar_localization::LocStopSrv::Request &service_request, sick_lidar_localization::LocStopSrv::Response &service_response) diff --git a/src/udp_message_parser.cpp b/src/udp_message_parser.cpp index 7ee58e3..6993a6c 100644 --- a/src/udp_message_parser.cpp +++ b/src/udp_message_parser.cpp @@ -194,75 +194,6 @@ std::vector sick_lidar_localization::UDPMessage::encodeHeader(size_t pa } } -#define OdometryPayload0101UdpInputMsgLidarLocVersion 1 // Note: odometry message type 1 version 1 (localization controller udp input message) in old lidar loc version 1 message format (NOT in lidar loc version 2) - -/** @brief Encodes the header of a OdometryPayload0101 message */ -template<> std::vector sick_lidar_localization::UDPMessage::encodeHeader(const sick_lidar_localization::UDPMessage::OdometryPayload0101& message_payload, size_t payload_size, bool header_big_endian, bool payload_big_endian, uint16_t source_id) -{ - int lidar_loc_version = OdometryPayload0101UdpInputMsgLidarLocVersion; // Note: odometry message type 1 version 1 in old lidar loc version 1 message format (NOT in lidar loc version 2) - return sick_lidar_localization::UDPMessage::encodeHeader(payload_size, header_big_endian, payload_big_endian, 1, 1, source_id, lidar_loc_version); -} - -/* -** @brief Encodes the payload of a OdometryPayload0101 udp message (odometry payload message type 1 version 1 -** @param[in] message_payload payload -** @param[in] encode_big_endian encode in big or little endiang -** @param[out] encoded vector of bytes -*/ -template<> std::vector sick_lidar_localization::UDPMessage::encodePayload(const sick_lidar_localization::UDPMessage::OdometryPayload0101& message_payload, bool encode_big_endian) -{ - int lidar_loc_version = OdometryPayload0101UdpInputMsgLidarLocVersion; // Note: odometry message type 1 version 1 in old lidar loc version 1 message format (NOT in lidar loc version 2) - if(lidar_loc_version == 2) - { - // Encode payload according to lidar localization specification version 2 (default) - std::vector buffer(32); - encode((uint64_t)message_payload.telegram_count, &buffer[0] + 0, 8, encode_big_endian); - encode((uint64_t)message_payload.timestamp, &buffer[0] + 8, 8, encode_big_endian); - encode((uint64_t)message_payload.x_velocity, &buffer[0] + 16, 4, encode_big_endian); - encode((uint64_t)message_payload.y_velocity, &buffer[0] + 20, 4, encode_big_endian); - encode((uint64_t)message_payload.angular_velocity, &buffer[0] + 24, 8, encode_big_endian); - return buffer; - } - else if(lidar_loc_version == 1) - { - // Encode payload according to lidar localization specification version 1 (for backward compatibility only) - std::vector buffer(16); - // Payload: 4 byte TelegramCount UInt32 - encode((uint64_t)message_payload.telegram_count, &buffer[0] + 0, 4, encode_big_endian); - // Payload: 4 byte Timestamp UInt32 in ms - encode((uint64_t)message_payload.timestamp, &buffer[0] + 4, 4, encode_big_endian); - // Payload: 2 byte vx Int16 in mm/s - encode((uint64_t)message_payload.x_velocity, &buffer[0] + 8, 2, encode_big_endian); - // Payload: 2 byte vy Int16 in mm/s - encode((uint64_t)message_payload.y_velocity, &buffer[0] + 10, 2, encode_big_endian); - // Payload: 4 byte omega Int32 in mdeg/s - encode((uint64_t)message_payload.angular_velocity, &buffer[0] + 12, 4, encode_big_endian); - return buffer; - } - else - { - ROS_ERROR_STREAM("## ERROR UDPMessage::encodePayload(OdometryPayload0101): lidar_loc_version = " << lidar_loc_version << " not supported, expected version 2 (default) or version 1 (backward compatibility)"); - return std::vector(); - } -} - -/* -** @brief Print the payload data. Implementation for odometry payload message type 1 version 1 -** @return payload data as human readable string -*/ -template<> std::string sick_lidar_localization::UDPMessage::printPayload(const sick_lidar_localization::UDPMessage::OdometryPayload0101& payload, bool print_sync_time) -{ - std::stringstream s; - s << "OdometryPayload0101:{" - << "telegram_count:" << payload.telegram_count - << ", timestamp:" << payload.timestamp - << ", x_velocity:" << payload.x_velocity - << ", y_velocity:" << payload.y_velocity - << ", angular_velocity:" << payload.angular_velocity - << "}"; - return s.str(); -} - /* ** @brief Decodes the payload of a udp message. Implementation for odometry payload message type 1 version 4 (24 byte payload) ** @param[in] udp_buffer payload bytes received by udp @@ -348,6 +279,7 @@ template<> std::string sick_lidar_localization::UDPMessage::printPayload(const s s << "OdometryPayload0104:{" << "telegram_count:" << payload.telegram_count << ", timestamp:" << payload.timestamp + << ", source_id:" << payload.source_id << ", x_velocity:" << payload.x_velocity << ", y_velocity:" << payload.y_velocity << ", angular_velocity:" << payload.angular_velocity @@ -412,6 +344,7 @@ template<> std::string sick_lidar_localization::UDPMessage::printPayload(const s s << "OdometryPayload0105:{" << "telegram_count:" << payload.telegram_count << ", timestamp:" << payload.timestamp + << ", source_id:" << payload.source_id << ", x_position:" << payload.x_position << ", y_position:" << payload.y_position << ", heading:" << payload.heading @@ -451,6 +384,7 @@ template<> std::string sick_lidar_localization::UDPMessage::printPayload(const s s << "EncoderMeasurementPayload0202:{" << "telegram_count:" << payload.telegram_count << ", timestamp:" << payload.timestamp + << ", source_id:" << payload.source_id << ", code:" << payload.encoder_value << "}"; return s.str(); @@ -487,11 +421,83 @@ template<> std::string sick_lidar_localization::UDPMessage::printPayload(const s s << "CodeMeasurementPayload0303:{" << "telegram_count:" << payload.telegram_count << ", timestamp:" << payload.timestamp + << ", source_id:" << payload.source_id << ", code:" << payload.code << "}"; return s.str(); } +/** @brief Encodes the header of a CodeMeasurementPayload0701 message */ +template<> std::vector sick_lidar_localization::UDPMessage::encodeHeader(const sick_lidar_localization::UDPMessage::CodeMeasurementPayload0701& message_payload, size_t payload_size, bool header_big_endian, bool payload_big_endian, uint16_t source_id) +{ + return sick_lidar_localization::UDPMessage::encodeHeader(payload_size, header_big_endian, payload_big_endian, 7, 1, source_id); +} + +/* +** @brief Encodes the payload of a CodeMeasurementPayload0701 udp message (code measurement payload message type 7 version 1 (variable length payload) +** @param[in] message_payload payload +** @param[in] encode_big_endian encode in big or little endiang +** @param[out] encoded vector of bytes +*/ +template<> std::vector sick_lidar_localization::UDPMessage::encodePayload(const sick_lidar_localization::UDPMessage::CodeMeasurementPayload0701& message_payload, bool encode_big_endian) +{ + int buffer_length = (int)(sizeof(message_payload.telegram_count) + sizeof(message_payload.timestamp) + 2 + message_payload.code.length() + + sizeof(message_payload.x_position) + sizeof(message_payload.y_position) + sizeof(message_payload.heading)); + std::vector buffer(buffer_length); + + uint8_t* p_data = &buffer[0]; + int len = sizeof(message_payload.telegram_count); + encode((uint64_t)message_payload.telegram_count, p_data, len, encode_big_endian); + p_data += len; + + len = sizeof(message_payload.timestamp); + encode((uint64_t)message_payload.timestamp, p_data, len, encode_big_endian); + p_data += len; + + uint16_t codelength = (uint16_t)message_payload.code.length(); + len = sizeof(codelength); + encode((uint16_t)codelength, p_data, len, encode_big_endian); + p_data += len; + + for(int n = 0; n < codelength; n++, p_data++) + { + *p_data = (uint8_t)message_payload.code[n]; + } + + len = sizeof(message_payload.x_position); + encode((uint64_t)message_payload.x_position, p_data, len, encode_big_endian); + p_data += len; + + len = sizeof(message_payload.y_position); + encode((uint64_t)message_payload.y_position, p_data, len, encode_big_endian); + p_data += len; + + len = sizeof(message_payload.heading); + encode((uint64_t)message_payload.heading, p_data, len, encode_big_endian); + p_data += len; + + return buffer; +} + +/* +** @brief Print the payload data. Implementation for code measurement payload message type 7 version 1 +** @return payload data as human readable string +*/ +template<> std::string sick_lidar_localization::UDPMessage::printPayload(const sick_lidar_localization::UDPMessage::CodeMeasurementPayload0701& payload, bool print_sync_time) +{ + std::stringstream s; + s << "CodeMeasurementPayload0701:{" + << "telegram_count:" << payload.telegram_count + << ", timestamp:" << payload.timestamp + << ", source_id:" << payload.source_id + << ", code:" << payload.code + << ", x_position:" << payload.x_position + << ", y_position:" << payload.y_position + << ", heading:" << payload.heading + << "}"; + return s.str(); +} + /* ** @brief Decodes the payload of a udp message. Implementation for code measurement payload message type 3 version 4 (24 byte payload) ** @param[in] udp_buffer payload bytes received by udp @@ -524,6 +530,7 @@ template<> std::string sick_lidar_localization::UDPMessage::printPayload(const s s << "CodeMeasurementPayload0304:{" << "telegram_count:" << payload.telegram_count << ", timestamp:" << payload.timestamp + << ", source_id:" << payload.source_id << ", code:" << payload.code << ", distance:" << payload.distance << printSyncTime(payload, print_sync_time) @@ -599,6 +606,7 @@ template<> std::string sick_lidar_localization::UDPMessage::printPayload(const s s << "LineMeasurementPayload0403:{" << "telegram_count:" << payload.telegram_count << ", timestamp:" << payload.timestamp + << ", source_id:" << payload.source_id << ", num_lanes:" << (int)payload.num_lanes << ", lanes:["; for (int n = 0; n < payload.num_lanes; n++) @@ -673,6 +681,7 @@ template<> std::string sick_lidar_localization::UDPMessage::printPayload(const s s << "LineMeasurementPayload0404:{" << "telegram_count:" << payload.telegram_count << ", timestamp:" << payload.timestamp + << ", source_id:" << payload.source_id << ", lcp1:" << (int)payload.lcp1 << ", lcp2:" << (int)payload.lcp2 << ", lcp3:" << (int)payload.lcp3 @@ -718,6 +727,7 @@ template<> std::string sick_lidar_localization::UDPMessage::printPayload(const s s << "LocalizationControllerResultPayload0502:{" << "telegram_count:" << payload.telegram_count << ", timestamp:" << payload.timestamp + << ", source_id:" << payload.source_id << ", x:" << payload.x << ", y:" << payload.y << ", heading:" << payload.heading diff --git a/src/udp_receiver_thread.cpp b/src/udp_receiver_thread.cpp index be34e6b..38825bc 100644 --- a/src/udp_receiver_thread.cpp +++ b/src/udp_receiver_thread.cpp @@ -86,12 +86,12 @@ static std::string getErrorMessage(void) { return std::to_string(errno) + " (" + /* ** @brief Default constructor ** @param[in] services time sync services -** @param[in] udp_ip_sim_output IP address for output UDP messages, or "" for broadcast (INADDR_ANY), default: "", use IP address of your local machine -** @param[in] udp_port_sim_output UDP port of output messages, default: 5010 -** @param[in] udp_sim_output_logfile Optional logfile for human readable output messages, default: "" (no outputlogfile) +** @param[in] udp_ip_lls_output IP address for output UDP messages, or "" for broadcast (INADDR_ANY), default: "", use IP address of your local machine +** @param[in] udp_port_lls_output UDP port of output messages, default: 5010 +** @param[in] udp_lls_output_logfile Optional logfile for human readable output messages, default: "" (no outputlogfile) */ -sick_lidar_localization::UDPReceiverThread::UDPReceiverThread(sick_lidar_localization::SickServices* services, const std::string& udp_ip_sim_output, int udp_port_sim_output, const std::string& udp_sim_output_logfile) - : m_services(services), m_udp_ip_sim_output(udp_ip_sim_output), m_udp_port_sim_output(udp_port_sim_output), m_udp_sim_output_logfile(udp_sim_output_logfile), m_run_receiver_thread(false), m_receiver_thread(0) +sick_lidar_localization::UDPReceiverThread::UDPReceiverThread(sick_lidar_localization::SickServices* services, const std::string& udp_ip_lls_output, int udp_port_lls_output, const std::string& udp_lls_output_logfile) + : m_services(services), m_udp_ip_lls_output(udp_ip_lls_output), m_udp_port_lls_output(udp_port_lls_output), m_udp_lls_output_logfile(udp_lls_output_logfile), m_run_receiver_thread(false), m_receiver_thread(0) { } @@ -131,7 +131,7 @@ void sick_lidar_localization::UDPReceiverThread::stop() } /* -** @brief Register a listener for upd messages. The callback functions of the listener will be called after receiving a new udp message. +** @brief Register a listener for udp messages. The callback functions of the listener will be called after receiving a new udp message. ** Overwrite the functions defined in sick_lidar_localization::UDPMessage::Listener with customized code to handle udp messages. */ void sick_lidar_localization::UDPReceiverThread::registerListener(sick_lidar_localization::UDPMessage::Listener* listener) @@ -165,8 +165,8 @@ void sick_lidar_localization::UDPReceiverThread::sleep(double seconds) */ bool sick_lidar_localization::UDPReceiverThread::runReceiver(void) { - if (!m_udp_sim_output_logfile.empty()) - UNLINK(m_udp_sim_output_logfile.c_str()); + if (!m_udp_lls_output_logfile.empty()) + UNLINK(m_udp_lls_output_logfile.c_str()); SOCKET udp_socket = INVALID_SOCKET; while (rosOk() && m_run_receiver_thread) { @@ -178,15 +178,15 @@ bool sick_lidar_localization::UDPReceiverThread::runReceiver(void) } // int broadcast_opt = 1; // setsockopt(udp_socket, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)); - struct sockaddr_in sim_servaddr = { 0 }; - if(m_udp_ip_sim_output.empty()) - sim_servaddr.sin_addr.s_addr = htonl(INADDR_ANY); + struct sockaddr_in lls_servaddr = { 0 }; + if(m_udp_ip_lls_output.empty()) + lls_servaddr.sin_addr.s_addr = htonl(INADDR_ANY); else - sim_servaddr.sin_addr.s_addr = inet_addr(m_udp_ip_sim_output.c_str()); - sim_servaddr.sin_family = AF_INET; - sim_servaddr.sin_port = htons(m_udp_port_sim_output); - ROS_INFO_STREAM("sick_lidar_localization::UDPReceiverThread: udp socket created, binding to port " << ntohs(sim_servaddr.sin_port) << " ... "); - while (rosOk() && m_run_receiver_thread && udp_socket != INVALID_SOCKET && bind(udp_socket, (SOCKADDR*)&sim_servaddr, sizeof(sim_servaddr)) < 0) + lls_servaddr.sin_addr.s_addr = inet_addr(m_udp_ip_lls_output.c_str()); + lls_servaddr.sin_family = AF_INET; + lls_servaddr.sin_port = htons(m_udp_port_lls_output); + ROS_INFO_STREAM("sick_lidar_localization::UDPReceiverThread: udp socket created, binding to port " << ntohs(lls_servaddr.sin_port) << " ... "); + while (rosOk() && m_run_receiver_thread && udp_socket != INVALID_SOCKET && bind(udp_socket, (SOCKADDR*)&lls_servaddr, sizeof(lls_servaddr)) < 0) { ROS_INFO_STREAM("sick_lidar_localization::UDPReceiverThread: socket bind failed, error " << getErrorMessage() << ", retrying..."); sleep(1.0); // Retry after 1 second @@ -205,8 +205,8 @@ bool sick_lidar_localization::UDPReceiverThread::runReceiver(void) while (rosOk() && m_run_receiver_thread && udp_socket != INVALID_SOCKET && bytes_received < bytes_required) { int n = recv(udp_socket, (char*)&udp_buffer[bytes_received], (int)sizeof(udp_buffer) - bytes_received, 0); - // socklen_t sim_servaddr_len = sizeof(sim_servaddr); - // int n = recvfrom(udp_socket, (char*)&udp_buffer[bytes_received], (int)sizeof(udp_buffer) - bytes_received, 0, (SOCKADDR*)&sim_servaddr, &sim_servaddr_len); + // socklen_t lls_servaddr_len = sizeof(lls_servaddr); + // int n = recvfrom(udp_socket, (char*)&udp_buffer[bytes_received], (int)sizeof(udp_buffer) - bytes_received, 0, (SOCKADDR*)&lls_servaddr, &lls_servaddr_len); if (n <= 0) { if (n < 0) @@ -269,6 +269,7 @@ bool sick_lidar_localization::UDPReceiverThread::runReceiver(void) } if (payload && payload->decodePayload(udp_payload_buffer, udp_payload_len, payload_is_big_endian)) { + payload->sourceID() = msg_header.sourceid; sick_lidar_localization::SyncTimeStamp sync_time_stamp = sick_lidar_localization::makeSyncTimeStamp(0, 0, false); if(m_services) { @@ -283,12 +284,12 @@ bool sick_lidar_localization::UDPReceiverThread::runReceiver(void) // Notify listener (and publish UDP output messages on ROS-1 or ROS-2) payload->notifyListener(m_udp_message_listener); // Log output messages - FILE* fp_udp_sim_output_logfile = 0; - if (!m_udp_sim_output_logfile.empty() && (fp_udp_sim_output_logfile = fopen(m_udp_sim_output_logfile.c_str(), "a")) != 0) + FILE* fp_udp_lls_output_logfile = 0; + if (!m_udp_lls_output_logfile.empty() && (fp_udp_lls_output_logfile = fopen(m_udp_lls_output_logfile.c_str(), "a")) != 0) { std::string payload_str = payload->toString(false); - fprintf(fp_udp_sim_output_logfile, "%s\n", payload_str.c_str()); - fclose(fp_udp_sim_output_logfile); + fprintf(fp_udp_lls_output_logfile, "%s\n", payload_str.c_str()); + fclose(fp_udp_lls_output_logfile); } } else diff --git a/src/udp_sender.cpp b/src/udp_sender.cpp index 9e84c71..8ed67e8 100644 --- a/src/udp_sender.cpp +++ b/src/udp_sender.cpp @@ -92,22 +92,22 @@ namespace sick_lidar_localization /* ** @brief Default constructor */ - UDPSenderImpl() : m_udp_socket(INVALID_SOCKET), m_sim_ip_address(), m_udp_port_sim_input(0) + UDPSenderImpl() : m_udp_socket(INVALID_SOCKET), m_lls_ip_address(), m_udp_port_lls_input(0) { } /* ** @brief Initializes the socket - ** @param[in] sim_ip_address IP address of localization controller, or "" for broadcast - ** @param[in] udp_port_sim_input UDP port of input messages, default: 5009 + ** @param[in] lls_ip_address IP address of localization controller, or "" for broadcast + ** @param[in] udp_port_lls_input UDP port of input messages, default: 5009 */ - bool initSocket(const std::string & sim_ip_address = "192.168.0.1", int udp_port_sim_input = 5009) + bool initSocket(const std::string & lls_ip_address = "192.168.0.1", int udp_port_lls_input = 5009) { - m_sim_ip_address = sim_ip_address; - m_udp_port_sim_input = udp_port_sim_input; + m_lls_ip_address = lls_ip_address; + m_udp_port_lls_input = udp_port_lls_input; if ((m_udp_socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == INVALID_SOCKET) { - ROS_ERROR_STREAM("## ERROR UDPSenderImpl::init(" << udp_port_sim_input << "): can't create socket"); + ROS_ERROR_STREAM("## ERROR UDPSenderImpl::init(" << udp_port_lls_input << "): can't create socket"); return false; } #if defined WIN32 || defined _MSC_VER @@ -116,7 +116,7 @@ namespace sick_lidar_localization int broadcast_opt = 1; #endif if (setsockopt(m_udp_socket, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) - ROS_ERROR_STREAM("## ERROR UDPSenderImpl::init(" << udp_port_sim_input << "): setsockopt(SO_BROADCAST) failed, error: " << getErrorMessage()); + ROS_ERROR_STREAM("## ERROR UDPSenderImpl::init(" << udp_port_lls_input << "): setsockopt(SO_BROADCAST) failed, error: " << getErrorMessage()); return true; } @@ -134,34 +134,34 @@ namespace sick_lidar_localization { if (m_udp_socket != INVALID_SOCKET) { - struct sockaddr_in sim_servaddr = { 0 }; - if(m_sim_ip_address.empty()) + struct sockaddr_in lls_servaddr = { 0 }; + if(m_lls_ip_address.empty()) { - sim_servaddr.sin_addr.s_addr = htonl(INADDR_BROADCAST); + lls_servaddr.sin_addr.s_addr = htonl(INADDR_BROADCAST); } else { #if defined WIN32 || defined _MSC_VER - sim_servaddr.sin_addr.s_addr = inet_addr(m_sim_ip_address.c_str()); + lls_servaddr.sin_addr.s_addr = inet_addr(m_lls_ip_address.c_str()); #else - struct in_addr sim_in_addr; - if (inet_aton(m_sim_ip_address.c_str(), &sim_in_addr) != 0) + struct in_addr lls_in_addr; + if (inet_aton(m_lls_ip_address.c_str(), &lls_in_addr) != 0) { - sim_servaddr.sin_addr.s_addr = sim_in_addr.s_addr; + lls_servaddr.sin_addr.s_addr = lls_in_addr.s_addr; } else { - ROS_ERROR_STREAM("## ERROR UDPSenderImpl::sendData(): inet_aton(" << m_sim_ip_address << ") failed (invalid address)"); - sim_servaddr.sin_addr.s_addr = inet_addr(m_sim_ip_address.c_str()); + ROS_ERROR_STREAM("## ERROR UDPSenderImpl::sendData(): inet_aton(" << m_lls_ip_address << ") failed (invalid address)"); + lls_servaddr.sin_addr.s_addr = inet_addr(m_lls_ip_address.c_str()); } #endif } - sim_servaddr.sin_family = AF_INET; - sim_servaddr.sin_port = htons(m_udp_port_sim_input); - int bytes_send = sendto(m_udp_socket, (const char*)buffer, num_bytes, 0, (SOCKADDR*)&sim_servaddr, sizeof(sim_servaddr)); + lls_servaddr.sin_family = AF_INET; + lls_servaddr.sin_port = htons(m_udp_port_lls_input); + int bytes_send = sendto(m_udp_socket, (const char*)buffer, num_bytes, 0, (SOCKADDR*)&lls_servaddr, sizeof(lls_servaddr)); if (bytes_send != num_bytes) { - ROS_ERROR_STREAM("## ERROR UDPSenderImpl::sendData(): " << bytes_send << " of " << num_bytes << " bytes send to " << m_sim_ip_address << ":" << m_udp_port_sim_input << ", error: " << getErrorMessage()); + ROS_ERROR_STREAM("## ERROR UDPSenderImpl::sendData(): " << bytes_send << " of " << num_bytes << " bytes send to " << m_lls_ip_address << ":" << m_udp_port_lls_input << ", error: " << getErrorMessage()); return false; } return true; @@ -172,25 +172,25 @@ namespace sick_lidar_localization protected: SOCKET m_udp_socket; // OS socket descriptor - std::string m_sim_ip_address; // IP address of localization controller, or "" for broadcast - int m_udp_port_sim_input; // UDP port of input messages, default: 5009 + std::string m_lls_ip_address; // IP address of localization controller, or "" for broadcast + int m_udp_port_lls_input; // UDP port of input messages, default: 5009 }; } /* ** @brief Default constructor ** @param[in] nh ros node handle (always 0 for native linus or windows) -** @param[in] sim_ip_address IP address of localization controller, or "" for broadcast -** @param[in] udp_port_sim_input UDP port of input messages, default: 5009 -** @param[in] udp_sim_input_source_id source_id of UDP input messages, default: 1 +** @param[in] lls_ip_address IP address of localization controller, or "" for broadcast +** @param[in] udp_port_lls_input UDP port of input messages, default: 5009 +** @param[in] source_id_cfg source_id map of UDP input messages, default source id: 1 ** @param[in] verbose print informational messages if verbose > 0, otherwise silent mode (error messages only) -** @param[in] ros_odom_to_udp_msg Convert ros odom message to upd: 0 = map velocity to OdometryPayload0101 (Type 1, Version 1, LidarLoc 1), +** @param[in] ros_odom_to_udp_msg Convert ros odom message to udp: ** 1 = map velocity to OdometryPayload0104 (Type 1, Version 4, LidarLoc 2), ** 2 = map position to OdometryPayload0105 (Type 1, Version 5, LidarLoc 2), ** 3 = map velocity to OdometryPayload0104 and position to OdometryPayload0105 */ -sick_lidar_localization::UDPSender::UDPSender(rosNodePtr nh, const std::string& sim_ip_address, int udp_port_sim_input, int udp_sim_input_source_id, int verbose, const std::string& odom_topic, int ros_odom_to_udp_msg) -: m_sim_ip_address(sim_ip_address), m_udp_port_sim_input(udp_port_sim_input), m_source_id(udp_sim_input_source_id), m_verbose(verbose), m_ros_odom_to_udp_msg(ros_odom_to_udp_msg), m_udp_sender_impl(0) +sick_lidar_localization::UDPSender::UDPSender(rosNodePtr nh, const std::string& lls_ip_address, int udp_port_lls_input, const sick_lidar_localization::UDPDefaultInputSourceId& source_id_cfg, int verbose, const std::string& odom_topic, int ros_odom_to_udp_msg) +: m_lls_ip_address(lls_ip_address), m_udp_port_lls_input(udp_port_lls_input), m_source_id_cfg(source_id_cfg), m_verbose(verbose), m_ros_odom_to_udp_msg(ros_odom_to_udp_msg), m_udp_sender_impl(0) { #if __ROS_VERSION > 0 if(nh != 0) @@ -198,39 +198,39 @@ sick_lidar_localization::UDPSender::UDPSender(rosNodePtr nh, const std::string& // Subscribe to odometry messages #if __ROS_VERSION == 1 auto messageCbOdomROS = &sick_lidar_localization::UDPSender::messageCbOdomROS; - auto messageCbOdometryMessage0101 = &sick_lidar_localization::UDPSender::messageCbOdometryMessage0101; auto messageCbOdometryMessage0104 = &sick_lidar_localization::UDPSender::messageCbOdometryMessage0104; auto messageCbOdometryMessage0105 = &sick_lidar_localization::UDPSender::messageCbOdometryMessage0105; auto messageCbEncoderMeasurementMessage0202 = &sick_lidar_localization::UDPSender::messageCbEncoderMeasurementMessage0202; auto messageCbCodeMeasurementMessage0303 = &sick_lidar_localization::UDPSender::messageCbCodeMeasurementMessage0303; + auto messageCbCodeMeasurementMessage0701 = &sick_lidar_localization::UDPSender::messageCbCodeMeasurementMessage0701; auto messageCbLineMeasurementMessage0403 = &sick_lidar_localization::UDPSender::messageCbLineMeasurementMessage0403; auto messageCbLineMeasurementMessage0404 = &sick_lidar_localization::UDPSender::messageCbLineMeasurementMessage0404; #else auto messageCbOdomROS = &sick_lidar_localization::UDPSender::messageCbOdomROS2; - auto messageCbOdometryMessage0101 = &sick_lidar_localization::UDPSender::messageCbOdometryMessage0101ROS2; auto messageCbOdometryMessage0104 = &sick_lidar_localization::UDPSender::messageCbOdometryMessage0104ROS2; auto messageCbOdometryMessage0105 = &sick_lidar_localization::UDPSender::messageCbOdometryMessage0105ROS2; auto messageCbEncoderMeasurementMessage0202 = &sick_lidar_localization::UDPSender::messageCbEncoderMeasurementMessage0202ROS2; auto messageCbCodeMeasurementMessage0303 = &sick_lidar_localization::UDPSender::messageCbCodeMeasurementMessage0303ROS2; + auto messageCbCodeMeasurementMessage0701 = &sick_lidar_localization::UDPSender::messageCbCodeMeasurementMessage0701ROS2; auto messageCbLineMeasurementMessage0403 = &sick_lidar_localization::UDPSender::messageCbLineMeasurementMessage0403ROS2; auto messageCbLineMeasurementMessage0404 = &sick_lidar_localization::UDPSender::messageCbLineMeasurementMessage0404ROS2; #endif # ifdef _MSC_VER m_subOdomROS = rosSubscriber(nh->create_subscription(odom_topic, 10, std::bind(&sick_lidar_localization::UDPSender::messageCbOdomROS2, this, std::placeholders::_1))); - m_subOdometryMessage0101 = rosSubscriber(nh->create_subscription("/localizationcontroller/in/odometry_message_0101", 10, std::bind(&sick_lidar_localization::UDPSender::messageCbOdometryMessage0101ROS2, this, std::placeholders::_1))); m_subOdometryMessage0104 = rosSubscriber(nh->create_subscription("/localizationcontroller/in/odometry_message_0104", 10, std::bind(&sick_lidar_localization::UDPSender::messageCbOdometryMessage0104ROS2, this, std::placeholders::_1))); m_subOdometryMessage0105 = rosSubscriber(nh->create_subscription("/localizationcontroller/in/odometry_message_0105", 10, std::bind(&sick_lidar_localization::UDPSender::messageCbOdometryMessage0105ROS2, this, std::placeholders::_1))); m_subEncoderMeasurementMessage0202 = rosSubscriber(nh->create_subscription("/localizationcontroller/in/encoder_measurement_message_0202", 10, std::bind(&sick_lidar_localization::UDPSender::messageCbEncoderMeasurementMessage0202ROS2, this, std::placeholders::_1))); m_subCodeMeasurementMessage0303 = rosSubscriber(nh->create_subscription("/localizationcontroller/in/code_measurement_message_0303", 10, std::bind(&sick_lidar_localization::UDPSender::messageCbCodeMeasurementMessage0303ROS2, this, std::placeholders::_1))); + m_subCodeMeasurementMessage0701 = rosSubscriber(nh->create_subscription("/localizationcontroller/in/code_measurement_message_0701", 10, std::bind(&sick_lidar_localization::UDPSender::messageCbCodeMeasurementMessage0701ROS2, this, std::placeholders::_1))); m_subLineMeasurementMessage0403 = rosSubscriber(nh->create_subscription("/localizationcontroller/in/line_measurement_message_0403", 10, std::bind(&sick_lidar_localization::UDPSender::messageCbLineMeasurementMessage0403ROS2, this, std::placeholders::_1))); m_subLineMeasurementMessage0404 = rosSubscriber(nh->create_subscription("/localizationcontroller/in/line_measurement_message_0404", 10, std::bind(&sick_lidar_localization::UDPSender::messageCbLineMeasurementMessage0404ROS2, this, std::placeholders::_1))); #else m_subOdomROS = rosSubscribe(nh, odom_topic, messageCbOdomROS, this); - m_subOdometryMessage0101 = rosSubscribe(nh, "/localizationcontroller/in/odometry_message_0101", messageCbOdometryMessage0101, this); m_subOdometryMessage0104 = rosSubscribe(nh, "/localizationcontroller/in/odometry_message_0104", messageCbOdometryMessage0104, this); m_subOdometryMessage0105 = rosSubscribe(nh, "/localizationcontroller/in/odometry_message_0105", messageCbOdometryMessage0105, this); m_subEncoderMeasurementMessage0202 = rosSubscribe(nh, "/localizationcontroller/in/encoder_measurement_message_0202", messageCbEncoderMeasurementMessage0202, this); m_subCodeMeasurementMessage0303 = rosSubscribe(nh, "/localizationcontroller/in/code_measurement_message_0303", messageCbCodeMeasurementMessage0303, this); + m_subCodeMeasurementMessage0701 = rosSubscribe(nh, "/localizationcontroller/in/code_measurement_message_0701", messageCbCodeMeasurementMessage0701, this); m_subLineMeasurementMessage0403 = rosSubscribe(nh, "/localizationcontroller/in/line_measurement_message_0403", messageCbLineMeasurementMessage0403, this); m_subLineMeasurementMessage0404 = rosSubscribe(nh, "/localizationcontroller/in/line_measurement_message_0404", messageCbLineMeasurementMessage0404, this); #endif @@ -258,11 +258,11 @@ bool sick_lidar_localization::UDPSender::init(void) close(); } m_udp_sender_impl = new sick_lidar_localization::UDPSenderImpl(); - if (!m_udp_sender_impl->initSocket(m_sim_ip_address, m_udp_port_sim_input)) + if (!m_udp_sender_impl->initSocket(m_lls_ip_address, m_udp_port_lls_input)) { delete(m_udp_sender_impl); m_udp_sender_impl = 0; - ROS_ERROR_STREAM("## ERROR UDPSender::init(" << m_sim_ip_address << ", " << m_udp_port_sim_input << ") failed"); + ROS_ERROR_STREAM("## ERROR UDPSender::init(" << m_lls_ip_address << ", " << m_udp_port_lls_input << ") failed"); return false; } return true; @@ -294,7 +294,7 @@ bool sick_lidar_localization::UDPSender::sendData(const std::vector& ud } if (!m_udp_sender_impl) { - ROS_ERROR_STREAM("## ERROR UDPSender: can't init udp socket, UDPSender(" << m_sim_ip_address << ", " << m_udp_port_sim_input << ") failed"); + ROS_ERROR_STREAM("## ERROR UDPSender: can't init udp socket, UDPSender(" << m_lls_ip_address << ", " << m_udp_port_lls_input << ") failed"); return false; } bool ok = m_udp_sender_impl->sendData(udp_data.data(), (int)udp_data.size()); @@ -320,6 +320,17 @@ static double quaternionToYawAngle(double w, double x, double y, double z) return std::atan2(siny_cosp, cosy_cosp); } +/** returns the default source id from configuration file */ +int sick_lidar_localization::UDPDefaultInputSourceId::getSourceId(int msg_type, int msg_version) +{ + if (m_msgtype_version_sourceid_map.find(msg_type) == m_msgtype_version_sourceid_map.end() + || m_msgtype_version_sourceid_map[msg_type].find(msg_version) == m_msgtype_version_sourceid_map[msg_type].end()) + { + m_msgtype_version_sourceid_map[msg_type][msg_version] = m_default_source_id; + } + return m_msgtype_version_sourceid_map[msg_type][msg_version]; +} + #if __ROS_VERSION > 0 /** subscriber callback function for ros odom messages */ @@ -329,7 +340,6 @@ void sick_lidar_localization::UDPSender::messageCbOdomROS(const ros_nav_msgs::Od rosTime timestamp_msg = msg.header.stamp; uint64_t timestamp_sec = sec(timestamp_msg); uint64_t timestamp_nsec = nsec(timestamp_msg); - uint64_t timestamp_msec = 1000 * timestamp_sec + timestamp_nsec / 1000000;// message timestamp in milliseconds int64_t timestamp_mircrosec = 1000000 * timestamp_sec + timestamp_nsec / 1000;// message timestamp in microseconds double vx_ms = msg.twist.twist.linear.x; // vx in m/s double vy_ms = msg.twist.twist.linear.y; // vy in m/s @@ -344,21 +354,6 @@ void sick_lidar_localization::UDPSender::messageCbOdomROS(const ros_nav_msgs::Od static uint64_t s_telegram_count = 0; s_telegram_count++; - // Convert to OdometryPayload0101 message - if(m_ros_odom_to_udp_msg == 0) - { - sick_lidar_localization::UDPMessage::OdometryPayload0101 odometry0101; - odometry0101.telegram_count = s_telegram_count; - odometry0101.timestamp = timestamp_msec; - odometry0101.x_velocity = vx_mms; - odometry0101.y_velocity = vy_mms; - odometry0101.angular_velocity = omega_mdegs; - // Send OdometryMessage0101 - if (m_verbose) - ROS_INFO_STREAM("sick_lidar_localization::UDPSender: sending odom message (vx=" << vx_ms << ", vy=" << vy_ms << ", omega=" << omega_rs << ") converted to OdometryPayload0101"); - if (!sendUDPPayload(odometry0101, true, false, m_source_id)) - ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::messageCbOdomROS(): sendUDPPayload(" << sick_lidar_localization::UDPMessage::printPayload(odometry0101) << ") failed"); - } // Convert to OdometryPayload0104 message if(m_ros_odom_to_udp_msg == 1 || m_ros_odom_to_udp_msg == 3) { @@ -368,10 +363,11 @@ void sick_lidar_localization::UDPSender::messageCbOdomROS(const ros_nav_msgs::Od odometry0104.x_velocity = vx_mms; odometry0104.y_velocity = vy_mms; odometry0104.angular_velocity = omega_mdegs; + odometry0104.source_id = m_source_id_cfg.getSourceId(1, 4); // Send OdometryMessage0104 if (m_verbose) ROS_INFO_STREAM("sick_lidar_localization::UDPSender: sending odom message (vx=" << vx_ms << ", vy=" << vy_ms << ", omega=" << omega_rs << ") converted to OdometryPayload0104"); - if (!sendUDPPayload(odometry0104, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT, m_source_id)) + if (!sendUDPPayload(odometry0104, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT)) ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::messageCbOdometryMessage0104(): sendUDPPayload(" << sick_lidar_localization::UDPMessage::printPayload(odometry0104) << ") failed"); } // Convert to OdometryPayload0105 message @@ -383,10 +379,11 @@ void sick_lidar_localization::UDPSender::messageCbOdomROS(const ros_nav_msgs::Od odometry0105.x_position = x_pos_mm; odometry0105.y_position = y_pos_mm; odometry0105.heading = heading_mdeg; + odometry0105.source_id = m_source_id_cfg.getSourceId(1, 5); // Send OdometryMessage0105 if (m_verbose) ROS_INFO_STREAM("sick_lidar_localization::UDPSender: sending odom message (x=" << x_pos_mm << ", y=" << y_pos_mm << ", heading=" << heading_mdeg << ") converted to OdometryPayload0105"); - if (!sendUDPPayload(odometry0105, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT, m_source_id)) + if (!sendUDPPayload(odometry0105, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT)) ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::OdometryPayload0105(): sendUDPPayload(" << sick_lidar_localization::UDPMessage::printPayload(odometry0105) << ") failed"); } if(m_ros_odom_to_udp_msg < 0 || m_ros_odom_to_udp_msg > 3) @@ -395,19 +392,6 @@ void sick_lidar_localization::UDPSender::messageCbOdomROS(const ros_nav_msgs::Od } } -/** subscriber callback function for input udp messages type OdometryMessage0101 */ -void sick_lidar_localization::UDPSender::messageCbOdometryMessage0101(const sick_lidar_localization::OdometryMessage0101 & msg) -{ - sick_lidar_localization::UDPMessage::OdometryPayload0101 odometry0101; - odometry0101.telegram_count = msg.telegram_count; - odometry0101.timestamp = msg.timestamp; - odometry0101.x_velocity = msg.x_velocity; - odometry0101.y_velocity = msg.y_velocity; - odometry0101.angular_velocity = msg.angular_velocity; - if (!sendUDPPayload(odometry0101, true, false, m_source_id)) - ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::messageCbOdometryMessage0101(): sendUDPPayload(" << sick_lidar_localization::UDPMessage::printPayload(odometry0101) << ") failed"); -} - /** subscriber callback function for input udp messages type OdometryMessage0104 */ void sick_lidar_localization::UDPSender::messageCbOdometryMessage0104(const sick_lidar_localization::OdometryMessage0104 & msg) { @@ -417,7 +401,10 @@ void sick_lidar_localization::UDPSender::messageCbOdometryMessage0104(const sick odometry0104.x_velocity = msg.x_velocity; odometry0104.y_velocity = msg.y_velocity; odometry0104.angular_velocity = msg.angular_velocity; - if (!sendUDPPayload(odometry0104, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT, m_source_id)) + odometry0104.source_id = msg.source_id; + if (odometry0104.source_id <= 0) + odometry0104.source_id = m_source_id_cfg.getSourceId(1, 4); + if (!sendUDPPayload(odometry0104, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT)) ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::messageCbOdometryMessage0104(): sendUDPPayload(" << sick_lidar_localization::UDPMessage::printPayload(odometry0104) << ") failed"); } @@ -430,7 +417,10 @@ void sick_lidar_localization::UDPSender::messageCbOdometryMessage0105(const sick odometry0105.x_position = msg.x_position; odometry0105.y_position = msg.y_position; odometry0105.heading = msg.heading; - if (!sendUDPPayload(odometry0105, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT, m_source_id)) + odometry0105.source_id = msg.source_id; + if (odometry0105.source_id <= 0) + odometry0105.source_id = m_source_id_cfg.getSourceId(1, 5); + if (!sendUDPPayload(odometry0105, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT)) ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::OdometryPayload0105(): sendUDPPayload(" << sick_lidar_localization::UDPMessage::printPayload(odometry0105) << ") failed"); } @@ -441,7 +431,10 @@ void sick_lidar_localization::UDPSender::messageCbEncoderMeasurementMessage0202( encoder_measurement0202.telegram_count = msg.telegram_count; encoder_measurement0202.timestamp = msg.timestamp; encoder_measurement0202.encoder_value = msg.encoder_value; - if (!sendUDPPayload(encoder_measurement0202, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT, m_source_id)) + encoder_measurement0202.source_id = msg.source_id; + if (encoder_measurement0202.source_id <= 0) + encoder_measurement0202.source_id = m_source_id_cfg.getSourceId(2, 2); + if (!sendUDPPayload(encoder_measurement0202, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT)) ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::messageCbEncoderMeasurementMessage0202(): sendUDPPayload(" << sick_lidar_localization::UDPMessage::printPayload(encoder_measurement0202) << ") failed"); } @@ -452,10 +445,30 @@ void sick_lidar_localization::UDPSender::messageCbCodeMeasurementMessage0303(con code_measurement0303.telegram_count = msg.telegram_count; code_measurement0303.timestamp = msg.timestamp; code_measurement0303.code = msg.code; - if (!sendUDPPayload(code_measurement0303, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT, m_source_id)) + code_measurement0303.source_id = msg.source_id; + if (code_measurement0303.source_id <= 0) + code_measurement0303.source_id = m_source_id_cfg.getSourceId(3, 3); + if (!sendUDPPayload(code_measurement0303, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT)) ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::messageCbCodeMeasurementMessage0303(): sendUDPPayload(" << sick_lidar_localization::UDPMessage::printPayload(code_measurement0303) << ") failed"); } +/** subscriber callback function for input udp messages type messageCbCodeMeasurementMessage0701 */ +void sick_lidar_localization::UDPSender::messageCbCodeMeasurementMessage0701(const sick_lidar_localization::CodeMeasurementMessage0701 & msg) +{ + sick_lidar_localization::UDPMessage::CodeMeasurementPayload0701 code_measurement0701; + code_measurement0701.telegram_count = msg.telegram_count; + code_measurement0701.timestamp = msg.timestamp; + code_measurement0701.code = msg.code; + code_measurement0701.x_position = msg.x_position; + code_measurement0701.y_position = msg.y_position; + code_measurement0701.heading = msg.heading; + code_measurement0701.source_id = msg.source_id; + if (code_measurement0701.source_id <= 0) + code_measurement0701.source_id = m_source_id_cfg.getSourceId(7, 1); + if (!sendUDPPayload(code_measurement0701, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT)) + ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::messageCbCodeMeasurementMessage0701(): sendUDPPayload(" << sick_lidar_localization::UDPMessage::printPayload(code_measurement0701) << ") failed"); +} + /** subscriber callback function for input udp messages type messageCbLineMeasurementMessage0403 */ void sick_lidar_localization::UDPSender::messageCbLineMeasurementMessage0403(const sick_lidar_localization::LineMeasurementMessage0403 & msg) { @@ -464,7 +477,10 @@ void sick_lidar_localization::UDPSender::messageCbLineMeasurementMessage0403(con line_measurement0403.timestamp = msg.timestamp; line_measurement0403.num_lanes = msg.num_lanes; line_measurement0403.lanes = msg.lanes; - if (!sendUDPPayload(line_measurement0403, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT, m_source_id)) + line_measurement0403.source_id = msg.source_id; + if (line_measurement0403.source_id <= 0) + line_measurement0403.source_id = m_source_id_cfg.getSourceId(4, 3); + if (!sendUDPPayload(line_measurement0403, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT)) ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::messageCbLineMeasurementMessage0403(): sendUDPPayload(" << sick_lidar_localization::UDPMessage::printPayload(line_measurement0403) << ") failed"); } @@ -479,7 +495,10 @@ void sick_lidar_localization::UDPSender::messageCbLineMeasurementMessage0404(con line_measurement0404.lcp3 = msg.lcp3; line_measurement0404.cnt_lpc = msg.cnt_lpc; line_measurement0404.reserved = 0; - if (!sendUDPPayload(line_measurement0404, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT, m_source_id)) + line_measurement0404.source_id = msg.source_id; + if (line_measurement0404.source_id <= 0) + line_measurement0404.source_id = m_source_id_cfg.getSourceId(4, 4); + if (!sendUDPPayload(line_measurement0404, UDP_HEADER_BIG_ENDIAN_DEFAULT, UDP_PAYLOAD_BIG_ENDIAN_DEFAULT)) ROS_ERROR_STREAM("## ERROR sick_lidar_localization::UDPSender::messageCbLineMeasurementMessage0404(): sendUDPPayload(" << sick_lidar_localization::UDPMessage::printPayload(line_measurement0404) << ") failed"); } #endif // __ROS_VERSION > 0 diff --git a/srv/LocStartLocalizingSrv.srv b/srv/LocStartSrv.srv similarity index 64% rename from srv/LocStartLocalizingSrv.srv rename to srv/LocStartSrv.srv index e0e9e3d..9e15ea7 100644 --- a/srv/LocStartLocalizingSrv.srv +++ b/srv/LocStartSrv.srv @@ -1,7 +1,7 @@ -# Definition of ROS service LocStartLocalizing for sick localization. +# Definition of ROS service LocStart for sick localization. # -# ROS service LocStartLocalizing starts localization -# by sending cola command ("sMN LocStartLocalizing"). +# ROS service LocStart starts localization +# by sending cola command ("sMN LocStart"). # # See Telegram-Listing-v1.1.0.241R.pdf for further details about # Cola telegrams and this command. diff --git a/test/config/rviz2_sick_lidar_localization_pointcloud.rviz2 b/test/config/rviz2_sick_lidar_localization_pointcloud.rviz2 index a13734f..2bc6a93 100644 --- a/test/config/rviz2_sick_lidar_localization_pointcloud.rviz2 +++ b/test/config/rviz2_sick_lidar_localization_pointcloud.rviz2 @@ -6,8 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /PointCloud21 - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 Splitter Ratio: 0.5 Tree Height: 617 - Class: rviz_common/Selection @@ -44,63 +45,42 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 50 - Size (m): 0.10000000149011612 - 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/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true - cloud: + lls: Value: true - sick_lidar_localization: + map: Value: true Marker Scale: 5 Name: TF Show Arrows: true Show Axes: true - Show Names: false + Show Names: true Tree: - cloud: - sick_lidar_localization: + map: + lls: {} Update Interval: 0 Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lls/marker + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: cloud + Fixed Frame: map Frame Rate: 30 Name: root Tools: @@ -174,5 +154,5 @@ Window Geometry: Views: collapsed: true Width: 1200 - X: 344 - Y: 60 + X: 1284 + Y: 66 diff --git a/test/config/rviz2_win64_sick_lidar_localization_pointcloud.rviz b/test/config/rviz2_win64_sick_lidar_localization_pointcloud.rviz index a4f5d62..9bed1bd 100644 --- a/test/config/rviz2_win64_sick_lidar_localization_pointcloud.rviz +++ b/test/config/rviz2_win64_sick_lidar_localization_pointcloud.rviz @@ -6,8 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /PointCloud21 - /TF1 + - /TF1/Frames1 + - /MarkerArray1/Topic1 Splitter Ratio: 0.5 Tree Height: 757 - Class: rviz_common/Selection @@ -44,63 +45,42 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /cloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true - cloud: + lls: Value: true - sick_lidar_localization: + map: Value: true Marker Scale: 5 Name: TF Show Arrows: true Show Axes: true - Show Names: false + Show Names: true Tree: - cloud: - sick_lidar_localization: + map: + lls: {} Update Interval: 0 Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lls/marker + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: cloud + Fixed Frame: map Frame Rate: 30 Name: root Tools: @@ -174,5 +154,5 @@ Window Geometry: Views: collapsed: true Width: 1656 - X: 404 - Y: 121 + X: 641 + Y: 71 diff --git a/test/config/rviz_sick_lidar_localization_pointcloud.rviz b/test/config/rviz_sick_lidar_localization_pointcloud.rviz index e0bf245..8d35953 100644 --- a/test/config/rviz_sick_lidar_localization_pointcloud.rviz +++ b/test/config/rviz_sick_lidar_localization_pointcloud.rviz @@ -5,10 +5,9 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /PointCloud21 - /TF1 - /TF1/Frames1 - - /TF1/Tree1 + - /MarkerArray1/Namespaces1 Splitter Ratio: 0.5 Tree Height: 848 - Class: rviz/Selection @@ -26,10 +25,9 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 - SyncSource: PointCloud2 + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: @@ -55,61 +53,49 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 10 - Size (m): 0.009999999776482582 - Style: Points - Topic: /cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Class: rviz/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true - cloud: + lls: Value: true - sick_lidar_localization: + map: Value: true + Marker Alpha: 1 Marker Scale: 5 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - cloud: - sick_lidar_localization: + map: + lls: {} Update Interval: 0 Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /lls/marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: false + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: cloud + Fixed Frame: map Frame Rate: 30 Name: root Tools: @@ -133,12 +119,13 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 14.366720199584961 + Distance: 18.02161407470703 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: -4.796201705932617 Y: 4.4470672607421875 @@ -150,7 +137,6 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.7803980112075806 Target Frame: - Value: Orbit (rviz) Yaw: 0.7153980731964111 Saved: ~ Window Geometry: @@ -159,7 +145,7 @@ Window Geometry: Height: 1145 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000003dbfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003db000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006340000003efc0100000002fb0000000800540069006d0065010000000000000634000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004d8000003db00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000003dbfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003db000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006340000003efc0100000002fb0000000800540069006d0065010000000000000634000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004d8000003db00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -169,5 +155,5 @@ Window Geometry: Views: collapsed: true Width: 1588 - X: 724 + X: 908 Y: 27 diff --git a/test/data/simu_unittests/linux_unittest_gen_sick_caller.txt b/test/data/simu_unittests/linux_unittest_gen_sick_caller.txt index 713dc56..ece34cd 100644 --- a/test/data/simu_unittests/linux_unittest_gen_sick_caller.txt +++ b/test/data/simu_unittests/linux_unittest_gen_sick_caller.txt @@ -16,7 +16,7 @@ { "request": "LocSetOdometryActive", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, { "request": "LocSetRecordingActive", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, { "request": "LocSetRingBufferRecordingActive", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, -{ "request": "LocStartLocalizing", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, +{ "request": "LocStart", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, { "request": "LocStop", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, { "request": "LocSwitchMap", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, { "request": "LocGetLocalizationStatus", "response": { "data": { "details": "LOCALIZING", "locStatus": 1 }, "header": { "message": "Ok", "status": 0 } } }, diff --git a/test/data/simu_unittests/win64_unittest_gen_sick_caller.txt b/test/data/simu_unittests/win64_unittest_gen_sick_caller.txt index 713dc56..ece34cd 100644 --- a/test/data/simu_unittests/win64_unittest_gen_sick_caller.txt +++ b/test/data/simu_unittests/win64_unittest_gen_sick_caller.txt @@ -16,7 +16,7 @@ { "request": "LocSetOdometryActive", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, { "request": "LocSetRecordingActive", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, { "request": "LocSetRingBufferRecordingActive", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, -{ "request": "LocStartLocalizing", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, +{ "request": "LocStart", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, { "request": "LocStop", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, { "request": "LocSwitchMap", "response": { "data": { "success": true }, "header": { "message": "Ok", "status": 0 } } }, { "request": "LocGetLocalizationStatus", "response": { "data": { "details": "LOCALIZING", "locStatus": 1 }, "header": { "message": "Ok", "status": 0 } } }, diff --git a/test/rest_server/python/README.md b/test/rest_server/python/README.md new file mode 100644 index 0000000..5a9a853 --- /dev/null +++ b/test/rest_server/python/README.md @@ -0,0 +1,61 @@ +# Test scripts for sick_lidar_localization + +Folder `test/rest_server/python` contains the following python test scripts for sick_lidar_localization: + +* [lls_pcapng_player.py](lls_pcapng_player.py): plays a pcapng-file recorded by Wireshark and broadcasts udp messages to simulate a localization controller.
+ Usage example: + ``` + python3 test/rest_server/python/lls_pcapng_player.py --pcap_filename test/data/wireshark/20210816_lidarloc2_2.0.0.14R_moving.pcapng + ``` + +* [lls_udp_receiver.py](lls_udp_receiver.py): receives udp messages from sick_lidar_localization, parses the messages and prints received localization data.
+ Usage example: + ``` + python3 test/rest_server/python/lls_udp_receiver.py --udp_port=5009 + ``` + +* [lls_udp_sender.py](lls_udp_sender.py): generates synthetical datagrams to emulate a local localization controller and sends corresponding UDP messages.
+ Usage example: + ``` + python3 test/rest_server/python/lls_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --max_message_count=300 + ``` + +* [send_odometry_message_0104.py](send_odometry_message_0104.py): creates and publishes odometry messages type 1 version 4 (ROS-1)
+ Usage example: + ``` + python test/rest_server/python/send_odometry_message_0104.py + ``` + Alternatively or on ROS-2, odometry messages can be published e.g. by + ``` + ros2 topic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 '{telegram_count: 1000001, timestamp: 123456789, source_id: 0, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' + ``` + +* [send_odometry_message_0105.py](send_odometry_message_0105.py): creates and publishes odometry messages type 1 version 5 (ROS-1)
+ Usage example: + ``` + python test/rest_server/python/send_odometry_message_0105.py + ``` + Alternatively or on ROS-2, odometry messages can be published e.g. by + ``` + ros2 topic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 '{telegram_count: 1000002, timestamp: 123456780, source_id: 0, x_position: -1234, y_position: -1234, heading: 1234}' + ``` + +* [send_ros_odom_messages.py](send_ros_odom_messages.py): creates and publishes ROS messages of type `nav_msgs.msg.Odometry` (ROS-1)
+ Usage example: + ``` + python test/rest_server/python/send_ros_odom_messages.py + ``` + +* [send_ros2_odom_messages.py](send_ros2_odom_messages.py): creates and publishes ROS messages of type `nav_msgs.msg.Odometry` (ROS-2)
+ Usage example: + ``` + python3 test/rest_server/python/send_ros2_odom_messages.py + ``` + +* [sick_rest_server.py](sick_rest_server.py): implements a tiny rest server, responds to http get- and post-request for local offline tests.
+ Usage example: + ``` + sudo python3 ../rest_server/python/sick_rest_server.py + ``` + +See the bash- and cmd-scripts in folder `test/scripts` for complete usage examples. diff --git a/test/rest_server/python/sim_pcapng_player.py b/test/rest_server/python/lls_pcapng_player.py similarity index 99% rename from test/rest_server/python/sim_pcapng_player.py rename to test/rest_server/python/lls_pcapng_player.py index 13fbf4a..eacd569 100644 --- a/test/rest_server/python/sim_pcapng_player.py +++ b/test/rest_server/python/lls_pcapng_player.py @@ -37,7 +37,7 @@ udp_port = cli_args.udp_port print("pcapng_player {} initializing, udp port {} ...".format(pcap_filename, udp_port)) - # Init upd sender + # Init udp 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("pcapng_player: sending on udp port {}".format(udp_port)) diff --git a/test/rest_server/python/sim_udp_receiver.py b/test/rest_server/python/lls_udp_receiver.py similarity index 85% rename from test/rest_server/python/sim_udp_receiver.py rename to test/rest_server/python/lls_udp_receiver.py index 855ee1f..7d0684f 100644 --- a/test/rest_server/python/sim_udp_receiver.py +++ b/test/rest_server/python/lls_udp_receiver.py @@ -29,12 +29,12 @@ def decode(payload, big_endian = True, signed_value = False): cli_args = arg_parser.parse_args() udp_port = cli_args.udp_port - # Init upd receiver + # Init udp receiver udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) # UDP socket udp_socket.bind(("", udp_port)) # Receive udp datagrams - print("sim_udp_receiver listening on port {} ...".format(udp_port)) + print("lls_udp_receiver listening on port {} ...".format(udp_port)) while True: data, addr = udp_socket.recvfrom(1024) # buffer size is 1024 bytes if len(data) >= 16 and data[0:4] == b"\x4d\x4f\x50\x53": # 4 byte Magic 0x4d 0x4f 0x50 0x53 ("MOPS") @@ -56,7 +56,7 @@ def decode(payload, big_endian = True, signed_value = False): header_is_big_endian = False payload_is_big_endian = True else: - print("## ERROR sim_udp_receiver: unknown message header encoding") + print("## ERROR lls_udp_receiver: unknown message header encoding") continue payloadlen = decode(data[6:8], header_is_big_endian) @@ -64,15 +64,7 @@ def decode(payload, big_endian = True, signed_value = False): msgtypeversion = decode(data[12:13], header_is_big_endian) # int(data[13]) print("{} byte udp message received, msgtype {}, msgtypeversion {}, {} byte payload".format(len(data), msgtype, msgtypeversion, payloadlen)) - if msgtype == 1 and msgtypeversion == 1 and payloadlen == 32: - telegram_count = decode(data[16:24], payload_is_big_endian) - timestamp = decode(data[24:32], payload_is_big_endian) - x_velocity = decode(data[32:36], payload_is_big_endian, True) - y_velocity = decode(data[36:40], payload_is_big_endian, True) - angular_velocity = decode(data[40:48], payload_is_big_endian, True) - print("OdometryMessage0101:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", x_velocity:" + str(x_velocity) + ", y_velocity:" + str(y_velocity) + ", angular_velocity:" + str(angular_velocity) + "}") - - elif msgtype == 1 and msgtypeversion == 4 and payloadlen == 24: + if msgtype == 1 and msgtypeversion == 4 and payloadlen == 24: telegram_count = decode(data[16:24], payload_is_big_endian) timestamp = decode(data[24:32], payload_is_big_endian) x_velocity = decode(data[32:34], payload_is_big_endian, True) @@ -121,8 +113,18 @@ def decode(payload, big_endian = True, signed_value = False): cnt_lpc = int(data[38]) # decode(data[38:39], payload_is_big_endian) print("LineMeasurementMessage0404:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", lcp1:" + str(lcp1) + ", lcp2:" + str(lcp2) + ", lcp3:" + str(lcp3) + ", cnt_lpc:" + str(cnt_lpc) + "}") + elif msgtype == 7 and msgtypeversion == 1 and payloadlen >= 30: + telegram_count = decode(data[16:24], payload_is_big_endian) + timestamp = decode(data[24:32], payload_is_big_endian) + codelength = decode(data[32:34], payload_is_big_endian, True) + code = data[34:(34+codelength)].decode("utf-8") + x_position = decode(data[(34+codelength):(34+codelength+4)], payload_is_big_endian, True) + y_position = decode(data[(34+codelength+4):(34+codelength+8)], payload_is_big_endian, True) + heading = decode(data[(34+codelength+8):(34+codelength+12)], payload_is_big_endian, True) + print("CodeMeasurementMessage0701:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", code:" + str(code) + ", x_position:" + str(x_position) + ", y_position:" + str(y_position) + ", heading:" + str(heading) + "}") + else: - print("## ERROR sim_udp_receiver: unknown message type or wrong payload length") + print("## ERROR lls_udp_receiver: unknown message type or wrong payload length") \ No newline at end of file diff --git a/test/rest_server/python/sim_udp_sender.py b/test/rest_server/python/lls_udp_sender.py similarity index 93% rename from test/rest_server/python/sim_udp_sender.py rename to test/rest_server/python/lls_udp_sender.py index 04fc14c..5aeb768 100644 --- a/test/rest_server/python/sim_udp_sender.py +++ b/test/rest_server/python/lls_udp_sender.py @@ -1,5 +1,5 @@ """ - sim_udp_sender generates synthetical datagrams to emulate a local localization server + lls_udp_sender generates synthetical datagrams to emulate a local localization server (specified by LocalizationController 2.0 UDP Data Format) and sends the UDP packages on port 5010. """ @@ -9,6 +9,8 @@ import socket import time +source_id = 0x0001 + # Encodes an integer to a bytearray of size in little or big endian. def encode(value, length, big_endian = False): payload = bytearray(length) @@ -45,7 +47,7 @@ def createHeader(msg_type, msg_type_version, payload_length, big_endian): payload[8:10] = encode(0x06c2, 2, True) # 2 byte PayloadType 0x06c2 for little endian payload[10:12] = encode(msg_type, 2, True) # 2 byte MsgType payload[12:14] = encode(msg_type_version, 2, True) # 2 byte MsgTypeVersion - payload[14:16] = encode(0x0001, 2, True) # 2 byte SourceID 0x0001 + payload[14:16] = encode(source_id, 2, True) # 2 byte SourceID 0x0001 return payload # Creates and returns an odometry message (type 1, version 4, 16 byte message header + 24 byte payload) @@ -57,7 +59,7 @@ def createOdometryMessage0104(telegram_count, timestamp, x_velocity, y_velocity, message[32:34] = encode(x_velocity, 2, big_endian) # 2 byte X-velocity int16 [mm/s] message[34:36] = encode(y_velocity, 2, big_endian) # 2 byte Y-velocity int16 [mm/s] message[36:40] = encode(angular_velocity, 4, big_endian) # 4 byte angular velocity int32 [mdeg/s] - message_str = "OdometryPayload0104:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", x_velocity:" + str(x_velocity) + ", y_velocity:" + str(y_velocity) + ", angular_velocity:" + str(angular_velocity) + "}" + message_str = "OdometryPayload0104:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", source_id:" + str(source_id) + ", x_velocity:" + str(x_velocity) + ", y_velocity:" + str(y_velocity) + ", angular_velocity:" + str(angular_velocity) + "}" return message, message_str # Creates and returns an odometry message (type 1, version 5, 16 byte message header + 40 byte payload) @@ -69,7 +71,7 @@ def createOdometryMessage0105(telegram_count, timestamp, x_position, y_position, message[32:40] = encode(x_position, 8, big_endian) # 8 byte X-position int64 [mm] message[40:48] = encode(y_position, 8, big_endian) # 8 byte Y-position int64 [mm] message[48:56] = encode(heading, 8, big_endian) # 8 byte heading int64 [mdeg] - message_str = "OdometryPayload0105:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", x_position:" + str(x_position) + ", y_position:" + str(y_position) + ", heading:" + str(heading) + "}" + message_str = "OdometryPayload0105:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", source_id:" + str(source_id) + ", x_position:" + str(x_position) + ", y_position:" + str(y_position) + ", heading:" + str(heading) + "}" return message, message_str # Creates and returns an code measurement message (type 3, version 4, 16 byte message header + 24 byte payload) @@ -80,7 +82,7 @@ def createCodeMeasurementMessage0304(telegram_count, timestamp, code, distance, message[24:32] = encode(timestamp, 8, big_endian) # 8 byte Timestamp uint64 [microseconds] message[32:36] = encode(code, 4, big_endian) # 4 byte Code int32 message[36:40] = encode(distance, 4, big_endian) # 4 byte Distance int32 [mm] - message_str = "CodeMeasurementPayload0304:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", code:" + str(code) + ", distance:" + str(distance) + "}" + message_str = "CodeMeasurementPayload0304:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", source_id:" + str(source_id) + ", code:" + str(code) + ", distance:" + str(distance) + "}" return message, message_str # Creates and returns a line measurement message (type 4, version 3, 16 byte message header + variable payload) @@ -93,7 +95,7 @@ def createLineMeasurementMessage0403(telegram_count, timestamp, lanes, big_endia message[32] = len(lanes) # 1 byte NumOfLanes uint8 for n, lane in enumerate(lanes): # variable number of lanes message[33+2*n+0:33+2*n+2] = encode(lane, 2, big_endian) # 2 byte lane int16 - message_str = "LineMeasurementPayload0403:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", num_lanes:" + str(len(lanes)) + ", lanes:[" + message_str = "LineMeasurementPayload0403:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", source_id:" + str(source_id) + ", num_lanes:" + str(len(lanes)) + ", lanes:[" for n, lane in enumerate(lanes): if n > 0: message_str = message_str + "," @@ -112,7 +114,7 @@ def createLineMeasurementMessage0404(telegram_count, timestamp, lcp1, lcp2, lcp3 message[36:38] = encode(lcp3, 2, big_endian) # 2 byte lcp3 int16 [mm] message[38] = cnt_lpc # 1 byte cnt_lpc uint8 message[39] = 0 # 1 byte memory alignment - message_str = "LineMeasurementPayload0404:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", lcp1:" + str(lcp1) + ", lcp2:" + str(lcp2) + ", lcp3:" + str(lcp3) + ", cnt_lpc:" + str(cnt_lpc) + "}" + message_str = "LineMeasurementPayload0404:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", source_id:" + str(source_id) + ", lcp1:" + str(lcp1) + ", lcp2:" + str(lcp2) + ", lcp3:" + str(lcp3) + ", cnt_lpc:" + str(cnt_lpc) + "}" return message, message_str # Creates and returns a LocalizationController result message (type 5, version 2, 16 byte message header + 40 byte payload) @@ -128,7 +130,7 @@ def createResultMessage0502(telegram_count, timestamp, x, y, heading, big_endian message[53] = 90 # 1 byte MapMatchingStatus [0...100, 90: Good] message[54] = 0 # 2 byte Reserved for memory alignment message[55] = 0 # 2 byte Reserved for memory alignment - message_str = "LocalizationControllerResultPayload0502:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", x:" + str(x) + ", y:" + str(y) + ", heading:" + str(heading) + ", loc_status:10, map_match_status:90}" + message_str = "LocalizationControllerResultPayload0502:{" + "telegram_count:" + str(telegram_count) + ", timestamp:" + str(timestamp) + ", source_id:" + str(source_id) + ", x:" + str(x) + ", y:" + str(y) + ", heading:" + str(heading) + ", loc_status:10, map_match_status:90}" return message, message_str if __name__ == "__main__": @@ -149,14 +151,14 @@ def createResultMessage0502(telegram_count, timestamp, x, y, heading, big_endian udp_send_rate = cli_args.udp_send_rate udp_output_logfile = cli_args.udp_output_logfile max_message_count = cli_args.max_message_count - print("sim_udp_sender --udp_port={} --udp_send_rate={} --udp_output_logfile={} --max_message_count={} initializing...".format(udp_port, udp_send_rate, udp_output_logfile, max_message_count)) + print("lls_udp_sender --udp_port={} --udp_send_rate={} --udp_output_logfile={} --max_message_count={} initializing...".format(udp_port, udp_send_rate, udp_output_logfile, max_message_count)) # Setup if udp_output_logfile != "" and os.path.exists(udp_output_logfile): os.remove(udp_output_logfile) unittestEncode() - # Init upd sender + # Init udp 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("udp_sender: sending on udp port {}".format(udp_port)) @@ -227,7 +229,7 @@ def createResultMessage0502(telegram_count, timestamp, x, y, heading, big_endian if message is None: telegram_idx = -1 msg_type_idx = msg_type_idx + 1 - if msg_type_idx > 5: + if msg_type_idx > 7: msg_type_idx = 0 continue; print("udp_sender: sending {} byte message = {}".format(len(message), message.hex())) diff --git a/test/rest_server/python/scripts_LLS1-OdometryUDPSender.py b/test/rest_server/python/scripts_LLS1-OdometryUDPSender.py deleted file mode 100644 index fe6dcc7..0000000 --- a/test/rest_server/python/scripts_LLS1-OdometryUDPSender.py +++ /dev/null @@ -1,182 +0,0 @@ -import socket -import math - -import time -starttime=time.time() - -# Network Socket -global sock - -# IP and Port for UDP Telegrams -UDP_IP = "192.168.0.1" # "192.168.1.1" -UDP_PORT = 5009 - -# Source ID from odom sender -> has to match the ID in the configuration file -SourceID = 21 # 31 - -global telegramCount -telegramCount = 0 - -#Found on https://stackoverflow.com/questions/25239423/crc-ccitt-16-bit-python-manual-calculation -#Using preset 0xffff instead of 0 (like in edp) -class CRC16: - def __init__(self): - self.poly = 0x1021 #CCITT polynom of CRC16 - self.preset = 0xffff - self._table = [ self._initial(i) for i in range(256)] - - def _initial(self, c): - crc = 0 - c = c << 8 - for _ in range(8): - if (crc ^ c) & 0x8000: - crc = (crc << 1) ^ self.poly - else: - crc = crc << 1 - c = c << 1 - return crc - - def _update_crc(self, crc, c): - cc = 0xff & c - tmp = (crc >> 8) ^ cc - crc = (crc << 8) ^ self._table[tmp & 0xff] - crc = crc & 0xffff - return crc - - def compute(self, msg): - crc = self.preset - for c in msg: - crc = self._update_crc(crc, c) - return crc - - - -def toHexPad(decINT, digits): - # Number of bits given the number of digits the hex string has (e.g. 4 Hex digits = 2 Byte = 16 Bit) - numBits = digits*4 - # Convert to hex (considers also negative numbers) + remove "\x" prefix - decINTHex = hex((decINT + (1 << numBits)) % (1 << numBits))[2:] - # Ensure correct length of hex string - return decINTHex.zfill(digits) - -#------------------------------------------------------------------------------------------ - -def sendOdometryTelegram(vx, vy, omega, ts): - - # Import socket - global sock - #Telegram counter - global telegramCount - - # ------------------------------------------------------------------------------------------------------------------------------------------------------ - #| HEADER | PAYLOAD | - # ------------------------------------------------------------------------------------------------------------------------------------------------------ - # MagicWord PayloadLength payloadType(endianess) MsgType MsgTypeVersion SourceID | telegramCounter timestamp Vx Vy Omega | - # 534d4f50 000C 0642 0000 0000 0000 | 00000001 00000000 0001 FFFF 00000001 | - # ------------------------------------------------------------------------------------------------------------------------------------------------------ - - - #---------------- HEADER --------------------------- - # Magic Word - magicWordInByte = 'SMOP'.encode('utf-8') - message = magicWordInByte.hex() - # Payload length (2Byte) in hex (length = 12 byte here) - message = message + toHexPad(16, 4) - # Using big endian - message = message + toHexPad(1602, 4) - # MsgType - message = message + toHexPad(1, 4) - # MsgType Version - message = message + toHexPad(1, 4) - # SourceID - message = message + toHexPad(SourceID, 4) - - #---------------- PAYLOAD --------------------------- - - # Add Telegram count - message = message + toHexPad(telegramCount, 8) - # Timestamp [in ms] - message = message + toHexPad(ts, 8) - # Vx [in mm/s] - message = message + toHexPad(vx, 4) - # Vy [in mm/s] - message = message + toHexPad(vy, 4) - # Omega [in mdeg/s] - message = message + toHexPad(omega, 8) - - - #print('########### DECIMAL ###################') - #print(telegramCount) - #print ("Timestamp :", ts) - #print ("Vx :", vx) - #print ("Vy :", vy) - #print ("Vomega :", omega) - - #print('########### HEX ###################') - #print(toHexPad(telegramCount, 8)) - #print(toHexPad(toHexPad(ts, 8)) - #print(toHexPad(vx, 4)) - #print(toHexPad(vy, 4)) - #print(toHexPad(omega, 8)) - - - # Compute CRC - CRC = CRC16() - crcIN = CRC.compute(bytes.fromhex(message)) - msgCRC16 = toHexPad(crcIN,4) - #print(msgCRC16) - - #Append crc16 to message - #message = message + msgCRC16 - #print ("message:", message) - - # Convert to bytes object - MESSAGEHEX = bytes.fromhex(message) - #print ("message:", MESSAGEHEX) - - # Send datagram - sock.sendto(MESSAGEHEX, (UDP_IP, UDP_PORT)) - - # Increment telegram counter - telegramCount = telegramCount + 1 - - -def startSendingTelegrams(): - - # Send a few telegram - #for i in range(0,5000,25): - # sendOdometryTelegram(1,0,-1, i) - ts = 0 - prevts = 0 - angVel = 10000 - while True: - ts = int(time.time()*1000) - sendOdometryTelegram(200,0,angVel, ts) - #if angVel == 0: - # angVel=25000 - #else: - # angVel=0 - # Delta between telegram timestamps - print("Delta ts: ", ts - prevts, " ms") - # Update previous telegram timestamps - prevts = ts - # Sleep in order to reach 25ms odom telegram cycle time - time.sleep(0.025 - (time.time() % 0.025)) - - - -def initilize(): - global sock - - print ("UDP target IP:", UDP_IP) - print ("UDP target port:", UDP_PORT) - - # UDP - sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - - # Start sending odometry messages - startSendingTelegrams() - - -if __name__ == '__main__': - initilize() \ No newline at end of file diff --git a/test/rest_server/python/scripts_LLS2-OdometryUDPSender.py b/test/rest_server/python/scripts_LLS2-OdometryUDPSender.py deleted file mode 100644 index 3cc5589..0000000 --- a/test/rest_server/python/scripts_LLS2-OdometryUDPSender.py +++ /dev/null @@ -1,122 +0,0 @@ -import socket -from struct import * -import time - -# IP of localization controller -LLS_DEVICE_IP = "192.168.0.1" # "192.168.0.70" - -# Input port specified in lidarloc_config.yml (communication/smopUdp/input/port) -CONFIG_UDP_PORT = 5009 -# Source ID for odom messages specified in lidarloc_config.yml (vehicle/odometer/external/interface/sourceId) -CONFIG_ODOM_SOURCE_ID = 21 # 100 - -# Network Socket -global sock -# Telegram Count -global telegramCount -telegramCount = 0 - -#------------------------------------------------------------------------------------------ - -def sendOdometryTelegram(vx, vy, omega, ts): - - # Import socket - global sock - #Telegram counter - global telegramCount - - # Odom Telegram Header Data - magicWord = "MOPS" - headerVersion = 2 - payloadLength = 24 - endianess = 1602 - msgType = 1 - msgTypeVersion = 4 - - # Odom Telegram (Header + Payload) - odom_telegram = pack(">4sHHHHHHQQhhi", - magicWord.encode("ASCII"), - headerVersion, - payloadLength, - endianess, - msgType, - msgTypeVersion, - CONFIG_ODOM_SOURCE_ID, - telegramCount, - ts, - vx, - vy, - omega) - - - print('########### DATA ###################') - print("MsgTypeVersion :", msgTypeVersion) - print("SourceID :", CONFIG_ODOM_SOURCE_ID) - print("Vx :", vx) - print("Vy :", vy) - print("Vomega: ", omega) - - # Send datagram - sock.sendto(odom_telegram, (LLS_DEVICE_IP, CONFIG_UDP_PORT)) - - # Increment telegram counter - telegramCount = telegramCount + 1 - - -def startSendingTelegrams(): - # Current and previous timestamp - ts = 0 - prevts = 0 - # Velocity - velX = 0 - velY = 0 - velAng = 0 - # Odom telegram frequency [in seconds] - odom_freq = 0.025 - # Odom message loop - while True: - # Generate timestamp in [us] - ts = int(time.time()*1000000) - # Send odom telegram - sendOdometryTelegram(velX,velY,velAng, ts) - # Delta between telegram timestamps - print("Delta ts: ", (ts - prevts)/1000, " ms") - # Update previous telegram timestamps - prevts = ts - # Sleep in order to reach 25ms odom telegram cycle time - time.sleep(odom_freq - (time.time() % odom_freq)) - - - -def initilize(): - global sock - - print ("UDP target IP:", LLS_DEVICE_IP) - print ("UDP target port:", CONFIG_UDP_PORT) - - # UDP - sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - - # Start sending odometry messages - startSendingTelegrams() - - -if __name__ == '__main__': - initilize() - - - - - - - - - - - - - - - - - diff --git a/test/rest_server/python/send_odometry_message_0101.py b/test/rest_server/python/send_odometry_message_0101.py deleted file mode 100644 index f825c2c..0000000 --- a/test/rest_server/python/send_odometry_message_0101.py +++ /dev/null @@ -1,66 +0,0 @@ -import math -import os -import rospy -import time -starttime=time.time() - -from sick_lidar_localization.msg import OdometryMessage0101 - -global telegramCount -telegramCount = 0 - -#------------------------------------------------------------------------------------------ - -def sendOdometryTelegram(pub, vx, vy, omega, ts): - - #Telegram counter - global telegramCount - - #odometry_msg_str = "{{telegram_count: {}, timestamp: {}, x_velocity: {}, y_velocity: {}, angular_velocity: {}}}".format(telegramCount, ts, vx, vy, omega) - #command = "rostopic pub --once /localizationcontroller/in/odometry_message_0101 sick_lidar_localization/OdometryMessage0101 \"{}\"".format(odometry_msg_str) - #print(command) - #os.system(command) - - odom_msg = OdometryMessage0101() - odom_msg.telegram_count = telegramCount - odom_msg.timestamp = ts - odom_msg.x_velocity = vx - odom_msg.y_velocity = vy - odom_msg.angular_velocity = omega - pub.publish(odom_msg) - - # Increment telegram counter - telegramCount = telegramCount + 1 - -def startSendingTelegrams(pub): - - # Send a few telegram - ts = 0 - prevts = 0 - angVel = -10000 - while telegramCount < 300: - if (telegramCount % 200) == 0: - angVel = -angVel - ts = int(time.time()*1000) - sendOdometryTelegram(pub, 200, 0, angVel, ts) - #if angVel == 0: - # angVel=25000 - #else: - # angVel=0 - # Delta between telegram timestamps - print("Delta ts: ", ts - prevts, " ms") - # Update previous telegram timestamps - prevts = ts - # Sleep in order to reach 25ms odom telegram cycle time - time.sleep(0.025 - (time.time() % 0.025)) - -def initialize(): - - rospy.init_node("send_odometry_message_0101", anonymous=True) - pub = rospy.Publisher("/localizationcontroller/in/odometry_message_0101", OdometryMessage0101, queue_size=10) - - # Start sending odometry messages - startSendingTelegrams(pub) - -if __name__ == '__main__': - initialize() \ No newline at end of file diff --git a/test/rest_server/python/send_odometry_message_0105.py b/test/rest_server/python/send_odometry_message_0105.py index 6700b65..b5165d3 100644 --- a/test/rest_server/python/send_odometry_message_0105.py +++ b/test/rest_server/python/send_odometry_message_0105.py @@ -38,7 +38,7 @@ def startSendingTelegrams(pub): if (telegramCount % 200) == 0: delta_heading = -delta_heading ts = int(time.time()*1000000) - sendOdometryTelegram(pub, 2000, 1000, heading, ts) + sendOdometryTelegram(pub, 2000, 1000, int(heading), ts) heading = heading + delta_heading # Delta between telegram timestamps print("Delta ts: ", ts - prevts, " microsec") diff --git a/test/rest_server/python/sick_rest_server.py b/test/rest_server/python/sick_rest_server.py index 7b149ca..b1b0eff 100644 --- a/test/rest_server/python/sick_rest_server.py +++ b/test/rest_server/python/sick_rest_server.py @@ -44,7 +44,7 @@ "LocSetOdometryActive" : { "header": std_header_ok, "data": { "success": True } }, "LocSetRecordingActive" : { "header": std_header_ok, "data": { "success": True } }, "LocSetRingBufferRecordingActive" : { "header": std_header_ok, "data": { "success": True } }, - "LocStartLocalizing" : { "header": std_header_ok, "data": { "success": True } }, + "LocStart" : { "header": std_header_ok, "data": { "success": True } }, "LocStop" : { "header": std_header_ok, "data": { "success": True } }, "LocSwitchMap" : { "header": std_header_ok, "data": { "success": True } }, "LocGetLocalizationStatus" : { "header": std_header_ok, "data": { "locStatus": 1, "details": "LOCALIZING" } }, diff --git a/test/scripts/make_ros2.cmd b/test/scripts/make_ros2.cmd index a171d23..b61bf26 100644 --- a/test/scripts/make_ros2.cmd +++ b/test/scripts/make_ros2.cmd @@ -34,7 +34,6 @@ start "sick_lidar_localization.sln" .\build\sick_lidar_localization\sick_lidar_l @echo. if not exist .\build\sick_lidar_localization\Release\gen_service_call.exe ( @echo colcon build gen_service_call.exe failed & @pause ) else ( @echo Successfully build gen_service_call.exe for ROS-2 Windows ) if not exist .\build\sick_lidar_localization\Release\sick_lidar_localization.exe ( @echo colcon build sick_lidar_localization.exe failed & @pause ) else ( @echo Successfully build sick_lidar_localization.exe for ROS-2 Windows ) -if not exist .\build\sick_lidar_localization\Release\pointcloud_converter.exe ( @echo colcon build pointcloud_converter.exe failed & @pause ) else ( @echo Successfully build pointcloud_converter.exe for ROS-2 Windows ) popd popd diff --git a/test/scripts/run_linux_ros1_simu.bash b/test/scripts/run_linux_ros1_simu.bash index 014b7c8..e96b031 100644 --- a/test/scripts/run_linux_ros1_simu.bash +++ b/test/scripts/run_linux_ros1_simu.bash @@ -41,7 +41,7 @@ function unittest_services() call_service LocSetOdometryActive "{active: 1}" "success: True" call_service LocSetRecordingActive "{active: 1}" "success: True" call_service LocSetRingBufferRecordingActive "{active: 1}" "success: True" - call_service LocStartLocalizing "{}" "success: True" + call_service LocStart "{}" "success: True" call_service LocStop "{}" "success: True" call_service LocSwitchMap "{submapname: \"test.vmap\"}" "success: True" call_service LocGetLocalizationStatus "{}" "locstatus: 1" @@ -50,7 +50,7 @@ function unittest_services() } # -# Run sick_lidar_localization simu on native linux +# Run sick_lidar_localization simu on Linux ROS-1 # printf "\033c" @@ -72,7 +72,7 @@ source /opt/ros/noetic/setup.bash source ./install/setup.bash SICK_LIDAR_LOCALIZATION_ROOT=./src/sick_lidar_localization if [ -d ./src/sick_lidar_localization2_pretest ] ; then SICK_LIDAR_LOCALIZATION_ROOT=./src/sick_lidar_localization2_pretest ; fi -roslaunch sick_lidar_localization sick_lidar_localization.launch hostname:=localhost udp_ip_sim_input:=127.0.0.1 udp_ip_sim_output:=localhost verbose:=1 & +roslaunch sick_lidar_localization sick_lidar_localization.launch hostname:=localhost udp_ip_lls_input:=127.0.0.1 udp_ip_lls_output:=localhost verbose:=1 & sleep 3 # @@ -94,16 +94,15 @@ rostopic echo /localizationcontroller/out/code_measurement_message_0304 & rostopic echo /localizationcontroller/out/line_measurement_message_0403 & rostopic echo /localizationcontroller/out/line_measurement_message_0404 & rostopic echo /localizationcontroller/out/localizationcontroller_result_message_0502 & -python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/sim_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --max_message_count=300 +python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/lls_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --max_message_count=300 sleep 3 # -# Run pointcloud_converter and visualize PointCloud2 messages by rviz: -# rviz -> Add by topic /cloud/PointCloud2 +# Run lls_transform and visualize TF messages by rviz: # rviz -> Add by display type TF # -roslaunch sick_lidar_localization pointcloud_converter.launch & +roslaunch sick_lidar_localization lls_transform.launch & sleep 3 rosrun rviz rviz -d $SICK_LIDAR_LOCALIZATION_ROOT/test/config/rviz_sick_lidar_localization_pointcloud.rviz & sleep 3 @@ -112,35 +111,43 @@ sleep 3 # Start pcapng player sending recorded UDP messages # -python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/sim_pcapng_player.py --pcap_filename $SICK_LIDAR_LOCALIZATION_ROOT/test/data/wireshark/20210816_lidarloc2_2.0.0.14R_moving.pcapng # 20210816_lidarloc2_2.0.0.14R_nonmoving.pcapng +python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/lls_pcapng_player.py --pcap_filename $SICK_LIDAR_LOCALIZATION_ROOT/test/data/wireshark/20210816_lidarloc2_2.0.0.14R_moving.pcapng # 20210816_lidarloc2_2.0.0.14R_nonmoving.pcapng sleep 3 # # Start udp receiver and send UDP input message examples # -python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/sim_udp_receiver.py --udp_port=5009 & -rostopic pub --once /localizationcontroller/in/odometry_message_0101 sick_lidar_localization/OdometryMessage0101 '{telegram_count: 1000001, timestamp: 123456789, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' -rostopic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 '{telegram_count: 1000001, timestamp: 123456789, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' -rostopic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 '{telegram_count: 1000002, timestamp: 123456780, x_position: -1234, y_position: -1234, heading: 1234}' -rostopic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 '{telegram_count: 1000003, timestamp: 123456781, encoder_value: 123456789}' -rostopic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 '{telegram_count: 1000004, timestamp: 123456782, code: 1234}' -rostopic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 '{telegram_count: 1000005, timestamp: 123456783, num_lanes: 1, lanes: [1234]}' -rostopic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 '{telegram_count: 1000006, timestamp: 123456784, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}' -python $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/send_ros_odom_messages.py -python $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/send_odometry_message_0101.py -python $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/send_odometry_message_0104.py -python $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/send_odometry_message_0105.py +python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/lls_udp_receiver.py --udp_port=5009 & +rostopic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 '{telegram_count: 1000001, timestamp: 123456789, source_id: 0, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' +rostopic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 '{telegram_count: 1000001, timestamp: 123456789, source_id: 2, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' +rostopic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 '{telegram_count: 1000002, timestamp: 123456780, source_id: 0, x_position: -1234, y_position: -1234, heading: 1234}' +rostopic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 '{telegram_count: 1000002, timestamp: 123456780, source_id: 3, x_position: -1234, y_position: -1234, heading: 1234}' +rostopic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 '{telegram_count: 1000003, timestamp: 123456781, source_id: 0, encoder_value: 123456789}' +rostopic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 '{telegram_count: 1000003, timestamp: 123456781, source_id: 4, encoder_value: 123456789}' +rostopic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 '{telegram_count: 1000004, timestamp: 123456782, source_id: 0, code: 1234}' +rostopic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 '{telegram_count: 1000004, timestamp: 123456782, source_id: 5, code: 1234}' +rostopic pub --once /localizationcontroller/in/code_measurement_message_0701 sick_lidar_localization/CodeMeasurementMessage0701 '{telegram_count: 1000004, timestamp: 123456782, source_id: 0, code: "1234", x_position: -1234, y_position: -2345, heading: -3456}' +rostopic pub --once /localizationcontroller/in/code_measurement_message_0701 sick_lidar_localization/CodeMeasurementMessage0701 '{telegram_count: 1000004, timestamp: 123456782, source_id: 6, code: "1234", x_position: -1234, y_position: -2345, heading: -3456}' +rostopic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 '{telegram_count: 1000005, timestamp: 123456783, source_id: 0, num_lanes: 1, lanes: [1234]}' +rostopic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 '{telegram_count: 1000005, timestamp: 123456783, source_id: 7, num_lanes: 1, lanes: [1234]}' +rostopic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 '{telegram_count: 1000006, timestamp: 123456784, source_id: 0, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}' +rostopic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 '{telegram_count: 1000006, timestamp: 123456784, source_id: 8, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}' +python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/send_ros_odom_messages.py +python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/send_odometry_message_0104.py +python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/send_odometry_message_0105.py # # Finish simulation # +echo -e "\nsick_lidar_localization simu finished, cleaning up\n" sleep 3 popd rosnode kill -a -pkill -f sim_udp_receiver.py +pkill -f lls_udp_receiver.py sudo pkill -f sick_rest_server.py killall sick_lidar_localization +killall lls_transform echo -e "\nsick_lidar_localization simu finished.\n" diff --git a/test/scripts/run_linux_ros2_simu.bash b/test/scripts/run_linux_ros2_simu.bash index 621ef58..ab4ae12 100644 --- a/test/scripts/run_linux_ros2_simu.bash +++ b/test/scripts/run_linux_ros2_simu.bash @@ -41,7 +41,7 @@ function unittest_services() call_service LocSetOdometryActive sick_lidar_localization/srv/LocSetOdometryActiveSrv "{active: 1}" "success=True" call_service LocSetRecordingActive sick_lidar_localization/srv/LocSetRecordingActiveSrv "{active: 1}" "success=True" call_service LocSetRingBufferRecordingActive sick_lidar_localization/srv/LocSetRingBufferRecordingActiveSrv "{active: 1}" "success=True" - call_service LocStartLocalizing sick_lidar_localization/srv/LocStartLocalizingSrv "{}" "success=True" + call_service LocStart sick_lidar_localization/srv/LocStartSrv "{}" "success=True" call_service LocStop sick_lidar_localization/srv/LocStopSrv "{}" "success=True" call_service LocSwitchMap sick_lidar_localization/srv/LocSwitchMapSrv "{submapname: \"test.vmap\"}" "success=True" call_service LocGetLocalizationStatus sick_lidar_localization/srv/LocGetLocalizationStatusSrv "{}" "locstatus=1" @@ -50,7 +50,7 @@ function unittest_services() } # -# Run sick_lidar_localization simu on native linux +# Run sick_lidar_localization simu on Linux ROS-2 # killall sick_lidar_localization printf "\033c" @@ -80,7 +80,7 @@ fi source ./install/setup.bash SICK_LIDAR_LOCALIZATION_ROOT=./src/sick_lidar_localization if [ -d ./src/sick_lidar_localization2_pretest ] ; then SICK_LIDAR_LOCALIZATION_ROOT=./src/sick_lidar_localization2_pretest ; fi -ros2 run sick_lidar_localization sick_lidar_localization $SICK_LIDAR_LOCALIZATION_ROOT/launch/sick_lidar_localization.launch --ros-args -p hostname:=localhost -p udp_ip_sim_input:=127.0.0.1 -p udp_ip_sim_output:=localhost -p verbose:=1 & +ros2 run sick_lidar_localization sick_lidar_localization $SICK_LIDAR_LOCALIZATION_ROOT/launch/sick_lidar_localization.launch --ros-args -p hostname:=localhost -p udp_ip_lls_input:=127.0.0.1 -p udp_ip_lls_output:=localhost -p verbose:=1 & sleep 3 # @@ -102,16 +102,15 @@ ros2 topic echo /localizationcontroller/out/code_measurement_message_0304 & ros2 topic echo /localizationcontroller/out/line_measurement_message_0403 & ros2 topic echo /localizationcontroller/out/line_measurement_message_0404 & ros2 topic echo /localizationcontroller/out/localizationcontroller_result_message_0502 & -python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/sim_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --max_message_count=300 +python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/lls_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --max_message_count=300 sleep 3 # -# Run pointcloud_converter and visualize PointCloud2 messages by rviz: -# rviz -> Add by topic /cloud/PointCloud2 +# Run lls_transform and visualize TF messages by rviz: # rviz -> Add by display type TF # -ros2 run sick_lidar_localization pointcloud_converter $SICK_LIDAR_LOCALIZATION_ROOT/launch/pointcloud_converter.launch & +ros2 run sick_lidar_localization lls_transform $SICK_LIDAR_LOCALIZATION_ROOT/launch/lls_transform.launch & sleep 3 rviz2 -d $SICK_LIDAR_LOCALIZATION_ROOT/test/config/rviz2_sick_lidar_localization_pointcloud.rviz2 & sleep 3 @@ -120,21 +119,28 @@ sleep 3 # Start pcapng player sending recorded UDP messages # -python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/sim_pcapng_player.py --pcap_filename $SICK_LIDAR_LOCALIZATION_ROOT/test/data/wireshark/20210816_lidarloc2_2.0.0.14R_moving.pcapng # 20210816_lidarloc2_2.0.0.14R_nonmoving.pcapng +python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/lls_pcapng_player.py --pcap_filename $SICK_LIDAR_LOCALIZATION_ROOT/test/data/wireshark/20210816_lidarloc2_2.0.0.14R_moving.pcapng # 20210816_lidarloc2_2.0.0.14R_nonmoving.pcapng sleep 3 # # Start udp receiver and send UDP input message examples # -python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/sim_udp_receiver.py --udp_port=5009 & -ros2 topic pub --once /localizationcontroller/in/odometry_message_0101 sick_lidar_localization/OdometryMessage0101 '{telegram_count: 1000001, timestamp: 123456789, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' -ros2 topic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 '{telegram_count: 1000001, timestamp: 123456789, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' -ros2 topic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 '{telegram_count: 1000002, timestamp: 123456780, x_position: -1234, y_position: -1234, heading: 1234}' -ros2 topic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 '{telegram_count: 1000003, timestamp: 123456781, encoder_value: 123456789}' -ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 '{telegram_count: 1000004, timestamp: 123456782, code: 1234}' -ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 '{telegram_count: 1000005, timestamp: 123456783, num_lanes: 1, lanes: [1234]}' -ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 '{telegram_count: 1000006, timestamp: 123456784, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}' +python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/lls_udp_receiver.py --udp_port=5009 & +ros2 topic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 '{telegram_count: 1000001, timestamp: 123456789, source_id: 0, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' +ros2 topic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 '{telegram_count: 1000001, timestamp: 123456789, source_id: 2, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}' +ros2 topic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 '{telegram_count: 1000002, timestamp: 123456780, source_id: 0, x_position: -1234, y_position: -1234, heading: 1234}' +ros2 topic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 '{telegram_count: 1000002, timestamp: 123456780, source_id: 3, x_position: -1234, y_position: -1234, heading: 1234}' +ros2 topic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 '{telegram_count: 1000003, timestamp: 123456781, source_id: 0, encoder_value: 123456789}' +ros2 topic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 '{telegram_count: 1000003, timestamp: 123456781, source_id: 4, encoder_value: 123456789}' +ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 '{telegram_count: 1000004, timestamp: 123456782, source_id: 0, code: 1234}' +ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 '{telegram_count: 1000004, timestamp: 123456782, source_id: 5, code: 1234}' +ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0701 sick_lidar_localization/CodeMeasurementMessage0701 '{telegram_count: 1000004, timestamp: 123456782, source_id: 0, code: "1234", x_position: -1234, y_position: -2345, heading: -3456}' +ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0701 sick_lidar_localization/CodeMeasurementMessage0701 '{telegram_count: 1000004, timestamp: 123456782, source_id: 6, code: "1234", x_position: -1234, y_position: -2345, heading: -3456}' +ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 '{telegram_count: 1000005, timestamp: 123456783, source_id: 0, num_lanes: 1, lanes: [1234]}' +ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 '{telegram_count: 1000005, timestamp: 123456783, source_id: 7, num_lanes: 1, lanes: [1234]}' +ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 '{telegram_count: 1000006, timestamp: 123456784, source_id: 0, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}' +ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 '{telegram_count: 1000006, timestamp: 123456784, source_id: 8, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}' python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/send_ros2_odom_messages.py # @@ -144,8 +150,8 @@ python3 $SICK_LIDAR_LOCALIZATION_ROOT/test/rest_server/python/send_ros2_odom_mes sleep 3 popd killall sick_lidar_localization -killall pointcloud_converter -pkill -f sim_udp_receiver.py +killall lls_transform +pkill -f lls_udp_receiver.py sudo pkill -f sick_rest_server.py sudo pkill -9 -f sick_rest_server.py pkill -f "ros2 topic echo" diff --git a/test/scripts/run_linux_simu.bash b/test/scripts/run_linux_simu.bash index 3e6ae78..66191ba 100644 --- a/test/scripts/run_linux_simu.bash +++ b/test/scripts/run_linux_simu.bash @@ -34,7 +34,7 @@ sleep 3 # pushd ../../build -./sick_lidar_localization ../launch/sick_lidar_localization.launch hostname:=localhost udp_ip_sim_input:=127.0.0.1 udp_ip_sim_output:=localhost verbose:=1 & +./sick_lidar_localization ../launch/sick_lidar_localization.launch hostname:=localhost udp_ip_lls_input:=127.0.0.1 udp_ip_lls_output:=localhost verbose:=1 & sleep 5 popd @@ -42,7 +42,7 @@ popd # Start pcapng player sending recorded UDP messages # -python3 ../rest_server/python/sim_pcapng_player.py --pcap_filename ../data/wireshark/20210816_lidarloc2_2.0.0.14R_nonmoving.pcapng +python3 ../rest_server/python/lls_pcapng_player.py --pcap_filename ../data/wireshark/20210816_lidarloc2_2.0.0.14R_nonmoving.pcapng sleep 5 # @@ -53,7 +53,7 @@ sleep 5 # sleep 30 echo -e "\nShutdown sick_lidar_localization simu...\n" killall sick_lidar_localization -pkill -f sim_pcapng_player.py +pkill -f lls_pcapng_player.py sudo pkill -f sick_rest_server.py pkill -f roslaunch echo -e "\nsick_lidar_localization simu finished.\n" diff --git a/test/scripts/run_linux_unittest_gen_sick_caller.bash b/test/scripts/run_linux_unittest_gen_sick_caller.bash index b96411e..f5d4e0c 100644 --- a/test/scripts/run_linux_unittest_gen_sick_caller.bash +++ b/test/scripts/run_linux_unittest_gen_sick_caller.bash @@ -38,7 +38,7 @@ pushd ../../build ./gen_service_call LocSetOdometryActive POST "{\"data\":{\"active\":1}}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocSetRecordingActive POST "{\"data\":{\"active\":1}}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocSetRingBufferRecordingActive POST "{\"data\":{\"active\":1}}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log -./gen_service_call LocStartLocalizing POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log +./gen_service_call LocStart POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocStop POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocSwitchMap POST "{\"data\":{\"subMapName\":\"test.vmap\"}}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocGetLocalizationStatus POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log diff --git a/test/scripts/run_linux_unittest_gen_sick_caller_2.bash b/test/scripts/run_linux_unittest_gen_sick_caller_2.bash index 06bafac..713274d 100644 --- a/test/scripts/run_linux_unittest_gen_sick_caller_2.bash +++ b/test/scripts/run_linux_unittest_gen_sick_caller_2.bash @@ -38,7 +38,7 @@ pushd ../../build ./gen_service_call LocSetOdometryActive POST "{active: 1}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocSetRecordingActive POST "{active: 1}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocSetRingBufferRecordingActive POST "{active: 1}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log -./gen_service_call LocStartLocalizing POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log +./gen_service_call LocStart POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocStop POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocSwitchMap POST "{subMapName: \"test.vmap\"}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocGetLocalizationStatus POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log diff --git a/test/scripts/run_linux_unittest_gen_sick_caller_3.bash b/test/scripts/run_linux_unittest_gen_sick_caller_3.bash index a920f7f..1506fc1 100644 --- a/test/scripts/run_linux_unittest_gen_sick_caller_3.bash +++ b/test/scripts/run_linux_unittest_gen_sick_caller_3.bash @@ -38,7 +38,7 @@ pushd ../../build ./gen_service_call LocSetOdometryActive "{active: 1}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocSetRecordingActive "{active: 1}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocSetRingBufferRecordingActive "{active: 1}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log -./gen_service_call LocStartLocalizing "{}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log +./gen_service_call LocStart "{}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocStop --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocSwitchMap "{subMapName: \"test.vmap\"}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log ./gen_service_call LocGetLocalizationStatus POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=linux_unittest_gen_sick_caller.log diff --git a/test/scripts/run_linux_unittest_time_sync.bash b/test/scripts/run_linux_unittest_time_sync.bash index b68b00c..9949274 100644 --- a/test/scripts/run_linux_unittest_time_sync.bash +++ b/test/scripts/run_linux_unittest_time_sync.bash @@ -18,7 +18,7 @@ sleep 3 # pushd ../../build -./sick_lidar_localization ../launch/sick_lidar_localization.launch hostname:=localhost udp_ip_sim_input:=127.0.0.1 udp_ip_sim_output:=localhost verbose:=1 | tee sick_lidar_localization.log & +./sick_lidar_localization ../launch/sick_lidar_localization.launch hostname:=localhost udp_ip_lls_input:=127.0.0.1 udp_ip_lls_output:=localhost verbose:=1 | tee sick_lidar_localization.log & sleep 3 popd @@ -27,7 +27,7 @@ popd # sleep 7 -python3 ../rest_server/python/sim_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --udp_output_logfile=../../build/udp_sender.log --max_message_count=1000 +python3 ../rest_server/python/lls_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --udp_output_logfile=../../build/udp_sender.log --max_message_count=1000 sleep 15 grep "WARN" ../../build/sick_lidar_localization.log > ../../build/sick_lidar_localization_error.log grep "ERR" ../../build/sick_lidar_localization.log >> ../../build/sick_lidar_localization_error.log diff --git a/test/scripts/run_linux_unittest_udp_receiver.bash b/test/scripts/run_linux_unittest_udp_receiver.bash index ce425a8..3648da5 100644 --- a/test/scripts/run_linux_unittest_udp_receiver.bash +++ b/test/scripts/run_linux_unittest_udp_receiver.bash @@ -19,7 +19,7 @@ sleep 3 # pushd ../../build -./sick_lidar_localization ../launch/sick_lidar_localization.launch hostname:=localhost udp_ip_sim_input:=127.0.0.1 udp_ip_sim_output:=localhost verbose:=1 udp_sim_output_logfile:=udp_sim_output.log & +./sick_lidar_localization ../launch/sick_lidar_localization.launch hostname:=localhost udp_ip_lls_input:=127.0.0.1 udp_ip_lls_output:=localhost verbose:=1 udp_lls_output_logfile:=udp_lls_output.log & sleep 3 popd @@ -27,7 +27,7 @@ popd # Start udp sender # -python3 ../rest_server/python/sim_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --udp_output_logfile=../../build/udp_sender.log --max_message_count=300 +python3 ../rest_server/python/lls_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --udp_output_logfile=../../build/udp_sender.log --max_message_count=300 # # Shutdown rest server and sick_lidar_localization @@ -38,14 +38,14 @@ killall sick_lidar_localization sudo pkill -f sick_rest_server.py # -# Compare received udp_sim_output.log with udp_sender.log, should be identical +# Compare received udp_lls_output.log with udp_sender.log, should be identical # echo -e "\nrun_linux_unittest_udp_receiver finished.\n" -cat ../../build/udp_sim_output.log -diff ../../build/udp_sim_output.log ../../build/udp_sender.log +cat ../../build/udp_lls_output.log +diff ../../build/udp_lls_output.log ../../build/udp_sender.log if [ $? != 0 ] ; then - echo -e "\n## ERROR unittest for udp messages: files ../../build/udp_sim_output.log and ../../build/udp_sender.log different, should be identical\n" + echo -e "\n## ERROR unittest for udp messages: files ../../build/udp_lls_output.log and ../../build/udp_sender.log different, should be identical\n" echo -e "## ERROR: unittest for udp messagesr failed. Press any key to continue..." read -n1 -s key else diff --git a/test/scripts/run_win64_ros2_simu.cmd b/test/scripts/run_win64_ros2_simu.cmd index bb41e3f..d8cc34b 100644 --- a/test/scripts/run_win64_ros2_simu.cmd +++ b/test/scripts/run_win64_ros2_simu.cmd @@ -62,7 +62,7 @@ ros2 service call LocSetMappingActive sick_lidar_localization/srv/LocSetMappingA ros2 service call LocSetOdometryActive sick_lidar_localization/srv/LocSetOdometryActiveSrv "{\"active\":1}" ros2 service call LocSetRecordingActive sick_lidar_localization/srv/LocSetRecordingActiveSrv "{\"active\":1}" ros2 service call LocSetRingBufferRecordingActive sick_lidar_localization/srv/LocSetRingBufferRecordingActiveSrv "{\"active\":1}" -ros2 service call LocStartLocalizing sick_lidar_localization/srv/LocStartLocalizingSrv "{}" +ros2 service call LocStart sick_lidar_localization/srv/LocStartSrv "{}" ros2 service call LocStop sick_lidar_localization/srv/LocStopSrv "{}" ros2 service call LocSwitchMap sick_lidar_localization/srv/LocSwitchMapSrv "{\"submapname\":\"test.vmap\"}" ros2 service call LocGetLocalizationStatus sick_lidar_localization/srv/LocGetLocalizationStatusSrv "{}" @@ -71,12 +71,11 @@ ros2 service call LocLoadPersistentConfig sick_lidar_localization/srv/LocLoadPer @timeout /t 3 REM -REM Start pointcloud_converter and visualize PointCloud2 messages by rviz -REM rviz -> Add by topic /cloud/PointCloud2 +REM Start lls_transform and visualize TF messages by rviz REM rviz -> Add by display type TF REM -start "pointcloud_converter" /min ros2 run sick_lidar_localization pointcloud_converter %SICK_LIDAR_LOCALIZATION_ROOT%/launch/pointcloud_converter.launch +start "lls_transform" /min ros2 run sick_lidar_localization lls_transform %SICK_LIDAR_LOCALIZATION_ROOT%/launch/lls_transform.launch start "rviz2" ros2 run rviz2 rviz2 -d %SICK_LIDAR_LOCALIZATION_ROOT%/test/config/rviz2_win64_sick_lidar_localization_pointcloud.rviz start "ros2 topic echo /localizationcontroller/out" cmd /k .\src\sick_lidar_localization2_pretest\test\scripts\simu_echo_localizationcontroller_out_topics.cmd @@ -84,14 +83,28 @@ REM REM Start udp sender resp. pcapng player to send UDP output messages REM -rem python %SICK_LIDAR_LOCALIZATION_ROOT%/test/rest_server/python/sim_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --max_message_count=300 -python %SICK_LIDAR_LOCALIZATION_ROOT%/test/rest_server/python/sim_pcapng_player.py --pcap_filename %SICK_LIDAR_LOCALIZATION_ROOT%/test/data/wireshark/20210803_moving_lidarloc2.pcapng +rem python %SICK_LIDAR_LOCALIZATION_ROOT%/test/rest_server/python/lls_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --max_message_count=300 +python %SICK_LIDAR_LOCALIZATION_ROOT%/test/rest_server/python/lls_pcapng_player.py --pcap_filename %SICK_LIDAR_LOCALIZATION_ROOT%/test/data/wireshark/20210803_moving_lidarloc2.pcapng REM REM Start odometry sender REM python %SICK_LIDAR_LOCALIZATION_ROOT%/test/rest_server/python/send_ros2_odom_messages.py +ros2 topic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 "{telegram_count: 1000001, timestamp: 123456789, source_id: 0, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}" +ros2 topic pub --once /localizationcontroller/in/odometry_message_0104 sick_lidar_localization/OdometryMessage0104 "{telegram_count: 1000001, timestamp: 123456789, source_id: 2, x_velocity: -1234, y_velocity: -1234, angular_velocity: 1234}" +ros2 topic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 "{telegram_count: 1000002, timestamp: 123456780, source_id: 0, x_position: -1234, y_position: -1234, heading: 1234}" +ros2 topic pub --once /localizationcontroller/in/odometry_message_0105 sick_lidar_localization/OdometryMessage0105 "{telegram_count: 1000002, timestamp: 123456780, source_id: 3, x_position: -1234, y_position: -1234, heading: 1234}" +ros2 topic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 "{telegram_count: 1000003, timestamp: 123456781, source_id: 0, encoder_value: 123456789}" +ros2 topic pub --once /localizationcontroller/in/encoder_measurement_message_0202 sick_lidar_localization/EncoderMeasurementMessage0202 "{telegram_count: 1000003, timestamp: 123456781, source_id: 4, encoder_value: 123456789}" +ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 "{telegram_count: 1000004, timestamp: 123456782, source_id: 0, code: 1234}" +ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0303 sick_lidar_localization/CodeMeasurementMessage0303 "{telegram_count: 1000004, timestamp: 123456782, source_id: 5, code: 1234}" +ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0701 sick_lidar_localization/CodeMeasurementMessage0701 "{telegram_count: 1000004, timestamp: 123456782, source_id: 0, code: "1234", x_position: -1234, y_position: -2345, heading: -3456}" +ros2 topic pub --once /localizationcontroller/in/code_measurement_message_0701 sick_lidar_localization/CodeMeasurementMessage0701 "{telegram_count: 1000004, timestamp: 123456782, source_id: 6, code: "1234", x_position: -1234, y_position: -2345, heading: -3456}" +ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 "{telegram_count: 1000005, timestamp: 123456783, source_id: 0, num_lanes: 1, lanes: [1234]}" +ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0403 sick_lidar_localization/LineMeasurementMessage0403 "{telegram_count: 1000005, timestamp: 123456783, source_id: 7, num_lanes: 1, lanes: [1234]}" +ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 "{telegram_count: 1000006, timestamp: 123456784, source_id: 0, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}" +ros2 topic pub --once /localizationcontroller/in/line_measurement_message_0404 sick_lidar_localization/LineMeasurementMessage0404 "{telegram_count: 1000006, timestamp: 123456784, source_id: 8, lcp1: 12, lcp2: 34, lcp3: 56, cnt_lpc: 78}" popd @pause diff --git a/test/scripts/run_win64_simu.cmd b/test/scripts/run_win64_simu.cmd index 84abad2..b41074b 100644 --- a/test/scripts/run_win64_simu.cmd +++ b/test/scripts/run_win64_simu.cmd @@ -14,7 +14,7 @@ REM Run sick_lidar_localization REM pushd ..\..\build -start "sick_lidar_localization" .\Debug\sick_lidar_localization ../launch/sick_lidar_localization.launch hostname:=localhost verbose:=1 udp_sim_output_logfile:=udp_sim_output.log +start "sick_lidar_localization" .\Debug\sick_lidar_localization ../launch/sick_lidar_localization.launch hostname:=localhost verbose:=1 udp_lls_output_logfile:=udp_lls_output.log @timeout /t 3 popd @@ -23,15 +23,15 @@ REM Start udp sender and run unittest for udp messages REM python --version -python ../rest_server/python/sim_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --udp_output_logfile=..\..\build\udp_sender.log --max_message_count=300 +python ../rest_server/python/lls_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --udp_output_logfile=..\..\build\udp_sender.log --max_message_count=300 REM -REM Compare received udp_sim_output.log with udp_sender.log, should be identical +REM Compare received udp_lls_output.log with udp_sender.log, should be identical REM -comp /a/l/m ..\..\build\udp_sim_output.log ..\..\build\udp_sender.log +comp /a/l/m ..\..\build\udp_lls_output.log ..\..\build\udp_sender.log @if not "%errorlevel%"=="0" ( - @echo ## ERROR: files ..\..\build\udp_sim_output.log and ..\..\build\udp_sender.log different, should be identical & @pause + @echo ## ERROR: files ..\..\build\udp_lls_output.log and ..\..\build\udp_sender.log different, should be identical & @pause ) else ( @echo unittest for sick_lidar_localization udp messages passed ) @@ -41,6 +41,6 @@ REM Start pcapng player sending recorded UDP messages REM python --version -python ../rest_server/python/sim_pcapng_player.py --pcap_filename ../data/wireshark/20210816_lidarloc2_2.0.0.14R_moving.pcapng +python ../rest_server/python/lls_pcapng_player.py --pcap_filename ../data/wireshark/20210816_lidarloc2_2.0.0.14R_moving.pcapng @pause diff --git a/test/scripts/run_win64_unittest_gen_sick_caller.cmd b/test/scripts/run_win64_unittest_gen_sick_caller.cmd index 2a2562f..8afa3c9 100644 --- a/test/scripts/run_win64_unittest_gen_sick_caller.cmd +++ b/test/scripts/run_win64_unittest_gen_sick_caller.cmd @@ -6,7 +6,7 @@ REM REM Start rest server REM -start "rest server" /min .\simu_run_sim_rest_server.cmd +start "rest server" /min .\simu_run_lls_rest_server.cmd @timeout /t 3 REM @@ -33,7 +33,7 @@ pushd ..\..\build .\Debug\gen_service_call LocSetOdometryActive POST "{\"data\":{\"active\":1}}" --hostname=localhost --verbose=1 --dump=1 --append=win64_unittest_gen_sick_caller.log .\Debug\gen_service_call LocSetRecordingActive POST "{\"data\":{\"active\":1}}" --hostname=localhost --verbose=1 --dump=1 --append=win64_unittest_gen_sick_caller.log .\Debug\gen_service_call LocSetRingBufferRecordingActive POST "{\"data\":{\"active\":1}}" --hostname=localhost --verbose=1 --dump=1 --append=win64_unittest_gen_sick_caller.log -.\Debug\gen_service_call LocStartLocalizing POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=win64_unittest_gen_sick_caller.log +.\Debug\gen_service_call LocStart POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=win64_unittest_gen_sick_caller.log .\Debug\gen_service_call LocStop POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=win64_unittest_gen_sick_caller.log .\Debug\gen_service_call LocSwitchMap POST "{\"data\":{\"subMapName\":\"test.vmap\"}}" --hostname=localhost --verbose=1 --dump=1 --append=win64_unittest_gen_sick_caller.log .\Debug\gen_service_call LocGetLocalizationStatus POST "{}" --hostname=localhost --verbose=1 --dump=1 --append=win64_unittest_gen_sick_caller.log diff --git a/test/scripts/run_win64_unittest_time_sync.cmd b/test/scripts/run_win64_unittest_time_sync.cmd index 48cf80f..8a46b29 100644 --- a/test/scripts/run_win64_unittest_time_sync.cmd +++ b/test/scripts/run_win64_unittest_time_sync.cmd @@ -6,7 +6,7 @@ REM REM Start rest server for LocTimestampRequests REM -start "rest server" /min .\simu_run_sim_rest_server_time_sync.cmd +start "rest server" /min .\simu_run_lls_rest_server_time_sync.cmd @timeout /t 3 REM @@ -23,7 +23,7 @@ REM Wait 7 seconds until Software-PLL is initialized and start udp sender REM @timeout /t 7 -python ../rest_server/python/sim_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --udp_output_logfile=..\..\build\udp_sender.log --max_message_count=1000 +python ../rest_server/python/lls_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --udp_output_logfile=..\..\build\udp_sender.log --max_message_count=1000 REM REM Check the timestamps calculated from sensor tics by Software-PLL: diff --git a/test/scripts/run_win64_unittest_udp_receiver.cmd b/test/scripts/run_win64_unittest_udp_receiver.cmd index 8b681e3..95c6f90 100644 --- a/test/scripts/run_win64_unittest_udp_receiver.cmd +++ b/test/scripts/run_win64_unittest_udp_receiver.cmd @@ -16,7 +16,7 @@ REM Start sick_lidar_localization REM pushd ..\..\build -start "sick_lidar_localization" .\Debug\sick_lidar_localization ../launch/sick_lidar_localization.launch hostname:=localhost verbose:=1 udp_sim_output_logfile:=udp_sim_output.log +start "sick_lidar_localization" .\Debug\sick_lidar_localization ../launch/sick_lidar_localization.launch hostname:=localhost verbose:=1 udp_lls_output_logfile:=udp_lls_output.log @timeout /t 10 popd @@ -24,15 +24,15 @@ REM REM Start udp sender REM -python ../rest_server/python/sim_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --udp_output_logfile=..\..\build\udp_sender.log --max_message_count=300 +python ../rest_server/python/lls_udp_sender.py --udp_port=5010 --udp_send_rate=30.0 --udp_output_logfile=..\..\build\udp_sender.log --max_message_count=300 REM -REM Compare received udp_sim_output.log with udp_sender.log, should be identical +REM Compare received udp_lls_output.log with udp_sender.log, should be identical REM -comp /a/l/m ..\..\build\udp_sim_output.log ..\..\build\udp_sender.log +comp /a/l/m ..\..\build\udp_lls_output.log ..\..\build\udp_sender.log @if not "%errorlevel%"=="0" ( - @echo ## ERROR: files ..\..\build\udp_sim_output.log and ..\..\build\udp_sender.log different, should be identical & @pause + @echo ## ERROR: files ..\..\build\udp_lls_output.log and ..\..\build\udp_sender.log different, should be identical & @pause ) else ( @echo unittest for sick_lidar_localization udp messages passed ) diff --git a/test/scripts/simu_run_sim_pcap_player.cmd b/test/scripts/simu_run_lls_pcap_player.cmd similarity index 89% rename from test/scripts/simu_run_sim_pcap_player.cmd rename to test/scripts/simu_run_lls_pcap_player.cmd index f7412d6..e50df10 100644 --- a/test/scripts/simu_run_sim_pcap_player.cmd +++ b/test/scripts/simu_run_lls_pcap_player.cmd @@ -16,5 +16,5 @@ REM REM Run sim udp sender REM -python ../rest_server/python/sim_pcapng_player.py --pcap_filename ../data/wireshark/20210816_lidarloc2_2.0.0.14R_moving.pcapng +python ../rest_server/python/lls_pcapng_player.py --pcap_filename ../data/wireshark/20210816_lidarloc2_2.0.0.14R_moving.pcapng @pause diff --git a/test/scripts/simu_run_sim_rest_server.cmd b/test/scripts/simu_run_lls_rest_server.cmd similarity index 100% rename from test/scripts/simu_run_sim_rest_server.cmd rename to test/scripts/simu_run_lls_rest_server.cmd diff --git a/test/scripts/simu_run_sim_rest_server_time_sync.cmd b/test/scripts/simu_run_lls_rest_server_time_sync.cmd similarity index 100% rename from test/scripts/simu_run_sim_rest_server_time_sync.cmd rename to test/scripts/simu_run_lls_rest_server_time_sync.cmd diff --git a/test/scripts/simu_run_sim_udp_sender.cmd b/test/scripts/simu_run_lls_udp_sender.cmd similarity index 90% rename from test/scripts/simu_run_sim_udp_sender.cmd rename to test/scripts/simu_run_lls_udp_sender.cmd index b6364a3..834a98f 100644 --- a/test/scripts/simu_run_sim_udp_sender.cmd +++ b/test/scripts/simu_run_lls_udp_sender.cmd @@ -16,5 +16,5 @@ REM REM Run sim udp sender REM -python ../rest_server/python/sim_udp_sender.py +python ../rest_server/python/lls_udp_sender.py @pause diff --git a/test/scripts/verify_sim_input_messages.cmd b/test/scripts/verify_lls_input_messages.cmd similarity index 68% rename from test/scripts/verify_sim_input_messages.cmd rename to test/scripts/verify_lls_input_messages.cmd index 9979c26..9873b3e 100644 --- a/test/scripts/verify_sim_input_messages.cmd +++ b/test/scripts/verify_lls_input_messages.cmd @@ -6,11 +6,11 @@ REM REM Run sim udp receiver REM -start "sim_udp_receiver" python ../rest_server/python/sim_udp_receiver.py --udp_port=5009 +start "lls_udp_receiver" python ../rest_server/python/lls_udp_receiver.py --udp_port=5009 REM REM Run sim udp sender REM -python ../rest_server/python/sim_pcapng_player.py --pcap_filename ../data/wireshark/20210804_UDP_Port_5009_RecvData.pcapng --udp_port=5009 +python ../rest_server/python/lls_pcapng_player.py --pcap_filename ../data/wireshark/20210804_UDP_Port_5009_RecvData.pcapng --udp_port=5009 @pause diff --git a/test/tools/python/ros_service_prep.py b/test/tools/python/ros_service_prep.py index c5a0143..e3e5d98 100644 --- a/test/tools/python/ros_service_prep.py +++ b/test/tools/python/ros_service_prep.py @@ -4,13 +4,13 @@ ros_service_names = [ "LocAutoStartSavePoseSrv", "LocClearMapCacheSrv", "LocGetErrorLevelSrv", "LocGetMapSrv", "LocGetSystemStateSrv", "LocInitializeAtPoseSrv", "LocInitializePoseSrv", "LocIsSystemReadySrv", "LocLoadMapToCacheSrv", "LocRequestTimestampSrv", "LocResumeAtPoseSrv", "LocSaveRingBufferRecordingSrv", "LocSetKinematicVehicleModelActiveSrv", "LocSetLinesForSupportActiveSrv", "LocSetMappingActiveSrv", - "LocSetMapSrv", "LocSetOdometryActiveSrv", "LocSetRecordingActiveSrv", "LocSetRingBufferRecordingActiveSrv", "LocStartLocalizingSrv", + "LocSetMapSrv", "LocSetOdometryActiveSrv", "LocSetRecordingActiveSrv", "LocSetRingBufferRecordingActiveSrv", "LocStartSrv", "LocStateSrv", "LocStopSrv", "LocSwitchMapSrv" ] rest_service_names = [ "LocAutoStartSavePose", "LocClearMapCache", "GetErrorLevel", "LocGetMap", "LocGetSystemState", "LocInitializeAtPose", "IsSystemReady", "LocLoadMapToCache", "LocRequestTimestamp", "LocResumeAtPose", "LocSaveRingBufferRecording", "LocSetKinematicVehicleModelActive", "LocSetLinesForSupportActive", "LocSetMappingActive", - "LocSetMap", "LocSetOdometryActive", "LocSetRecordingActive", "LocSetRingBufferRecordingActive", "LocStartLocalizing", + "LocSetMap", "LocSetOdometryActive", "LocSetRecordingActive", "LocSetRingBufferRecordingActive", "LocStart", "LocStop", "LocSwitchMap" ] if False: # print ros service callback declarations