diff --git a/.gitignore b/.gitignore index b38867c..49a4808 100644 --- a/.gitignore +++ b/.gitignore @@ -31,4 +31,11 @@ *.out *.app -test/scripts/log/ +test/ros1_scripts/log/ +test/ros1_scripts/log/cola_examples/ +test/ros1_scripts/log/error_simu/ +test/ros2_scripts/log/ +test/ros2_scripts/log/cola_examples/ +test/ros2_scripts/log/error_simu/ + +package\.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 320fe09..0d1942f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,13 +1,30 @@ -cmake_minimum_required(VERSION 2.8.3) +# cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(sick_lidar_localization) -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11 -g -Wall -Wno-reorder -Wno-sign-compare -Wno-unused-local-typedefs -Wno-unused-parameter) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS +## Compile as C++14, supported in ROS Kinetic and newer +set(CMAKE_CXX_STANDARD 14) +add_compile_options(-std=c++14 -g -Wall -Wno-reorder -Wno-sign-compare -Wno-unused-local-typedefs -Wno-unused-parameter) + +if("${CMAKE_BUILD_TYPE}" STREQUAL "Release") + add_compile_options(-O3) +endif() +if("${CMAKE_BUILD_TYPE}" STREQUAL "Debug") + add_compile_options(-g) +endif() + +if(DEFINED ROS_VERSION) + add_compile_options(-D__ROS_VERSION=${ROS_VERSION}) +endif() + +## catkin and colcon packages +# message("cmake option ROS_VERSION=${ROS_VERSION}") +if(ROS_VERSION EQUAL 1) + + ## Find catkin macros and libraries + ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) + ## is used, also find other catkin packages + find_package(catkin REQUIRED COMPONENTS # cmake_modules # Note: find_package(catkin ... "cmake_modules" ...) causes cmake error "Target ... links to target "UUID::UUID", caused by some dependency issue with gazebo # see https://bitbucket.org/ignitionrobotics/ign-cmake/issues/40/target-uuid-uuid-does-not-exist for further details message_generation @@ -15,13 +32,30 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy geometry_msgs + nav_msgs sensor_msgs std_msgs tf -) + ) + +elseif(ROS_VERSION EQUAL 2) + + find_package(ament_cmake REQUIRED) + find_package(rclcpp REQUIRED) + find_package(rosidl_default_generators REQUIRED) + find_package(geometry_msgs REQUIRED) + find_package(nav_msgs REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(std_msgs REQUIRED) + find_package(tf2 REQUIRED) + find_package(tf2_ros REQUIRED) + +else() + message(SEND_ERROR "ROS_VERSION not configured or unsupported, run with --cmake-args -DROS_VERSION=1 on ROS1 or --cmake-args -DROS_VERSION=2 on ROS2") +endif() ## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) +find_package(Boost REQUIRED COMPONENTS system thread) find_package(Doxygen) # find_package(OpenCV 3 REQUIRED) # find_package(cmake_modules REQUIRED) # find_package(cmake_modules) required for TinyXML. find_package(catkin ... "cmake_modules" ...) causes issues by gazebo dependencies (see note above). @@ -37,28 +71,30 @@ find_package(Doxygen) ## Declare ROS messages, services and actions ## ################################################ -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -add_message_files( +if(ROS_VERSION EQUAL 1) + + ## To declare and build messages, services or actions from within this + ## package, follow these steps: + ## * Let MSG_DEP_SET be the set of packages whose message types you use in + ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). + ## * In the file package.xml: + ## * add a build_depend tag for "message_generation" + ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET + ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in + ## but can be declared for certainty nonetheless: + ## * add a exec_depend tag for "message_runtime" + ## * In this file (CMakeLists.txt): + ## * add "message_generation" and every package in MSG_DEP_SET to + ## find_package(catkin REQUIRED COMPONENTS ...) + ## * add "message_runtime" and every package in MSG_DEP_SET to + ## catkin_package(CATKIN_DEPENDS ...) + ## * uncomment the add_*_files sections below as needed + ## and list every .msg/.srv/.action file to be processed + ## * uncomment the generate_messages entry below + ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + + ## Generate messages in the 'msg' folder + add_message_files( FILES SickLocColaTelegramMsg.msg SickLocDiagnosticMsg.msg @@ -67,16 +103,16 @@ add_message_files( SickLocResultPortCrcMsg.msg SickLocResultPortTelegramMsg.msg SickLocResultPortTestcaseMsg.msg -) - -## Generate services in the 'srv' folder -add_service_files( + ) + + ## Generate services in the 'srv' folder + add_service_files( FILES + # Servicefiles supported in release 3 and later SickLocColaTelegramSrv.srv SickLocRequestTimestampSrv.srv SickLocSetResultModeSrv.srv SickLocSetResultPoseIntervalSrv.srv - SickLocStopAndSaveSrv.srv SickLocIsSystemReadySrv.srv SickLocSetPoseSrv.srv SickLocSetResultPortSrv.srv @@ -87,22 +123,131 @@ add_service_files( SickLocSetResultPoseEnabledSrv.srv SickLocStateSrv.srv SickLocTimeSyncSrv.srv -) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( + # Servicefiles supported in release 4 and later + SickDevGetLidarConfigSrv.srv + SickDevGetLidarIdentSrv.srv + SickDevGetLidarStateSrv.srv + SickDevSetLidarConfigSrv.srv + SickGetSoftwareVersionSrv.srv + SickLocAutoStartActiveSrv.srv + SickLocAutoStartSavePoseIntervalSrv.srv + SickLocAutoStartSavePoseSrv.srv + SickLocForceUpdateSrv.srv + SickLocInitializePoseSrv.srv + SickLocInitialPoseSrv.srv + SickLocMapSrv.srv + SickLocMapStateSrv.srv + SickLocOdometryActiveSrv.srv + SickLocOdometryPortSrv.srv + SickLocOdometryRestrictYMotionSrv.srv + SickLocReflectorsForSupportActiveSrv.srv + SickLocResultEndiannessSrv.srv + SickLocResultModeSrv.srv + SickLocResultPortSrv.srv + SickLocResultPoseIntervalSrv.srv + SickLocResultStateSrv.srv + SickLocRingBufferRecordingActiveSrv.srv + SickLocSaveRingBufferRecordingSrv.srv + SickLocSetAutoStartActiveSrv.srv + SickLocSetAutoStartSavePoseIntervalSrv.srv + SickLocSetMapSrv.srv + SickLocSetOdometryActiveSrv.srv + SickLocSetOdometryPortSrv.srv + SickLocSetOdometryRestrictYMotionSrv.srv + SickLocSetReflectorsForSupportActiveSrv.srv + SickLocSetRingBufferRecordingActiveSrv.srv + SickLocStartDemoMappingSrv.srv + SickReportUserMessageSrv.srv + SickSavePermanentSrv.srv + SickDevSetIMUActiveSrv.srv + SickDevIMUActiveSrv.srv + ) + + ## Generate actions in the 'action' folder + # add_action_files( + # FILES + # Action1.action + # Action2.action + # ) + + ## Generate added messages and services with any dependencies listed here + generate_messages( DEPENDENCIES geometry_msgs + nav_msgs sensor_msgs std_msgs -) + ) + +elseif(ROS_VERSION EQUAL 2) + + rosidl_generate_interfaces(${PROJECT_NAME} + "msg/SickLocColaTelegramMsg.msg" + "msg/SickLocDiagnosticMsg.msg" + "msg/SickLocResultPortHeaderMsg.msg" + "msg/SickLocResultPortPayloadMsg.msg" + "msg/SickLocResultPortCrcMsg.msg" + "msg/SickLocResultPortTelegramMsg.msg" + "msg/SickLocResultPortTestcaseMsg.msg" + # Servicefiles supported in release 3 and later + "srv/SickLocColaTelegramSrv.srv" + "srv/SickLocRequestTimestampSrv.srv" + "srv/SickLocSetResultModeSrv.srv" + "srv/SickLocSetResultPoseIntervalSrv.srv" + "srv/SickLocIsSystemReadySrv.srv" + "srv/SickLocSetPoseSrv.srv" + "srv/SickLocSetResultPortSrv.srv" + "srv/SickLocStartLocalizingSrv.srv" + "srv/SickLocStopSrv.srv" + "srv/SickLocRequestResultDataSrv.srv" + "srv/SickLocSetResultEndiannessSrv.srv" + "srv/SickLocSetResultPoseEnabledSrv.srv" + "srv/SickLocStateSrv.srv" + "srv/SickLocTimeSyncSrv.srv" + # Servicefiles supported in release 4 and later + "srv/SickDevGetLidarConfigSrv.srv" + "srv/SickDevGetLidarIdentSrv.srv" + "srv/SickDevGetLidarStateSrv.srv" + "srv/SickDevSetLidarConfigSrv.srv" + "srv/SickGetSoftwareVersionSrv.srv" + "srv/SickLocAutoStartActiveSrv.srv" + "srv/SickLocAutoStartSavePoseIntervalSrv.srv" + "srv/SickLocAutoStartSavePoseSrv.srv" + "srv/SickLocForceUpdateSrv.srv" + "srv/SickLocInitializePoseSrv.srv" + "srv/SickLocInitialPoseSrv.srv" + "srv/SickLocMapSrv.srv" + "srv/SickLocMapStateSrv.srv" + "srv/SickLocOdometryActiveSrv.srv" + "srv/SickLocOdometryPortSrv.srv" + "srv/SickLocOdometryRestrictYMotionSrv.srv" + "srv/SickLocReflectorsForSupportActiveSrv.srv" + "srv/SickLocResultEndiannessSrv.srv" + "srv/SickLocResultModeSrv.srv" + "srv/SickLocResultPortSrv.srv" + "srv/SickLocResultPoseIntervalSrv.srv" + "srv/SickLocResultStateSrv.srv" + "srv/SickLocRingBufferRecordingActiveSrv.srv" + "srv/SickLocSaveRingBufferRecordingSrv.srv" + "srv/SickLocSetAutoStartActiveSrv.srv" + "srv/SickLocSetAutoStartSavePoseIntervalSrv.srv" + "srv/SickLocSetMapSrv.srv" + "srv/SickLocSetOdometryActiveSrv.srv" + "srv/SickLocSetOdometryPortSrv.srv" + "srv/SickLocSetOdometryRestrictYMotionSrv.srv" + "srv/SickLocSetReflectorsForSupportActiveSrv.srv" + "srv/SickLocSetRingBufferRecordingActiveSrv.srv" + "srv/SickLocStartDemoMappingSrv.srv" + "srv/SickReportUserMessageSrv.srv" + "srv/SickSavePermanentSrv.srv" + "srv/SickDevSetIMUActiveSrv.srv" + "srv/SickDevIMUActiveSrv.srv" + DEPENDENCIES builtin_interfaces std_msgs + ) + +else() + message(SEND_ERROR "ROS_VERSION not configured or unsupported, run with --cmake-args -DROS_VERSION=1 on ROS1 or --cmake-args -DROS_VERSION=2 on ROS2") +endif() ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -133,11 +278,13 @@ generate_messages( ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( +if(ROS_VERSION EQUAL 1) + catkin_package( INCLUDE_DIRS include LIBRARIES sick_localization_lib - CATKIN_DEPENDS message_runtime roscpp rospy geometry_msgs sensor_msgs std_msgs -) + CATKIN_DEPENDS message_runtime roscpp rospy geometry_msgs nav_msgs sensor_msgs std_msgs + ) +endif() ########### ## Build ## @@ -159,15 +306,18 @@ add_library(sick_localization_lib src/client_socket.cpp src/cola_configuration.cpp src/cola_converter.cpp + src/cola_encoder.cpp src/cola_parser.cpp src/cola_services.cpp src/cola_transmitter.cpp src/driver_check_thread.cpp src/driver_monitor.cpp src/driver_thread.cpp + src/odom_converter.cpp src/pointcloud_converter_thread.cpp src/random_generator.cpp src/result_port_parser.cpp + src/ros_wrapper.cpp src/SoftwarePLL.cpp src/testcase_generator.cpp src/time_sync_service.cpp @@ -175,11 +325,6 @@ add_library(sick_localization_lib src/crc/crc16ccitt_false.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide @@ -187,9 +332,7 @@ add_library(sick_localization_lib ## Executables for ros driver sick_lidar_localization add_executable(sim_loc_driver src/driver.cpp) add_executable(sim_loc_driver_check src/driver_check.cpp) -add_executable(sim_loc_time_sync src/time_sync.cpp) add_executable(pointcloud_converter src/pointcloud_converter.cpp) -add_executable(cola_service_node src/cola_service_node.cpp) ## Executables for test purposes add_executable(sim_loc_test_server test/src/test_server.cpp test/src/test_server_thread.cpp) @@ -204,50 +347,51 @@ add_executable(verify_sim_loc_driver test/src/verify_sim_loc_driver.cpp test/src ## Add cmake target dependencies of the executable ## same as for the library above -add_dependencies(sick_localization_lib - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) -add_dependencies(sim_loc_driver - sick_localization_lib - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} - ) -add_dependencies(sim_loc_driver_check - sick_localization_lib +if(ROS_VERSION EQUAL 1) + + add_dependencies(sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) -add_dependencies(sim_loc_test_server + add_dependencies(sim_loc_driver sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) -add_dependencies(sim_loc_time_sync + add_dependencies(sim_loc_driver_check sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) -add_dependencies(pointcloud_converter + add_dependencies(sim_loc_test_server sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) -add_dependencies(cola_service_node + add_dependencies(pointcloud_converter sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) -add_dependencies(unittest_sim_loc_parser + add_dependencies(unittest_sim_loc_parser sick_localization_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) -add_dependencies(verify_sim_loc_driver + add_dependencies(verify_sim_loc_driver 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) + rosidl_target_interfaces(sick_localization_lib ${PROJECT_NAME} "rosidl_typesupport_cpp") + +else() + message(SEND_ERROR "ROS_VERSION not configured or unsupported, run with --cmake-args -DROS_VERSION=1 on ROS1 or --cmake-args -DROS_VERSION=2 on ROS2") +endif() ## Specify libraries to link a library or executable target against target_link_libraries(sick_localization_lib @@ -269,21 +413,11 @@ target_link_libraries(sim_loc_test_server ${Boost_LIBRARIES} ${catkin_LIBRARIES} ) -target_link_libraries(sim_loc_time_sync - sick_localization_lib - ${Boost_LIBRARIES} - ${catkin_LIBRARIES} - ) target_link_libraries(pointcloud_converter sick_localization_lib ${Boost_LIBRARIES} ${catkin_LIBRARIES} ) -target_link_libraries(cola_service_node - sick_localization_lib - ${Boost_LIBRARIES} - ${catkin_LIBRARIES} - ) target_link_libraries(unittest_sim_loc_parser sick_localization_lib ${Boost_LIBRARIES} @@ -308,27 +442,48 @@ target_link_libraries(verify_sim_loc_driver # scripts/my_python_script # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) - -## Mark executables and/or libraries for installation -install(TARGETS sick_localization_lib sim_loc_driver sim_loc_driver_check sim_loc_test_server sim_loc_time_sync pointcloud_converter cola_service_node unittest_sim_loc_parser verify_sim_loc_driver +if(ROS_VERSION EQUAL 1) + + ## Mark executables and/or libraries for installation + install(TARGETS sick_localization_lib sim_loc_driver sim_loc_driver_check sim_loc_test_server pointcloud_converter unittest_sim_loc_parser verify_sim_loc_driver ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ + + ## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY + + ## Mark other files for installation (e.g. launch and bag files, etc.) + install(DIRECTORY launch yaml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +elseif(ROS_VERSION EQUAL 2) + + ament_export_dependencies(sick_localization_lib rclcpp geometry_msgs nav_msgs sensor_msgs std_msgs tf2_ros) + ament_export_include_directories(include/${PROJECT_NAME}/) + ament_export_libraries(sick_localization_lib sim_loc_driver sim_loc_driver_check sim_loc_test_server pointcloud_converter unittest_sim_loc_parser verify_sim_loc_driver) + ament_package() + + # install targets and launch files + install(TARGETS + sick_localization_lib sim_loc_driver sim_loc_driver_check sim_loc_test_server pointcloud_converter unittest_sim_loc_parser verify_sim_loc_driver + DESTINATION lib/${PROJECT_NAME} + ) + install(DIRECTORY + launch + yaml + DESTINATION share/${PROJECT_NAME} + ) + +endif() ############# ## Doxygen ## diff --git a/README.md b/README.md index 8ca0fa3..445391c 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,13 @@ # sick_lidar_localization sick_lidar_localization is an open-source project to connect the LiDAR-LOC software of the company SICK -with the ROS-framework +with the ROS-framework. It supports both ROS1 and ROS2 on Linux. The LiDAR-LOC software has to be purchased separately. -## Install and build +## Install and build on ROS1 -To install and build ROS support for SICK LiDAR Localization, run the following commands: +To install and build ROS1 support for SICK LiDAR Localization, run the following commands: ```console cd ~/catkin_ws/src @@ -17,18 +17,38 @@ cd ~/catkin_ws/src git clone https://github.com/SICKAG/sick_lidar_localization.git # sick_lidar_localization release version cd .. source /opt/ros/melodic/setup.bash -catkin_make -catkin_make install +cp ./src/sick_lidar_localization/package_ros1.xml ./src/sick_lidar_localization/package.xml +catkin_make --cmake-args -DROS_VERSION=1 +catkin_make install --cmake-args -DROS_VERSION=1 source ./devel/setup.bash ``` -## Run +## Install and build on ROS2 -To run SICK LiDAR Localization under ROS, install the SICK localization controller and run the ros driver: +To install and build ROS2 support for SICK LiDAR Localization, run the following commands: + +```console +cd ~/catkin_ws/src +# sudo apt-get install expect # install unbuffer for logging console output, development only +# git clone https://github.com/madler/crcany.git # get crc implementation, development only +# git clone https://github.com/michael1309/sick_lidar_localization_pretest.git # sick_lidar_localization development +git clone https://github.com/SICKAG/sick_lidar_localization.git # sick_lidar_localization release version +cd .. +source /opt/ros/eloquent/setup.bash +# export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH # export CMAKE_PREFIX_PATH can be required depending on system settings +cp ./src/sick_lidar_localization/package_ros2.xml ./src/sick_lidar_localization/package.xml +BUILDTYPE=Debug +colcon build --packages-select sick_lidar_localization --cmake-args " -DROS_VERSION=2" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" +source ./install/setup.bash +``` + +## Run on ROS1 + +To run SICK LiDAR Localization under ROS1, install the SICK localization controller and run the ros driver: 1. Install and run the SICK localization controller. See [doc/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/Product_notes/lidar-loc-operating-instructions/. +https://supportportal.sick.com/Product_notes/lidar-loc-operation-instruction/. 2. Start the sick_lidar_localization driver: @@ -51,6 +71,37 @@ messages with rostopic echo "/sick_lidar_localization/driver/result_telegrams" ``` +## Run on ROS2 + +To run SICK LiDAR Localization under ROS2, install the SICK localization controller and run the ros driver: + +1. Install and run the SICK localization controller. See [doc/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/Product_notes/lidar-loc-operation-instruction/. + +2. Start the sick_lidar_localization driver: + +```console +cd ~/catkin_ws +source ./install/setup.bash +LD_LIBRARY_PATH=$COLCON_PREFIX_PATH/sick_lidar_localization/lib/sick_lidar_localization:$LD_LIBRARY_PATH # set LD_LIBRARY_PATH can be required depending on ros version +export localization_controller_ip_address=192.168.0.1 +ros2 launch sick_lidar_localization sim_loc_driver.launch.py +``` + +Note: The IP address of the SICK localization controller is 192.168.0.1 by default. Depending on your network configuration, +different IP addresses can be set by environment variable `localization_controller_ip_address=` or +by changing the value `localization_controller_default_ip_address: ""` in the drivers launch file +[launch/sim_loc_driver.launch.py](launch/sim_loc_driver.launch.py). + +The sick_lidar_localization driver connects to the localization controller, receives result port telegrams and publishes +them on ros topic "/sick_lidar_localization/driver/result_telegrams". After successful installation, you can view the telegram +messages with + +```console +ros2 topic echo "/sick_lidar_localization/driver/result_telegrams" +``` + ## LiDAR-LOC configuration The LiDAR localization and the controller can be configured by Cola telegrams. @@ -80,32 +131,176 @@ LocRequestResultData | SickLocRequestResultData | [srv/SickLocRequestRes LocSetPose | SickLocSetPose | [srv/SickLocSetPoseSrv.srv](srv/SickLocSetPoseSrv.srv) | Initialize vehicle pose **Timestamp Telegrams**||| LocRequestTimestamp | SickLocRequestTimestamp | [srv/SickLocRequestTimestampSrv.srv](srv/SickLocRequestTimestampSrv.srv) | Query timestamp, see "Time synchronization" - -The following examples show how to call these services: - -``` -# ROS services for States Telegrams +**Advanced services in release 4 and later**||| +DevGetLidarConfig | SickDevGetLidarConfig | [srv/SickDevGetLidarConfigSrv.srv](srv/SickDevGetLidarConfigSrv.srv) | Reads the configuration for a lidar +DevGetLidarIdent | SickDevGetLidarIdent | [srv/SickDevGetLidarIdentSrv.srv](srv/SickDevGetLidarIdentSrv.srv) | Returns the scanner ident of the specified lidar. +DevGetLidarState | SickDevGetLidarState | [srv/SickDevGetLidarStateSrv.srv](srv/SickDevGetLidarStateSrv.srv) | Returns the lidar state of the given lidar. +DevIMUActive | SickDevIMUActive | [srv/SickDevIMUActiveSrv.srv](srv/SickDevIMUActiveSrv.srv) | Read IMU Active status +DevSetIMUActive | SickDevSetIMUActive | [srv/SickDevSetIMUActiveSrv.srv](srv/SickDevSetIMUActiveSrv.srv) | Set IMU Active +DevSetLidarConfig | SickDevSetLidarConfig | [srv/SickDevSetLidarConfigSrv.srv](srv/SickDevSetLidarConfigSrv.srv) | Sets the configuration for a lidar +GetSoftwareVersion | SickGetSoftwareVersion | [srv/SickGetSoftwareVersionSrv.srv](srv/SickGetSoftwareVersionSrv.srv) | Returns the version string of the localization system. +LocAutoStartActive | SickLocAutoStartActive | [srv/SickLocAutoStartActiveSrv.srv](srv/SickLocAutoStartActiveSrv.srv) | Returns whether autostart is used or not. +LocAutoStartSavePose | SickLocAutoStartSavePose | [srv/SickLocAutoStartSavePoseSrv.srv](srv/SickLocAutoStartSavePoseSrv.srv) | Saves the current pose. If no further pose writing occurs the system will reinitialize itself at this position on restart if LocAutoStart is enabled. +LocAutoStartSavePoseInterval | SickLocAutoStartSavePoseInterval | [srv/SickLocAutoStartSavePoseIntervalSrv.srv](srv/SickLocAutoStartSavePoseIntervalSrv.srv) | Returns the interval in seconds for saving the pose automatically for auto start while localizing +LocForceUpdate | SickLocForceUpdate | [srv/SickLocForceUpdateSrv.srv](srv/SickLocForceUpdateSrv.srv) | Forces an update of the map localization with the next scan. This should be used with care because it is not garanteed that this converges to the correct pose. Moving the LiDAR instead should be preferred because it produces more robust updates. +LocInitializePose | SickLocInitializePose | [srv/SickLocInitializePoseSrv.srv](srv/SickLocInitializePoseSrv.srv) | Automatically adjusts the given input pose according to the map of the environment and the current LiDAR measurements. +LocInitialPose | SickLocInitialPose | [srv/SickLocInitialPoseSrv.srv](srv/SickLocInitialPoseSrv.srv) | Reads the initial pose +LocMap | SickLocMap | [srv/SickLocMapSrv.srv](srv/SickLocMapSrv.srv) | Returns the currently loaded map +LocMapState | SickLocMapState | [srv/SickLocMapStateSrv.srv](srv/SickLocMapStateSrv.srv) | Reads the current state of the map. 0: not active, 1: active. +LocOdometryActive | SickLocOdometryActive | [srv/SickLocOdometryActiveSrv.srv](srv/SickLocOdometryActiveSrv.srv) | Returns the usage of odometry data in Scan Matching +LocOdometryPort | SickLocOdometryPort | [srv/SickLocOdometryPortSrv.srv](srv/SickLocOdometryPortSrv.srv) | Returns the UDP port of the UDP socket for odometry measurement input. +LocOdometryRestrictYMotion | SickLocOdometryRestrictYMotion | [srv/SickLocOdometryRestrictYMotionSrv.srv](srv/SickLocOdometryRestrictYMotionSrv.srv) | Returns the variable that Indicates that the vehicle will be able to move in Y-Direction or not. If true the mounting pose of the sensor matters. For omnidirektional vehicles this must be set to false. +LocReflectorsForSupportActive | SickLocReflectorsForSupportActive | [srv/SickLocReflectorsForSupportActiveSrv.srv](srv/SickLocReflectorsForSupportActiveSrv.srv) | Returns usage of mapped reflectors for map based localization robustification +LocResultEndianness | SickLocResultEndianness | [srv/SickLocResultEndiannessSrv.srv](srv/SickLocResultEndiannessSrv.srv) | Returns the endianness of the result port: 0 BIG_ENDIAN (default), 1 LITTLE_ENDIAN +LocResultMode | SickLocResultMode | [srv/SickLocResultModeSrv.srv](srv/SickLocResultModeSrv.srv) | Returns the current result mode: 0 STREAM (default), 1 POLL +LocResultPort | SickLocResultPort | [srv/SickLocResultPortSrv.srv](srv/SickLocResultPortSrv.srv) | Read the TCP port of the localization controller for result output. +LocResultPoseInterval | SickLocResultPoseInterval | [srv/SickLocResultPoseIntervalSrv.srv](srv/SickLocResultPoseIntervalSrv.srv) | Reads the interval of the pose result output. +LocResultState | SickLocResultState | [srv/SickLocResultStateSrv.srv](srv/SickLocResultStateSrv.srv) | Returns the output state of the result interface +LocRingBufferRecordingActive | SickLocRingBufferRecordingActive | [srv/SickLocRingBufferRecordingActiveSrv.srv](srv/SickLocRingBufferRecordingActiveSrv.srv) | Returns whether rolling buffer log file recording is activated or deactivated +LocSaveRingBufferRecording | SickLocSaveRingBufferRecording | [srv/SickLocSaveRingBufferRecordingSrv.srv](srv/SickLocSaveRingBufferRecordingSrv.srv) | Saves the current sensor data ring buffer as log file. The log file can be downloaded via FTP. +LocSetAutoStartActive | SickLocSetAutoStartActive | [srv/SickLocSetAutoStartActiveSrv.srv](srv/SickLocSetAutoStartActiveSrv.srv) | Sets whether autostart should be used or not. +LocSetAutoStartSavePoseInterval | SickLocSetAutoStartSavePoseInterval | [srv/SickLocSetAutoStartSavePoseIntervalSrv.srv](srv/SickLocSetAutoStartSavePoseIntervalSrv.srv) | Set the interval in seconds for saving the pose automatically for auto start while localizing +LocSetMap | SickLocSetMap | [srv/SickLocSetMapSrv.srv](srv/SickLocSetMapSrv.srv) | Loads a given map +LocSetOdometryActive | SickLocSetOdometryActive | [srv/SickLocSetOdometryActiveSrv.srv](srv/SickLocSetOdometryActiveSrv.srv) | Enables or disables the usage of odometry data in Scan Matching +LocSetOdometryPort | SickLocSetOdometryPort | [srv/SickLocSetOdometryPortSrv.srv](srv/SickLocSetOdometryPortSrv.srv) | Sets the UDP port of the UDP socket for odometry measurement input. +LocSetOdometryRestrictYMotion | SickLocSetOdometryRestrictYMotion | [srv/SickLocSetOdometryRestrictYMotionSrv.srv](srv/SickLocSetOdometryRestrictYMotionSrv.srv) | Method to set the variable that Indicates that the vehicle will be able to move in Y-Direction or not. If true the mounting pose of the sensor matters. For omnidirektional vehicles this must be set to false. +LocSetReflectorsForSupportActive | SickLocSetReflectorsForSupportActive | [srv/SickLocSetReflectorsForSupportActiveSrv.srv](srv/SickLocSetReflectorsForSupportActiveSrv.srv) | Enables or disables usage of mapped reflectors for map based localization robustification +LocSetRingBufferRecordingActive | SickLocSetRingBufferRecordingActive | [srv/SickLocSetRingBufferRecordingActiveSrv.srv](srv/SickLocSetRingBufferRecordingActiveSrv.srv) | Activates or deactivates rolling buffer log file recording on error report +LocStartDemoMapping | SickLocStartDemoMapping | [srv/SickLocStartDemoMappingSrv.srv](srv/SickLocStartDemoMappingSrv.srv) | If all conditions are met, starts the demo mapping system. +ReportUserMessage | SickReportUserMessage | [srv/SickReportUserMessageSrv.srv](srv/SickReportUserMessageSrv.srv) | Method to report messages to the localization system. +SavePermanent | SickSavePermanent | [srv/SickSavePermanentSrv.srv](srv/SickSavePermanentSrv.srv) | Saves the parameters permanently on the device. They will be reloaded on reboot. + +The following examples show how to call these services under ROS1: + +``` +# ROS1 services for States Telegrams rosservice call SickLocIsSystemReady "{}" # expected answer: "success: True" # Check if the system is ready rosservice call SickLocState "{}" # expected answer: "state: 2, success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING rosservice call SickLocStop "{}" # expected answer: "success: True" # Stop localization or demo mapping rosservice call SickLocStartLocalizing "{}" # expected answer: "success: True" # Start localization -# ROS services for Result Output Configuration Telegrams +# ROS1 services for Result Output Configuration Telegrams rosservice call SickLocSetResultPort "{port: 2201}" # expected answer: "success: True" # Set TCP-port for result output rosservice call SickLocSetResultMode "{mode: 0}" # expected answer: "success: True" # Set mode of result output (stream or: poll) rosservice call SickLocSetResultPoseEnabled "{enabled: 1}" # expected answer: "success: True" # Disable/enable result output rosservice call SickLocSetResultEndianness "{endianness: 0}" # expected answer: "success: True" # Set endianness of result output rosservice call SickLocSetResultPoseInterval "{interval: 1}" # expected answer: "success: True" # Set interval of result output rosservice call SickLocRequestResultData "{}" # expected answer: "success: True" # If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. -# ROS services for SetPose Telegrams +# ROS1 services for SetPose Telegrams rosservice call SickLocSetPose "{posex: 10300, posey: -5200, yaw: 30000, uncertainty: 1000}" # expected answer: "success: True" # Initialize vehicle pose -# ROS services for Timestamp Telegrams +# ROS1 services for Timestamp Telegrams rosservice call SickLocRequestTimestamp "{}" # expected reponse: "timestamp_lidar_ms: , mean_time_vehicle_ms: , delta_time_ms: , ..." # Query timestamp, see "Time synchronization" -``` +# ROS1 services release 4 and later +rosservice call SickLocIsSystemReady "{}" # expected reponse: "success: True" +rosservice call SickLocStartLocalizing "{}" # expected reponse: "success: True" +rosservice call SickDevSetLidarConfig "{index: # expected reponse: 0, minrange: 100, maxrange: 200000, minangle: -15000, maxangle: 15000, x: 1000, y: -1000, yaw: 2000, upsidedown: true, ip: 192.168.0.123, port: 2111, interfacetype: 0, maplayer: 0, active: true}" "set: True executed: True" +rosservice call SickDevGetLidarConfig "{scannerindex: # expected reponse: 0}" "minrange: 100 maxrange: 200000 minangle: -15000 maxangle: 15000 x: 1000 y: -1000 yaw: 2000 upsidedown: True ip: \"192.168.0.123\" port: 2111 interfacetype: 0 maplayer: 0 active: True" +rosservice call SickLocSetMap "{mapfilename: # expected reponse: test.map}" "set: True executed: True" +rosservice call SickLocMap "{}" # expected reponse: "map: \"test.map\" success: True" +rosservice call SickLocMapState "{}" # expected reponse: "mapstate: True success: True" +rosservice call SickLocInitializePose "{x: # expected reponse: 100, y: -100, yaw: 2000, sigmatranslation: 1000}" "success: True" +rosservice call SickLocInitialPose "{}" # expected reponse: "x: 100 y: -100 yaw: 2000 sigmatranslation: 1000 success: True" +rosservice call SickLocSetReflectorsForSupportActive "{active: # expected reponse: 1}" "success: True" +rosservice call SickLocReflectorsForSupportActive "{}" # expected reponse: "active: True success: True" +rosservice call SickLocSetOdometryActive "{active: # expected reponse: 1}" "set: True executed: True" +rosservice call SickLocOdometryActive "{}" # expected reponse: "active: True success: True" +rosservice call SickLocSetOdometryPort "{port: # expected reponse: 3000}" "set: True executed: True" +rosservice call SickLocOdometryPort "{}" # expected reponse: "port: 3000 success: True" +rosservice call SickLocSetOdometryRestrictYMotion "{active: # expected reponse: 1}" "success: True" +rosservice call SickLocOdometryRestrictYMotion "{}" # expected reponse: "active: True success: True" +rosservice call SickLocSetAutoStartActive "{active: # expected reponse: 1}" "success: True" +rosservice call SickLocAutoStartActive "{}" # expected reponse: "active: True success: True" +rosservice call SickLocSetAutoStartSavePoseInterval "{interval: # expected reponse: 5}" "success: True" +rosservice call SickLocAutoStartSavePoseInterval "{}" # expected reponse: "interval: 5 success: True" +rosservice call SickLocSetRingBufferRecordingActive "{active: # expected reponse: 1}" "success: True" +rosservice call SickLocRingBufferRecordingActive "{}" # expected reponse: "active: True success: True" +rosservice call SickDevGetLidarIdent "{index: # expected reponse: 0}" "scannerident: \"TestcaseGenerator0\" success: True" +rosservice call SickDevGetLidarState "{index: # expected reponse: 0}" "devicestatus: 2 deviceconnected: 2 receivingdata: 2 success: True" +rosservice call SickGetSoftwareVersion "{}" # expected reponse: "version: \"1.0\" success: True" +rosservice call SickLocAutoStartSavePose "{}" # expected reponse: "success: True" +rosservice call SickLocForceUpdate "{}" # expected reponse: "success: True" +rosservice call SickLocSaveRingBufferRecording "{reason: # expected reponse: test}" "success: True" +rosservice call SickLocStartDemoMapping "{}" # expected reponse: "success: True" +rosservice call SickReportUserMessage "{usermessage: # expected reponse: test_message}" "success: True" +rosservice call SickSavePermanent "{}" # expected reponse: "success: True" +rosservice call SickLocResultPort "{}" # expected reponse: "port: 2201 success: True" +rosservice call SickLocResultMode "{}" # expected reponse: "mode: 0 success: True" +rosservice call SickLocResultEndianness "{}" # expected reponse: "endianness: 0 success: True" +rosservice call SickLocResultState "{}" # expected reponse: "state: 1 success: True" +rosservice call SickLocResultPoseInterval "{}" # expected reponse: "interval: 1 success: True" +rosservice call SickDevSetIMUActive "{active: # expected reponse: 1}" "success: True" +rosservice call SickDevIMUActive "{}" # expected reponse: "active: True success: True" +``` + +The following examples show how to call these services under ROS2: + +``` +# ROS2 services for States Telegrams +ros2 service call SickLocIsSystemReady sick_lidar_localization/srv/SickLocIsSystemReadySrv "{}" # expected answer: "success: True" # Check if the system is ready +ros2 service call SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" # expected answer: "state: 2, success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING +ros2 service call SickLocStop sick_lidar_localization/srv/SickLocStopSrv "{}" # expected answer: "success: True" # Stop localization or demo mapping +ros2 service call SickLocStartLocalizing sick_lidar_localization/srv/SickLocStartLocalizingSrv "{}" # expected answer: "success: True" # Start localization +# ROS2 services for Result Output Configuration Telegrams +ros2 service call SickLocSetResultPort sick_lidar_localization/srv/SickSickLocSetResultPortSrv "{port: 2201}" # expected answer: "success: True" # Set TCP-port for result output +ros2 service call SickLocSetResultMode sick_lidar_localization/srv/SickSickLocSetResultModeSrv "{mode: 0}" # expected answer: "success: True" # Set mode of result output (stream or: poll) +ros2 service call SickLocSetResultPoseEnabled sick_lidar_localization/srv/SickSickLocSetResultPoseEnabledSrv "{enabled: 1}" # expected answer: "success: True" # Disable/enable result output +ros2 service call SickLocSetResultEndianness sick_lidar_localization/srv/SickSickLocSetResultEndiannessSrv "{endianness: 0}" # expected answer: "success: True" # Set endianness of result output +ros2 service call SickLocSetResultPoseInterval sick_lidar_localization/srv/SickSickLocSetResultPoseIntervalSrv "{interval: 1}" # expected answer: "success: True" # Set interval of result output +ros2 service call SickLocRequestResultData sick_lidar_localization/srv/SickSickLocRequestResultDataSrv "{}" # expected answer: "success: True" # If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. +# ROS2 services for SetPose Telegrams +ros2 service call SickLocSetPose sick_lidar_localization/srv/SickSickLocSetPoseSrv "{posex: 10300, posey: -5200, yaw: 30000, uncertainty: 1000}" # expected answer: "success: True" # Initialize vehicle pose +# ROS2 services for Timestamp Telegrams +ros2 service call SickLocRequestTimestamp sick_lidar_localization/srv/SickSickLocRequestTimestampSrv "{}" # expected reponse: "timestamp_lidar_ms: , mean_time_vehicle_ms: , delta_time_ms: , ..." # Query timestamp, see "Time synchronization" +# ROS2 services release 4 and later +ros2 service call SickLocIsSystemReady sick_lidar_localization/srv/SickLocIsSystemReadySrv "{}" # expected reponse: "success=True" +ros2 service call SickLocStartLocalizing sick_lidar_localization/srv/SickLocStartLocalizingSrv "{}" # expected reponse: "success=True" +ros2 service call SickDevSetLidarConfig sick_lidar_localization/srv/SickDevSetLidarConfigSrv "{index: # expected reponse: 0, minrange: 100, maxrange: 200000, minangle: -15000, maxangle: 15000, x: 1000, y: -1000, yaw: 2000, upsidedown: true, ip: 192.168.0.123, port: 2111, interfacetype: 0, maplayer: 0, active: true}" "set=True, executed=True" +ros2 service call SickDevGetLidarConfig sick_lidar_localization/srv/SickDevGetLidarConfigSrv "{scannerindex: # expected reponse: 0}" "minrange=100, maxrange=200000, minangle=-15000, maxangle=15000, x=1000, y=-1000, yaw=2000, upsidedown=True, ip='192.168.0.123', port=2111, interfacetype=0, maplayer=0, active=True" +ros2 service call SickLocSetMap sick_lidar_localization/srv/SickLocSetMapSrv "{mapfilename: # expected reponse: test.map}" "set=True, executed=True" +ros2 service call SickLocMap sick_lidar_localization/srv/SickLocMapSrv "{}" # expected reponse: "map='test.map', success=True" +ros2 service call SickLocMapState sick_lidar_localization/srv/SickLocMapStateSrv "{}" # expected reponse: "mapstate=True, success=True" +ros2 service call SickLocInitializePose sick_lidar_localization/srv/SickLocInitializePoseSrv "{x: # expected reponse: 100, y: -100, yaw: 2000, sigmatranslation: 1000}" "success=True" +ros2 service call SickLocInitialPose sick_lidar_localization/srv/SickLocInitialPoseSrv "{}" # expected reponse: "x=100, y=-100, yaw=2000, sigmatranslation=1000, success=True" +ros2 service call SickLocSetReflectorsForSupportActive sick_lidar_localization/srv/SickLocSetReflectorsForSupportActiveSrv "{active: # expected reponse: 1}" "success=True" +ros2 service call SickLocReflectorsForSupportActive sick_lidar_localization/srv/SickLocReflectorsForSupportActiveSrv "{}" # expected reponse: "active=True, success=True" +ros2 service call SickLocSetOdometryActive sick_lidar_localization/srv/SickLocSetOdometryActiveSrv "{active: # expected reponse: 1}" "set=True, executed=True" +ros2 service call SickLocOdometryActive sick_lidar_localization/srv/SickLocOdometryActiveSrv "{}" # expected reponse: "active=True, success=True" +ros2 service call SickLocSetOdometryPort sick_lidar_localization/srv/SickLocSetOdometryPortSrv "{port: # expected reponse: 3000}" "set=True, executed=True" +ros2 service call SickLocOdometryPort sick_lidar_localization/srv/SickLocOdometryPortSrv "{}" # expected reponse: "port=3000, success=True" +ros2 service call SickLocSetOdometryRestrictYMotion sick_lidar_localization/srv/SickLocSetOdometryRestrictYMotionSrv "{active: # expected reponse: 1}" "success=True" +ros2 service call SickLocOdometryRestrictYMotion sick_lidar_localization/srv/SickLocOdometryRestrictYMotionSrv "{}" # expected reponse: "active=True, success=True" +ros2 service call SickLocSetAutoStartActive sick_lidar_localization/srv/SickLocSetAutoStartActiveSrv "{active: # expected reponse: 1}" "success=True" +ros2 service call SickLocAutoStartActive sick_lidar_localization/srv/SickLocAutoStartActiveSrv "{}" # expected reponse: "active=True, success=True" +ros2 service call SickLocSetAutoStartSavePoseInterval sick_lidar_localization/srv/SickLocSetAutoStartSavePoseIntervalSrv "{interval: # expected reponse: 5}" "success=True" +ros2 service call SickLocAutoStartSavePoseInterval sick_lidar_localization/srv/SickLocAutoStartSavePoseIntervalSrv "{}" # expected reponse: "interval=5, success=True" +ros2 service call SickLocSetRingBufferRecordingActive sick_lidar_localization/srv/SickLocSetRingBufferRecordingActiveSrv "{active: # expected reponse: 1}" "success=True" +ros2 service call SickLocRingBufferRecordingActive sick_lidar_localization/srv/SickLocRingBufferRecordingActiveSrv "{}" # expected reponse: "active=True, success=True" +ros2 service call SickDevGetLidarIdent sick_lidar_localization/srv/SickDevGetLidarIdentSrv "{index: # expected reponse: 0}" "scannerident='TestcaseGenerator0', success=True" +ros2 service call SickDevGetLidarState sick_lidar_localization/srv/SickDevGetLidarStateSrv "{index: # expected reponse: 0}" "devicestatus=2, deviceconnected=2, receivingdata=2, success=True" +ros2 service call SickGetSoftwareVersion sick_lidar_localization/srv/SickGetSoftwareVersionSrv "{}" # expected reponse: "version='1.0', success=True" +ros2 service call SickLocAutoStartSavePose sick_lidar_localization/srv/SickLocAutoStartSavePoseSrv "{}" # expected reponse: "success=True" +ros2 service call SickLocForceUpdate sick_lidar_localization/srv/SickLocForceUpdateSrv "{}" # expected reponse: "success=True" +ros2 service call SickLocSaveRingBufferRecording sick_lidar_localization/srv/SickLocSaveRingBufferRecordingSrv "{reason: # expected reponse: test}" "success=True" +ros2 service call SickLocStartDemoMapping sick_lidar_localization/srv/SickLocStartDemoMappingSrv "{}" # expected reponse: "success=True" +ros2 service call SickReportUserMessage sick_lidar_localization/srv/SickReportUserMessageSrv "{usermessage: # expected reponse: test_message}" "success=True" +ros2 service call SickSavePermanent sick_lidar_localization/srv/SickSavePermanentSrv "{}" # expected reponse: "success=True" +ros2 service call SickLocResultPort sick_lidar_localization/srv/SickLocResultPortSrv "{}" # expected reponse: "port=2201, success=True" +ros2 service call SickLocResultMode sick_lidar_localization/srv/SickLocResultModeSrv "{}" # expected reponse: "mode=0, success=True" +ros2 service call SickLocResultEndianness sick_lidar_localization/srv/SickLocResultEndiannessSrv "{}" # expected reponse: "endianness=0, success=True" +ros2 service call SickLocResultState sick_lidar_localization/srv/SickLocResultStateSrv "{}" # expected reponse: "state=1, success=True" +ros2 service call SickLocResultPoseInterval sick_lidar_localization/srv/SickLocResultPoseIntervalSrv "{}" # expected reponse: "interval=1, success=True" +ros2 service call SickDevSetIMUActive sick_lidar_localization/srv/SickDevSetIMUActiveSrv "{active: # expected reponse: 1}" "success=True" +ros2 service call SickDevIMUActive sick_lidar_localization/srv/SickDevIMUActiveSrv "{}" # expected reponse: "active=True, success=True" +``` + +See files [test/ros1_scripts/send_cola_examples.bash](test/ros1_scripts/send_cola_examples.bash) and +[test/ros1_scripts/send_cola_advanced.bash](test/ros1_scripts/send_cola_advanced.bash)(ROS1) resp. +[test/ros2_scripts/send_cola_examples.bash](test/ros2_scripts/send_cola_examples.bash) and +[test/ros2_scripts/send_cola_advanced.bash](test/ros2_scripts/send_cola_advanced.bash) (ROS2) for further examples. ### LiDAR-LOC generic CoLa telegrams The following Cola telegrams are examples for the usage of the generic ros service "SickLocColaTelegram". They can be used as a -substituion of the above described excerpt ROS LiDAR-LOC services. +substituion of the ROS LiDAR-LOC services described above. Note: Other commands like "sMN mSCreboot" (reboot controller) can be send to the localization controller using ros service "SickLocColaTelegram", too. @@ -126,7 +321,7 @@ Request | Response | Parameter | Description **Timestamp Telegrams**||| "sMN LocRequestTimestamp" | "sAN LocRequestTimestamp \" | 4 byte timestamp (ticks in milliseconds) | Query timestamp, see "Time synchronization" -The following examples show how to call Cola telegrams supported and tested by ros service "SickLocColaTelegram": +The following examples show how to call Cola telegrams supported and tested by service "SickLocColaTelegram" (ROS1): ``` # States Telegrams @@ -145,7 +340,8 @@ rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocSetPose +10300 rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocRequestTimestamp', wait_response_timeout: 1}" # expected answer: "sAN LocRequestTimestamp " # 4 byte timestamp (ticks in milliseconds) # Query timestamp, see "Time synchronization" ``` -See file [test/scripts/send_cola_examples.bash](test/scripts/send_cola_examples.bash) for further examples. +See file [test/ros1_scripts/send_cola_examples.bash](test/ros1_scripts/send_cola_examples.bash) (ROS1) resp. +[test/ros2_scripts/send_cola_examples.bash](test/ros2_scripts/send_cola_examples.bash) (ROS2) for further examples. An initial result output configuration can be set by launch file [launch/sim_loc_driver.launch](launch/sim_loc_driver.launch), too: @@ -186,34 +382,37 @@ Type | Name | Description uint32 | header.seq | ROS sequence identifier (consecutively increasing id) time | header.stamp | ROS timestamp in seconds and nanoseconds string | header.frame_id | ROS frame identifier -uint32 | telegram_header.MagicWord | Magic word SICK (0x53 0x49 0x43 0x4B) -uint32 | telegram_header.Length | Length of telegram incl. header, payload, and trailer -uint16 | telegram_header.PayloadType | Payload type: 0x06c2 = Little Endian, 0x0642 = Big Endian -uint16 | telegram_header.PayloadVersion | Version of PayloadType structure -uint32 | telegram_header.OrderNumber | Order number of the localization controller -uint32 | telegram_header.SerialNumber | Serial number of the localization controller -uint8[] | telegram_header.FW_Version | Software version of the localization controller, 20 byte -uint32 | telegram_header.TelegramCounter | Telegram counter since last start-up -uint64 | telegram_header.SystemTime | Not used -uint16 | telegram_payload.ErrorCode | ErrorCode 0: OK, ErrorCode 1: UNKNOWNERROR -uint32 | telegram_payload.ScanCounter | Counter of related scan data -uint32 | telegram_payload.Timestamp | Time stamp of the pose [ms], indicating the time at which the pose is calculated -int32 | telegram_payload.PoseX | Position X of the vehicle on the map in cartesian global coordinates [mm] -int32 | telegram_payload.PoseY | Position Y of the vehicle on the map in cartesian global coordinates [mm] -int32 | telegram_payload.PoseYaw | Orientation (yaw) of the vehicle on the map [mdeg] -uint32 | telegram_payload.Reserved1 | Reserved -int32 | telegram_payload.Reserved2 | Reserved -uint8 | telegram_payload.Quality | Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality -uint8 | telegram_payload.OutliersRatio | Ratio of beams that cannot be assigned to the current reference map [%] -int32 | telegram_payload.CovarianceX | Covariance c1 of the pose X [mm^2] -int32 | telegram_payload.CovarianceY | Covariance c5 of the pose Y [mm^2] -int32 | telegram_payload.CovarianceYaw | Covariance c9 of the pose Yaw [mdeg^2] -uint64 | telegram_payload.Reserved3 | Reserved -uint16 | telegram_trailer.Checksum | CRC16-CCITT over length of header (52 bytes) and payload (52 bytes) without 2 bytes of the trailer +uint32 | telegram_header.magicword | Magic word SICK (0x53 0x49 0x43 0x4B) +uint32 | telegram_header.length | Length of telegram incl. header, payload, and trailer +uint16 | telegram_header.payloadtype | Payload type: 0x06c2 = Little Endian, 0x0642 = Big Endian +uint16 | telegram_header.payloadversion | Version of PayloadType structure +uint32 | telegram_header.ordernumber | Order number of the localization controller +uint32 | telegram_header.serialnumber | Serial number of the localization controller +uint8[] | telegram_header.fw_version | Software version of the localization controller, 20 byte +uint32 | telegram_header.telegramcounter | Telegram counter since last start-up +uint64 | telegram_header.systemtime | Not used +uint16 | telegram_payload.errorcode | ErrorCode 0: OK, ErrorCode 1: UNKNOWNERROR +uint32 | telegram_payload.scancounter | Counter of related scan data +uint32 | telegram_payload.timestamp | Time stamp of the pose [ms], indicating the time at which the pose is calculated +int32 | telegram_payload.posex | Position X of the vehicle on the map in cartesian global coordinates [mm] +int32 | telegram_payload.posey | Position Y of the vehicle on the map in cartesian global coordinates [mm] +int32 | telegram_payload.poseyaw | Orientation (yaw) of the vehicle on the map [mdeg] +uint32 | telegram_payload.reserved1 | Reserved +int32 | telegram_payload.reserved2 | Reserved +uint8 | telegram_payload.quality | Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality +uint8 | telegram_payload.outliersratio | Ratio of beams that cannot be assigned to the current reference map [%] +int32 | telegram_payload.covariancex | Covariance c1 of the pose X [mm^2] +int32 | telegram_payload.covariancey | Covariance c5 of the pose Y [mm^2] +int32 | telegram_payload.covarianceyaw | Covariance c9 of the pose Yaw [mdeg^2] +uint64 | telegram_payload.reserved3 | Reserved +uint16 | telegram_trailer.checksum | CRC16-CCITT over length of header (52 bytes) and payload (52 bytes) without 2 bytes of the trailer bool | vehicle_time_valid | true: vehicle_time_sec and vehicle_time_nsec valid, false: software pll still in initial phase uint32 | vehicle_time_sec | System time of vehicles pose calculated by software pll (seconds part of the system time) uint32 | vehicle_time_nsec | System time of vehicles pose calculated by software pll (nano seconds part of the system time) +Note, that ROS2 requires lowercase fieldnames in messages, while ROS1 allowed camel case field names. For identical and +interoperable messages, all fieldnames have been changed to lowercase in release 4.0 and later. + Example output of a result telegram (ros message of type [msg/SickLocResultPortTelegramMsg.msg](msg/SickLocResultPortTelegramMsg.msg)): ``` @@ -224,43 +423,43 @@ header: nsecs: 914320492 frame_id: "sick_lidar_localization" telegram_header: - MagicWord: 1397310283 - Length: 106 - PayloadType: 1602 - PayloadVersion: 1 - OrderNumber: 1097816 - SerialNumber: 19047026 - FW_Version: [0, 0, 0, 0, 0, 0, 0, 76, 76, 83, 32, 86, 48, 46, 49, 46, 57, 46, 120, 66] - TelegramCounter: 621 - SystemTime: 9487549550560573440 + magicword: 1397310283 + length: 106 + payloadtype: 1602 + payloadversion: 1 + ordernumber: 1097816 + serialnumber: 19047026 + fw_version: [0, 0, 0, 0, 0, 0, 0, 76, 76, 83, 32, 86, 48, 46, 49, 46, 57, 46, 120, 66] + telegramcounter: 621 + systemtime: 9487549550560573440 telegram_payload: - ErrorCode: 0 - ScanCounter: 623 - Timestamp: 3468531 - PoseX: 93 - PoseY: 33 - PoseYaw: 17895 - Reserved1: 0 - Reserved2: 0 - Quality: 55 - OutliersRatio: 0 - CovarianceX: 32905 - CovarianceY: 39315 - CovarianceYaw: 1210527 - Reserved3: 0 + errorcode: 0 + scancounter: 623 + timestamp: 3468531 + posex: 93 + posey: 33 + poseyaw: 17895 + reserved1: 0 + reserved2: 0 + quality: 55 + outliersratio: 0 + covariancex: 32905 + covariancey: 39315 + covarianceyaw: 1210527 + reserved3: 0 telegram_trailer: - Checksum: 25105 + checksum: 25105 vehicle_time_valid: True vehicle_time_sec: 1571732539 vehicle_time_nsec: 854719042 ``` -Note: Result telegrams always have the same MagicWord: `1397310283` (`0x5349434B` hex resp. `SICK` in ascii/ansi) and a +Note: Result telegrams always have the same magicword: `1397310283` (`0x5349434B` hex resp. `SICK` in ascii/ansi) and a length of 106 bytes (`Length: 106`). ## Time synchronization -The localization controller sends the timestamp of a pose in millisecond ticks (telegram_payload.Timestamp of a result +The localization controller sends the timestamp of a pose in millisecond ticks (telegram_payload.timestamp of a result port telegram). This timestamp in milliseconds is accurate, but differs from the ros system time by a time offset. This time offset can be queried by a LocRequestTimestamp command. @@ -272,7 +471,10 @@ sim_loc_driver and defined in file [srv/SickLocColaTelegramSrv.srv](srv/SickLocC Example for a Cola command to query a LocRequestTimestamp: ``` +> # ROS1: > rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocRequestTimestamp', wait_response_timeout: 1}" +> # ROS2: +> ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN LocRequestTimestamp', wait_response_timeout: 1}" cola_ascii_response: "sAN LocRequestTimestamp 1EDB" send_timestamp_sec: 1573118218 send_timestamp_nsec: 365014292 @@ -298,7 +500,10 @@ sim_loc_driver and defined in file [srv/SickLocRequestTimestampSrv.srv](srv/Sick Example to query the time offset: ``` +> # ROS1: > rosservice call SickLocRequestTimestamp "{}" +> # ROS2: +> ros2 service call SickLocRequestTimestamp sick_lidar_localization/srv/SickLocRequestTimestampSrv "{}" timestamp_lidar_ms: 23745 mean_time_vehicle_ms: 1573118234209 delta_time_ms: 1573118210464 @@ -321,7 +526,10 @@ not need to care about time synchronization itself. It's sufficient to use `vehi the result telegrams. Example output of a result telegram: ``` +> # ROS1: > rostopic echo "/sick_lidar_localization/driver/result_telegrams" +> # ROS2: +> ros2 topic echo "/sick_lidar_localization/driver/result_telegrams" Timestamp: 525924 # Lidar timestamp in millisecond ticks vehicle_time_valid: True # System timestamp by software pll is valid vehicle_time_sec: 1573119569 # System timestamp of vehicle pose (second part) @@ -333,31 +541,48 @@ returns the system timestamp from ticks using the software pll running in the dr [srv/SickLocTimeSyncSrv.srv](srv/SickLocTimeSyncSrv.srv). Example: ``` +> # ROS1: > rosservice call SickLocTimeSync "{timestamp_lidar_ms: 123t456}" +> # ROS2: +> ros2 service call SickLocTimeSync sick_lidar_localization/srv/SickLocTimeSyncSrv "{timestamp_lidar_ms: 123t456}" vehicle_time_valid: True vehicle_time_sec: 1573119167 vehicle_time_nsec: 380565047 ``` **Important note:** The driver sends LocRequestTimestamp commands to the localization controller with a constant rate, -each 10 seconds by default (configurable by parameter `time_sync_rate` in file [yaml/sim_loc_driver.yaml](yaml/sim_loc_driver.yaml)). +each 10 seconds by default (configurable by parameter `time_sync_rate` in file [yaml/sim_loc_driver.yaml](yaml/sim_loc_driver.yaml) +(ROS1) resp. [launch/sim_loc_driver.launch.py](launch/sim_loc_driver.launch.py) (ROS2). The software pll is updated after each successful LocRequestTimestamp with the current lidar ticks and the current system time. The software pll uses a fifo buffer to calculate a regression, with a buffer size of 7 by default (configurable by -parameter `software_pll_fifo_length` in file [yaml/sim_loc_driver.yaml](yaml/sim_loc_driver.yaml)). Therefore, it takes +parameter `software_pll_fifo_length` in file [yaml/sim_loc_driver.yaml](yaml/sim_loc_driver.yaml) (ROS1) resp. +[launch/sim_loc_driver.launch.py](launch/sim_loc_driver.launch.py) (ROS2). Therefore, it takes at least 7 LocRequestTimestamp commands before the software pll is fully initialized and time synchronization becomes valid. Within the initialization phase (at least 7 LocRequestTimestamp commands), a system time can not be calculated from -lidar ticks. During initialization phase, `vehicle_time_valid` will be false, `vehicle_time_sec` and `vehicle_time_nsec` -will have value 0. +lidar ticks. During the initial phase, the time synchronization service is not available and the vehicle system time is not valid +(`vehicle_time_valid` will be false, `vehicle_time_sec` and `vehicle_time_nsec` will have value 0). + +## Odometry -SICK recommends a time_sync_rate of 0.1 or below („SICK recommends you set the request cycle time from the vehicle -controller to 10 seconds or higher”). Therefore, the initial phase after start will take round about 70 seconds (7 LocRequestTimestamp -commands with 10 seconds delay by default). During the initial phase, the time synchronization service is not available -and the vehicle system time is not valid. +The driver subscribes to odometry messages and sends timestamp and velocity to the localization server by udp packages. +The odometry topic and udp connection can be configured by [yaml/sim_loc_driver.yaml](yaml/sim_loc_driver.yaml) (ROS1) +resp. [launch/sim_loc_driver.launch.py](launch/sim_loc_driver.launch.py) (ROS2): + +``` +# Odometry configuration +{'/sick_lidar_localization/driver/odom_telegrams_udp_port': 3000}, # Udp port to send odom packages to the localization controller +{'/sick_lidar_localization/driver/odom_topic': "/odom"}, # ROS topic for odometry messages +{'/sick_lidar_localization/driver/odom_telegrams_bigendian': 1}, # Send udp odometry telegrams big endian (true) or little endian (false) +{'/sick_lidar_localization/driver/odom_telegrams_source_id': 100}, # SourceID of udp odometry telegrams, e.g. vehicle controller 1 +``` + +See the operation manual for further details about odometry support. ## Diagnostics The sick_lidar_localization driver publishes diagnostic messages on ros topic "/sick_lidar_localization/driver/diagnostic", -which can be examined by command `rostopic echo "/sick_lidar_localization/driver/diagnostic"`. +which can be examined by command `rostopic echo "/sick_lidar_localization/driver/diagnostic"` (ROS1) resp. +`ros2 topic echo "/sick_lidar_localization/driver/diagnostic"` (ROS2). Example diagnostic messages (ros message of type [msg/SickLocDiagnosticMsg.msg](msg/SickLocDiagnosticMsg.msg)): ``` @@ -393,7 +618,8 @@ INTERNAL_ERROR | 4 | Internal error (should never happen) ## Driver configuration -The sick_lidar_localization driver is configured by file [yaml/sim_loc_driver.yaml](yaml/sim_loc_driver.yaml): +The sick_lidar_localization driver is configured by file [yaml/sim_loc_driver.yaml](yaml/sim_loc_driver.yaml) (ROS1) resp. +[launch/sim_loc_driver.launch.py](launch/sim_loc_driver.launch.py) (ROS2): Parametername | Defaultvalue | Description --- | --- | --- @@ -424,6 +650,7 @@ Note: The IP address of the SICK localization controller (192.168.0.1 by default To test the sick_lidar_localization ros driver, just connect your ros system with the SICK localization controller, start the driver and observe the result port telegrams and diagnostic messages: +ROS1: ```console cd ~/catkin_ws source ./devel/setup.bash @@ -432,6 +659,16 @@ rostopic echo "/sick_lidar_localization/driver/result_telegrams" & roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=192.168.0.1 ``` +ROS2: +```console +cd ~/catkin_ws +source ./install/setup.bash +ros2 topic echo "/sick_lidar_localization/driver/diagnostic" & +ros2 topic echo "/sick_lidar_localization/driver/result_telegrams" & +export localization_controller_ip_address=192.168.0.1 +ros2 launch sick_lidar_localization sim_loc_driver.launch +``` + Warnings and error messages will be printed in case of failures like unreachable controller, connection losts or invalid telegrams. @@ -439,16 +676,23 @@ For automated tests over long time, the values of result telegram messages can b and maximal limits. These limits - i.e. min. and max. allowed values for each element in a result telegram message - can be configured by a yaml-file and can be automatically checked by sim_loc_driver_check: +ROS1: +```console +roslaunch sick_lidar_localization sim_loc_driver_check.launch +``` + +ROS2: ```console -roslaunch sick_lidar_localization sim_loc_driver_check.launch sim_loc_driver_check_cfg:=message_check_demo.yaml +ros2 launch sick_lidar_localization sim_loc_driver_check.launch.py ``` -Each result telegram message which violates the limits defined in file [yaml/message_check_demo.yaml](yaml/message_check_demo.yaml) +Each result telegram message which violates the limits defined in file [yaml/message_check_demo.yaml](yaml/message_check_demo.yaml) (ROS1)) +resp. [launch/sim_loc_driver.launch.py](launch/sim_loc_driver.launch.py) (ROS2) will result in warnings and error messages. By providing limits adapted to a specific scenario, result telegram messages can be checked automatically over long time. -Configuration file [yaml/message_check_demo.yaml](yaml/message_check_demo.yaml) defines lower bounds in section result_telegram_min_values -and upper bounds in section result_telegram_max_values for all values of a result telegram. By default, these limits are +Configuration file [yaml/message_check_demo.yaml](yaml/message_check_demo.yaml) (ROS1)) resp. [launch/sim_loc_driver.launch.py](launch/sim_loc_driver.launch.py) (ROS2) +defines lower bounds in section result_telegram_min_values and upper bounds in section result_telegram_max_values for all values of a result telegram. By default, these limits are configured to fit all scenarios. Feel free to provide a configuration file with narrower limits; this might help to track occasionally or otherwise hard to find problems. @@ -461,6 +705,7 @@ when using hardware controllers. sim_loc_test_server simulates a localization controller and generates random based result port telegrams. To run an offline simulation, start the test server and the sick_lidar_localization ros driver on you local system: +ROS1: ```console cd ~/catkin_ws source ./devel/setup.bash @@ -469,6 +714,16 @@ sleep 3 # make sure ros core and sim_loc_test_server are up and running roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 ``` +ROS2: +```console +cd ~/catkin_ws +source ./install/setup.bash +ros2 launch sick_lidar_localization sim_loc_test_server.launch.py & # start test server to generate result port telegrams +sleep 3 # make sure ros core and sim_loc_test_server are up and running +export localization_controller_ip_address=127.0.0.1 +ros2 launch sick_lidar_localization sim_loc_driver.launch.py +``` + The sick_lidar_localization ros driver will connect to the local test server, receive random based result port telegrams and publish them on ros topic "/sick_lidar_localization/driver/result_telegrams". Telegram messages can be viewed with @@ -476,6 +731,11 @@ and publish them on ros topic "/sick_lidar_localization/driver/result_telegrams" rostopic echo "/sick_lidar_localization/driver/diagnostic" & rostopic echo "/sick_lidar_localization/driver/result_telegrams" & ``` +resp. +```console +ros2 topic echo "/sick_lidar_localization/driver/diagnostic" & +ros2 topic echo "/sick_lidar_localization/driver/result_telegrams" & +``` Since the driver input (i.e. the binary result port telegrams) and the expected output (i.e. the generated telegrams) are known, the driver output can be verified. The test server sim_loc_test_server publishes testcases with both the @@ -489,6 +749,10 @@ To run the verification, start driver and test server as described above, and la ```console roslaunch sick_lidar_localization verify_sim_loc_driver.launch ``` +resp. +```console +ros2 launch sick_lidar_localization verify_sim_loc_driver.launch.py +``` After stopping the test, a summary note is printed. Example output by verify_sim_loc_driver: @@ -496,10 +760,16 @@ After stopping the test, a summary note is printed. Example output by verify_sim VerifierThread: verification thread summary: 588 messages checked, 0 failures. ``` -Use `run_simu.bash` in folder src/sick_lidar_localization/test/scripts to run an automated offline test: +Use `run_simu.bash` in folder src/sick_lidar_localization/test/ros1_scripts (ROS1) resp. src/sick_lidar_localization/test/ros2_scripts (ROS2) +to run an automated offline test: ```console -cd ~/catkin_ws/src/sick_lidar_localization/test/scripts +cd ~/catkin_ws/src/sick_lidar_localization/test/ros1_scripts +./run_simu.bash +``` +resp. +```console +cd ~/catkin_ws/src/sick_lidar_localization/test/ros2_scripts ./run_simu.bash ``` @@ -510,11 +780,16 @@ MessageCheckThread: check messages thread summary: 599 messages checked, 0 failu VerifierThread: verification thread summary: 585 messages checked, 0 failures. ``` -Use `run_cola_examples.bash` in folder src/sick_lidar_localization/test/scripts to run an automated offline test -of ros services for SIM configuration: +Use `run_cola_examples.bash` in folder src/sick_lidar_localization/test/ros1_scripts (ROS1) resp. src/sick_lidar_localization/test/ros2_scripts (ROS2) +to run an automated offline test of ros services for SIM configuration: ```console -cd ~/catkin_ws/src/sick_lidar_localization/test/scripts +cd ~/catkin_ws/src/sick_lidar_localization/test/ros1_scripts +./run_cola_examples.bash +``` +resp. +```console +cd ~/catkin_ws/src/sick_lidar_localization/test/ros2_scripts ./run_cola_examples.bash ``` @@ -537,11 +812,17 @@ f.e. with error code 1 (NO_TCP_CONNECTION) after connect lost: sick_lidar_localization,1,sim_loc_driver: no tcp connection to localization controller 192.168.0.1:2201 ``` -Errors can be simulated and tested using sim_loc_test_server with commandline argument `error_simulation:=true`: +Errors can be simulated and tested using sim_loc_test_server with argument `error_simulation`: +ROS1: ```console roslaunch sick_lidar_localization sim_loc_test_server.launch error_simulation:=true & # run test server in error simulation mode ``` +ROS2: +```console +export sim_loc_test_server_error_simulation=1 +ros2 launch sick_lidar_localization sim_loc_test_server.launch.py & # run test server in error simulation mode +``` In error simulation mode, sim_loc_test_server will toggle between errors and correct execution each 10 seconds. After switching to normal mode, sim_loc_test_server checks for telegram messages from the driver and displays an @@ -562,10 +843,16 @@ sim_loc_test_server runs each of these error modes for 10 seconds and switches t After switching to normal mode, the driver has to reconnect and the test server expects new telegram messages. In case of missing telegram messages from the driver, the test will fail. -Use `run_error_simu.bash` in folder src/sick_lidar_localization/test/scripts to run an automated error simulation and error test: +Use `run_error_simu.bash` in folder src/sick_lidar_localization/test/ros1_scripts (ROS1) resp. src/sick_lidar_localization/test/ros2_scripts (ROS2) +to run an automated error simulation and error test: ```console -cd ~/catkin_ws/src/sick_lidar_localization/test/scripts +cd ~/catkin_ws/src/sick_lidar_localization/test/ros1_scripts +./run_error_simu.bash +``` +resp. +```console +cd ~/catkin_ws/src/sick_lidar_localization/test/ros2_scripts ./run_error_simu.bash ``` @@ -588,6 +875,7 @@ which can be viewed by rviz. To run and visualize an example with a simulated vehicle moving in circles, run the following commands: +ROS1: ```console cd ~/catkin_ws source ./devel/setup.bash @@ -598,7 +886,26 @@ sleep 3 # make sure ros core and sim_loc_test_server are up and running roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 & # Visualize PointCloud2 and TF messages by rviz: rosrun tf static_transform_publisher 0 0 0 0 0 0 map pointcloud_sick_lidar_localization 10 & -rosrun rviz rviz & +rosrun rviz rviz -d ./src/sick_lidar_localization_pretest/test/config/rviz_sick_lidar_localization_demo_pointcloud.rviz & +rosrun rviz rviz -d ./src/sick_lidar_localization_pretest/test/config/rviz_sick_lidar_localization_demo_tf.rviz & +``` + +ROS2: +```console +cd ~/catkin_ws +source ./install/setup.bash +# Run test server, simulate localization controller with a vehicle moving in circles. +export localization_controller_ip_address=127.0.0.1 +export sim_loc_test_server_demo_circles=1 +export sim_loc_test_server_error_simulation=0 +ros2 launch sick_lidar_localization sim_loc_test_server.launch.py & +sleep 3 # make sure ros core and sim_loc_test_server are up and running +# Run ros driver, connect to localization controller, receive, convert and publish report telegrams +ros2 launch sick_lidar_localization sim_loc_driver.launch.py & +# Visualize PointCloud2 and TF messages by rviz: +ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map pointcloud_sick_lidar_localization & +ros2 run rviz2 rviz2 -d ./src/sick_lidar_localization_pretest/test/config/rviz2_sick_lidar_localization_demo_pointcloud.rviz & +ros2 run rviz2 rviz2 -d ./src/sick_lidar_localization_pretest/test/config/rviz2_sick_lidar_localization_demo_tf.rviz & ``` To view vehicles poses by TF messages, add a display with type TF and select frame tf_demo_map in global options: @@ -643,7 +950,7 @@ in file [src/result_port_parser.cpp](src/result_port_parser.cpp). sick_lidar_localization::DriverThread. In case of errors or missing telegram messages, the tcp connection to the localization controller is closed and re-established. -Doxygen source code documentation is supported. To build the doxygen source code documentation, run the following commands: +Doxygen source code documentation is supported. To build the doxygen source code documentation, run the following commands (ROS1): ```console cd ~/catkin_ws diff --git a/include/sick_lidar_localization/SoftwarePLL.h b/include/sick_lidar_localization/SoftwarePLL.h index 54a1448..fa88b3f 100644 --- a/include/sick_lidar_localization/SoftwarePLL.h +++ b/include/sick_lidar_localization/SoftwarePLL.h @@ -183,7 +183,7 @@ class SoftwarePLL bool NearSameTimeStamp(double relTimeStamp1, double relTimeStamp2); bool UpdateInterpolationSlope(); uint32_t ExtrapolationDivergenceCounter_; - SoftwarePLL(int fifo_length = 7) : FifoSize_(fifo_length), TickFifo_(fifo_length,0), ClockFifo_(fifo_length,0) + SoftwarePLL(int fifo_length = 7) : FifoSize_(fifo_length), TickFifo_(fifo_length,0), ClockFifo_(fifo_length,0), IsInitialized_(false), FirstTimeStamp_(0), FirstTick_(0), InterpolationSlope_(0) { AllowedTimeDeviation(SoftwarePLL::MaxAllowedTimeDeviation_); // 1 ms NumberValInFifo_ = 0; diff --git a/include/sick_lidar_localization/cola_configuration.h b/include/sick_lidar_localization/cola_configuration.h index 35cebfc..569846e 100644 --- a/include/sick_lidar_localization/cola_configuration.h +++ b/include/sick_lidar_localization/cola_configuration.h @@ -76,8 +76,10 @@ namespace sick_lidar_localization /*! * Constructor + * @param[in] nh ros node handle + * @param[in] cola_services cola service callbacks (converts requests to cola telegrams, sends the cola telegrams and decodes the response from localization server) */ - ColaConfiguration(ros::NodeHandle* nh = 0); + ColaConfiguration(ROS::NodePtr nh = 0, sick_lidar_localization::ColaServices* cola_services = 0); /*! * Destructor @@ -109,31 +111,55 @@ namespace sick_lidar_localization virtual void runConfigurationThreadCb(void); /*! - * Calls a ros service until successfully executed or configuration thread cancelled. - * @param[in/out] service_telegram service request and response - * @param[in] service_client ros service client + * Calls a ROS1 service until successfully executed or configuration thread cancelled. + * @param[in,out] service_telegram service request and response + * @param[in] service_client ROS1 service client * @param[in] retry_delay delay before next retry in case of errors * @return true on success or false on failure */ - template bool callService(T & service_telegram, ros::ServiceClient & service_client, double retry_delay) + template bool callServiceROS1(TelegramType & service_telegram, ClientType & service_client, double retry_delay) { service_telegram.response.success = false; - while(ros::ok() && m_nh && m_configuration_thread_running && (!service_client.call(service_telegram) || !service_telegram.response.success)) + while(ROS::ok() && m_nh && m_configuration_thread_running && (!service_client.call(service_telegram) || !service_telegram.response.success)) { ROS_WARN_STREAM("## ERROR ColaConfiguration: service call(" << sick_lidar_localization::Utils::flattenToString(service_telegram.request) << ") failed, response: " << sick_lidar_localization::Utils::flattenToString(service_telegram.response) << ", retrying"); - ros::Duration(retry_delay).sleep(); + ROS::sleep(retry_delay); } - ROS_INFO_STREAM("ColaConfiguration " << typeid(T).name() << " " << (service_telegram.response.success ? "successfull" : "failed")); + ROS_INFO_STREAM("ColaConfiguration " << typeid(TelegramType).name() << " " << (service_telegram.response.success ? "successfull" : "failed")); return service_telegram.response.success; } + /*! + * Calls a ROS1 service until successfully executed or configuration thread cancelled. + * @param[in] service_request service request + * @param[in] service_client ROS1 service client + * @param[in] retry_delay delay before next retry in case of errors + * @return true on success or false on failure + */ + template bool callServiceROS2(RequestType & service_request, ClientType & service_client, double retry_delay) + { + while(ROS::ok() && m_nh && m_configuration_thread_running) + { + auto service_future = service_client->async_send_request(service_request); + if(service_future.wait_for(std::chrono::milliseconds((int64_t)(1000*retry_delay))) == std::future_status::ready && service_future.get()->success) + { + ROS_INFO_STREAM("ColaConfiguration " << typeid(RequestType).name() << " successfull"); + return true; // service successfull completed + } + ROS_WARN_STREAM("## ERROR ColaConfiguration: service call type " << typeid(*service_request).name() << " failed, retrying"); + ROS::sleep(retry_delay); + } + return false; + } + /* * member variables */ - ros::NodeHandle* m_nh; ///< ros node handle + ROS::NodePtr m_nh; ///< ros node handle boost::thread* m_configuration_thread; ///< thread to transmit configuration bool m_configuration_thread_running; ///< true: m_configuration_thread is running, otherwise false + sick_lidar_localization::ColaServices* m_cola_services; ///< cola service callbacks (converts requests to cola telegrams, sends the cola telegrams and decodes the response from localization server) }; // class ColaServices diff --git a/include/sick_lidar_localization/cola_encoder.h b/include/sick_lidar_localization/cola_encoder.h new file mode 100755 index 0000000..1bb912c --- /dev/null +++ b/include/sick_lidar_localization/cola_encoder.h @@ -0,0 +1,633 @@ +/* + * @brief cola_encoder encodes service requests to cola telegrams, parses cola responses and + * converts them to service responses. + * + * 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 + * + */ +#ifndef __SIM_LOC_COLA_ENCODER_H_INCLUDED +#define __SIM_LOC_COLA_ENCODER_H_INCLUDED + +#include +#include +#include "sick_lidar_localization/ros_wrapper.h" +#include "sick_lidar_localization/cola_parser.h" + +namespace sick_lidar_localization +{ + /*! + * @brief class ColaEncoder encodes service requests to cola telegrams, parses cola responses and + * converts them to service responses. + */ + class ColaEncoder + { + public: + + /*! + * Converts the service request for service SickDevSetLidarConfigSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevSetLidarConfigSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickDevSetLidarConfigSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickDevSetLidarConfigSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevSetLidarConfigSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevSetLidarConfigSrv::Response& service_response); + + /*! + * Converts the service request for service SickDevGetLidarConfigSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevGetLidarConfigSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickDevGetLidarConfigSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickDevGetLidarConfigSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevGetLidarConfigSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevGetLidarConfigSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocSetMapSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetMapSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocSetMapSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetMapSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetMapSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetMapSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocMapSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocMapSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocMapSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocMapSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocMapSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocMapSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocMapStateSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocMapStateSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocMapStateSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocMapStateSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocMapStateSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocMapStateSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocInitializePoseSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocInitializePoseSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocInitializePoseSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocInitializePoseSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocInitializePoseSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocInitializePoseSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocInitialPoseSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocInitialPoseSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocInitialPoseSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocInitialPoseSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocInitialPoseSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocInitialPoseSrv::Response& service_response); + + + /*! + * Converts the service request for service SickLocSetReflectorsForSupportActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetReflectorsForSupportActiveSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetReflectorsForSupportActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetReflectorsForSupportActiveSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocReflectorsForSupportActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocReflectorsForSupportActiveSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocReflectorsForSupportActiveSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocReflectorsForSupportActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocReflectorsForSupportActiveSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocReflectorsForSupportActiveSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocSetOdometryActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetOdometryActiveSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocSetOdometryActiveSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetOdometryActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetOdometryActiveSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetOdometryActiveSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocOdometryActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocOdometryActiveSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocOdometryActiveSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocOdometryActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocOdometryActiveSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocOdometryActiveSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocSetOdometryPortSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetOdometryPortSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocSetOdometryPortSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetOdometryPortSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetOdometryPortSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetOdometryPortSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocOdometryPortSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocOdometryPortSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocOdometryPortSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocOdometryPortSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocOdometryPortSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocOdometryPortSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocSetOdometryRestrictYMotionSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetOdometryRestrictYMotionSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetOdometryRestrictYMotionSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetOdometryRestrictYMotionSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocOdometryRestrictYMotionSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocOdometryRestrictYMotionSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocOdometryRestrictYMotionSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocOdometryRestrictYMotionSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocOdometryRestrictYMotionSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocOdometryRestrictYMotionSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocSetAutoStartActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetAutoStartActiveSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocSetAutoStartActiveSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetAutoStartActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetAutoStartActiveSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetAutoStartActiveSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocAutoStartActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocAutoStartActiveSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocAutoStartActiveSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocAutoStartActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocAutoStartActiveSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocAutoStartActiveSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocSetAutoStartSavePoseIntervalSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetAutoStartSavePoseIntervalSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetAutoStartSavePoseIntervalSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetAutoStartSavePoseIntervalSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocAutoStartSavePoseIntervalSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocAutoStartSavePoseIntervalSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocAutoStartSavePoseIntervalSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocAutoStartSavePoseIntervalSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocAutoStartSavePoseIntervalSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocAutoStartSavePoseIntervalSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocSetRingBufferRecordingActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetRingBufferRecordingActiveSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetRingBufferRecordingActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetRingBufferRecordingActiveSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocRingBufferRecordingActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocRingBufferRecordingActiveSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocRingBufferRecordingActiveSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocRingBufferRecordingActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocRingBufferRecordingActiveSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocRingBufferRecordingActiveSrv::Response& service_response); + + /*! + * Converts the service request for service SickDevGetLidarIdentSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevGetLidarIdentSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickDevGetLidarIdentSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickDevGetLidarIdentSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevGetLidarIdentSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevGetLidarIdentSrv::Response& service_response); + + /*! + * Converts the service request for service SickDevGetLidarStateSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevGetLidarStateSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickDevGetLidarStateSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickDevGetLidarStateSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevGetLidarStateSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevGetLidarStateSrv::Response& service_response); + + /*! + * Converts the service request for service SickGetSoftwareVersionSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickGetSoftwareVersionSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickGetSoftwareVersionSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickGetSoftwareVersionSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickGetSoftwareVersionSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickGetSoftwareVersionSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocAutoStartSavePoseSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocAutoStartSavePoseSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocAutoStartSavePoseSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocAutoStartSavePoseSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocAutoStartSavePoseSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocAutoStartSavePoseSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocForceUpdateSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocForceUpdateSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocForceUpdateSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocForceUpdateSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocForceUpdateSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocForceUpdateSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocSaveRingBufferRecordingSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSaveRingBufferRecordingSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocSaveRingBufferRecordingSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocSaveRingBufferRecordingSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSaveRingBufferRecordingSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSaveRingBufferRecordingSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocStartDemoMappingSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocStartDemoMappingSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocStartDemoMappingSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocStartDemoMappingSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocStartDemoMappingSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocStartDemoMappingSrv::Response& service_response); + + /*! + * Converts the service request for service SickReportUserMessageSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickReportUserMessageSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickReportUserMessageSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickReportUserMessageSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickReportUserMessageSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickReportUserMessageSrv::Response& service_response); + + /*! + * Converts the service request for service SickSavePermanentSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickSavePermanentSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickSavePermanentSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickSavePermanentSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickSavePermanentSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickSavePermanentSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocResultPortSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocResultPortSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocResultPortSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocResultPortSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocResultPortSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocResultPortSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocResultModeSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocResultModeSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocResultModeSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocResultModeSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocResultModeSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocResultModeSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocResultEndiannessSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocResultEndiannessSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocResultEndiannessSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocResultEndiannessSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocResultEndiannessSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocResultEndiannessSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocResultStateSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocResultStateSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocResultStateSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocResultStateSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocResultStateSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocResultStateSrv::Response& service_response); + + /*! + * Converts the service request for service SickLocResultPoseIntervalSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocResultPoseIntervalSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickLocResultPoseIntervalSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickLocResultPoseIntervalSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocResultPoseIntervalSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocResultPoseIntervalSrv::Response& service_response); + + /*! + * Converts the service request for service SickDevSetIMUActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevSetIMUActiveSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickDevSetIMUActiveSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickDevSetIMUActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevSetIMUActiveSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevSetIMUActiveSrv::Response& service_response); + + /*! + * Converts the service request for service SickDevIMUActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevIMUActiveSrv + * @return cola ascii telegram + */ + static std::string encodeServiceRequest(const sick_lidar_localization::SickDevIMUActiveSrv::Request& service_request); + + /*! + * Parses a cola response and converts the arguments to a service response for service SickDevIMUActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevIMUActiveSrv + * @return true on succes or false in case of parse errors + */ + static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevIMUActiveSrv::Response& service_response); + + }; // class ColaEncoder + +} // namespace sick_lidar_localization +#endif // __SIM_LOC_COLA_ENCODER_H_INCLUDED diff --git a/include/sick_lidar_localization/cola_parser.h b/include/sick_lidar_localization/cola_parser.h index 2470086..c18d133 100644 --- a/include/sick_lidar_localization/cola_parser.h +++ b/include/sick_lidar_localization/cola_parser.h @@ -61,7 +61,7 @@ #ifndef __SIM_LOC_COLA_PARSER_H_INCLUDED #define __SIM_LOC_COLA_PARSER_H_INCLUDED -#include "sick_lidar_localization/SickLocColaTelegramMsg.h" +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/cola_converter.h" namespace sick_lidar_localization @@ -189,7 +189,33 @@ namespace sick_lidar_localization * @return COLA_SOPAS_COMMAND from string. */ static COLA_SOPAS_COMMAND convertSopasCommand(const std::string & sopas_command); + + /*! + * Converts and returns the parameter of a cola ascii telegram into a numeric value. + * @param[in] cola_arg parameter of a cola ascii telegram + * @param[in] base numeric base (10 for decimal values, 16 for hex strings or -1 for autodetection with base 10 in case of +/-sign, otherwise hex) + * @param[in] default_value default value returned in case of parse errors + * @return parameter converted to integer value + */ + static int32_t convertColaArg(const std::string & cola_arg, int base = 10, int32_t default_value = 0); + /*! + * Converts and returns the parameter of a cola ascii telegram into an unsigned numeric value. + * @param[in] cola_arg parameter of a cola ascii telegram + * @param[in] base numeric base (10 for decimal values, 16 for hex strings or -1 for autodetection with base 10 in case of +/-sign, otherwise hex) + * @param[in] default_value default value returned in case of parse errors + * @return parameter converted to integer value + */ + static uint32_t convertColaArg(const std::string & cola_arg, int base = 10, uint32_t default_value = 0); + + /*! + * Converts and returns the parameter of a cola ascii response into a boolean value. + * @param[in] cola_response_arg parameter of a cola ascii response + * @param[in] default_value default value returned in case of parse errors + * @return parameter converted to boolean value + */ + static bool convertColaResponseBool(const std::string & cola_response_arg, bool default_value); + protected: static const std::string s_command_type_string[MAX_COLA_COMMAND_NUMBER]; ///< static table to convert COLA_SOPAS_COMMAND to string, f.e. s_command_type_string[sRN]:="sRN", s_command_type_string[sRA]:="sRA" and so on diff --git a/include/sick_lidar_localization/cola_services.h b/include/sick_lidar_localization/cola_services.h index 7dc63b3..95ca75e 100644 --- a/include/sick_lidar_localization/cola_services.h +++ b/include/sick_lidar_localization/cola_services.h @@ -6,7 +6,6 @@ * "SickLocState" | srv/SickLocStateSrv.srv | "sRN LocState" | Read localization state * "SickLocStartLocalizing" | srv/SickLocStartLocalizingSrv.srv | "sMN LocStartLocalizing" | Start localization * "SickLocStop" | srv/SickLocStopSrv.srv | "sMN LocStop" | Stop localization or demo mapping - * "SickLocStopAndSave" | srv/SickLocStopAndSaveSrv.srv | "sMN LocStopAndSave" | Stop localization, save settings * "SickLocSetResultPort" | srv/SickLocSetResultPortSrv.srv | "sMN LocSetResultPort " | Set TCP-port for result output (default: 2201) * "SickLocSetResultMode" | srv/SickLocSetResultModeSrv.srv | "sMN LocSetResultMode " | Set mode of result output (default: stream) * "SickLocSetResultPoseEnabled" | srv/SickLocSetResultPoseEnabledSrv.srv | "sMN LocSetResultPoseEnabled " | Disable/enable result output @@ -71,20 +70,9 @@ #ifndef __SIM_LOC_COLA_SERVICES_H_INCLUDED #define __SIM_LOC_COLA_SERVICES_H_INCLUDED +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/cola_parser.h" -#include "sick_lidar_localization/SickLocColaTelegramSrv.h" -#include "sick_lidar_localization/SickLocIsSystemReadySrv.h" -#include "sick_lidar_localization/SickLocStateSrv.h" -#include "sick_lidar_localization/SickLocStartLocalizingSrv.h" -#include "sick_lidar_localization/SickLocStopSrv.h" -#include "sick_lidar_localization/SickLocStopAndSaveSrv.h" -#include "sick_lidar_localization/SickLocSetResultPortSrv.h" -#include "sick_lidar_localization/SickLocSetResultModeSrv.h" -#include "sick_lidar_localization/SickLocSetResultPoseEnabledSrv.h" -#include "sick_lidar_localization/SickLocSetResultEndiannessSrv.h" -#include "sick_lidar_localization/SickLocSetResultPoseIntervalSrv.h" -#include "sick_lidar_localization/SickLocRequestResultDataSrv.h" -#include "sick_lidar_localization/SickLocSetPoseSrv.h" +#include "sick_lidar_localization/driver_monitor.h" #include "sick_lidar_localization/utils.h" namespace sick_lidar_localization @@ -98,7 +86,6 @@ namespace sick_lidar_localization * "SickLocState" | srv/SickLocStateSrv.srv | "sRN LocState" | Read localization state * "SickLocStartLocalizing" | srv/SickLocStartLocalizingSrv.srv | "sMN LocStartLocalizing" | Start localization * "SickLocStop" | srv/SickLocStopSrv.srv | "sMN LocStop" | Stop localization or demo mapping - * "SickLocStopAndSave" | srv/SickLocStopAndSaveSrv.srv | "sMN LocStopAndSave" | Stop localization, save settings * "SickLocSetResultPort" | srv/SickLocSetResultPortSrv.srv | "sMN LocSetResultPort " | Set TCP-port for result output (default: 2201) * "SickLocSetResultMode" | srv/SickLocSetResultModeSrv.srv | "sMN LocSetResultMode " | Set mode of result output (default: stream) * "SickLocSetResultPoseEnabled" | srv/SickLocSetResultPoseEnabledSrv.srv | "sMN LocSetResultPoseEnabled " | Disable/enable result output @@ -116,7 +103,7 @@ namespace sick_lidar_localization /*! * Constructor */ - ColaServices(ros::NodeHandle* nh = 0); + ColaServices(ROS::NodePtr nh = 0, sick_lidar_localization::DriverMonitor* driver_monitor = 0); /*! * Destructor @@ -132,7 +119,12 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbIsSystemReady(sick_lidar_localization::SickLocIsSystemReadySrv::Request & service_request, sick_lidar_localization::SickLocIsSystemReadySrv::Response & service_response); - + /*! ROS2 version of function serviceCbIsSystemReady */ + virtual bool serviceCbIsSystemReadyROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbIsSystemReady(*service_request, *service_response); + } + /*! * Callback for service messages (SickLocStateSrv, Read localization state). * Sends a cola telegram "sRN LocState" and receives the response from localization controller @@ -142,6 +134,11 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbLocState(sick_lidar_localization::SickLocStateSrv::Request & service_request, sick_lidar_localization::SickLocStateSrv::Response & service_response); + /*! ROS2 version of function serviceCbLocState */ + virtual bool serviceCbLocStateROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocState(*service_request, *service_response); + } /*! * Callback for service messages (SickLocStartLocalizingSrv, Stop localization, save settings). @@ -152,6 +149,11 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbLocStartLocalizing(sick_lidar_localization::SickLocStartLocalizingSrv::Request & service_request, sick_lidar_localization::SickLocStartLocalizingSrv::Response & service_response); + /*! ROS2 version of function serviceCbLocStartLocalizing */ + virtual bool serviceCbLocStartLocalizingROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocStartLocalizing(*service_request, *service_response); + } /*! * Callback for service messages (SickLocStopSrv, Stop localization, save settings). @@ -162,16 +164,11 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbLocStop(sick_lidar_localization::SickLocStopSrv::Request & service_request, sick_lidar_localization::SickLocStopSrv::Response & service_response); - - /*! - * Callback for service messages (SickLocStopAndSaveSrv, Stop localization, save settings). - * Sends a cola telegram "sMN LocStopAndSave" and receives the response from localization controller - * using ros service "SickLocColaTelegramSrv". - * @param[in] service_request ros service request to localization controller - * @param[out] service_response service response from localization controller - * @return true on success, false in case of errors. - */ - virtual bool serviceCbLocStopAndSave(sick_lidar_localization::SickLocStopAndSaveSrv::Request & service_request, sick_lidar_localization::SickLocStopAndSaveSrv::Response & service_response); + /*! ROS2 version of function serviceCbLocStop */ + virtual bool serviceCbLocStopROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocStop(*service_request, *service_response); + } /*! * Callback for service messages (SickLocSetResultPortSrv, Set TCP-port for result output, default: 2201). @@ -182,6 +179,11 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbLocSetResultPort(sick_lidar_localization::SickLocSetResultPortSrv::Request & service_request, sick_lidar_localization::SickLocSetResultPortSrv::Response & service_response); + /*! ROS2 version of function serviceCbLocSetResultPort */ + virtual bool serviceCbLocSetResultPortROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetResultPort(*service_request, *service_response); + } /*! * Callback for service messages (SickLocSetResultModeSrv, Set mode of result output, default: stream). @@ -192,6 +194,11 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbLocSetResultMode(sick_lidar_localization::SickLocSetResultModeSrv::Request & service_request, sick_lidar_localization::SickLocSetResultModeSrv::Response & service_response); + /*! ROS2 version of function serviceCbLocSetResultMode */ + virtual bool serviceCbLocSetResultModeROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetResultMode(*service_request, *service_response); + } /*! * Callback for service messages (SickLocSetResultPoseEnabledSrv, Disable/enable result output). @@ -202,6 +209,11 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbLocSetResultPoseEnabled(sick_lidar_localization::SickLocSetResultPoseEnabledSrv::Request & service_request, sick_lidar_localization::SickLocSetResultPoseEnabledSrv::Response & service_response); + /*! ROS2 version of function serviceCbLocSetResultPoseEnabled */ + virtual bool serviceCbLocSetResultPoseEnabledROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetResultPoseEnabled(*service_request, *service_response); + } /*! * Callback for service messages (SickLocSetResultEndiannessSrv, Set endianness of result output). @@ -212,6 +224,11 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbLocSetResultEndianness(sick_lidar_localization::SickLocSetResultEndiannessSrv::Request & service_request, sick_lidar_localization::SickLocSetResultEndiannessSrv::Response & service_response); + /*! ROS2 version of function serviceCbLocSetResultEndianness */ + virtual bool serviceCbLocSetResultEndiannessROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetResultEndianness(*service_request, *service_response); + } /*! * Callback for service messages (SickLocSetResultPoseIntervalSrv, Set interval of result output). @@ -222,6 +239,11 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbLocSetResultPoseInterval(sick_lidar_localization::SickLocSetResultPoseIntervalSrv::Request & service_request, sick_lidar_localization::SickLocSetResultPoseIntervalSrv::Response & service_response); + /*! ROS2 version of function serviceCbLocSetResultPoseInterval */ + virtual bool serviceCbLocSetResultPoseIntervalROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetResultPoseInterval(*service_request, *service_response); + } /*! * Callback for service messages (SickLocRequestResultDataSrv, If in poll mode, trigger sending the localization result of the next processed scan via TCP interface). @@ -232,6 +254,11 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbLocRequestResultData(sick_lidar_localization::SickLocRequestResultDataSrv::Request & service_request, sick_lidar_localization::SickLocRequestResultDataSrv::Response & service_response); + /*! ROS2 version of function serviceCbLocRequestResultData */ + virtual bool serviceCbLocRequestResultDataROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocRequestResultData(*service_request, *service_response); + } /*! * Callback for service messages (SickLocSetPoseSrv, Initialize vehicle pose). @@ -242,7 +269,567 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbLocSetPose(sick_lidar_localization::SickLocSetPoseSrv::Request & service_request, sick_lidar_localization::SickLocSetPoseSrv::Response & service_response); - + /*! ROS2 version of function serviceCbLocSetPose */ + virtual bool serviceCbLocSetPoseROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetPose(*service_request, *service_response); + } + + /*! + * Callback for service "SickDevSetLidarConfigSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbDevSetLidarConfig(sick_lidar_localization::SickDevSetLidarConfigSrv::Request& service_request, sick_lidar_localization::SickDevSetLidarConfigSrv::Response& service_response); + /*! ROS2 version of function serviceCbDevSetLidarConfig */ + virtual bool serviceCbDevSetLidarConfigROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbDevSetLidarConfig(*service_request, *service_response); + } + + /*! + * Callback for service "SickDevGetLidarConfigSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbDevGetLidarConfig(sick_lidar_localization::SickDevGetLidarConfigSrv::Request& service_request, sick_lidar_localization::SickDevGetLidarConfigSrv::Response& service_response); + /*! ROS2 version of function serviceCbDevGetLidarConfig */ + virtual bool serviceCbDevGetLidarConfigROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbDevGetLidarConfig(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocSetMapSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocSetMap(sick_lidar_localization::SickLocSetMapSrv::Request& service_request, sick_lidar_localization::SickLocSetMapSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocSetMap */ + virtual bool serviceCbLocSetMapROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetMap(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocMapSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocMap(sick_lidar_localization::SickLocMapSrv::Request& service_request, sick_lidar_localization::SickLocMapSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocMap */ + virtual bool serviceCbLocMapROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocMap(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocMapStateSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocMapState(sick_lidar_localization::SickLocMapStateSrv::Request& service_request, sick_lidar_localization::SickLocMapStateSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocMapState */ + virtual bool serviceCbLocMapStateROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocMapState(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocInitializePoseSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocInitializePose(sick_lidar_localization::SickLocInitializePoseSrv::Request& service_request, sick_lidar_localization::SickLocInitializePoseSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocInitializePose */ + virtual bool serviceCbLocInitializePoseROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocInitializePose(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocInitialPoseSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocInitialPose(sick_lidar_localization::SickLocInitialPoseSrv::Request& service_request, sick_lidar_localization::SickLocInitialPoseSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocInitialPose */ + virtual bool serviceCbLocInitialPoseROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocInitialPose(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocSetReflectorsForSupportActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocSetReflectorsForSupportActive(sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrv::Request& service_request, sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocSetReflectorsForSupportActive */ + virtual bool serviceCbLocSetReflectorsForSupportActiveROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetReflectorsForSupportActive(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocReflectorsForSupportActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocReflectorsForSupportActive(sick_lidar_localization::SickLocReflectorsForSupportActiveSrv::Request& service_request, sick_lidar_localization::SickLocReflectorsForSupportActiveSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocReflectorsForSupportActive */ + virtual bool serviceCbLocReflectorsForSupportActiveROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocReflectorsForSupportActive(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocSetOdometryActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocSetOdometryActive(sick_lidar_localization::SickLocSetOdometryActiveSrv::Request& service_request, sick_lidar_localization::SickLocSetOdometryActiveSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocSetOdometryActive */ + virtual bool serviceCbLocSetOdometryActiveROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetOdometryActive(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocOdometryActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocOdometryActive(sick_lidar_localization::SickLocOdometryActiveSrv::Request& service_request, sick_lidar_localization::SickLocOdometryActiveSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocOdometryActive */ + virtual bool serviceCbLocOdometryActiveROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocOdometryActive(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocSetOdometryPortSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocSetOdometryPort(sick_lidar_localization::SickLocSetOdometryPortSrv::Request& service_request, sick_lidar_localization::SickLocSetOdometryPortSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocSetOdometryPort */ + virtual bool serviceCbLocSetOdometryPortROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetOdometryPort(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocOdometryPortSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocOdometryPort(sick_lidar_localization::SickLocOdometryPortSrv::Request& service_request, sick_lidar_localization::SickLocOdometryPortSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocOdometryPort */ + virtual bool serviceCbLocOdometryPortROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocOdometryPort(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocSetOdometryRestrictYMotionSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocSetOdometryRestrictYMotion(sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrv::Request& service_request, sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocSetOdometryRestrictYMotion */ + virtual bool serviceCbLocSetOdometryRestrictYMotionROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetOdometryRestrictYMotion(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocOdometryRestrictYMotionSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocOdometryRestrictYMotion(sick_lidar_localization::SickLocOdometryRestrictYMotionSrv::Request& service_request, sick_lidar_localization::SickLocOdometryRestrictYMotionSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocOdometryRestrictYMotion */ + virtual bool serviceCbLocOdometryRestrictYMotionROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocOdometryRestrictYMotion(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocSetAutoStartActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocSetAutoStartActive(sick_lidar_localization::SickLocSetAutoStartActiveSrv::Request& service_request, sick_lidar_localization::SickLocSetAutoStartActiveSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocSetAutoStartActive */ + virtual bool serviceCbLocSetAutoStartActiveROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetAutoStartActive(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocAutoStartActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocAutoStartActive(sick_lidar_localization::SickLocAutoStartActiveSrv::Request& service_request, sick_lidar_localization::SickLocAutoStartActiveSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocAutoStartActive */ + virtual bool serviceCbLocAutoStartActiveROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocAutoStartActive(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocSetAutoStartSavePoseIntervalSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocSetAutoStartSavePoseInterval(sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrv::Request& service_request, sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocSetAutoStartSavePoseInterval */ + virtual bool serviceCbLocSetAutoStartSavePoseIntervalROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetAutoStartSavePoseInterval(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocAutoStartSavePoseIntervalSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocAutoStartSavePoseInterval(sick_lidar_localization::SickLocAutoStartSavePoseIntervalSrv::Request& service_request, sick_lidar_localization::SickLocAutoStartSavePoseIntervalSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocAutoStartSavePoseInterval */ + virtual bool serviceCbLocAutoStartSavePoseIntervalROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocAutoStartSavePoseInterval(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocSetRingBufferRecordingActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocSetRingBufferRecordingActive(sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrv::Request& service_request, sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocSetRingBufferRecordingActive */ + virtual bool serviceCbLocSetRingBufferRecordingActiveROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSetRingBufferRecordingActive(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocRingBufferRecordingActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocRingBufferRecordingActive(sick_lidar_localization::SickLocRingBufferRecordingActiveSrv::Request& service_request, sick_lidar_localization::SickLocRingBufferRecordingActiveSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocRingBufferRecordingActive */ + virtual bool serviceCbLocRingBufferRecordingActiveROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocRingBufferRecordingActive(*service_request, *service_response); + } + + /*! + * Callback for service "SickDevGetLidarIdentSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbDevGetLidarIdent(sick_lidar_localization::SickDevGetLidarIdentSrv::Request& service_request, sick_lidar_localization::SickDevGetLidarIdentSrv::Response& service_response); + /*! ROS2 version of function serviceCbDevGetLidarIdent */ + virtual bool serviceCbDevGetLidarIdentROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbDevGetLidarIdent(*service_request, *service_response); + } + + /*! + * Callback for service "SickDevGetLidarStateSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbDevGetLidarState(sick_lidar_localization::SickDevGetLidarStateSrv::Request& service_request, sick_lidar_localization::SickDevGetLidarStateSrv::Response& service_response); + /*! ROS2 version of function serviceCbDevGetLidarState */ + virtual bool serviceCbDevGetLidarStateROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbDevGetLidarState(*service_request, *service_response); + } + + /*! + * Callback for service "SickGetSoftwareVersionSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbGetSoftwareVersion(sick_lidar_localization::SickGetSoftwareVersionSrv::Request& service_request, sick_lidar_localization::SickGetSoftwareVersionSrv::Response& service_response); + /*! ROS2 version of function serviceCbGetSoftwareVersion */ + virtual bool serviceCbGetSoftwareVersionROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbGetSoftwareVersion(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocAutoStartSavePoseSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocAutoStartSavePose(sick_lidar_localization::SickLocAutoStartSavePoseSrv::Request& service_request, sick_lidar_localization::SickLocAutoStartSavePoseSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocAutoStartSavePose */ + virtual bool serviceCbLocAutoStartSavePoseROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocAutoStartSavePose(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocForceUpdateSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocForceUpdate(sick_lidar_localization::SickLocForceUpdateSrv::Request& service_request, sick_lidar_localization::SickLocForceUpdateSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocForceUpdate */ + virtual bool serviceCbLocForceUpdateROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocForceUpdate(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocSaveRingBufferRecordingSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocSaveRingBufferRecording(sick_lidar_localization::SickLocSaveRingBufferRecordingSrv::Request& service_request, sick_lidar_localization::SickLocSaveRingBufferRecordingSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocSaveRingBufferRecording */ + virtual bool serviceCbLocSaveRingBufferRecordingROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocSaveRingBufferRecording(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocStartDemoMappingSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocStartDemoMapping(sick_lidar_localization::SickLocStartDemoMappingSrv::Request& service_request, sick_lidar_localization::SickLocStartDemoMappingSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocStartDemoMapping */ + virtual bool serviceCbLocStartDemoMappingROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocStartDemoMapping(*service_request, *service_response); + } + + /*! + * Callback for service "SickReportUserMessageSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbReportUserMessage(sick_lidar_localization::SickReportUserMessageSrv::Request& service_request, sick_lidar_localization::SickReportUserMessageSrv::Response& service_response); + /*! ROS2 version of function serviceCbReportUserMessage */ + virtual bool serviceCbReportUserMessageROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbReportUserMessage(*service_request, *service_response); + } + + /*! + * Callback for service "SickSavePermanentSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbSavePermanent(sick_lidar_localization::SickSavePermanentSrv::Request& service_request, sick_lidar_localization::SickSavePermanentSrv::Response& service_response); + /*! ROS2 version of function serviceCbSavePermanent */ + virtual bool serviceCbSavePermanentROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbSavePermanent(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocResultPortSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocResultPort(sick_lidar_localization::SickLocResultPortSrv::Request& service_request, sick_lidar_localization::SickLocResultPortSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocResultPort */ + virtual bool serviceCbLocResultPortROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocResultPort(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocResultModeSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocResultMode(sick_lidar_localization::SickLocResultModeSrv::Request& service_request, sick_lidar_localization::SickLocResultModeSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocResultMode */ + virtual bool serviceCbLocResultModeROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocResultMode(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocResultEndiannessSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocResultEndianness(sick_lidar_localization::SickLocResultEndiannessSrv::Request& service_request, sick_lidar_localization::SickLocResultEndiannessSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocResultEndianness */ + virtual bool serviceCbLocResultEndiannessROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocResultEndianness(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocResultStateSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocResultState(sick_lidar_localization::SickLocResultStateSrv::Request& service_request, sick_lidar_localization::SickLocResultStateSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocResultState */ + virtual bool serviceCbLocResultStateROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocResultState(*service_request, *service_response); + } + + /*! + * Callback for service "SickLocResultPoseIntervalSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbLocResultPoseInterval(sick_lidar_localization::SickLocResultPoseIntervalSrv::Request& service_request, sick_lidar_localization::SickLocResultPoseIntervalSrv::Response& service_response); + /*! ROS2 version of function serviceCbLocResultPoseInterval */ + virtual bool serviceCbLocResultPoseIntervalROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbLocResultPoseInterval(*service_request, *service_response); + } + + /*! + * Callback for service "SickDevSetIMUActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbDevSetIMUActive(sick_lidar_localization::SickDevSetIMUActiveSrv::Request& service_request, sick_lidar_localization::SickDevSetIMUActiveSrv::Response& service_response); + /*! ROS2 version of function serviceCbDevSetIMUActive */ + virtual bool serviceCbDevSetIMUActiveROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbDevSetIMUActive(*service_request, *service_response); + } + + /*! + * Callback for service "SickDevIMUActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ + virtual bool serviceCbDevIMUActive(sick_lidar_localization::SickDevIMUActiveSrv::Request& service_request, sick_lidar_localization::SickDevIMUActiveSrv::Response& service_response); + /*! ROS2 version of function serviceCbDevIMUActive */ + virtual bool serviceCbDevIMUActiveROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbDevIMUActive(*service_request, *service_response); + } + protected: /*! @@ -252,23 +839,6 @@ namespace sick_lidar_localization */ virtual sick_lidar_localization::SickLocColaTelegramMsg sendColaTelegram(const std::string & cola_ascii_request); - /*! - * Converts and returns the parameter of a cola ascii telegram into a numeric value. - * @param[in] cola_arg parameter of a cola ascii telegram - * @param[in] base numeric base (10 for decimal values or 16 for hex strings) - * @param[in] default_value default value returned in case of parse errors - * @return parameter converted to integer value - */ - virtual int32_t convertColaArg(const std::string & cola_arg, int base = 10, int32_t default_value = 0); - - /*! - * Converts and returns the parameter of a cola ascii response into a boolean value. - * @param[in] cola_response_arg parameter of a cola ascii response - * @param[in] default_value default value returned in case of parse errors - * @return parameter converted to boolean value - */ - virtual bool convertColaResponseBool(const std::string & cola_response_arg, bool default_value); - /*! * Sends a cola telegram using ros service "SickLocColaTelegram", receives the response from localization controller * and sets service_response.success to the parameter value of the recived response. Specialization of function @@ -289,7 +859,7 @@ namespace sick_lidar_localization sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii.str()); if(cola_response.command_name == cola_command_name && cola_response.parameter.size() == 1) { - service_response.success = convertColaResponseBool(cola_response.parameter[0], false); + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); return true; } ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(\"" << cola_ascii.str() << "\") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); @@ -300,10 +870,61 @@ namespace sick_lidar_localization * member variables */ - double m_cola_response_timeout; ///< Timeout in seconds for cola responses from localization controller, default: 1 - std::vector m_service_server; ///< list of ros service provider for services listed above - ros::ServiceClient m_service_client; ///< client to call ros service "SickLocColaTelegram" to send cola telegrams and receive cola responses from localization controller - + ROS::NodePtr m_nh; ///< ros node + sick_lidar_localization::DriverMonitor* m_driver_monitor; ///< implements the cola telegram services + double m_cola_response_timeout; ///< Timeout in seconds for cola responses from localization controller, default: 1 + boost::mutex m_service_cb_mutex; ///< Mutex to protect sendColaTelegram (one service request at a time) + sick_lidar_localization::SickLocColaTelegramSrvClient m_service_client; ///< ROS client to call service "SickLocColaTelegram" to send cola telegrams and receive cola responses from localization controller + /** list of ros service provider for services listed above (release 3 and later) */ + sick_lidar_localization::SickLocIsSystemReadySrvServer m_srv_server_01; ///< "SickLocIsSystemReady",&sick_lidar_localization::ColaServices::serviceCbIsSystemReady + sick_lidar_localization::SickLocStateSrvServer m_srv_server_02; ///< "SickLocState",&sick_lidar_localization::ColaServices::serviceCbLocState + sick_lidar_localization::SickLocStartLocalizingSrvServer m_srv_server_03; ///< "SickLocStartLocalizing",&sick_lidar_localization::ColaServices::serviceCbLocStartLocalizing + sick_lidar_localization::SickLocStopSrvServer m_srv_server_04; ///< "SickLocStop",&sick_lidar_localization::ColaServices::serviceCbLocStop + sick_lidar_localization::SickLocSetResultPortSrvServer m_srv_server_06; ///< "SickLocSetResultPort",&sick_lidar_localization::ColaServices::serviceCbLocSetResultPort + sick_lidar_localization::SickLocSetResultModeSrvServer m_srv_server_07; ///< "SickLocSetResultMode",&sick_lidar_localization::ColaServices::serviceCbLocSetResultMode + sick_lidar_localization::SickLocSetResultPoseEnabledSrvServer m_srv_server_08; ///< "SickLocSetResultPoseEnabled",&sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseEnabled + sick_lidar_localization::SickLocSetResultEndiannessSrvServer m_srv_server_09; ///< "SickLocSetResultEndianness",&sick_lidar_localization::ColaServices::serviceCbLocSetResultEndianness + sick_lidar_localization::SickLocSetResultPoseIntervalSrvServer m_srv_server_10; ///< "SickLocSetResultPoseInterval",&sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseInterval + sick_lidar_localization::SickLocRequestResultDataSrvServer m_srv_server_11; ///< "SickLocRequestResultData",&sick_lidar_localization::ColaServices::serviceCbLocRequestResultData + sick_lidar_localization::SickLocSetPoseSrvServer m_srv_server_12; ///< "SickLocSetPose",&sick_lidar_localization::ColaServices::serviceCbLocSetPose + /** list of ros service provider for services listed above (release 4 and later) */ + sick_lidar_localization::SickDevSetLidarConfigSrvServer m_SickDevSetLidarConfigSrvServer; ///< service "DevSetLidarConfig", callback &sick_lidar_localization::ColaServices::serviceCbDevSetLidarConfig + sick_lidar_localization::SickDevGetLidarConfigSrvServer m_SickDevGetLidarConfigSrvServer; ///< service "DevGetLidarConfig", callback &sick_lidar_localization::ColaServices::serviceCbDevGetLidarConfig + sick_lidar_localization::SickLocSetMapSrvServer m_SickLocSetMapSrvServer; ///< service "LocSetMap", callback &sick_lidar_localization::ColaServices::serviceCbLocSetMap + sick_lidar_localization::SickLocMapSrvServer m_SickLocMapSrvServer; ///< service "LocMap", callback &sick_lidar_localization::ColaServices::serviceCbLocMap + sick_lidar_localization::SickLocMapStateSrvServer m_SickLocMapStateSrvServer; ///< service "LocMapState", callback &sick_lidar_localization::ColaServices::serviceCbLocMapState + sick_lidar_localization::SickLocInitializePoseSrvServer m_SickLocInitializePoseSrvServer; ///< service "LocInitializePose", callback &sick_lidar_localization::ColaServices::serviceCbLocInitializePose + sick_lidar_localization::SickLocInitialPoseSrvServer m_SickLocInitialPoseSrvServer; ///< service "LocInitialPose", callback &sick_lidar_localization::ColaServices::serviceCbLocInitialPose + sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrvServer m_SickLocSetReflectorsForSupportActiveSrvServer; ///< service "LocSetReflectorsForSupportActive", callback &sick_lidar_localization::ColaServices::serviceCbLocSetReflectorsForSupportActive + sick_lidar_localization::SickLocReflectorsForSupportActiveSrvServer m_SickLocReflectorsForSupportActiveSrvServer; ///< service "LocReflectorsForSupportActive", callback &sick_lidar_localization::ColaServices::serviceCbLocReflectorsForSupportActive + sick_lidar_localization::SickLocSetOdometryActiveSrvServer m_SickLocSetOdometryActiveSrvServer; ///< service "LocSetOdometryActive", callback &sick_lidar_localization::ColaServices::serviceCbLocSetOdometryActive + sick_lidar_localization::SickLocOdometryActiveSrvServer m_SickLocOdometryActiveSrvServer; ///< service "LocOdometryActive", callback &sick_lidar_localization::ColaServices::serviceCbLocOdometryActive + sick_lidar_localization::SickLocSetOdometryPortSrvServer m_SickLocSetOdometryPortSrvServer; ///< service "LocSetOdometryPort", callback &sick_lidar_localization::ColaServices::serviceCbLocSetOdometryPort + sick_lidar_localization::SickLocOdometryPortSrvServer m_SickLocOdometryPortSrvServer; ///< service "LocOdometryPort", callback &sick_lidar_localization::ColaServices::serviceCbLocOdometryPort + sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrvServer m_SickLocSetOdometryRestrictYMotionSrvServer; ///< service "LocSetOdometryRestrictYMotion", callback &sick_lidar_localization::ColaServices::serviceCbLocSetOdometryRestrictYMotion + sick_lidar_localization::SickLocOdometryRestrictYMotionSrvServer m_SickLocOdometryRestrictYMotionSrvServer; ///< service "LocOdometryRestrictYMotion", callback &sick_lidar_localization::ColaServices::serviceCbLocOdometryRestrictYMotion + sick_lidar_localization::SickLocSetAutoStartActiveSrvServer m_SickLocSetAutoStartActiveSrvServer; ///< service "LocSetAutoStartActive", callback &sick_lidar_localization::ColaServices::serviceCbLocSetAutoStartActive + sick_lidar_localization::SickLocAutoStartActiveSrvServer m_SickLocAutoStartActiveSrvServer; ///< service "LocAutoStartActive", callback &sick_lidar_localization::ColaServices::serviceCbLocAutoStartActive + sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrvServer m_SickLocSetAutoStartSavePoseIntervalSrvServer; ///< service "LocSetAutoStartSavePoseInterval", callback &sick_lidar_localization::ColaServices::serviceCbLocSetAutoStartSavePoseInterval + sick_lidar_localization::SickLocAutoStartSavePoseIntervalSrvServer m_SickLocAutoStartSavePoseIntervalSrvServer; ///< service "LocAutoStartSavePoseInterval", callback &sick_lidar_localization::ColaServices::serviceCbLocAutoStartSavePoseInterval + sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrvServer m_SickLocSetRingBufferRecordingActiveSrvServer; ///< service "LocSetRingBufferRecordingActive", callback &sick_lidar_localization::ColaServices::serviceCbLocSetRingBufferRecordingActive + sick_lidar_localization::SickLocRingBufferRecordingActiveSrvServer m_SickLocRingBufferRecordingActiveSrvServer; ///< service "LocRingBufferRecordingActive", callback &sick_lidar_localization::ColaServices::serviceCbLocRingBufferRecordingActive + sick_lidar_localization::SickDevGetLidarIdentSrvServer m_SickDevGetLidarIdentSrvServer; ///< service "DevGetLidarIdent", callback &sick_lidar_localization::ColaServices::serviceCbDevGetLidarIdent + sick_lidar_localization::SickDevGetLidarStateSrvServer m_SickDevGetLidarStateSrvServer; ///< service "DevGetLidarState", callback &sick_lidar_localization::ColaServices::serviceCbDevGetLidarState + sick_lidar_localization::SickGetSoftwareVersionSrvServer m_SickGetSoftwareVersionSrvServer; ///< service "GetSoftwareVersion", callback &sick_lidar_localization::ColaServices::serviceCbGetSoftwareVersion + sick_lidar_localization::SickLocAutoStartSavePoseSrvServer m_SickLocAutoStartSavePoseSrvServer; ///< service "LocAutoStartSavePose", callback &sick_lidar_localization::ColaServices::serviceCbLocAutoStartSavePose + sick_lidar_localization::SickLocForceUpdateSrvServer m_SickLocForceUpdateSrvServer; ///< service "LocForceUpdate", callback &sick_lidar_localization::ColaServices::serviceCbLocForceUpdate + sick_lidar_localization::SickLocSaveRingBufferRecordingSrvServer m_SickLocSaveRingBufferRecordingSrvServer; ///< service "LocSaveRingBufferRecording", callback &sick_lidar_localization::ColaServices::serviceCbLocSaveRingBufferRecording + sick_lidar_localization::SickLocStartDemoMappingSrvServer m_SickLocStartDemoMappingSrvServer; ///< service "LocStartDemoMapping", callback &sick_lidar_localization::ColaServices::serviceCbLocStartDemoMapping + sick_lidar_localization::SickReportUserMessageSrvServer m_SickReportUserMessageSrvServer; ///< service "ReportUserMessage", callback &sick_lidar_localization::ColaServices::serviceCbReportUserMessage + sick_lidar_localization::SickSavePermanentSrvServer m_SickSavePermanentSrvServer; ///< service "SavePermanent", callback &sick_lidar_localization::ColaServices::serviceCbSavePermanent + sick_lidar_localization::SickLocResultPortSrvServer m_SickLocResultPortSrvServer; ///< service "LocResultPort", callback &sick_lidar_localization::ColaServices::serviceCbLocResultPort + sick_lidar_localization::SickLocResultModeSrvServer m_SickLocResultModeSrvServer; ///< service "LocResultMode", callback &sick_lidar_localization::ColaServices::serviceCbLocResultMode + sick_lidar_localization::SickLocResultEndiannessSrvServer m_SickLocResultEndiannessSrvServer; ///< service "LocResultEndianness", callback &sick_lidar_localization::ColaServices::serviceCbLocResultEndianness + sick_lidar_localization::SickLocResultStateSrvServer m_SickLocResultStateSrvServer; ///< service "LocResultState", callback &sick_lidar_localization::ColaServices::serviceCbLocResultState + sick_lidar_localization::SickLocResultPoseIntervalSrvServer m_SickLocResultPoseIntervalSrvServer; ///< service "LocResultPoseInterval", callback &sick_lidar_localization::ColaServices::serviceCbLocResultPoseInterval + sick_lidar_localization::SickDevSetIMUActiveSrvServer m_SickDevSetIMUActiveSrvServer; ///< service "DevSetIMUActive", callback &sick_lidar_localization::ColaServices::serviceCbDevSetIMUActive + sick_lidar_localization::SickDevIMUActiveSrvServer m_SickDevIMUActiveSrvServer; ///< service "DevIMUActive", callback &sick_lidar_localization::ColaServices::serviceCbSetIMUActive }; // class ColaServices } // namespace sick_lidar_localization diff --git a/include/sick_lidar_localization/cola_transmitter.h b/include/sick_lidar_localization/cola_transmitter.h index 8633405..b231759 100644 --- a/include/sick_lidar_localization/cola_transmitter.h +++ b/include/sick_lidar_localization/cola_transmitter.h @@ -100,7 +100,7 @@ namespace sick_lidar_localization * @param[out] send_timestamp send timestamp in seconds (ros timestamp immediately before tcp send) * @return true on success, false on failure */ - virtual bool send(const std::vector & data, ros::Time & send_timestamp); + virtual bool send(const std::vector & data, ROS::Time & send_timestamp); /*! * Send data to the localization server. @@ -109,7 +109,7 @@ namespace sick_lidar_localization * @param[out] send_timestamp send timestamp in seconds (ros timestamp immediately before tcp send) * @return true on success, false on failure */ - static bool send(boost::asio::ip::tcp::socket & socket, const std::vector & data, ros::Time & send_timestamp); + static bool send(boost::asio::ip::tcp::socket & socket, const std::vector & data, ROS::Time & send_timestamp); /*! * Receive a cola telegram from the localization server. @@ -118,7 +118,7 @@ namespace sick_lidar_localization * @param[out] receive_timestamp receive timestamp in seconds (ros timestamp immediately after first response byte received) * @return true on success, false on failure (connection error or timeout) */ - virtual bool receive(std::vector & telegram, double timeout, ros::Time & receive_timestamp); + virtual bool receive(std::vector & telegram, double timeout, ROS::Time & receive_timestamp); /*! * Receive a cola telegram from a socket. @@ -128,7 +128,7 @@ namespace sick_lidar_localization * @param[out] receive_timestamp receive timestamp in seconds (ros timestamp immediately after first response byte received) * @return true on success, false on failure (connection error or timeout) */ - static bool receive(boost::asio::ip::tcp::socket & socket, std::vector & telegram, double timeout,ros::Time & receive_timestamp); + static bool receive(boost::asio::ip::tcp::socket & socket, std::vector & telegram, double timeout,ROS::Time & receive_timestamp); /*! * Starts a thread to receive response telegrams from the localization server. @@ -153,7 +153,7 @@ namespace sick_lidar_localization * @param[out] receive_timestamp receive timestamp in seconds (ros timestamp immediately after first response byte received) * @return true on success, false on failure (connection error or timeout) */ - virtual bool waitPopResponse(std::vector & telegram, double timeout, ros::Time & receive_timestamp); + virtual bool waitPopResponse(std::vector & telegram, double timeout, ROS::Time & receive_timestamp); protected: @@ -164,10 +164,10 @@ namespace sick_lidar_localization class ColaResponseContainer { public: - ColaResponseContainer(const std::vector & data = std::vector(),const ros::Time & timestamp = ros::Time::now()) + ColaResponseContainer(const std::vector & data = std::vector(),const ROS::Time & timestamp = ROS::now()) : telegram_data(data), receive_timestamp(timestamp) {} ///< Constructor std::vector telegram_data; ///< received telegram_data (Cola-Ascii or Cola-Binary) - ros::Time receive_timestamp; ///< receive timestamp in seconds (ros timestamp immediately after first response byte received) + ROS::Time receive_timestamp; ///< receive timestamp in seconds (ros timestamp immediately after first response byte received) }; /*! diff --git a/include/sick_lidar_localization/driver_check_thread.h b/include/sick_lidar_localization/driver_check_thread.h index 983ee83..e1e2a2b 100644 --- a/include/sick_lidar_localization/driver_check_thread.h +++ b/include/sick_lidar_localization/driver_check_thread.h @@ -65,10 +65,7 @@ #include #include -#include "sick_lidar_localization/SickLocResultPortHeaderMsg.h" -#include "sick_lidar_localization/SickLocResultPortPayloadMsg.h" -#include "sick_lidar_localization/SickLocResultPortCrcMsg.h" -#include "sick_lidar_localization/SickLocResultPortTelegramMsg.h" +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/fifo_buffer.h" namespace sick_lidar_localization @@ -85,8 +82,9 @@ namespace sick_lidar_localization /*! * Constructor, reads the configuration parameter incl. the * min and max allowed values in sim_loc_driver messages. + * @param[in] nh ros node handle */ - MessageCheckThread(); + MessageCheckThread(ROS::NodePtr nh = 0); /*! * Destructor @@ -110,6 +108,8 @@ namespace sick_lidar_localization * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) */ virtual void messageCbResultPortTelegrams(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg); + /*! ROS2 version of function messageCbResultPortTelegrams */ + virtual void messageCbResultPortTelegramsROS2(const std::shared_ptr msg) { messageCbResultPortTelegrams(*msg); } protected: @@ -121,10 +121,11 @@ namespace sick_lidar_localization /*! * Reads and returns a result port telegram from yaml configuration file. + * @param[in] nh ros node handle * @param[in] param_section section name in yaml configuration file, f.e. "/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values" * @return result port telegram with values initialized from yaml file. */ - sick_lidar_localization::SickLocResultPortTelegramMsg readYamlResultPortTelegram(const std::string param_section); + sick_lidar_localization::SickLocResultPortTelegramMsg readYamlResultPortTelegram(ROS::NodePtr nh, const std::string param_section); /*! * Checks a result telegram messages (SickLocResultPortTelegramMsg) against min and max values. @@ -153,7 +154,7 @@ namespace sick_lidar_localization double m_vehicle_time_delta_max; ///< max. allowed time diff in seconds between vehicle time (system time from ticks by software pll) and ros::Time::now() bool m_vehicle_time_check_enabled; ///< true: check of vehicle time is enabled (default), false in case of simulated network errors (LocRequestTimestamp not available) double m_software_pll_expected_initialization_duration; ///< expected initialization time for software pll (system time from lidar ticks not yet available) - ros::Time m_timestamp_valid_telegram; ///< timestamp of a telegram with valid vehicle time + ROS::Time m_timestamp_valid_telegram; ///< timestamp of a telegram with valid vehicle time bool m_message_check_thread_running; ///< true: m_message_check_thread is running, otherwise false boost::thread* m_message_check_thread; ///< thread to check sim_loc_driver messages double m_message_check_frequency; ///< frequency to check sim_loc_driver messages (default: 100) diff --git a/include/sick_lidar_localization/driver_monitor.h b/include/sick_lidar_localization/driver_monitor.h index 3f19352..8a96854 100644 --- a/include/sick_lidar_localization/driver_monitor.h +++ b/include/sick_lidar_localization/driver_monitor.h @@ -86,7 +86,7 @@ namespace sick_lidar_localization * @param[in] ip_port_results ip port for result telegrams, default: 2201 * @param[in] ip_port_cola ip port for command requests and responses, default: 2111 */ - DriverMonitor(ros::NodeHandle * nh = 0, const std::string & server_adress = "192.168.0.1", int ip_port_results = 2201, int ip_port_cola = 2111); + DriverMonitor(ROS::NodePtr nh = 0, const std::string & server_adress = "192.168.0.1", int ip_port_results = 2201, int ip_port_cola = 2111); /*! * Destructor. Stops the driver thread and monitor and closes all tcp connections. @@ -110,7 +110,9 @@ namespace sick_lidar_localization * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) */ virtual void messageCbResultPortTelegrams(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg); - + /*! ROS2 version of function messageCbResultPortTelegrams */ + virtual void messageCbResultPortTelegramsROS2(const std::shared_ptr msg) { messageCbResultPortTelegrams(*msg); } + /*! * Callback for service messages (SickLocColaTelegramSrv). Handles Cola requests and responses, sends and receives Cola-ASCII telegrams to resp. from * the localization controller @@ -118,6 +120,11 @@ namespace sick_lidar_localization * @param[out] cola_response Cola command response from localization controller */ virtual bool serviceCbColaTelegram(sick_lidar_localization::SickLocColaTelegramSrv::Request & cola_request, sick_lidar_localization::SickLocColaTelegramSrv::Response & cola_response); + /*! ROS2 version of function serviceCbColaTelegram */ + virtual bool serviceCbColaTelegramROS2(std::shared_ptr cola_request, std::shared_ptr cola_response) + { + return serviceCbColaTelegram(*cola_request, *cola_response); + } protected: @@ -153,14 +160,14 @@ namespace sick_lidar_localization */ bool m_initialized; ///< true after successfull configuration, otherwise false - ros::NodeHandle* m_nh; ///< ros node handle + ROS::NodePtr m_nh; ///< ros node handle std::string m_server_adress; ///< ip adress of the localization controller, default: 192.168.0.1 int m_ip_port_results; ///< ip port for result telegrams, default: The localization controller uses IP port number 2201 to send localization results int m_ip_port_cola; ///< ip port for for command requests and responses, default: The localization controller uses IP port number 2111 and 2112 to send telegrams and to request data bool m_cola_binary; ///< false: send Cola-ASCII (default), true: send Cola-Binary bool m_monitoring_thread_running; ///< true: m_monitoring_thread is running, otherwise false boost::thread* m_monitoring_thread; ///< thread to monitor tcp-connection, restarts DriverThread in case of tcp-errors. - sick_lidar_localization::SetGet m_driver_message_recv_timestamp; ///< time of the last received driver message (threadsafe) + sick_lidar_localization::SetGet m_driver_message_recv_timestamp; ///< time of the last received driver message (threadsafe) double m_monitoring_rate; ///< frequency to monitor driver messages, default: once per second double m_receive_telegrams_timeout; ///< timeout for driver messages, shutdown tcp-sockets and reconnect after message timeout, default: 1 second double m_cola_response_timeout; ///< timeout in seconds for cola responses from localization controller, default: 1 diff --git a/include/sick_lidar_localization/driver_thread.h b/include/sick_lidar_localization/driver_thread.h index a9a60b0..e6ef39f 100644 --- a/include/sick_lidar_localization/driver_thread.h +++ b/include/sick_lidar_localization/driver_thread.h @@ -90,7 +90,7 @@ namespace sick_lidar_localization * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 * @param[in] tcp_port tcp port of the localization controller, default: The localization controller uses IP port number 2201 to send localization results */ - DriverThread(ros::NodeHandle * nh = 0, const std::string & server_adress = "192.168.0.1", int tcp_port = 2201); + DriverThread(ROS::NodePtr nh = 0, const std::string & server_adress = "192.168.0.1", int tcp_port = 2201); /*! * Destructor. Stops the driver thread and closes all tcp connections. @@ -170,9 +170,9 @@ namespace sick_lidar_localization boost::thread* m_converter_thread; ///< thread to convert and publish localization data bool m_converter_thread_running; ///< true: m_converter_thread is running, otherwise false sick_lidar_localization::FifoBuffer, boost::mutex> m_fifo_buffer; ///< fifo buffer to transfer data from receiver thread to converter thread - ros::Publisher m_result_telegrams_publisher; ///< ros publisher for result port telegram messages (type SickLocResultPortTelegramMsg) + sick_lidar_localization::SickLocResultPortTelegramMsgPublisher m_result_telegrams_publisher; ///< ros publisher for result port telegram messages (type SickLocResultPortTelegramMsg) std::string m_result_telegrams_frame_id; ///< ros frame id of result port telegram messages (type SickLocResultPortTelegramMsg), default: "sick_lidar_localization" - ros::ServiceClient m_timesync_service_client; ///< client to call ros service "SickLocTimeSync" to calculate system time from ticks by software pll + sick_lidar_localization::SickLocTimeSyncSrvClient m_timesync_service_client; ///< client to call ros service "SickLocTimeSync" to calculate system time from ticks by software pll double m_software_pll_expected_initialization_duration; ///< expected initialization time for software pll (system time from lidar ticks not yet available) /* @@ -194,8 +194,8 @@ namespace sick_lidar_localization */ void publishDiagnosticMessage(const DRIVER_ERROR_CODES & error_code, const std::string & message); - ros::Publisher m_diagnostic_publisher; ///< ros publisher for diagnostic messages (type SickLocDiagnosticMsg) - std::string m_diagnostic_frame_id; ///< ros frame id of diagnostic messages (type SickLocDiagnosticMsg), default: "sick_lidar_localization" + sick_lidar_localization::SickLocDiagnosticMsgPublisher m_diagnostic_publisher; ///< ros publisher for diagnostic messages (type SickLocDiagnosticMsg) + std::string m_diagnostic_frame_id; ///< ros frame id of diagnostic messages (type SickLocDiagnosticMsg), default: "sick_lidar_localization" }; // class DriverThread diff --git a/include/sick_lidar_localization/fifo_buffer.h b/include/sick_lidar_localization/fifo_buffer.h index 29504b8..679b3b5 100644 --- a/include/sick_lidar_localization/fifo_buffer.h +++ b/include/sick_lidar_localization/fifo_buffer.h @@ -128,7 +128,7 @@ namespace sick_lidar_localization */ void waitForElement() { - while (ros::ok() && empty()) + while (ROS::ok() && empty()) { waitForNotify(); } @@ -139,7 +139,7 @@ namespace sick_lidar_localization */ void waitOnceForElement() { - if (ros::ok() && empty()) + if (ROS::ok() && empty()) { waitForNotify(); } diff --git a/include/sick_lidar_localization/odom_converter.h b/include/sick_lidar_localization/odom_converter.h new file mode 100755 index 0000000..11a3735 --- /dev/null +++ b/include/sick_lidar_localization/odom_converter.h @@ -0,0 +1,180 @@ +/* + * @brief odom_converter subscribes to odometry messages, converts (vx, vy, omega) to binary odometry telegrams + * and sends the telegrams to the localization server by udp. + * See Operating-Instructions-EN-LLS-1.2.0.300R.pdf, chapter 5.9 "About Odometry for support", page 44-48 + * and Odometry_LLS.py for further details. + * + * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2020 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2020 SICK AG + * Copyright 2020 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SIM_LOC_ODOM_CONVERTER_H_INCLUDED +#define __SIM_LOC_ODOM_CONVERTER_H_INCLUDED + +#include +#include +#include + +#include "sick_lidar_localization/ros_wrapper.h" +#include "sick_lidar_localization/utils.h" + +namespace sick_lidar_localization +{ + class UdpSenderSocketImpl; ///< forward declaration of udp sender implementation + + /*! + * @brief class OdomConverter subscribes to odometry messages, converts (timestamp, vx, vy, omega) to binary odometry telegrams + * and sends the telegrams to the localization server by udp. + * See Operating-Instructions-EN-LLS-1.2.0.300R.pdf, chapter 5.9 "About Odometry for support", page 44-48 + * and Odometry_LLS.py for further details. + */ + class OdomConverter + { + public: + + /*! + * Constructor. + * @param[in] nh ros node handle + * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 + * @param[in] udp_port_odom udp port for odometry telegrams, default: 3000 + * @param[in] run_unittest run a short unittest, default: true + */ + OdomConverter(ROS::NodePtr nh = 0, const std::string & server_adress = "192.168.0.1", int udp_port_odom = 3000, bool run_unittest = true); + + /*! + * Callback for ros odometry messages. Converts the message to a binary odom telegram and sends the telegram to the localization server by udp. + * @param[in] msg odometry message + */ + virtual void messageCbOdometry(const sick_lidar_localization::OdomMsg & msg); + /*! ROS2 version of function messageCbOdometry */ + virtual void messageCbOdometryROS2(const std::shared_ptr msg) { messageCbOdometry(*msg); } + + /*! + * @brief Runs a unit test by converting some odometry messages and checking the result. + * @param[in] verbose true: print info messages, false: print error messages only + * @return unit test passed (true) or failed (false) + */ + static bool Unittest(bool verbose = false); + + /*! + * @brief Converts (timestamp, vx, vy, omega) to binary odometry telegrams. + * Telegram format: + * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + * | H E A D E R | P A Y L O A D | + * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + * | MagicWord PayloadLength payloadType(endianess) MsgType MsgTypeVersion SourceID | telegramCounter timestamp Vx Vy Omega | + * | 534d4f50 000C 0642 0000 0000 0000 | 00000001 00000000 0001 FFFF 00000001 | + * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + * Note: The header always has Big Endian format + * @param[in] bigendian send payload big endian (true) or little endian (false) + * @param[in] SourceID: 1 (e.g. vehicle controller 1) + * @param[in] TelegramCount telegram counter + * @param[in] Timestamp timestamp in ms + * @param[in] vx x-component of velocity in mm/s + * @param[in] vy y-component of velocity in mm/s + * @param[in] omega angular velocity in mdeg/s + * @return binary odometry telegram + */ + static std::vector Encode(bool bigendian = true, uint16_t SourceID = 1, uint32_t TelegramCount = 0, uint32_t Timestamp = 0, int16_t vx = 0, int16_t vy = 0, int32_t omega = 0); + + protected: + + /*! + * Converts and returns a byte vector to hex string + */ + static std::string ToHexString(const std::vector & binary_telegram); + + /*! + * @brief Runs a single test by converting a odometry messages and checking the result. + * @param[in] bigendian send payload big endian (true) or little endian (false) + * @param[in] SourceID: 1 (e.g. vehicle controller 1) + * @param[in] TelegramCount telegram counter + * @param[in] Timestamp timestamp in ms + * @param[in] vx x-component of velocity in mm/s + * @param[in] vy y-component of velocity in mm/s + * @param[in] omega angular velocity in mdeg/s + * @param[in] hex_result expected result (binary odometry telegram as hex string) + * @param[in] verbose true: print info messages, false: print error messages only + * @return unit test passed (true) or failed (false) + */ + static bool Unittest(bool bigendian, uint16_t SourceID, uint32_t TelegramCount, uint32_t Timestamp, int16_t vx, int16_t vy, int32_t omega, const std::string & hex_result, bool verbose = false); + + /*! + * @brief Converts and pushes 2 byte to binary odometry telegram. + * @param[out] binary_telegram byte vetor to append 2 bytes + * @param[in] value: bytes to encode in big or little endian order + * @param[in] bigendian send payload big endian (true) or little endian (false) + */ + static void EncodeAndPush(std::vector & binary_telegram, uint16_t value, bool bigendian); + + /*! + * @brief Converts and pushes 4 byte to binary odometry telegram. + * @param[out] binary_telegram byte vetor to append 4 bytes + * @param[in] value: bytes to encode in big or little endian order + * @param[in] bigendian send payload big endian (true) or little endian (false) + */ + static void EncodeAndPush(std::vector & binary_telegram, uint32_t value, bool bigendian); + + /* + * member data + */ + + ROS::NodePtr m_nh; ///< ros node handle + std::string m_server_adress; ///< ip adress of the localization controller, default: 192.168.0.1 + int m_udp_port_odom; ///< udp port for odometry telegrams, default: 3000 + int m_odom_telegrams_bigendian; ///< Send udp odometry telegrams big endian (1) or little endian (0) + int m_odom_telegrams_source_id; ///< SourceID of udp odometry telegrams, e.g. vehicle controller 1 + sick_lidar_localization::UdpSenderSocketImpl* m_udp_sender_impl; ///< udp sender for odometry telegrams + static sick_lidar_localization::SetGet32 s_odom_telegram_count; ///< static odometry telegram counter + + }; // class OdomConverter + +} // namespace sick_lidar_localization +#endif // __SIM_LOC_ODOM_CONVERTER_H_INCLUDED diff --git a/include/sick_lidar_localization/pointcloud_converter.h b/include/sick_lidar_localization/pointcloud_converter.h index 0e66bf1..f48f6f8 100644 --- a/include/sick_lidar_localization/pointcloud_converter.h +++ b/include/sick_lidar_localization/pointcloud_converter.h @@ -65,13 +65,7 @@ #include #include -#include -#include -#include -#include -#include -#include - +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/fifo_buffer.h" #include "sick_lidar_localization/result_port_parser.h" @@ -90,7 +84,7 @@ namespace sick_lidar_localization * Constructor * @param[in] nh ros node handle */ - PointCloudConverter(ros::NodeHandle* nh = 0); + PointCloudConverter(ROS::NodePtr nh = 0); /*! * Destructor @@ -116,6 +110,8 @@ namespace sick_lidar_localization * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) */ virtual void messageCbResultPortTelegrams(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg); + /*! ROS2 version of function messageCbResultPortTelegrams */ + virtual void messageCbResultPortTelegramsROS2(const std::shared_ptr msg) { messageCbResultPortTelegrams(*msg); } protected: @@ -157,11 +153,12 @@ namespace sick_lidar_localization * member data */ + ROS::NodePtr m_nh; ///< ROS node handle sick_lidar_localization::FifoBuffer m_result_port_telegram_fifo; ///< fifo buffer for result port telegrams from sim_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 - ros::Publisher m_point_cloud_publisher; ///< ros publisher for PointCloud2 messages + sick_lidar_localization::PointCloud2MsgPublisher m_point_cloud_publisher; ///< ros publisher for PointCloud2 messages bool m_converter_thread_running; ///< true: m_verification_thread is running, otherwise false boost::thread* m_converter_thread; ///< thread to verify sim_loc_driver diff --git a/include/sick_lidar_localization/result_port_parser.h b/include/sick_lidar_localization/result_port_parser.h index e9205e7..0446ffb 100644 --- a/include/sick_lidar_localization/result_port_parser.h +++ b/include/sick_lidar_localization/result_port_parser.h @@ -56,10 +56,7 @@ #ifndef __SIM_LOC_RESULT_PORT_PARSER_H_INCLUDED #define __SIM_LOC_RESULT_PORT_PARSER_H_INCLUDED -#include "sick_lidar_localization/SickLocResultPortHeaderMsg.h" -#include "sick_lidar_localization/SickLocResultPortPayloadMsg.h" -#include "sick_lidar_localization/SickLocResultPortCrcMsg.h" -#include "sick_lidar_localization/SickLocResultPortTelegramMsg.h" +#include "sick_lidar_localization/ros_wrapper.h" namespace sick_lidar_localization { diff --git a/include/sick_lidar_localization/ros_wrapper.h b/include/sick_lidar_localization/ros_wrapper.h new file mode 100755 index 0000000..44fcf3d --- /dev/null +++ b/include/sick_lidar_localization/ros_wrapper.h @@ -0,0 +1,612 @@ +/* + * @brief ros_wrapper encapsulates the ROS API and switches ROS specific implementation + * between ROS 1 and ROS 2. + * + * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2020 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2020 SICK AG + * Copyright 2020 Ing.-Buero Dr. Michael Lehning + * + */ +#ifndef __SIM_LOC_ROS_WRAPPER_H_INCLUDED +#define __SIM_LOC_ROS_WRAPPER_H_INCLUDED + +#include +#include +#include +#include + +#if defined __ROS_VERSION && __ROS_VERSION == 1 +/* + * Support for ROS 1 API + */ +#include +#include +#include +#include +#include +#include +#include +#include + +#define RCLCPP_DEBUG_STREAM(logger,msgstream) ROS_DEBUG_STREAM(msgstream) +#define RCLCPP_INFO_STREAM(logger,msgstream) ROS_INFO_STREAM(msgstream) +#define RCLCPP_WARN_STREAM(logger,msgstream) ROS_WARN_STREAM(msgstream) +#define RCLCPP_ERROR_STREAM(logger,msgstream) ROS_ERROR_STREAM(msgstream) + +// Message header +#include "sick_lidar_localization/SickLocColaTelegramMsg.h" +#include "sick_lidar_localization/SickLocDiagnosticMsg.h" +#include "sick_lidar_localization/SickLocResultPortCrcMsg.h" +#include "sick_lidar_localization/SickLocResultPortHeaderMsg.h" +#include "sick_lidar_localization/SickLocResultPortPayloadMsg.h" +#include "sick_lidar_localization/SickLocResultPortTelegramMsg.h" +#include "sick_lidar_localization/SickLocResultPortTestcaseMsg.h" +// Services supported in release 3 and later +#include "sick_lidar_localization/SickLocColaTelegramSrv.h" +#include "sick_lidar_localization/SickLocIsSystemReadySrv.h" +#include "sick_lidar_localization/SickLocRequestResultDataSrv.h" +#include "sick_lidar_localization/SickLocRequestTimestampSrv.h" +#include "sick_lidar_localization/SickLocSetPoseSrv.h" +#include "sick_lidar_localization/SickLocSetResultEndiannessSrv.h" +#include "sick_lidar_localization/SickLocSetResultModeSrv.h" +#include "sick_lidar_localization/SickLocSetResultPortSrv.h" +#include "sick_lidar_localization/SickLocSetResultPoseEnabledSrv.h" +#include "sick_lidar_localization/SickLocSetResultPoseIntervalSrv.h" +#include "sick_lidar_localization/SickLocStartLocalizingSrv.h" +#include "sick_lidar_localization/SickLocStateSrv.h" +#include "sick_lidar_localization/SickLocStopSrv.h" +#include "sick_lidar_localization/SickLocTimeSyncSrv.h" +// Services supported in release 4 and later +#include "sick_lidar_localization/SickDevGetLidarConfigSrv.h" +#include "sick_lidar_localization/SickDevGetLidarIdentSrv.h" +#include "sick_lidar_localization/SickDevGetLidarStateSrv.h" +#include "sick_lidar_localization/SickDevSetLidarConfigSrv.h" +#include "sick_lidar_localization/SickGetSoftwareVersionSrv.h" +#include "sick_lidar_localization/SickLocAutoStartActiveSrv.h" +#include "sick_lidar_localization/SickLocAutoStartSavePoseIntervalSrv.h" +#include "sick_lidar_localization/SickLocAutoStartSavePoseSrv.h" +#include "sick_lidar_localization/SickLocForceUpdateSrv.h" +#include "sick_lidar_localization/SickLocInitializePoseSrv.h" +#include "sick_lidar_localization/SickLocInitialPoseSrv.h" +#include "sick_lidar_localization/SickLocMapSrv.h" +#include "sick_lidar_localization/SickLocMapStateSrv.h" +#include "sick_lidar_localization/SickLocOdometryActiveSrv.h" +#include "sick_lidar_localization/SickLocOdometryPortSrv.h" +#include "sick_lidar_localization/SickLocOdometryRestrictYMotionSrv.h" +#include "sick_lidar_localization/SickLocReflectorsForSupportActiveSrv.h" +#include "sick_lidar_localization/SickLocResultEndiannessSrv.h" +#include "sick_lidar_localization/SickLocResultModeSrv.h" +#include "sick_lidar_localization/SickLocResultPortSrv.h" +#include "sick_lidar_localization/SickLocResultPoseIntervalSrv.h" +#include "sick_lidar_localization/SickLocResultStateSrv.h" +#include "sick_lidar_localization/SickLocRingBufferRecordingActiveSrv.h" +#include "sick_lidar_localization/SickLocSaveRingBufferRecordingSrv.h" +#include "sick_lidar_localization/SickLocSetAutoStartActiveSrv.h" +#include "sick_lidar_localization/SickLocSetAutoStartSavePoseIntervalSrv.h" +#include "sick_lidar_localization/SickLocSetMapSrv.h" +#include "sick_lidar_localization/SickLocSetOdometryActiveSrv.h" +#include "sick_lidar_localization/SickLocSetOdometryPortSrv.h" +#include "sick_lidar_localization/SickLocSetOdometryRestrictYMotionSrv.h" +#include "sick_lidar_localization/SickLocSetReflectorsForSupportActiveSrv.h" +#include "sick_lidar_localization/SickLocSetRingBufferRecordingActiveSrv.h" +#include "sick_lidar_localization/SickLocStartDemoMappingSrv.h" +#include "sick_lidar_localization/SickReportUserMessageSrv.h" +#include "sick_lidar_localization/SickSavePermanentSrv.h" +#include "sick_lidar_localization/SickDevSetIMUActiveSrv.h" +#include "sick_lidar_localization/SickDevIMUActiveSrv.h" + +namespace sick_lidar_localization +{ + typedef sensor_msgs::PointCloud2 PointCloud2Msg; + typedef ros::Publisher PointCloud2MsgPublisher; + + typedef nav_msgs::Odometry OdomMsg; + typedef ros::Subscriber OdomMsgSubscriber; + + typedef ros::Publisher SickLocResultPortTelegramMsgPublisher; + typedef ros::Publisher SickLocDiagnosticMsgPublisher; + typedef ros::Publisher SickLocResultPortTestcaseMsgPublisher; + + typedef ros::Subscriber SickLocResultPortTelegramMsgSubscriber; + typedef ros::Subscriber SickLocResultPortTestcaseMsgSubscriber; + + // Service types supported in release 3 and later + typedef ros::ServiceClient SickLocColaTelegramSrvClient; + typedef ros::ServiceServer SickLocColaTelegramSrvServer; + typedef ros::ServiceClient SickLocIsSystemReadySrvClient; + typedef ros::ServiceServer SickLocIsSystemReadySrvServer; + typedef ros::ServiceClient SickLocRequestResultDataSrvClient; + typedef ros::ServiceServer SickLocRequestResultDataSrvServer; + typedef ros::ServiceClient SickLocRequestTimestampSrvClient; + typedef ros::ServiceServer SickLocRequestTimestampSrvServer; + typedef ros::ServiceClient SickLocSetPoseSrvClient; + typedef ros::ServiceServer SickLocSetPoseSrvServer; + typedef ros::ServiceClient SickLocSetResultEndiannessSrvClient; + typedef ros::ServiceServer SickLocSetResultEndiannessSrvServer; + typedef ros::ServiceClient SickLocSetResultModeSrvClient; + typedef ros::ServiceServer SickLocSetResultModeSrvServer; + typedef ros::ServiceClient SickLocSetResultPortSrvClient; + typedef ros::ServiceServer SickLocSetResultPortSrvServer; + typedef ros::ServiceClient SickLocSetResultPoseEnabledSrvClient; + typedef ros::ServiceServer SickLocSetResultPoseEnabledSrvServer; + typedef ros::ServiceClient SickLocSetResultPoseIntervalSrvClient; + typedef ros::ServiceServer SickLocSetResultPoseIntervalSrvServer; + typedef ros::ServiceClient SickLocStartLocalizingSrvClient; + typedef ros::ServiceServer SickLocStartLocalizingSrvServer; + typedef ros::ServiceClient SickLocStateSrvClient; + typedef ros::ServiceServer SickLocStateSrvServer; + typedef ros::ServiceClient SickLocStopSrvClient; + typedef ros::ServiceServer SickLocStopSrvServer; + typedef ros::ServiceClient SickLocTimeSyncSrvClient; + typedef ros::ServiceServer SickLocTimeSyncSrvServer; + // Service types supported in release 4 and later + typedef ros::ServiceClient SickDevGetLidarConfigSrvClient; + typedef ros::ServiceServer SickDevGetLidarConfigSrvServer; + typedef ros::ServiceClient SickDevGetLidarIdentSrvClient; + typedef ros::ServiceServer SickDevGetLidarIdentSrvServer; + typedef ros::ServiceClient SickDevGetLidarStateSrvClient; + typedef ros::ServiceServer SickDevGetLidarStateSrvServer; + typedef ros::ServiceClient SickDevSetLidarConfigSrvClient; + typedef ros::ServiceServer SickDevSetLidarConfigSrvServer; + typedef ros::ServiceClient SickGetSoftwareVersionSrvClient; + typedef ros::ServiceServer SickGetSoftwareVersionSrvServer; + typedef ros::ServiceClient SickLocAutoStartActiveSrvClient; + typedef ros::ServiceServer SickLocAutoStartActiveSrvServer; + typedef ros::ServiceClient SickLocAutoStartSavePoseIntervalSrvClient; + typedef ros::ServiceServer SickLocAutoStartSavePoseIntervalSrvServer; + typedef ros::ServiceClient SickLocAutoStartSavePoseSrvClient; + typedef ros::ServiceServer SickLocAutoStartSavePoseSrvServer; + typedef ros::ServiceClient SickLocForceUpdateSrvClient; + typedef ros::ServiceServer SickLocForceUpdateSrvServer; + typedef ros::ServiceClient SickLocInitializePoseSrvClient; + typedef ros::ServiceServer SickLocInitializePoseSrvServer; + typedef ros::ServiceClient SickLocInitialPoseSrvClient; + typedef ros::ServiceServer SickLocInitialPoseSrvServer; + typedef ros::ServiceClient SickLocMapSrvClient; + typedef ros::ServiceServer SickLocMapSrvServer; + typedef ros::ServiceClient SickLocMapStateSrvClient; + typedef ros::ServiceServer SickLocMapStateSrvServer; + typedef ros::ServiceClient SickLocOdometryActiveSrvClient; + typedef ros::ServiceServer SickLocOdometryActiveSrvServer; + typedef ros::ServiceClient SickLocOdometryPortSrvClient; + typedef ros::ServiceServer SickLocOdometryPortSrvServer; + typedef ros::ServiceClient SickLocOdometryRestrictYMotionSrvClient; + typedef ros::ServiceServer SickLocOdometryRestrictYMotionSrvServer; + typedef ros::ServiceClient SickLocReflectorsForSupportActiveSrvClient; + typedef ros::ServiceServer SickLocReflectorsForSupportActiveSrvServer; + typedef ros::ServiceClient SickLocResultEndiannessSrvClient; + typedef ros::ServiceServer SickLocResultEndiannessSrvServer; + typedef ros::ServiceClient SickLocResultModeSrvClient; + typedef ros::ServiceServer SickLocResultModeSrvServer; + typedef ros::ServiceClient SickLocResultPortSrvClient; + typedef ros::ServiceServer SickLocResultPortSrvServer; + typedef ros::ServiceClient SickLocResultPoseIntervalSrvClient; + typedef ros::ServiceServer SickLocResultPoseIntervalSrvServer; + typedef ros::ServiceClient SickLocResultStateSrvClient; + typedef ros::ServiceServer SickLocResultStateSrvServer; + typedef ros::ServiceClient SickLocRingBufferRecordingActiveSrvClient; + typedef ros::ServiceServer SickLocRingBufferRecordingActiveSrvServer; + typedef ros::ServiceClient SickLocSaveRingBufferRecordingSrvClient; + typedef ros::ServiceServer SickLocSaveRingBufferRecordingSrvServer; + typedef ros::ServiceClient SickLocSetAutoStartActiveSrvClient; + typedef ros::ServiceServer SickLocSetAutoStartActiveSrvServer; + typedef ros::ServiceClient SickLocSetAutoStartSavePoseIntervalSrvClient; + typedef ros::ServiceServer SickLocSetAutoStartSavePoseIntervalSrvServer; + typedef ros::ServiceClient SickLocSetMapSrvClient; + typedef ros::ServiceServer SickLocSetMapSrvServer; + typedef ros::ServiceClient SickLocSetOdometryActiveSrvClient; + typedef ros::ServiceServer SickLocSetOdometryActiveSrvServer; + typedef ros::ServiceClient SickLocSetOdometryPortSrvClient; + typedef ros::ServiceServer SickLocSetOdometryPortSrvServer; + typedef ros::ServiceClient SickLocSetOdometryRestrictYMotionSrvClient; + typedef ros::ServiceServer SickLocSetOdometryRestrictYMotionSrvServer; + typedef ros::ServiceClient SickLocSetReflectorsForSupportActiveSrvClient; + typedef ros::ServiceServer SickLocSetReflectorsForSupportActiveSrvServer; + typedef ros::ServiceClient SickLocSetRingBufferRecordingActiveSrvClient; + typedef ros::ServiceServer SickLocSetRingBufferRecordingActiveSrvServer; + typedef ros::ServiceClient SickLocStartDemoMappingSrvClient; + typedef ros::ServiceServer SickLocStartDemoMappingSrvServer; + typedef ros::ServiceClient SickReportUserMessageSrvClient; + typedef ros::ServiceServer SickReportUserMessageSrvServer; + typedef ros::ServiceClient SickSavePermanentSrvClient; + typedef ros::ServiceServer SickSavePermanentSrvServer; + typedef ros::ServiceClient SickDevSetIMUActiveSrvClient; + typedef ros::ServiceServer SickDevSetIMUActiveSrvServer; + typedef ros::ServiceClient SickDevIMUActiveSrvClient; + typedef ros::ServiceServer SickDevIMUActiveSrvServer; + +} // namespace sick_lidar_localization + +namespace ROS +{ + using namespace ros; // map namespace ROS to ros in ROS1 and to rclcpp in ROS2, f.e. ROS::ok() to ROS::ok() in ROS1 and rclcpp::ok() in ROS2 + typedef ros::NodeHandle* NodePtr; + + template bool param(ROS::NodePtr & node, const std::string & param_name, T& value, const T& default_value) + { + return ros::param::param(param_name, value, default_value); + } + + /** ROS1-/ROS2-compatible shortcut for ros::spin(); */ + void spin(ROS:: NodePtr nh = 0); + +} // namespace ROS + +#define ROS_CREATE_SRV_CLIENT(nh,srv,name) nh->serviceClient(name) +#define ROS_CREATE_SRV_SERVER(nh,srv,name,cbfunction,cbobject) nh->advertiseService(name,cbfunction,cbobject) + +#define ROS_CREATE_PUBLISHER(nh,msgtype,topic) nh->advertise(topic,1); +#define ROS_PUBLISH(publisher,message) publisher.publish(message); + +#define ROS_CREATE_SUBSCRIBER(nh,msgtype,topic,cbfunction,cbobject) nh->subscribe(topic,1,cbfunction,cbobject) + +#define NSEC nsec // maps nanoseconds in std_msgs::Header::stamp to stamp.nsec in ROS1 resp. stamp.nanosec in ROS2 + +#elif defined __ROS_VERSION && __ROS_VERSION == 2 +/* + * Support for ROS 2 API + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define ROS_DEBUG_STREAM(msgstream) RCLCPP_DEBUG_STREAM(rclcpp::get_logger("sick_lidar_localization"),msgstream) +#define ROS_INFO_STREAM(msgstream) RCLCPP_INFO_STREAM(rclcpp::get_logger("sick_lidar_localization"),msgstream) +#define ROS_WARN_STREAM(msgstream) RCLCPP_WARN_STREAM(rclcpp::get_logger("sick_lidar_localization"),msgstream) +#define ROS_ERROR_STREAM(msgstream) RCLCPP_ERROR_STREAM(rclcpp::get_logger("sick_lidar_localization"),msgstream) + +// Message header +#include "sick_lidar_localization/msg/sick_loc_cola_telegram_msg.hpp" +#include "sick_lidar_localization/msg/sick_loc_diagnostic_msg.hpp" +#include "sick_lidar_localization/msg/sick_loc_result_port_crc_msg.hpp" +#include "sick_lidar_localization/msg/sick_loc_result_port_header_msg.hpp" +#include "sick_lidar_localization/msg/sick_loc_result_port_payload_msg.hpp" +#include "sick_lidar_localization/msg/sick_loc_result_port_telegram_msg.hpp" +#include "sick_lidar_localization/msg/sick_loc_result_port_testcase_msg.hpp" +// Services supported in release 3 and later +#include "sick_lidar_localization/srv/sick_loc_cola_telegram_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_is_system_ready_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_request_result_data_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_request_timestamp_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_pose_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_result_endianness_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_result_mode_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_result_port_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_result_pose_enabled_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_result_pose_interval_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_start_localizing_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_state_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_stop_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_time_sync_srv.hpp" +// Services supported in release 4 and later +#include "sick_lidar_localization/srv/sick_dev_get_lidar_config_srv.hpp" +#include "sick_lidar_localization/srv/sick_dev_get_lidar_ident_srv.hpp" +#include "sick_lidar_localization/srv/sick_dev_get_lidar_state_srv.hpp" +#include "sick_lidar_localization/srv/sick_dev_set_lidar_config_srv.hpp" +#include "sick_lidar_localization/srv/sick_get_software_version_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_auto_start_active_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_auto_start_save_pose_interval_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_auto_start_save_pose_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_force_update_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_initialize_pose_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_initial_pose_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_map_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_map_state_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_odometry_active_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_odometry_port_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_odometry_restrict_y_motion_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_reflectors_for_support_active_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_result_endianness_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_result_mode_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_result_port_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_result_pose_interval_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_result_state_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_ring_buffer_recording_active_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_save_ring_buffer_recording_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_auto_start_active_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_auto_start_save_pose_interval_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_map_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_odometry_active_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_odometry_port_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_odometry_restrict_y_motion_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_reflectors_for_support_active_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_set_ring_buffer_recording_active_srv.hpp" +#include "sick_lidar_localization/srv/sick_loc_start_demo_mapping_srv.hpp" +#include "sick_lidar_localization/srv/sick_report_user_message_srv.hpp" +#include "sick_lidar_localization/srv/sick_save_permanent_srv.hpp" +#include "sick_lidar_localization/srv/sick_dev_set_imu_active_srv.hpp" +#include "sick_lidar_localization/srv/sick_dev_imu_active_srv.hpp" + +namespace geometry_msgs +{ + using namespace msg; // maps ROS2-namespace geometry_msgs::msg to ROS1-namespace geometry_msgs, f.e. geometry_msgs::Point works on both ROS1 and ROS2 +} + +namespace sensor_msgs +{ + using namespace msg; // maps ROS2-namespace sensor_msgs::msg to ROS1-namespace sensor_msgs, f.e. sensor_msgs::PointCloud2 works on both ROS1 and ROS2 +} + +namespace nav_msgs +{ + using namespace msg; // maps ROS2-namespace nav_msgs::msg to ROS1-namespace nav_msgs, f.e. nav_msgs::Odometry works on both ROS1 and ROS2 +} + +namespace std_msgs +{ + using namespace msg; // maps ROS2-namespace std_msgs::msg to ROS1-namespace std_msgs, f.e. std_msgs::Header works on both ROS1 and ROS2 +} + +namespace sick_lidar_localization +{ + using namespace msg; + using namespace srv; + + typedef sensor_msgs::msg::PointCloud2 PointCloud2Msg; + typedef rclcpp::Publisher::SharedPtr PointCloud2MsgPublisher; + + typedef nav_msgs::msg::Odometry OdomMsg; + typedef rclcpp::Subscription::SharedPtr OdomMsgSubscriber; + + typedef rclcpp::Publisher::SharedPtr SickLocResultPortTelegramMsgPublisher; + typedef rclcpp::Publisher::SharedPtr SickLocDiagnosticMsgPublisher; + typedef rclcpp::Publisher::SharedPtr SickLocResultPortTestcaseMsgPublisher; + + typedef rclcpp::Subscription::SharedPtr SickLocResultPortTelegramMsgSubscriber; + typedef rclcpp::Subscription::SharedPtr SickLocResultPortTestcaseMsgSubscriber; + + // Service types supported in release 3 and later + typedef rclcpp::Client::SharedPtr SickLocColaTelegramSrvClient; + typedef rclcpp::Service::SharedPtr SickLocColaTelegramSrvServer; + typedef rclcpp::Client::SharedPtr SickLocIsSystemReadySrvClient; + typedef rclcpp::Service::SharedPtr SickLocIsSystemReadySrvServer; + typedef rclcpp::Client::SharedPtr SickLocRequestResultDataSrvClient; + typedef rclcpp::Service::SharedPtr SickLocRequestResultDataSrvServer; + typedef rclcpp::Client::SharedPtr SickLocRequestTimestampSrvClient; + typedef rclcpp::Service::SharedPtr SickLocRequestTimestampSrvServer; + typedef rclcpp::Client::SharedPtr SickLocSetPoseSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetPoseSrvServer; + typedef rclcpp::Client::SharedPtr SickLocSetResultEndiannessSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetResultEndiannessSrvServer; + typedef rclcpp::Client::SharedPtr SickLocSetResultModeSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetResultModeSrvServer; + typedef rclcpp::Client::SharedPtr SickLocSetResultPortSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetResultPortSrvServer; + typedef rclcpp::Client::SharedPtr SickLocSetResultPoseEnabledSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetResultPoseEnabledSrvServer; + typedef rclcpp::Client::SharedPtr SickLocSetResultPoseIntervalSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetResultPoseIntervalSrvServer; + typedef rclcpp::Client::SharedPtr SickLocStartLocalizingSrvClient; + typedef rclcpp::Service::SharedPtr SickLocStartLocalizingSrvServer; + typedef rclcpp::Client::SharedPtr SickLocStateSrvClient; + typedef rclcpp::Service::SharedPtr SickLocStateSrvServer; + typedef rclcpp::Client::SharedPtr SickLocStopSrvClient; + typedef rclcpp::Service::SharedPtr SickLocStopSrvServer; + typedef rclcpp::Client::SharedPtr SickLocTimeSyncSrvClient; + typedef rclcpp::Service::SharedPtr SickLocTimeSyncSrvServer; + // Service types supported in release 4 and later + typedef rclcpp::Client ::SharedPtr SickDevGetLidarConfigSrvClient; + typedef rclcpp::Service::SharedPtr SickDevGetLidarConfigSrvServer; + typedef rclcpp::Client ::SharedPtr SickDevGetLidarIdentSrvClient; + typedef rclcpp::Service::SharedPtr SickDevGetLidarIdentSrvServer; + typedef rclcpp::Client ::SharedPtr SickDevGetLidarStateSrvClient; + typedef rclcpp::Service::SharedPtr SickDevGetLidarStateSrvServer; + typedef rclcpp::Client ::SharedPtr SickDevSetLidarConfigSrvClient; + typedef rclcpp::Service::SharedPtr SickDevSetLidarConfigSrvServer; + typedef rclcpp::Client ::SharedPtr SickGetSoftwareVersionSrvClient; + typedef rclcpp::Service::SharedPtr SickGetSoftwareVersionSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocAutoStartActiveSrvClient; + typedef rclcpp::Service::SharedPtr SickLocAutoStartActiveSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocAutoStartSavePoseIntervalSrvClient; + typedef rclcpp::Service::SharedPtr SickLocAutoStartSavePoseIntervalSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocAutoStartSavePoseSrvClient; + typedef rclcpp::Service::SharedPtr SickLocAutoStartSavePoseSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocForceUpdateSrvClient; + typedef rclcpp::Service::SharedPtr SickLocForceUpdateSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocInitializePoseSrvClient; + typedef rclcpp::Service::SharedPtr SickLocInitializePoseSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocInitialPoseSrvClient; + typedef rclcpp::Service::SharedPtr SickLocInitialPoseSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocMapSrvClient; + typedef rclcpp::Service::SharedPtr SickLocMapSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocMapStateSrvClient; + typedef rclcpp::Service::SharedPtr SickLocMapStateSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocOdometryActiveSrvClient; + typedef rclcpp::Service::SharedPtr SickLocOdometryActiveSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocOdometryPortSrvClient; + typedef rclcpp::Service::SharedPtr SickLocOdometryPortSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocOdometryRestrictYMotionSrvClient; + typedef rclcpp::Service::SharedPtr SickLocOdometryRestrictYMotionSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocReflectorsForSupportActiveSrvClient; + typedef rclcpp::Service::SharedPtr SickLocReflectorsForSupportActiveSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocResultEndiannessSrvClient; + typedef rclcpp::Service::SharedPtr SickLocResultEndiannessSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocResultModeSrvClient; + typedef rclcpp::Service::SharedPtr SickLocResultModeSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocResultPortSrvClient; + typedef rclcpp::Service::SharedPtr SickLocResultPortSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocResultPoseIntervalSrvClient; + typedef rclcpp::Service::SharedPtr SickLocResultPoseIntervalSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocResultStateSrvClient; + typedef rclcpp::Service::SharedPtr SickLocResultStateSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocRingBufferRecordingActiveSrvClient; + typedef rclcpp::Service::SharedPtr SickLocRingBufferRecordingActiveSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocSaveRingBufferRecordingSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSaveRingBufferRecordingSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocSetAutoStartActiveSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetAutoStartActiveSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocSetAutoStartSavePoseIntervalSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetAutoStartSavePoseIntervalSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocSetMapSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetMapSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocSetOdometryActiveSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetOdometryActiveSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocSetOdometryPortSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetOdometryPortSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocSetOdometryRestrictYMotionSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetOdometryRestrictYMotionSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocSetReflectorsForSupportActiveSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetReflectorsForSupportActiveSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocSetRingBufferRecordingActiveSrvClient; + typedef rclcpp::Service::SharedPtr SickLocSetRingBufferRecordingActiveSrvServer; + typedef rclcpp::Client ::SharedPtr SickLocStartDemoMappingSrvClient; + typedef rclcpp::Service::SharedPtr SickLocStartDemoMappingSrvServer; + typedef rclcpp::Client ::SharedPtr SickReportUserMessageSrvClient; + typedef rclcpp::Service::SharedPtr SickReportUserMessageSrvServer; + typedef rclcpp::Client ::SharedPtr SickSavePermanentSrvClient; + typedef rclcpp::Service::SharedPtr SickSavePermanentSrvServer; + typedef rclcpp::Client ::SharedPtr SickDevSetIMUActiveSrvClient; + typedef rclcpp::Service::SharedPtr SickDevSetIMUActiveSrvServer; + typedef rclcpp::Client ::SharedPtr SickDevIMUActiveSrvClient; + typedef rclcpp::Service::SharedPtr SickDevIMUActiveSrvServer; + +} // namespace sick_lidar_localization + +namespace ROS +{ + using namespace rclcpp; // map namespace ROS to ros in ROS1 and to rclcpp in ROS2, f.e. ROS::ok() to ros::ok() in ROS1 and rclcpp::ok() in ROS2 + typedef rclcpp::Node::SharedPtr NodePtr; + + template bool param(ROS::NodePtr & node, const std::string & param_name, T& value, const T& default_value) + { + ROS_INFO_STREAM("ROS::param(" << node->get_name() << "," << param_name << "," << default_value << ")"); + try + { + if(!node->has_parameter(param_name)) + { + node->declare_parameter(param_name, default_value); + } + } + catch(const std::exception& exc) + { + ROS_WARN_STREAM("## ERROR ROS::param: declare_parameter(" << param_name << "," << default_value << ") failed, exception " << exc.what()); + } + try + { + return node->get_parameter(param_name, value); + } + catch(const std::exception& exc) + { + ROS_WARN_STREAM("## ERROR ROS::param: get_parameter(" << param_name << "," << default_value << ") failed, exception " << exc.what()); + } + return false; + } + + /** ROS1-/ROS2-compatible shortcut for rclcpp::init(argc, argv); */ + void init(int argc, char** argv, const std::string nodename = ""); + +} // namespace ROS + +#define ROS_CREATE_SRV_CLIENT(nh,srv,name) nh->create_client(name) +#define ROS_CREATE_SRV_SERVER(nh,srv,name,cbfunction,cbobject) nh->create_service(name,std::bind(cbfunction,cbobject,std::placeholders::_1,std::placeholders::_2)) + +#define ROS_CREATE_PUBLISHER(nh,msgtype,topic) nh->create_publisher(topic,rclcpp::SystemDefaultsQoS()); +#define ROS_PUBLISH(publisher,message) publisher->publish(message); + +#define ROS_CREATE_SUBSCRIBER(nh,msgtype,topic,cbfunction,cbobject) nh->create_subscription(topic,10,std::bind(cbfunction,cbobject,std::placeholders::_1)) + +#define NSEC nanosec // maps nanoseconds in std_msgs::Header::stamp to stamp.nsec in ROS1 resp. stamp.nanosec in ROS2 + +#else +#error __ROS_VERSION not defined, build with "--cmake-args -DROS_VERSION=1" or "--cmake-args -DROS_VERSION=2" +#endif + +namespace ROS +{ + /** Creates a new ros node, shortcut for new ros::NodeHandle() on ROS1 resp. rclcpp::Node::make_shared(node_name) on ROS2 */ + ROS::NodePtr createNode(const std::string& node_name = "sick_lidar_localization"); + + /** Deletes a ros node */ + void deleteNode(ROS::NodePtr & node); + + /** Shortcut to replace ros::Duration(seconds).sleep() by std::thread */ + void sleep(double seconds); + + /** Shortcut to return ros::Time::now() on ROS1 resp. rclcpp::Clock::now() on ROS2 */ + ROS::Time now(void); + + /** Splits a ROS::Duration into seconds and nanoseconds part */ + void splitTime(ROS::Duration time, uint32_t& seconds, uint32_t& nanoseconds); + + /** Splits a ROS::Time into seconds and nanoseconds part */ + void splitTime(ROS::Time time, uint32_t& seconds, uint32_t& nanoseconds); + + /** Shortcut to return ros::Time(msg_header->stamp.sec,msg_header->stamp.nsec) on ROS1 resp. rclcpp::Time(msg_header->stamp.sec,msg_header->stamp.nanosec) on ROS2 */ + ROS::Time timeFromHeader(const std_msgs::Header* msg_header); + + /** Returns a time (type ros::Time on ROS1 resp. rclcpp::Time on ROS2) from a given amount of seconds. + ** Note: ros::Time(1) initializes 1 second, while rclcpp::Time(1) initializes 1 nanosecond. + ** Do not use the Time constructor with one parameter! Always use ROS::timeFromSec(seconds) + ** or ROS::Time(int32_t seconds, uint32_t nanoseconds). */ + ROS::Time timeFromSec(double seconds); + + /** Returns a duration (type ros::Duration on ROS1 resp. rclcpp::Duration on ROS2) from a given amount of seconds. + ** Note: ros::Duration(1) initializes 1 second, while rclcpp::Duration(1) initializes 1 nanosecond. + ** Do not use the Duration constructor with one parameter! Always use ROS::durationFromSec(seconds) + ** or ROS::Duration(int32_t seconds, uint32_t nanoseconds). */ + ROS::Duration durationFromSec(double seconds); + + /** Shortcut to return ros::Duration::toSec() on ROS1 resp. rclcpp::Duration::seconds() on ROS2 */ + double seconds(ROS::Duration duration); + + /** Shortcut to return the time in seconds since program start (first node started) */ + double secondsSinceStart(const ROS::Time& time); + + /** Shortcut to return the timestamp in milliseconds from ROS1 resp. ROS2 time */ + uint64_t timestampMilliseconds(const ROS::Time& time); + +} // namespace ROS + +#endif // __SIM_LOC_ROS_WRAPPER_H_INCLUDED diff --git a/include/sick_lidar_localization/testcase_generator.h b/include/sick_lidar_localization/testcase_generator.h index f8cc4f9..52662b5 100644 --- a/include/sick_lidar_localization/testcase_generator.h +++ b/include/sick_lidar_localization/testcase_generator.h @@ -56,9 +56,8 @@ #ifndef __SIM_LOC_TESTCASE_GENERATOR_H_INCLUDED #define __SIM_LOC_TESTCASE_GENERATOR_H_INCLUDED +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/result_port_parser.h" -#include "sick_lidar_localization/SickLocColaTelegramMsg.h" -#include "sick_lidar_localization/SickLocResultPortTestcaseMsg.h" namespace sick_lidar_localization { @@ -158,7 +157,8 @@ namespace sick_lidar_localization static uint32_t createTimestampTicksMilliSec(void); static uint32_t s_u32ResultPoseInterval; ///< result pose interval, i.e. the interval in number of scans (default: 1, i.e. result telegram with each processed scan) - static std::map s_controller_settings; ///< test server settings, set by sMN or sRN requests + static std::map s_controller_settings; ///< test server int32 settings, set by sMN or sRN requests + static std::map s_controller_settings_str; ///< test server string settings, set by sMN or sRN requests }; // class TestcaseGenerator diff --git a/include/sick_lidar_localization/time_sync_service.h b/include/sick_lidar_localization/time_sync_service.h index f000758..251f4c2 100644 --- a/include/sick_lidar_localization/time_sync_service.h +++ b/include/sick_lidar_localization/time_sync_service.h @@ -85,6 +85,8 @@ #ifndef __SIM_LOC_TIME_SYNC_SERVICE_H_INCLUDED #define __SIM_LOC_TIME_SYNC_SERVICE_H_INCLUDED +#include "sick_lidar_localization/cola_parser.h" +#include "sick_lidar_localization/driver_monitor.h" #include "sick_lidar_localization/utils.h" namespace sick_lidar_localization @@ -102,7 +104,7 @@ namespace sick_lidar_localization /*! * Constructor */ - TimeSyncService(ros::NodeHandle* nh = 0); + TimeSyncService(ROS::NodePtr nh = 0, sick_lidar_localization::DriverMonitor* driver_monitor = 0); /*! * Destructor @@ -135,6 +137,11 @@ namespace sick_lidar_localization * @return true on success, false in case of errors. */ virtual bool serviceCbRequestTimestamp(sick_lidar_localization::SickLocRequestTimestampSrv::Request & service_request, sick_lidar_localization::SickLocRequestTimestampSrv::Response & service_response); + /*! ROS2 version of function serviceCbRequestTimestamp */ + virtual bool serviceCbRequestTimestampROS2(std::shared_ptr service_request, std::shared_ptr service_response) + { + return serviceCbRequestTimestamp(*service_request, *service_response); + } /*! * Callback for service messages (SickLocTimeSync). Calculates the system time of a vehicle pose from lidar ticks, @@ -149,6 +156,11 @@ namespace sick_lidar_localization * @return true on success, false in case of errors (software pll still in initialization phase or communication error). */ virtual bool serviceCbTimeSync(sick_lidar_localization::SickLocTimeSyncSrv::Request & time_sync_request, sick_lidar_localization::SickLocTimeSyncSrv::Response & time_sync_response); + /*! ROS2 version of function serviceCbTimeSync */ + virtual bool serviceCbTimeSyncROS2(std::shared_ptr time_sync_request, std::shared_ptr time_sync_response) + { + return serviceCbTimeSync(*time_sync_request, *time_sync_response); + } /*! * Thread callback, runs time synchronization, calls ros service "SickLocRequestTimestamp" each 10 seconds @@ -170,21 +182,23 @@ namespace sick_lidar_localization */ bool isSoftwarePllInitialized(void); - ros::ServiceServer m_timestamp_service_server; ///< provides ros service "SickLocRequestTimestamp" to send a LocRequestTimestamp, receive the response and to calculate the time offset - ros::ServiceServer m_timesync_service_server; ///< provides ros service "SickLocTimeSync" to calculate system time from ticks by software pll - ros::ServiceClient m_cola_service_client; ///< client to call ros service "SickLocColaTelegram" to send cola telegrams and receive cola responses from localization controller + ROS::NodePtr m_nh; ///< ros node handle + sick_lidar_localization::DriverMonitor* m_driver_monitor; ///< implements the cola telegram services + sick_lidar_localization::SickLocRequestTimestampSrvServer m_timestamp_service_server; ///< provides ros service "SickLocRequestTimestamp" to send a LocRequestTimestamp, receive the response and to calculate the time offset + sick_lidar_localization::SickLocTimeSyncSrvServer m_timesync_service_server; ///< provides ros service "SickLocTimeSync" to calculate system time from ticks by software pll + sick_lidar_localization::SickLocColaTelegramSrvClient m_cola_service_client; ///< client to call ros service "SickLocColaTelegram" to send cola telegrams and receive cola responses from localization controller + // sick_lidar_localization::SickLocRequestTimestampSrvClient m_request_timestamp_client; ///< client to call ros service "SickLocRequestTimestamp" bool m_time_sync_thread_running; ///< true: m_time_sync_thread is running, otherwise false boost::thread* m_time_sync_thread; ///< thread to synchronize timestamps, runs the software pll bool m_cola_binary; ///< false: send Cola-ASCII (default), true: send Cola-Binary int m_cola_binary_mode; ///< 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) int m_software_pll_fifo_length; ///< length of software pll fifo, default: 7 - ros::Rate m_time_sync_rate; ///< frequency to request timestamps using ros service "SickLocRequestTimestamp" and to update software pll, default: 0.1 (LocRequestTimestamp queries every 10 seconds) - ros::Rate m_time_sync_initial_rate; ///< frequency to request timestamps and to update software pll during initialization phase, default: 1.0 (LocRequestTimestamp queries every second) + double m_time_sync_rate; ///< frequency to request timestamps using ros service "SickLocRequestTimestamp" and to update software pll, default: 0.1 (LocRequestTimestamp queries every 10 seconds) + double m_time_sync_initial_rate; ///< frequency to request timestamps and to update software pll during initialization phase, default: 1.0 (LocRequestTimestamp queries every second) int m_time_sync_initial_length; ///< length of initialization phase with LocRequestTimestamps every second, default: 10 (i.e. 10 LocRequestTimestamp queries every second after start, otherwise LocRequestTimestamp queries every 10 seconds) double m_cola_response_timeout; ///< Timeout in seconds for cola responses from localization controller, default: 1 - ros::ServiceClient m_request_timestamp_client; ///< client to call ros service "SickLocRequestTimestamp" boost::mutex m_software_pll_mutex; ///< mutex to protect access to software pll used in service "SickLocTimeSync - + boost::mutex m_service_cb_mutex; ///< mutex to protect serviceCbRequestTimestamp (one service request at a time) }; // class TimeSyncService diff --git a/include/sick_lidar_localization/utils.h b/include/sick_lidar_localization/utils.h index af87e1f..527bc2c 100644 --- a/include/sick_lidar_localization/utils.h +++ b/include/sick_lidar_localization/utils.h @@ -61,14 +61,7 @@ #include #include -#include "sick_lidar_localization/SickLocColaTelegramSrv.h" -#include "sick_lidar_localization/SickLocResultPortHeaderMsg.h" -#include "sick_lidar_localization/SickLocResultPortPayloadMsg.h" -#include "sick_lidar_localization/SickLocResultPortCrcMsg.h" -#include "sick_lidar_localization/SickLocResultPortTelegramMsg.h" -#include "sick_lidar_localization/SickLocResultPortTestcaseMsg.h" -#include "sick_lidar_localization/SickLocRequestTimestampSrv.h" -#include "sick_lidar_localization/SickLocTimeSyncSrv.h" +#include "sick_lidar_localization/ros_wrapper.h" namespace sick_lidar_localization { @@ -110,7 +103,13 @@ namespace sick_lidar_localization { public: /*! Constructor with optional initialization of value */ - SetGet32(const uint32_t & value = 0) : SetGet(value) {} + SetGet32(const uint32_t & value = 0) : SetGet(value) {} + /*! Increments and returns the value */ + uint32_t inc(void) + { + boost::lock_guard value_lockguard(m_value_mutex); + return ++m_value; + } }; /*! @@ -144,6 +143,49 @@ namespace sick_lidar_localization return out; } + /** Shortcut to print (msg_header.stamp.sec,msg_header.stamp.nsec) on ROS1 resp. (msg_header.stamp.sec,msg_header.stamp.nanosec) on ROS2 */ + static std::string flattenToString(const std_msgs::Header* header) + { + std::stringstream s; + s << "header.stamp: " << header->stamp.sec << "." << header->stamp.NSEC; + return s.str(); + } + + /*! + * Shortcut to print a SickLocColaTelegramMsg in flatten format, replacing linefeeds by colon-separators + */ + static std::string flattenToString(const sick_lidar_localization::SickLocColaTelegramMsg & cola_telegram) + { + std::stringstream s; + s << flattenToString(&cola_telegram.header) + << ", command_type: " << cola_telegram.command_type + << ", command_name: " << cola_telegram.command_name << ", parameter: ["; + for(size_t n = 0; n < cola_telegram.parameter.size(); n++) + s << (n>0?",":"") << cola_telegram.parameter[n]; + s << "]"; + std::string out(s.str()); + flattenString(out); + return out; + } + + /*! + * Shortcut to print a SickLocColaTelegramMsg in flatten format, replacing linefeeds by colon-separators + */ + static std::string flattenToString(const sick_lidar_localization::SickLocResultPortTelegramMsg & telegram) + { + + std::stringstream s; + s << flattenToString(&telegram.header) + << ", telegram_payload: [posex:" << telegram.telegram_payload.posex << ",posey:" << telegram.telegram_payload.posey << ",poseyaw:" << telegram.telegram_payload.poseyaw + << ",scancounter:" << telegram.telegram_payload.scancounter << ",timestamp:" << telegram.telegram_payload.timestamp << ",quality:" << (int)(telegram.telegram_payload.quality&0xFF) + << ",covariancex:" << telegram.telegram_payload.covariancex << ",covariancey:" << telegram.telegram_payload.covariancey << ",covarianceyaw:" << telegram.telegram_payload.covarianceyaw + << ",outliersratio:" << telegram.telegram_payload.outliersratio << ",errorcode:" << telegram.telegram_payload.errorcode + << "], telegram_checksum: " << telegram.telegram_trailer.checksum; + std::string out(s.str()); + flattenString(out); + return out; + } + /*! * Compares two objects by streaming them to strings. * Returns true, if string representation of x and y is identical, @@ -157,6 +199,23 @@ namespace sick_lidar_localization return sx.str() == sy.str(); } + /*! + * Compares two vectors by streaming them to strings. + * Returns true, if string representation of x and y is identical, + * otherwise false. + */ + template static bool identicalByStream(const std::vector & x, const std::vector & y) + { + if(x.size() == y.size()) + { + for(size_t n = 0; n < x.size(); n++) + if(!identicalByStream(x[n], y[n])) + return false; + return true; + } + return false; + } + /*! * Compares two SickLocResultPortTelegramMsgs by streaming them to strings. * Returns true, if string representation of x and y is identical, @@ -164,9 +223,26 @@ namespace sick_lidar_localization */ static bool identicalByStream(const SickLocResultPortTelegramMsg & x, const SickLocResultPortTelegramMsg & y) { + #if defined __ROS_VERSION && __ROS_VERSION == 1 return identicalByStream(x.telegram_header, y.telegram_header) - && identicalByStream(x.telegram_payload, y.telegram_payload) - && identicalByStream(x.telegram_trailer, y.telegram_trailer); + && identicalByStream(x.telegram_payload, y.telegram_payload) + && identicalByStream(x.telegram_trailer, y.telegram_trailer); + #elif defined __ROS_VERSION && __ROS_VERSION == 2 + return x.telegram_header == y.telegram_header && x.telegram_payload == y.telegram_payload && x.telegram_trailer == y.telegram_trailer; + //return x == y; + #else + return false; + #endif + } + + /*! + * Compares two SickLocColaTelegramMsg by streaming them to strings. + * Returns true, if string representation of x and y is identical, + * otherwise false. The message headers with timestamp and frame_id are ignored. + */ + static bool identicalByStream(const SickLocColaTelegramMsg & x, const SickLocColaTelegramMsg & y) + { + return x.command_name == y.command_name && x.command_type == y.command_type && identicalByStream(x.parameter, y.parameter); } /*! diff --git a/launch/odom_msg_sender.launch b/launch/odom_msg_sender.launch new file mode 100755 index 0000000..8e0451d --- /dev/null +++ b/launch/odom_msg_sender.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/sim_loc_driver.launch b/launch/sim_loc_driver.launch index 903f24e..3d17674 100644 --- a/launch/sim_loc_driver.launch +++ b/launch/sim_loc_driver.launch @@ -6,6 +6,14 @@ + + + + + + + + @@ -14,22 +22,6 @@ - - - - - - - - - - - - - - - - diff --git a/launch/sim_loc_driver.launch.py b/launch/sim_loc_driver.launch.py new file mode 100755 index 0000000..ab664c8 --- /dev/null +++ b/launch/sim_loc_driver.launch.py @@ -0,0 +1,145 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +def generate_launch_description(): + ld = LaunchDescription() + + # sick_lidar_localization configuration + os_localization_controller_ip_address = os.getenv('localization_controller_ip_address', '') + sick_lidar_localization_parameters=[ + # Driver configuration. See Operation-Instruction-v1.1.0.241R.pdf, page 51, "IP port number and protocol" for default tcp ports. + {'/sick_lidar_localization/driver/localization_controller_default_ip_address': "192.168.0.1"}, # Default IP address "192.168.0.1" of the localization controller (if not otherwise set by parameter "localization_controller_ip_address") + {'/sim_loc_driver/localization_controller_ip_address': "192.168.0.1"}, # IP address "192.168.0.1" of the localization controller (if not otherwise set by parameter "localization_controller_ip_address") + {'/system/localization_controller_ip_address': "{}".format(os_localization_controller_ip_address)}, # IP address can optionally be set by system environment + {'/sick_lidar_localization/driver/result_telegrams_tcp_port': 2201}, # TCP port number of the localization controller sending localization results. To transmit the localization results to the vehicle controller, the localization controller uses IP port number 2201 to send localization results in a single direction to the external vehicle controller. + {'/sick_lidar_localization/driver/cola_telegrams_tcp_port': 2111}, # For requests and to transmit settings to the localization controller: IP port number 2111 and 2112 to send telegrams and to request data, SOPAS CoLa-A or CoLa-B protocols + {'/sick_lidar_localization/driver/cola_binary': 0}, # 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) + {'/sick_lidar_localization/driver/tcp_connection_retry_delay': 1.0}, # Delay in seconds to retry to connect to the localization controller, default 1 second + {'/sick_lidar_localization/driver/result_telegrams_topic': "/sick_lidar_localization/driver/result_telegrams"}, # ros topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) + {'/sick_lidar_localization/driver/result_telegrams_frame_id': "sick_lidar_localization"}, # ros frame id of result port telegram messages (type SickLocResultPortTelegramMsg) + {'/sick_lidar_localization/driver/diagnostic_topic': "/sick_lidar_localization/driver/diagnostic"}, # ros topic to publish diagnostic messages (type SickLocDiagnosticMsg) + {'/sick_lidar_localization/driver/diagnostic_frame_id': "sick_lidar_localization"}, # ros frame id of diagnostic messages (type SickLocDiagnosticMsg) + {'/sick_lidar_localization/driver/monitoring_rate': 1.0}, # frequency to monitor driver messages, once per second by default + {'/sick_lidar_localization/driver/monitoring_message_timeout': 1.0}, # timeout for driver messages, shutdown tcp-sockets and reconnect after message timeout, 1 second by default + # Configuration for time sync service + {'/sick_lidar_localization/time_sync/cola_response_timeout': 1.0}, # Timeout in seconds for cola responses from localization controller + {'/sick_lidar_localization/time_sync/software_pll_fifo_length': 7}, # Length of software pll fifo, default: 7 + {'/sick_lidar_localization/time_sync/time_sync_rate': 0.1}, # Frequency to request timestamps from localization controller using ros service "SickLocRequestTimestamp" and to update software pll, default: 0.1 + {'/sick_lidar_localization/time_sync/time_sync_initial_rate': 1.0}, # Frequency to request timestamps and to update software pll during initialization phase, default: 1.0 (LocRequestTimestamp queries every second) + {'/sick_lidar_localization/time_sync/time_sync_initial_length': 10}, # Length of initialization phase with LocRequestTimestamps every second, default: 10 (i.e. 10 LocRequestTimestamp queries every second after start, otherwise LocRequestTimestamp queries every 10 seconds) + # Pointcloud configuration + {'/sick_lidar_localization/driver/point_cloud_topic': "/cloud"}, # ros topic to publish PointCloud2 data + {'/sick_lidar_localization/driver/point_cloud_frame_id': "pointcloud_sick_lidar_localization"}, # ros frame id of PointCloud2 messages + {'/sick_lidar_localization/driver/tf_parent_frame_id': "tf_demo_map"}, # parent frame of tf messages of of vehicles pose (typically frame of the loaded map) + {'/sick_lidar_localization/driver/tf_child_frame_id': "tf_sick_lidar_localization"}, # child frame of tf messages of of vehicles pose + # Configuration for cola service + {'/sick_lidar_localization/time_sync/cola_response_timeout': 1.0}, # Timeout in seconds for cola responses from localization controller + {'/sick_lidar_localization/driver/tcp_connection_retry_delay': 1.0}, # Delay in seconds to retry to connect to the localization controller, default 1 second + # Odometry configuration + {'/sick_lidar_localization/driver/odom_telegrams_udp_port': 3000}, # Udp port to send odom packages to the localization controller + {'/sick_lidar_localization/driver/odom_topic': "/odom"}, # ROS topic for odometry messages + {'/sick_lidar_localization/driver/odom_telegrams_bigendian': 1}, # Send udp odometry telegrams big endian (true) or little endian (false) + {'/sick_lidar_localization/driver/odom_telegrams_source_id': 100}, # SourceID of udp odometry telegrams, e.g. vehicle controller 1 + # sim_loc_driver_check configuration + {'/sick_lidar_localization/sim_loc_driver_check/result_telegrams_topic': "/sick_lidar_localization/driver/result_telegrams"}, # ros topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) + {'/sick_lidar_localization/sim_loc_driver_check/message_check_frequency': 100.0}, # frequency to check sim_loc_driver messages + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/header/seq': 0.0}, # sequence ID, consecutively increasing ID, uint32, size:= 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/header/stamp': 0.0}, # time stamp + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/header/frame_id': "sick_lidar_localization"}, # frame id of ros header + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_header/MagicWord': 0x5349434B}, # Magic word SICK (0x53 0x49 0x43 0x4B). uint32, size:= 4 � UInt8 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_header/Length': 106}, # Length of telegram incl. header, payload, and trailer. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_header/PayloadType': 0x0642}, # Payload type, 0x06c2 = Little Endian, 0x0642 = Big Endian. uint16, size:= UInt16 = 2 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_header/PayloadVersion': 1}, # Version of PayloadType structure. uint16, size:= UInt16 = 2 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_header/OrderNumber': 0.0}, # Order number of the localization controller. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_header/SerialNumber': 0.0}, # Serial number of the localization controller. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_header/FW_Version': [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}, # Software version of the localization controller. uint8[], size:= 20 � UInt8 = 20 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_header/TelegramCounter': 0.0}, # Telegram counter since last start-up. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_header/SystemTime': 0.0}, # Not used. uint64, size:= NTP = 8 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/ErrorCode': 0}, # ErrorCode 0:= OK, ErrorCode 1:= UNKNOWNERROR. uint16, size:= UInt16 = 2 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/ScanCounter': 0.0}, # Counter of related scan data. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/Timestamp': 0.0}, # Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/PoseX': -2147483648}, # Position X of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/PoseY': -2147483648}, # Position Y of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/PoseYaw': -360000}, # Orientation (yaw) of the vehicle on the map [mdeg]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/Reserved1': 0.0}, # Reserved. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/Reserved2': -2147483648}, # Reserved. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/Quality': 0}, # Quality of pose [0 � 100], 1 = bad pose quality, 100 = good pose quality. uint8, size:= UInt8 = 1 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/OutliersRatio': 0}, # Ratio of beams that cannot be assigned to the current reference map [%]. uint8, size:= UInt8 = 1 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/CovarianceX': 0}, # Covariance c1 of the pose X [mm^2]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/CovarianceY': 0}, # Covariance c5 of the pose Y [mm^2]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/CovarianceYaw': 0}, # Covariance c9 of the pose Yaw [mdeg^2]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_payload/Reserved3': 0.0}, # Reserved. uint64, size:= UInt64 = 8 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/telegram_trailer/Checksum': 0}, # CRC16-CCITT checksum. uint16, size:= UInt16 = 2 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/vehicle_time_delta': -1.0e308}, # min. allowed time diff in seconds between vehicle time (system time from ticks by software pll) and ros::Time::now() + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/check_vehicle_time': False}, # we can't check the vehicle time in case of network errors + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/header/seq': 4294967296.0}, # 4294967296 = 0xFFFFFFFF # sequence ID, consecutively increasing ID, uint32, size:= 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/header/stamp': 2147483648.2147483648}, # time stamp (max. 2 int values, each 2^31 = 2147483648) + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/header/frame_id': "sick_lidar_localization"}, # frame id of ros header + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_header/MagicWord': 0x5349434B}, # Magic word SICK (0x53 0x49 0x43 0x4B). uint32, size:= 4 � UInt8 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_header/Length': 106}, # Length of telegram incl. header, payload, and trailer. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_header/PayloadType': 0x06c2}, # Payload type, 0x06c2 = Little Endian, 0x0642 = Big Endian. uint16, size:= UInt16 = 2 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_header/PayloadVersion': 0xFFFF}, # Version of PayloadType structure. uint16, size:= UInt16 = 2 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_header/OrderNumber': 4294967296.0}, # 4294967296 = 0xFFFFFFFF # Order number of the localization controller. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_header/SerialNumber': 4294967296.0}, # 4294967296 = 0xFFFFFFFF # Serial number of the localization controller. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_header/FW_Version': [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF]}, # Software version of the localization controller. uint8[], size:= 20 � UInt8 = 20 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_header/TelegramCounter': 4294967296.0}, # 4294967296 = 0xFFFFFFFF # Telegram counter since last start-up. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_header/SystemTime': 18446744073709551616.0}, # 18446744073709551616 = 0xFFFFFFFFFFFFFFFF # Not used. uint64, size:= NTP = 8 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/ErrorCode': 0xFFFF}, # ErrorCode 0:= OK, ErrorCode 1:= UNKNOWNERROR. uint16, size:= UInt16 = 2 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/ScanCounter': 4294967296.0}, # 4294967296 = 0xFFFFFFFF # Counter of related scan data. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/Timestamp': 4294967296.0}, # 4294967296 = 0xFFFFFFFF # Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/PoseX': 2147483647}, # Position X of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/PoseY': 2147483647}, # Position Y of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/PoseYaw': 360000}, # Orientation (yaw) of the vehicle on the map [mdeg]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/Reserved1': 4294967296.0}, # 4294967296 = 0xFFFFFFFF # Reserved. uint32, size:= UInt32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/Reserved2': 2147483647}, # Reserved. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/Quality': 100}, # Quality of pose [1 � 100], 1 = bad pose quality, 100 = good pose quality. uint8, size:= UInt8 = 1 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/OutliersRatio': 100}, # Ratio of beams that cannot be assigned to the current reference map [%]. uint8, size:= UInt8 = 1 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/CovarianceX': 2147483647}, # Covariance c1 of the pose X [mm^2]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/CovarianceY': 2147483647}, # Covariance c5 of the pose Y [mm^2]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/CovarianceYaw': 2147483647}, # Covariance c9 of the pose Yaw [mdeg^2]. int32, size:= Int32 = 4 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_payload/Reserved3': 18446744073709551616.0}, # 18446744073709551616 = 0xFFFFFFFFFFFFFFFF # Reserved. uint64, size:= UInt64 = 8 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/telegram_trailer/Checksum': 0xFFFF}, # CRC16-CCITT checksum. uint16, size:= UInt16 = 2 byte + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/vehicle_time_delta': 1.0e308}, # max. allowed time diff in seconds between vehicle time (system time from ticks by software pll) and ros::Time::now() + {'/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/check_vehicle_time': False} # we can't check the vehicle time in case of network errors + # Initial result output configuration. Unless configuration is activated here, default values are used. --> + # Uncomment and set result output configuration if required, otherwise SIM default configuration applies. --> + # {'SickLocSetResultPort': "2201"}, # LocSetResultPort: Set TCP-port for result output (default: 2201) --> + # {'SickLocSetResultMode': "0", # LocSetResultMode: Set mode of result output (0=stream or 1=poll, default:0) --> + # {'SickLocSetResultPoseEnabled': "1", # LocSetResultPoseEnabled: Disable (0) or enable (1) result output (default: 1, enabled) --> + # {'SickLocSetResultEndianness': "0", # LocSetResultEndianness: Set endianness of result output (0 = big endian, 1 = little endian, default: 0) --> + # {'SickLocSetResultPoseInterval': "1", # LocSetResultPoseInterval: Set interval of result output (0-255, interval in number of scans, 1: result with each processed scan, default: 1) --> + # {'SickLocRequestResultData': "0", # LocRequestResultData: If in poll mode, trigger sending the localization result of the next processed scan via TCP interface (default: 0) --> + ] + + # Launch sim_loc_driver + sim_loc_driver_node = Node( + package='sick_lidar_localization', + name = 'sim_loc_driver', + node_executable='sim_loc_driver', + output='screen', + parameters=sick_lidar_localization_parameters + ) + + # Launch sim_loc_driver_check to monitor sim_loc_driver + sim_loc_driver_check_node = Node( + package='sick_lidar_localization', + name = 'sim_loc_driver_check', + node_executable='sim_loc_driver_check', + output='screen', + parameters=sick_lidar_localization_parameters + ) + + # Launch pointcloud_converter to convert and publish PointCloud2 messages + pointcloud_converter_node = Node( + package='sick_lidar_localization', + name = 'pointcloud_converter', + node_executable='pointcloud_converter', + output='screen', + parameters=sick_lidar_localization_parameters + ) + + ld.add_action(sim_loc_driver_node) + ld.add_action(sim_loc_driver_check_node) + ld.add_action(pointcloud_converter_node) + return ld diff --git a/launch/sim_loc_test_server.launch.py b/launch/sim_loc_test_server.launch.py new file mode 100755 index 0000000..7f8e3ac --- /dev/null +++ b/launch/sim_loc_test_server.launch.py @@ -0,0 +1,33 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +def generate_launch_description(): + ld = LaunchDescription() + + # sim_loc_test_server configuration. See Operation-Instruction-v1.1.0.241R.pdf, page 51, "IP port number and protocol" for default tcp ports. + os_demo_circles = os.getenv('sim_loc_test_server_demo_circles', '') + os_error_simulation = os.getenv('sim_loc_test_server_error_simulation', '') + sim_loc_test_server_parameters=[ + {'/sim_loc_test_server/demo_circles': False}, # True: simulate a sensor moving in circles, False (default): create random based result port telegrams + {'//sim_loc_test_server/error_simulation': False}, # If true, test server simulates errors and communication failures + {'/system/test_server/demo_circles': "{}".format(os_demo_circles)}, # 1: simulate a sensor moving in circles, 0 (default): create random based result port telegrams + {'/system/test_server/error_simulation': "{}".format(os_error_simulation)}, # 0: false (no error simulation), if true, test server simulates errors and communication failures + {'/sick_lidar_localization/test_server/result_telegrams_tcp_port': 2201}, # Default tcp port for sim_loc_test_server is 2201 (ip port number of the localization controller sending localization results) + {'/sick_lidar_localization/test_server/cola_telegrams_tcp_port': 2111}, # For requests and to transmit settings to the localization controller: IP port number 2111 and 2112 to send telegrams and to request data, SOPAS CoLa-A or CoLa-B protocols + {'/sick_lidar_localization/test_server/result_telegrams_rate': 10.0}, # Rate to generate and send result port telegrams + {'/sick_lidar_localization/test_server/result_testcases_topic': "/sick_lidar_localization/test_server/result_testcases"}, # ROS topic to publish testcases with result port telegrams (type SickLocResultPortTestcaseMsg) + {'/sick_lidar_localization/test_server/result_testcases_frame_id': "result_testcases"} # ROS frame id of testcase messages (type SickLocResultPortTestcaseMsg) + ] + + # Launch sim_loc_test_server + sim_loc_test_server_node = Node( + package='sick_lidar_localization', + name = 'sim_loc_test_server', + node_executable='sim_loc_test_server', + output='screen', + parameters=sim_loc_test_server_parameters + ) + + ld.add_action(sim_loc_test_server_node) + return ld diff --git a/launch/unittest_sim_loc_parser.launch.py b/launch/unittest_sim_loc_parser.launch.py new file mode 100755 index 0000000..7764e7e --- /dev/null +++ b/launch/unittest_sim_loc_parser.launch.py @@ -0,0 +1,23 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +def generate_launch_description(): + ld = LaunchDescription() + + # unittest_sim_loc_parser configuration + unittest_sim_loc_parser_parameters=[ + {'/unittest_sim_loc_parser/number_result_port_testcases': 100} # number of random based result port testcases, default: 100 testcases + ] + + # Launch verify_sim_loc_driver + unittest_sim_loc_parser_node = Node( + package='sick_lidar_localization', + name = 'unittest_sim_loc_parser', + node_executable='unittest_sim_loc_parser', + output='screen', + parameters=unittest_sim_loc_parser_parameters + ) + + ld.add_action(unittest_sim_loc_parser_node) + return ld diff --git a/launch/verify_sim_loc_driver.launch.py b/launch/verify_sim_loc_driver.launch.py new file mode 100755 index 0000000..fa88c23 --- /dev/null +++ b/launch/verify_sim_loc_driver.launch.py @@ -0,0 +1,24 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +def generate_launch_description(): + ld = LaunchDescription() + + # sim_loc_test_server configuration. See Operation-Instruction-v1.1.0.241R.pdf, page 51, "IP port number and protocol" for default tcp ports. + verify_sim_loc_driver_parameters=[ + {'/sick_lidar_localization/driver/result_telegrams_topic': "/sick_lidar_localization/driver/result_telegrams"}, # default topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) + {'/sick_lidar_localization/test_server/result_testcases_topic': "/sick_lidar_localization/test_server/result_testcases"} # ROS topic to publish testcases with result port telegrams (type SickLocResultPortTestcaseMsg) + ] + + # Launch verify_sim_loc_driver + verify_sim_loc_driver_node = Node( + package='sick_lidar_localization', + name = 'verify_sim_loc_driver', + node_executable='verify_sim_loc_driver', + output='screen', + parameters=verify_sim_loc_driver_parameters + ) + + ld.add_action(verify_sim_loc_driver_node) + return ld diff --git a/msg/SickLocColaTelegramMsg.msg b/msg/SickLocColaTelegramMsg.msg index ef01586..f79c0b4 100644 --- a/msg/SickLocColaTelegramMsg.msg +++ b/msg/SickLocColaTelegramMsg.msg @@ -3,7 +3,7 @@ # See Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.PDF for further details # about Cola telgrams -Header header # ROS Header with sequence id, timestamp and frame id +std_msgs/Header header # ROS Header with sequence id, timestamp and frame id int32 command_type # One of the SOPAS Commands sRN (1, Read by name request) or sRA (2, Read by name response) or sMN (3, Method by name request) or sMA (4, Method by name response) or sWN (5, Write by name request) diff --git a/msg/SickLocDiagnosticMsg.msg b/msg/SickLocDiagnosticMsg.msg index 7bbe2df..fa2438f 100644 --- a/msg/SickLocDiagnosticMsg.msg +++ b/msg/SickLocDiagnosticMsg.msg @@ -2,6 +2,6 @@ # SickLocDiagnosticMsg publishes diagnostic messages # of the sick localization driver. -Header header # ROS Header with sequence id, timestamp and frame id +std_msgs/Header header # ROS Header with sequence id, timestamp and frame id int32 error_code # error code (0 := no error) string message # diagnostic message diff --git a/msg/SickLocResultPortCrcMsg.msg b/msg/SickLocResultPortCrcMsg.msg index 6e1b119..909881d 100644 --- a/msg/SickLocResultPortCrcMsg.msg +++ b/msg/SickLocResultPortCrcMsg.msg @@ -7,5 +7,5 @@ # * Initial value = 0xFFFF # See chapter 5.9 ("About result port telegrams") of the operation manual for further details. -uint16 Checksum # CRC16-CCITT over length of header (52 bytes) and payload (52 bytes) without 2 bytes of this trailer. Size: UInt16 = 2 byte +uint16 checksum # CRC16-CCITT over length of header (52 bytes) and payload (52 bytes) without 2 bytes of this trailer. Size: UInt16 = 2 byte diff --git a/msg/SickLocResultPortHeaderMsg.msg b/msg/SickLocResultPortHeaderMsg.msg index b546b4e..cf1ceb5 100644 --- a/msg/SickLocResultPortHeaderMsg.msg +++ b/msg/SickLocResultPortHeaderMsg.msg @@ -3,13 +3,13 @@ # for sick localization (52 byte). See chapter 5.9 ("About result port telegrams") # of the operation manual for further details. -uint32 MagicWord # Magic word SICK (0x53 0x49 0x43 0x4B). Size: 4 × UInt8 = 4 byte -uint32 Length # Length of telegram incl. header, payload, and trailer. Size: UInt32 = 4 byte -uint16 PayloadType # Payload type: 0x06c2 = Little Endian, 0x0642 = Big Endian. Size: UInt16 = 2 byte -uint16 PayloadVersion # Version of PayloadType structure. Size: UInt16 = 2 byte -uint32 OrderNumber # Order number of the localization controller. Size: UInt32 = 4 byte -uint32 SerialNumber # Serial number of the localization controller. Size: UInt32 = 4 byte -uint8[] FW_Version # Software version of the localization controller. Size: 20 × UInt8 = 20 byte -uint32 TelegramCounter # Telegram counter since last start-up. Size: UInt32 = 4 byte -uint64 SystemTime # Not used. Size: NTP = 8 byte +uint32 magicword # Magic word SICK (0x53 0x49 0x43 0x4B). Size: 4 × UInt8 = 4 byte +uint32 length # Length of telegram incl. header, payload, and trailer. Size: UInt32 = 4 byte +uint16 payloadtype # Payload type: 0x06c2 = Little Endian, 0x0642 = Big Endian. Size: UInt16 = 2 byte +uint16 payloadversion # Version of PayloadType structure. Size: UInt16 = 2 byte +uint32 ordernumber # Order number of the localization controller. Size: UInt32 = 4 byte +uint32 serialnumber # Serial number of the localization controller. Size: UInt32 = 4 byte +uint8[] fw_version # Software version of the localization controller. Size: 20 × UInt8 = 20 byte +uint32 telegramcounter # Telegram counter since last start-up. Size: UInt32 = 4 byte +uint64 systemtime # Not used. Size: NTP = 8 byte diff --git a/msg/SickLocResultPortPayloadMsg.msg b/msg/SickLocResultPortPayloadMsg.msg index d7371e0..13e7e7d 100644 --- a/msg/SickLocResultPortPayloadMsg.msg +++ b/msg/SickLocResultPortPayloadMsg.msg @@ -3,18 +3,18 @@ # for sick localization (52 byte). See chapter 5.9 ("About result port telegrams") # of the operation manual for further details. -uint16 ErrorCode # ErrorCode 0: OK, ErrorCode 1: UNKNOWNERROR. Size: UInt16 = 2 byte -uint32 ScanCounter # Counter of related scan data. Size: UInt32 = 4 byte -uint32 Timestamp # Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte -int32 PoseX # Position X of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte -int32 PoseY # Position Y of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte -int32 PoseYaw # Orientation (yaw) of the vehicle on the map [mdeg] Size: Int32 = 4 byte -uint32 Reserved1 # Reserved. Size: UInt32 = 4 byte -int32 Reserved2 # Reserved. Size: Int32 = 4 byte -uint8 Quality # Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality. Size: UInt8 = 1 byte -uint8 OutliersRatio # Ratio of beams that cannot be assigned to the current reference map [%]. Size: UInt8 = 1 byte -int32 CovarianceX # Covariance c1 of the pose X [mm^2]. Size: Int32 = 4 byte -int32 CovarianceY # Covariance c5 of the pose Y [mm^2]. Size: Int32 = 4 byte -int32 CovarianceYaw # Covariance c9 of the pose Yaw [mdeg^2]. Size: Int32 = 4 byte -uint64 Reserved3 # Reserved. Size: UInt64 = 8 byte +uint16 errorcode # ErrorCode 0: OK, ErrorCode 1: UNKNOWNERROR. Size: UInt16 = 2 byte +uint32 scancounter # Counter of related scan data. Size: UInt32 = 4 byte +uint32 timestamp # Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte +int32 posex # Position X of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte +int32 posey # Position Y of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte +int32 poseyaw # Orientation (yaw) of the vehicle on the map [mdeg] Size: Int32 = 4 byte +uint32 reserved1 # Reserved. Size: UInt32 = 4 byte +int32 reserved2 # Reserved. Size: Int32 = 4 byte +uint8 quality # Quality of pose [0 ... 100], 1 = bad pose quality, 100 = good pose quality. Size: UInt8 = 1 byte +uint8 outliersratio # Ratio of beams that cannot be assigned to the current reference map [%]. Size: UInt8 = 1 byte +int32 covariancex # Covariance c1 of the pose X [mm^2]. Size: Int32 = 4 byte +int32 covariancey # Covariance c5 of the pose Y [mm^2]. Size: Int32 = 4 byte +int32 covarianceyaw # Covariance c9 of the pose Yaw [mdeg^2]. Size: Int32 = 4 byte +uint64 reserved3 # Reserved. Size: UInt64 = 8 byte diff --git a/msg/SickLocResultPortTelegramMsg.msg b/msg/SickLocResultPortTelegramMsg.msg index 4535c90..ba749ac 100644 --- a/msg/SickLocResultPortTelegramMsg.msg +++ b/msg/SickLocResultPortTelegramMsg.msg @@ -7,7 +7,7 @@ # Header # -Header header # ROS Header with sequence id, timestamp and frame id +std_msgs/Header header # ROS Header with sequence id, timestamp and frame id # # Result port telegram diff --git a/msg/SickLocResultPortTestcaseMsg.msg b/msg/SickLocResultPortTestcaseMsg.msg index d6feea2..1bb5801 100644 --- a/msg/SickLocResultPortTestcaseMsg.msg +++ b/msg/SickLocResultPortTestcaseMsg.msg @@ -4,6 +4,6 @@ # and ros message format of type SickLocResultPortTelegramMsg (published by sim_loc_driver). # See chapter 5.9 ("About result port telegrams") of the operation manual for further details. -Header header # ROS Header with sequence id, timestamp and frame id +std_msgs/Header header # ROS Header with sequence id, timestamp and frame id uint8[] binary_data # binary encoded result port telegram SickLocResultPortTelegramMsg telegram_msg # decoded result port telegram diff --git a/package.xml b/package.xml deleted file mode 100644 index ee3cf8a..0000000 --- a/package.xml +++ /dev/null @@ -1,71 +0,0 @@ - - - sick_lidar_localization - 0.0.0 - sick_lidar_localization is an open-source project to support the LiDAR-LOC software of the company SICK using the ROS-framework - - - rostest - - - - - Apache - - - - https://github.com/michael1309/sick_lidar_localization - - - - Michael Lehning - - - - - - - - - - - - - - - - - - - - - - catkin - message_generation - message_runtime - roscpp - rospy - geometry_msgs - std_msgs - sensor_msgs - tf - message_generation - message_generation - message_runtime - roscpp - rospy - geometry_msgs - std_msgs - sensor_msgs - tf - message_runtime - roscpp - rospy - std_msgs - - - - - - - diff --git a/package.xml b/package.xml new file mode 120000 index 0000000..478b134 --- /dev/null +++ b/package.xml @@ -0,0 +1 @@ +./package_ros1.xml \ No newline at end of file diff --git a/package_ros1.xml b/package_ros1.xml new file mode 100755 index 0000000..e85b8c7 --- /dev/null +++ b/package_ros1.xml @@ -0,0 +1,73 @@ + + + sick_lidar_localization + 0.0.0 + sick_lidar_localization is an open-source project to support the LiDAR-LOC software of the company SICK using the ROS-framework + + + rostest + + + + + Apache + + + + https://github.com/michael1309/sick_lidar_localization + + + + Michael Lehning + + + + + + + + + + + + + + + + + + + + + + catkin + message_generation + message_runtime + roscpp + rospy + geometry_msgs + nav_msgs + std_msgs + sensor_msgs + tf + message_generation + message_generation + message_runtime + roscpp + rospy + geometry_msgs + nav_msgs + std_msgs + sensor_msgs + tf + message_runtime + roscpp + rospy + std_msgs + + + + + + + diff --git a/package_ros2.xml b/package_ros2.xml new file mode 100755 index 0000000..1d6abba --- /dev/null +++ b/package_ros2.xml @@ -0,0 +1,46 @@ + + + sick_lidar_localization + 0.0.0 + sick_lidar_localization is an open-source project to support the LiDAR-LOC software of the company SICK using the ROS-framework + + + rostest + + + + + Apache + + + + https://github.com/michael1309/sick_lidar_localization + + + + Michael Lehning + + + + rosidl_default_generators + ament_cmake + + geometry_msgs + nav_msgs + sensor_msgs + std_msgs + tf2 + tf2_ros + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + rosidl_interface_packages + + diff --git a/src/client_socket.cpp b/src/client_socket.cpp index 003e4c8..431641b 100644 --- a/src/client_socket.cpp +++ b/src/client_socket.cpp @@ -53,7 +53,7 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/client_socket.h" diff --git a/src/cola_configuration.cpp b/src/cola_configuration.cpp index 20fa59a..5c26f8d 100644 --- a/src/cola_configuration.cpp +++ b/src/cola_configuration.cpp @@ -56,15 +56,17 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/cola_configuration.h" /*! * Constructor + * @param[in] nh ros node handle + * @param[in] cola_services cola service callbacks (converts requests to cola telegrams, sends the cola telegrams and decodes the response from localization server) */ -sick_lidar_localization::ColaConfiguration::ColaConfiguration(ros::NodeHandle* nh) -: m_nh(nh), m_configuration_thread_running(false), m_configuration_thread(0) +sick_lidar_localization::ColaConfiguration::ColaConfiguration(ROS::NodePtr nh, sick_lidar_localization::ColaServices* cola_services) +: m_nh(nh), m_cola_services(cola_services), m_configuration_thread_running(false), m_configuration_thread(0) { } @@ -124,69 +126,87 @@ void sick_lidar_localization::ColaConfiguration::runConfigurationThreadCb(void) {"SickLocRequestResultData", -1} // "LocRequestResultData" value="0": If in poll mode, trigger sending the localization result of the next processed scan via TCP interface (default: 0) }; double retry_delay = 1.0; - ros::param::param("/sick_lidar_localization/driver/tcp_connection_retry_delay", retry_delay, retry_delay); + ROS::param(m_nh, "/sick_lidar_localization/driver/tcp_connection_retry_delay", retry_delay, retry_delay); ROS_INFO_STREAM("ColaConfiguration: configuration thread started"); - if(ros::ok() && m_nh && m_configuration_thread_running) + if(ROS::ok() && m_nh && m_configuration_thread_running) { for(std::map::iterator iter_config = sim_configuration_map.begin(); iter_config != sim_configuration_map.end(); iter_config++) { - ros::param::param(std::string("/cola_service_node/") + iter_config->first, iter_config->second, iter_config->second); + ROS::param(m_nh, std::string("/cola_service_node/") + iter_config->first, iter_config->second, iter_config->second); if(iter_config->second >= 0) ROS_INFO_STREAM("ColaConfiguration: \"" << iter_config->first << "\": " << iter_config->second); } } // Transmit SickLocSetResultPort setting to localization - if(sim_configuration_map["SickLocSetResultPort"] >= 0 && ros::ok() && m_nh && m_configuration_thread_running) + while(sim_configuration_map["SickLocSetResultPort"] >= 0 && ROS::ok() && m_nh && m_cola_services && m_configuration_thread_running) { - ros::ServiceClient service_client = m_nh->serviceClient("SickLocSetResultPort"); - sick_lidar_localization::SickLocSetResultPortSrv service_telegram; - service_telegram.request.port = sim_configuration_map["SickLocSetResultPort"]; - callService(service_telegram, service_client, retry_delay); + sick_lidar_localization::SickLocSetResultPortSrv::Request service_request; + sick_lidar_localization::SickLocSetResultPortSrv::Response service_response; + service_request.port = sim_configuration_map["SickLocSetResultPort"]; + if(m_cola_services->serviceCbLocSetResultPort(service_request, service_response) && service_response.success) + break; // service request successfull + ROS_WARN_STREAM("## ERROR ColaConfiguration: service call type " << typeid(service_request).name() << " failed, retrying"); + ROS::sleep(retry_delay); } // Transmit SickLocSetResultMode setting to localization - if(sim_configuration_map["SickLocSetResultMode"] >= 0 && ros::ok() && m_nh && m_configuration_thread_running) + while(sim_configuration_map["SickLocSetResultMode"] >= 0 && ROS::ok() && m_nh && m_cola_services && m_configuration_thread_running) { - ros::ServiceClient service_client = m_nh->serviceClient("SickLocSetResultMode"); - sick_lidar_localization::SickLocSetResultModeSrv service_telegram; - service_telegram.request.mode = sim_configuration_map["SickLocSetResultMode"]; - callService(service_telegram, service_client, retry_delay); + sick_lidar_localization::SickLocSetResultModeSrv::Request service_request; + sick_lidar_localization::SickLocSetResultModeSrv::Response service_response; + service_request.mode = sim_configuration_map["SickLocSetResultMode"]; + if(m_cola_services->serviceCbLocSetResultMode(service_request, service_response) && service_response.success) + break; // service request successfull + ROS_WARN_STREAM("## ERROR ColaConfiguration: service call type " << typeid(service_request).name() << " failed, retrying"); + ROS::sleep(retry_delay); } // Transmit SickLocSetResultPoseEnabled setting to localization - if(sim_configuration_map["SickLocSetResultPoseEnabled"] >= 0 && ros::ok() && m_nh && m_configuration_thread_running) + while(sim_configuration_map["SickLocSetResultPoseEnabled"] >= 0 && ROS::ok() && m_nh && m_cola_services && m_configuration_thread_running) { - ros::ServiceClient service_client = m_nh->serviceClient("SickLocSetResultPoseEnabled"); - sick_lidar_localization::SickLocSetResultPoseEnabledSrv service_telegram; - service_telegram.request.enabled = sim_configuration_map["SickLocSetResultPoseEnabled"]; - callService(service_telegram, service_client, retry_delay); + sick_lidar_localization::SickLocSetResultPoseEnabledSrv::Request service_request; + sick_lidar_localization::SickLocSetResultPoseEnabledSrv::Response service_response; + service_request.enabled = sim_configuration_map["SickLocSetResultPoseEnabled"]; + if(m_cola_services->serviceCbLocSetResultPoseEnabled(service_request, service_response) && service_response.success) + break; // service request successfull + ROS_WARN_STREAM("## ERROR ColaConfiguration: service call type " << typeid(service_request).name() << " failed, retrying"); + ROS::sleep(retry_delay); } // Transmit SickLocSetResultEndianness setting to localization - if(sim_configuration_map["SickLocSetResultEndianness"] >= 0 && ros::ok() && m_nh && m_configuration_thread_running) + while(sim_configuration_map["SickLocSetResultEndianness"] >= 0 && ROS::ok() && m_nh && m_cola_services && m_configuration_thread_running) { - ros::ServiceClient service_client = m_nh->serviceClient("SickLocSetResultEndianness"); - sick_lidar_localization::SickLocSetResultEndiannessSrv service_telegram; - service_telegram.request.endianness = sim_configuration_map["SickLocSetResultEndianness"]; - callService(service_telegram, service_client, retry_delay); + sick_lidar_localization::SickLocSetResultEndiannessSrv::Request service_request; + sick_lidar_localization::SickLocSetResultEndiannessSrv::Response service_response; + service_request.endianness = sim_configuration_map["SickLocSetResultEndianness"]; + if(m_cola_services->serviceCbLocSetResultEndianness(service_request, service_response) && service_response.success) + break; // service request successfull + ROS_WARN_STREAM("## ERROR ColaConfiguration: service call type " << typeid(service_request).name() << " failed, retrying"); + ROS::sleep(retry_delay); } // Transmit SickLocSetResultEndianness setting to localization - if(sim_configuration_map["SickLocSetResultPoseInterval"] >= 0 && ros::ok() && m_nh && m_configuration_thread_running) + while(sim_configuration_map["SickLocSetResultPoseInterval"] >= 0 && ROS::ok() && m_nh && m_cola_services && m_configuration_thread_running) { - ros::ServiceClient service_client = m_nh->serviceClient("SickLocSetResultPoseInterval"); - sick_lidar_localization::SickLocSetResultPoseIntervalSrv service_telegram; - service_telegram.request.interval = sim_configuration_map["SickLocSetResultPoseInterval"]; - callService(service_telegram, service_client, retry_delay); + sick_lidar_localization::SickLocSetResultPoseIntervalSrv::Request service_request; + sick_lidar_localization::SickLocSetResultPoseIntervalSrv::Response service_response; + service_request.interval = sim_configuration_map["SickLocSetResultPoseInterval"]; + if(m_cola_services->serviceCbLocSetResultPoseInterval(service_request, service_response) && service_response.success) + break; // service request successfull + ROS_WARN_STREAM("## ERROR ColaConfiguration: service call type " << typeid(service_request).name() << " failed, retrying"); + ROS::sleep(retry_delay); } // Transmit SickLocSetResultEndianness setting to localization - if(sim_configuration_map["SickLocRequestResultData"] > 0 && ros::ok() && m_nh && m_configuration_thread_running) + while(sim_configuration_map["SickLocRequestResultData"] > 0 && ROS::ok() && m_nh && m_cola_services && m_configuration_thread_running) { - ros::ServiceClient service_client = m_nh->serviceClient("SickLocRequestResultData"); - sick_lidar_localization::SickLocRequestResultDataSrv service_telegram; - callService(service_telegram, service_client, retry_delay); + sick_lidar_localization::SickLocRequestResultDataSrv::Request service_request; + sick_lidar_localization::SickLocRequestResultDataSrv::Response service_response; + if(m_cola_services->serviceCbLocRequestResultData(service_request, service_response) && service_response.success) + break; // service request successfull + ROS_WARN_STREAM("## ERROR ColaConfiguration: service call type " << typeid(service_request).name() << " failed, retrying"); + ROS::sleep(retry_delay); } m_configuration_thread_running = false; diff --git a/src/cola_converter.cpp b/src/cola_converter.cpp index 9f797cc..3db057d 100644 --- a/src/cola_converter.cpp +++ b/src/cola_converter.cpp @@ -56,7 +56,7 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include #include @@ -198,7 +198,7 @@ std::vector sick_lidar_localization::ColaAsciiBinaryConverter::ColaAsci uint8_t val = static_cast(std::stoul(parameter.substr(m - 1, 2), 0, 16) & 0xFF); binary_payload.push_back(val); } - catch (const std::exception exc) + catch (const std::exception & exc) { ROS_ERROR_STREAM("## ERROR in ColaAsciiToColaBinary(): can't convert parameter value " << parameter.substr(m - 1, 2) << " to hex, exception " << exc.what()); return cola_binary; diff --git a/src/cola_encoder.cpp b/src/cola_encoder.cpp new file mode 100755 index 0000000..a1f8988 --- /dev/null +++ b/src/cola_encoder.cpp @@ -0,0 +1,1209 @@ +/* + * @brief cola_encoder encodes service requests to cola telegrams, parses cola responses and + * converts them to service responses. + * + * 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 "sick_lidar_localization/ros_wrapper.h" +#include "sick_lidar_localization/cola_encoder.h" + +/*! + * Converts the service request for service SickDevSetLidarConfigSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevSetLidarConfigSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickDevSetLidarConfigSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN DevSetLidarConfig"; + cola_ascii << std::showpos << " +" << service_request.index << " +" << service_request.minrange << " +" << service_request.maxrange << " " << service_request.minangle << " " << service_request.maxangle + << " " << service_request.x << " " << service_request.y << " " << service_request.yaw + << std::noshowpos << " " << (service_request.upsidedown?1:0) + << " +" << service_request.ip.length() << " " << service_request.ip << " +" << service_request.port + << " " << service_request.interfacetype << " " << service_request.maplayer << " " << (service_request.active?1:0); + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickDevSetLidarConfigSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevSetLidarConfigSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevSetLidarConfigSrv::Response& service_response) +{ + service_response.set = false; + service_response.executed = false; + if(cola_response.parameter.size() == 2) + { + service_response.set = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + service_response.executed = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[1], false); + } + return service_response.set; +} + +/*! + * Converts the service request for service SickDevGetLidarConfigSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevGetLidarConfigSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickDevGetLidarConfigSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN DevGetLidarConfig"; + cola_ascii << std::showpos << " " << service_request.scannerindex; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickDevGetLidarConfigSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevGetLidarConfigSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevGetLidarConfigSrv::Response& service_response) +{ + if(cola_response.parameter.size() == 13) + { + service_response.minrange = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0U); + service_response.maxrange = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[1], -1, 0U); + service_response.minangle = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[2], -1, 0); + service_response.maxangle = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[3], -1, 0); + service_response.x = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[4], -1, 0); + service_response.y = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[5], -1, 0); + service_response.yaw = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[6], -1, 0); + service_response.upsidedown = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[7], false); + service_response.ip = cola_response.parameter[8]; + service_response.port = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[9], -1, 0U); + service_response.interfacetype = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[10], -1, 0U); + service_response.maplayer = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[11], -1, 0U); + service_response.active = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[12], false); + return true; + } + if(cola_response.parameter.size() >= 14) + { + service_response.minrange = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0U); + service_response.maxrange = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[1], -1, 0U); + service_response.minangle = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[2], -1, 0); + service_response.maxangle = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[3], -1, 0); + service_response.x = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[4], -1, 0); + service_response.y = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[5], -1, 0); + service_response.yaw = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[6], -1, 0); + service_response.upsidedown = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[7], false); + int n = 8; + int ip_strlen = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[n++], 10, 0U); + service_response.ip = cola_response.parameter[n++]; + while (n + 4 < cola_response.parameter.size() && service_response.ip.length() < ip_strlen) + { + service_response.ip = service_response.ip + " " + cola_response.parameter[n++]; + } + service_response.port = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[n++], -1, 0U); + service_response.interfacetype = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[n++], -1, 0U); + service_response.maplayer = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[n++], -1, 0U); + service_response.active = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[n++], false); + return true; + } + return false; +} + +/*! + * Converts the service request for service SickLocSetMapSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetMapSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocSetMapSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocSetMap +" << service_request.mapfilename.length() << " " << service_request.mapfilename; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetMapSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetMapSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetMapSrv::Response& service_response) +{ + service_response.set = false; + service_response.executed = false; + if(cola_response.parameter.size() == 2) + { + service_response.set = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + service_response.executed = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[1], false); + } + return service_response.set; +} + +/*! + * Converts the service request for service SickLocMapSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocMapSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocMapSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocMap"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocMapSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocMapSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocMapSrv::Response& service_response) +{ + service_response.success = false; + if(cola_response.parameter.size() == 1) + { + service_response.map = cola_response.parameter[0]; + service_response.success = true; + } + if(cola_response.parameter.size() > 1) + { + int str_len = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0U); + service_response.map = cola_response.parameter[1]; + for (int n = 2; n < cola_response.parameter.size() && service_response.map.length() < str_len; n++) + { + service_response.map = service_response.map + " " + cola_response.parameter[n]; + } + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocMapStateSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocMapStateSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocMapStateSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocMapState"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocMapStateSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocMapStateSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocMapStateSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.mapstate = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocInitializePoseSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocInitializePoseSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocInitializePoseSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocInitializePose"; + cola_ascii << std::showpos << " " << service_request.x << " " << service_request.y << " " << service_request.yaw << " +" << service_request.sigmatranslation; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocInitializePoseSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocInitializePoseSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocInitializePoseSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocInitialPoseSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocInitialPoseSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocInitialPoseSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocInitialPose"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocInitialPoseSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocInitialPoseSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocInitialPoseSrv::Response& service_response) +{ + service_response.success = false; + if(cola_response.parameter.size() >= 4) + { + service_response.x = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0); + service_response.y = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[1], -1, 0); + service_response.yaw = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[2], -1, 0); + service_response.sigmatranslation = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[3], -1, 0U); + service_response.success = true; + } + return service_response.success; +} + + +/*! + * Converts the service request for service SickLocSetReflectorsForSupportActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetReflectorsForSupportActiveSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocSetReflectorsForSupportActive"; + cola_ascii << std::noshowpos << " " << (service_request.active?1:0); + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetReflectorsForSupportActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetReflectorsForSupportActiveSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocReflectorsForSupportActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocReflectorsForSupportActiveSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocReflectorsForSupportActiveSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocReflectorsForSupportActive"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocReflectorsForSupportActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocReflectorsForSupportActiveSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocReflectorsForSupportActiveSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.active = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocSetOdometryActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetOdometryActiveSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocSetOdometryActiveSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocSetOdometryActive"; + cola_ascii << std::noshowpos << " " << (service_request.active?1:0); + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetOdometryActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetOdometryActiveSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetOdometryActiveSrv::Response& service_response) +{ + service_response.set = false; + service_response.executed = false; + if(!cola_response.parameter.empty()) + { + service_response.set = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + service_response.executed = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[1], false); + } + return service_response.set; +} + +/*! + * Converts the service request for service SickLocOdometryActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocOdometryActiveSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocOdometryActiveSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocOdometryActive"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocOdometryActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocOdometryActiveSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocOdometryActiveSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.active = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocSetOdometryPortSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetOdometryPortSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocSetOdometryPortSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocSetOdometryPort"; + cola_ascii << " +" << service_request.port; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetOdometryPortSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetOdometryPortSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetOdometryPortSrv::Response& service_response) +{ + service_response.set = false; + if(!cola_response.parameter.empty()) + { + service_response.set = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + service_response.executed = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[1], false); + } + return service_response.set; +} + +/*! + * Converts the service request for service SickLocOdometryPortSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocOdometryPortSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocOdometryPortSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocOdometryPort"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocOdometryPortSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocOdometryPortSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocOdometryPortSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.port = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocSetOdometryRestrictYMotionSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetOdometryRestrictYMotionSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocSetOdometryRestrictYMotion"; + cola_ascii << std::noshowpos << " " << (service_request.active?1:0); + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetOdometryRestrictYMotionSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetOdometryRestrictYMotionSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocOdometryRestrictYMotionSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocOdometryRestrictYMotionSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocOdometryRestrictYMotionSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocOdometryRestrictYMotion"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocOdometryRestrictYMotionSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocOdometryRestrictYMotionSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocOdometryRestrictYMotionSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.active = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocSetAutoStartActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetAutoStartActiveSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocSetAutoStartActiveSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocSetAutoStartActive"; + cola_ascii << std::noshowpos << " " << (service_request.active?1:0); + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetAutoStartActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetAutoStartActiveSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetAutoStartActiveSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocAutoStartActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocAutoStartActiveSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocAutoStartActiveSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocAutoStartActive"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocAutoStartActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocAutoStartActiveSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocAutoStartActiveSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.active = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocSetAutoStartSavePoseIntervalSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetAutoStartSavePoseIntervalSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocSetAutoStartSavePoseInterval"; + cola_ascii << std::showpos << " " << service_request.interval; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetAutoStartSavePoseIntervalSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetAutoStartSavePoseIntervalSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocAutoStartSavePoseIntervalSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocAutoStartSavePoseIntervalSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocAutoStartSavePoseIntervalSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocAutoStartSavePoseInterval"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocAutoStartSavePoseIntervalSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocAutoStartSavePoseIntervalSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocAutoStartSavePoseIntervalSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.interval = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocSetRingBufferRecordingActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSetRingBufferRecordingActiveSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocSetRingBufferRecordingActive"; + cola_ascii << std::noshowpos << " " << (service_request.active?1:0); + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocSetRingBufferRecordingActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSetRingBufferRecordingActiveSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocRingBufferRecordingActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocRingBufferRecordingActiveSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocRingBufferRecordingActiveSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocRingBufferRecordingActive"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocRingBufferRecordingActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocRingBufferRecordingActiveSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocRingBufferRecordingActiveSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.active = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickDevGetLidarIdentSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevGetLidarIdentSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickDevGetLidarIdentSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN DevGetLidarIdent"; + cola_ascii << std::showpos << " " << service_request.index; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickDevGetLidarIdentSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevGetLidarIdentSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevGetLidarIdentSrv::Response& service_response) +{ + service_response.success = false; + if(cola_response.parameter.size() == 1) + { + service_response.scannerident = cola_response.parameter[0]; + service_response.success = true; + } + if(cola_response.parameter.size() > 1) + { + int str_len = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0U); + service_response.scannerident = cola_response.parameter[1]; + for (int n = 2; n < cola_response.parameter.size() && service_response.scannerident.length() < str_len; n++) + { + service_response.scannerident = service_response.scannerident + " " + cola_response.parameter[n]; + } + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickDevGetLidarStateSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevGetLidarStateSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickDevGetLidarStateSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN DevGetLidarState"; + cola_ascii << std::showpos << " " << service_request.index; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickDevGetLidarStateSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevGetLidarStateSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevGetLidarStateSrv::Response& service_response) +{ + service_response.success = false; + if(cola_response.parameter.size() >= 3) + { + service_response.devicestatus = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0); + service_response.deviceconnected = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[1], -1, 0); + service_response.receivingdata = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[2], -1, 0); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickGetSoftwareVersionSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickGetSoftwareVersionSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickGetSoftwareVersionSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN GetSoftwareVersion"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickGetSoftwareVersionSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickGetSoftwareVersionSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickGetSoftwareVersionSrv::Response& service_response) +{ + service_response.success = false; + if(cola_response.parameter.size() == 1) + { + service_response.version = cola_response.parameter[0]; + service_response.success = true; + } + if(cola_response.parameter.size() > 1) + { + int str_len = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0U); + service_response.version = cola_response.parameter[1]; + for (int n = 2; n < cola_response.parameter.size() && service_response.version.length() < str_len; n++) + { + service_response.version = service_response.version + " " + cola_response.parameter[n]; + } + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocAutoStartSavePoseSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocAutoStartSavePoseSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocAutoStartSavePoseSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocAutoStartSavePose"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocAutoStartSavePoseSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocAutoStartSavePoseSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocAutoStartSavePoseSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocForceUpdateSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocForceUpdateSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocForceUpdateSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocForceUpdate"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocForceUpdateSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocForceUpdateSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocForceUpdateSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocSaveRingBufferRecordingSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocSaveRingBufferRecordingSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocSaveRingBufferRecordingSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocSaveRingBufferRecording"; + cola_ascii << " +" << service_request.reason.length() << " " << service_request.reason; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocSaveRingBufferRecordingSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocSaveRingBufferRecordingSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocSaveRingBufferRecordingSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocStartDemoMappingSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocStartDemoMappingSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocStartDemoMappingSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN LocStartDemoMapping"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocStartDemoMappingSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocStartDemoMappingSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocStartDemoMappingSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickReportUserMessageSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickReportUserMessageSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickReportUserMessageSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN ReportUserMessage"; + cola_ascii << " +" << service_request.usermessage.length() << " " << service_request.usermessage; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickReportUserMessageSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickReportUserMessageSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickReportUserMessageSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickSavePermanentSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickSavePermanentSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickSavePermanentSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN SavePermanent"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickSavePermanentSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickSavePermanentSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickSavePermanentSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocResultPortSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocResultPortSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocResultPortSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocResultPort"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocResultPortSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocResultPortSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocResultPortSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.port = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocResultModeSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocResultModeSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocResultModeSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocResultMode"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocResultModeSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocResultModeSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocResultModeSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.mode = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocResultEndiannessSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocResultEndiannessSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocResultEndiannessSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocResultEndianness"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocResultEndiannessSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocResultEndiannessSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocResultEndiannessSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.endianness = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocResultStateSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocResultStateSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocResultStateSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocResultState"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocResultStateSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocResultStateSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocResultStateSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.state = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickLocResultPoseIntervalSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickLocResultPoseIntervalSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickLocResultPoseIntervalSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN LocResultPoseInterval"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickLocResultPoseIntervalSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickLocResultPoseIntervalSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickLocResultPoseIntervalSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.interval = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], -1, 0); + service_response.success = true; + } + return service_response.success; +} + +/*! + * Converts the service request for service SickDevSetIMUActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevSetIMUActiveSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickDevSetIMUActiveSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sMN DevSetIMUActive"; + cola_ascii << std::noshowpos << " " << (service_request.active?1:0); + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickDevSetIMUActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevSetIMUActiveSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevSetIMUActiveSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + } + return service_response.success; +} + +/*! + * Converts the service request for service SickDevIMUActiveSrv into a cola ascii telegram + * @param[in] service_request ros request for service SickDevIMUActiveSrv + * @return cola ascii telegram + */ +std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::SickDevIMUActiveSrv::Request& service_request) +{ + std::stringstream cola_ascii; + cola_ascii << "sRN DevIMUActive"; + return cola_ascii.str(); +} + +/*! + * Parses a cola response and converts the arguments to a service response for service SickDevIMUActiveSrv + * @param[in] cola_response cola ascii telegram (response from localization server) + * @param[out] service_response converted response for service SickDevIMUActiveSrv + * @return true on succes or false in case of parse errors + */ +bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::SickDevIMUActiveSrv::Response& service_response) +{ + service_response.success = false; + if(!cola_response.parameter.empty()) + { + service_response.active = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); + service_response.success = true; + } + return service_response.success; +} diff --git a/src/cola_parser.cpp b/src/cola_parser.cpp index 910ceaa..46205d7 100644 --- a/src/cola_parser.cpp +++ b/src/cola_parser.cpp @@ -58,7 +58,7 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include #include @@ -101,7 +101,7 @@ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::ColaPar const std::string & command_name, const std::vector & parameter) { sick_lidar_localization::SickLocColaTelegramMsg cola_telegram; - cola_telegram.header.stamp = ros::Time::now(); + cola_telegram.header.stamp = ROS::now(); cola_telegram.command_type = command_type; cola_telegram.command_name = command_name; cola_telegram.parameter = parameter; @@ -229,3 +229,57 @@ sick_lidar_localization::ColaParser::COLA_SOPAS_COMMAND sick_lidar_localization: return command_type; } +/*! + * Converts and returns the parameter of a cola ascii telegram into a numeric value. + * @param[in] cola_arg parameter of a cola ascii telegram + * @param[in] base numeric base (10 for decimal values, 16 for hex strings or -1 for autodetection with base 10 in case of +/-sign, otherwise hex) + * @param[in] default_value default value returned in case of parse errors + * @return parameter converted to integer value + */ +int32_t sick_lidar_localization::ColaParser::convertColaArg(const std::string & cola_arg, int base, int32_t default_value) +{ + try + { + if(base < 0) + base = ((cola_arg.find_first_of("+-") != std::string::npos) ? 10 : 16); // base 10 if +/-sign in cola_arg, otherwise hex + return std::stoi(cola_arg, 0, base); + } + catch(const std::exception & exc) + { + ROS_WARN_STREAM("## ERROR ColaParser::convertColaArg(" << cola_arg << ") failed, exception " << exc.what()); + } + return default_value; +} + +/*! + * Converts and returns the parameter of a cola ascii telegram into a numeric value. + * @param[in] cola_arg parameter of a cola ascii telegram + * @param[in] base numeric base (10 for decimal values, 16 for hex strings or -1 for autodetection with base 10 in case of +/-sign, otherwise hex) + * @param[in] default_value default value returned in case of parse errors + * @return parameter converted to integer value + */ +uint32_t sick_lidar_localization::ColaParser::convertColaArg(const std::string & cola_arg, int base, uint32_t default_value) +{ + try + { + if(base < 0) + base = ((cola_arg.find_first_of("+-") != std::string::npos) ? 10 : 16); // base 10 if +/-sign in cola_arg, otherwise hex + return (uint32_t)std::stoul(cola_arg, 0, base); + } + catch(const std::exception & exc) + { + ROS_WARN_STREAM("## ERROR ColaParser::convertColaArg(" << cola_arg << ") failed, exception " << exc.what()); + } + return default_value; +} + +/*! + * Converts and returns the parameter of a cola ascii response into a boolean value. + * @param[in] cola_response_arg parameter of a cola ascii response + * @param[in] default_value default value returned in case of parse errors + * @return parameter converted to boolean value + */ +bool sick_lidar_localization::ColaParser::convertColaResponseBool(const std::string & cola_response_arg, bool default_value) +{ + return ((sick_lidar_localization::ColaParser::convertColaArg(cola_response_arg, 10, (default_value ? 1 : 0)) > 0) ? true : false); +} diff --git a/src/cola_service_node.cpp b/src/cola_service_node.cpp deleted file mode 100644 index 4c598ce..0000000 --- a/src/cola_service_node.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/* - * @brief cola_service_node advertises and runs the following ros services for sick_lidar_localization: - * - * ROS service name | Definition file | Cola request telegram | Description - * "SickLocIsSystemReady" | srv/SickLocIsSystemReadySrv.srv | "sMN IsSystemReady" | Check if the system is ready - * "SickLocState" | srv/SickLocStateSrv.srv | "sRN LocState" | Read localization state - * "SickLocStartLocalizing" | srv/SickLocStartLocalizingSrv.srv | "sMN LocStartLocalizing" | Start localization - * "SickLocStop" | srv/SickLocStopSrv.srv | "sMN LocStop" | Stop localization or demo mapping - * "SickLocStopAndSave" | srv/SickLocStopAndSaveSrv.srv | "sMN LocStopAndSave" | Stop localization, save settings - * "SickLocSetResultPort" | srv/SickLocSetResultPortSrv.srv | "sMN LocSetResultPort " | Set TCP-port for result output (default: 2201) - * "SickLocSetResultMode" | srv/SickLocSetResultModeSrv.srv | "sMN LocSetResultMode " | Set mode of result output (default: stream) - * "SickLocSetResultPoseEnabled" | srv/SickLocSetResultPoseEnabledSrv.srv | "sMN LocSetResultPoseEnabled " | Disable/enable result output - * "SickLocSetResultEndianness" | srv/SickLocSetResultEndiannessSrv.srv | "sMN LocSetResultEndianness " | Set endianness of result output - * "SickLocSetResultPoseInterval" | srv/SickLocSetResultPoseIntervalSrv.srv | "sMN LocSetResultPoseInterval " | Set interval of result output - * "SickLocRequestResultData" | srv/SickLocRequestResultDataSrv.srv | "sMN LocRequestResultData" | If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. - * "SickLocSetPose" | srv/SickLocSetPoseSrv.srv | "sMN LocSetPose " | Initialize vehicle pose - * - * See Telegram-Listing-v1.1.0.241R.pdf for further details about Cola telegrams. - * - * 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/cola_configuration.h" -#include "sick_lidar_localization/cola_services.h" - -int main(int argc, char** argv) -{ - // Ros configuration and initialization - ros::init(argc, argv, "cola_service_node"); - ros::NodeHandle nh; - ROS_INFO_STREAM("cola_service_node started."); - - // Initialize cola services - sick_lidar_localization::ColaServices cola_services(&nh); - - // Initial configuration from launch file - sick_lidar_localization::ColaConfiguration cola_configuration(&nh); - cola_configuration.start(); - - // Run ros event loop - ros::spin(); - - // Cleanup and exit - std::cout << "cola_service_node finished." << std::endl; - ROS_INFO_STREAM("cola_service_node finished."); - return EXIT_SUCCESS; -} diff --git a/src/cola_services.cpp b/src/cola_services.cpp index 7f8c5a0..7ab9de2 100644 --- a/src/cola_services.cpp +++ b/src/cola_services.cpp @@ -6,7 +6,6 @@ * "SickLocState" | srv/SickLocStateSrv.srv | "sRN LocState" | Read localization state * "SickLocStartLocalizing" | srv/SickLocStartLocalizingSrv.srv | "sMN LocStartLocalizing" | Start localization * "SickLocStop" | srv/SickLocStopSrv.srv | "sMN LocStop" | Stop localization or demo mapping - * "SickLocStopAndSave" | srv/SickLocStopAndSaveSrv.srv | "sMN LocStopAndSave" | Stop localization, save settings * "SickLocSetResultPort" | srv/SickLocSetResultPortSrv.srv | "sMN LocSetResultPort " | Set TCP-port for result output (default: 2201) * "SickLocSetResultMode" | srv/SickLocSetResultModeSrv.srv | "sMN LocSetResultMode " | Set mode of result output (default: stream) * "SickLocSetResultPoseEnabled" | srv/SickLocSetResultPoseEnabledSrv.srv | "sMN LocSetResultPoseEnabled " | Disable/enable result output @@ -68,33 +67,125 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include - +#include "sick_lidar_localization/ros_wrapper.h" +#include "sick_lidar_localization/cola_encoder.h" #include "sick_lidar_localization/cola_services.h" /*! * Constructor */ -sick_lidar_localization::ColaServices::ColaServices(ros::NodeHandle *nh) : m_cola_response_timeout(1.0) +sick_lidar_localization::ColaServices::ColaServices(ROS::NodePtr nh, sick_lidar_localization::DriverMonitor* driver_monitor) : m_nh(nh), m_driver_monitor(driver_monitor), m_cola_response_timeout(1.0) { if(nh) { - ros::param::param("/sick_lidar_localization/time_sync/cola_response_timeout", m_cola_response_timeout, m_cola_response_timeout); - // Advertise ros services - m_service_server.push_back(nh->advertiseService("SickLocIsSystemReady", &sick_lidar_localization::ColaServices::serviceCbIsSystemReady, this)); - m_service_server.push_back(nh->advertiseService("SickLocState", &sick_lidar_localization::ColaServices::serviceCbLocState, this)); - m_service_server.push_back(nh->advertiseService("SickLocStartLocalizing", &sick_lidar_localization::ColaServices::serviceCbLocStartLocalizing, this)); - m_service_server.push_back(nh->advertiseService("SickLocStop", &sick_lidar_localization::ColaServices::serviceCbLocStop, this)); - m_service_server.push_back(nh->advertiseService("SickLocStopAndSave", &sick_lidar_localization::ColaServices::serviceCbLocStopAndSave, this)); - m_service_server.push_back(nh->advertiseService("SickLocSetResultPort", &sick_lidar_localization::ColaServices::serviceCbLocSetResultPort, this)); - m_service_server.push_back(nh->advertiseService("SickLocSetResultMode", &sick_lidar_localization::ColaServices::serviceCbLocSetResultMode, this)); - m_service_server.push_back(nh->advertiseService("SickLocSetResultPoseEnabled", &sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseEnabled, this)); - m_service_server.push_back(nh->advertiseService("SickLocSetResultEndianness", &sick_lidar_localization::ColaServices::serviceCbLocSetResultEndianness, this)); - m_service_server.push_back(nh->advertiseService("SickLocSetResultPoseInterval", &sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseInterval, this)); - m_service_server.push_back(nh->advertiseService("SickLocRequestResultData", &sick_lidar_localization::ColaServices::serviceCbLocRequestResultData, this)); - m_service_server.push_back(nh->advertiseService("SickLocSetPose", &sick_lidar_localization::ColaServices::serviceCbLocSetPose, this)); + ROS::param(nh, "/sick_lidar_localization/time_sync/cola_response_timeout", m_cola_response_timeout, m_cola_response_timeout); + + // Advertise ros services supported by release 3 and later +#if defined __ROS_VERSION && __ROS_VERSION == 1 + m_srv_server_01 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocIsSystemReadySrv, "SickLocIsSystemReady",&sick_lidar_localization::ColaServices::serviceCbIsSystemReady, this); + m_srv_server_02 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocStateSrv, "SickLocState",&sick_lidar_localization::ColaServices::serviceCbLocState, this); + m_srv_server_03 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocStartLocalizingSrv, "SickLocStartLocalizing",&sick_lidar_localization::ColaServices::serviceCbLocStartLocalizing, this); + m_srv_server_04 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocStopSrv, "SickLocStop",&sick_lidar_localization::ColaServices::serviceCbLocStop, this); + m_srv_server_06 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetResultPortSrv, "SickLocSetResultPort",&sick_lidar_localization::ColaServices::serviceCbLocSetResultPort, this); + m_srv_server_07 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetResultModeSrv, "SickLocSetResultMode",&sick_lidar_localization::ColaServices::serviceCbLocSetResultMode, this); + m_srv_server_08 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetResultPoseEnabledSrv, "SickLocSetResultPoseEnabled",&sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseEnabled, this); + m_srv_server_09 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetResultEndiannessSrv, "SickLocSetResultEndianness",&sick_lidar_localization::ColaServices::serviceCbLocSetResultEndianness, this); + m_srv_server_10 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetResultPoseIntervalSrv, "SickLocSetResultPoseInterval",&sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseInterval, this); + m_srv_server_11 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocRequestResultDataSrv, "SickLocRequestResultData",&sick_lidar_localization::ColaServices::serviceCbLocRequestResultData, this); + m_srv_server_12 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetPoseSrv, "SickLocSetPose",&sick_lidar_localization::ColaServices::serviceCbLocSetPose, this); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + m_srv_server_01 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocIsSystemReadySrv, "SickLocIsSystemReady",&sick_lidar_localization::ColaServices::serviceCbIsSystemReadyROS2, this); + m_srv_server_02 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocStateSrv, "SickLocState",&sick_lidar_localization::ColaServices::serviceCbLocStateROS2, this); + m_srv_server_03 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocStartLocalizingSrv, "SickLocStartLocalizing",&sick_lidar_localization::ColaServices::serviceCbLocStartLocalizingROS2, this); + m_srv_server_04 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocStopSrv, "SickLocStop",&sick_lidar_localization::ColaServices::serviceCbLocStopROS2, this); + m_srv_server_06 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetResultPortSrv, "SickLocSetResultPort",&sick_lidar_localization::ColaServices::serviceCbLocSetResultPortROS2, this); + m_srv_server_07 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetResultModeSrv, "SickLocSetResultMode",&sick_lidar_localization::ColaServices::serviceCbLocSetResultModeROS2, this); + m_srv_server_08 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetResultPoseEnabledSrv, "SickLocSetResultPoseEnabled",&sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseEnabledROS2, this); + m_srv_server_09 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetResultEndiannessSrv, "SickLocSetResultEndianness",&sick_lidar_localization::ColaServices::serviceCbLocSetResultEndiannessROS2, this); + m_srv_server_10 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetResultPoseIntervalSrv, "SickLocSetResultPoseInterval",&sick_lidar_localization::ColaServices::serviceCbLocSetResultPoseIntervalROS2, this); + m_srv_server_11 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocRequestResultDataSrv, "SickLocRequestResultData",&sick_lidar_localization::ColaServices::serviceCbLocRequestResultDataROS2, this); + m_srv_server_12 = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetPoseSrv, "SickLocSetPose",&sick_lidar_localization::ColaServices::serviceCbLocSetPoseROS2, this); +#endif + // Advertise ros services supported by release 4 and later +#if defined __ROS_VERSION && __ROS_VERSION == 1 + m_SickDevSetLidarConfigSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevSetLidarConfigSrv, "SickDevSetLidarConfig", &sick_lidar_localization::ColaServices::serviceCbDevSetLidarConfig, this); + m_SickDevGetLidarConfigSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevGetLidarConfigSrv, "SickDevGetLidarConfig", &sick_lidar_localization::ColaServices::serviceCbDevGetLidarConfig, this); + m_SickLocSetMapSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetMapSrv, "SickLocSetMap", &sick_lidar_localization::ColaServices::serviceCbLocSetMap, this); + m_SickLocMapSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocMapSrv, "SickLocMap", &sick_lidar_localization::ColaServices::serviceCbLocMap, this); + m_SickLocMapStateSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocMapStateSrv, "SickLocMapState", &sick_lidar_localization::ColaServices::serviceCbLocMapState, this); + m_SickLocInitializePoseSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocInitializePoseSrv, "SickLocInitializePose", &sick_lidar_localization::ColaServices::serviceCbLocInitializePose, this); + m_SickLocInitialPoseSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocInitialPoseSrv, "SickLocInitialPose", &sick_lidar_localization::ColaServices::serviceCbLocInitialPose, this); + m_SickLocSetReflectorsForSupportActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrv, "SickLocSetReflectorsForSupportActive", &sick_lidar_localization::ColaServices::serviceCbLocSetReflectorsForSupportActive, this); + m_SickLocReflectorsForSupportActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocReflectorsForSupportActiveSrv, "SickLocReflectorsForSupportActive", &sick_lidar_localization::ColaServices::serviceCbLocReflectorsForSupportActive, this); + m_SickLocSetOdometryActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetOdometryActiveSrv, "SickLocSetOdometryActive", &sick_lidar_localization::ColaServices::serviceCbLocSetOdometryActive, this); + m_SickLocOdometryActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocOdometryActiveSrv, "SickLocOdometryActive", &sick_lidar_localization::ColaServices::serviceCbLocOdometryActive, this); + m_SickLocSetOdometryPortSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetOdometryPortSrv, "SickLocSetOdometryPort", &sick_lidar_localization::ColaServices::serviceCbLocSetOdometryPort, this); + m_SickLocOdometryPortSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocOdometryPortSrv, "SickLocOdometryPort", &sick_lidar_localization::ColaServices::serviceCbLocOdometryPort, this); + m_SickLocSetOdometryRestrictYMotionSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrv, "SickLocSetOdometryRestrictYMotion", &sick_lidar_localization::ColaServices::serviceCbLocSetOdometryRestrictYMotion, this); + m_SickLocOdometryRestrictYMotionSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocOdometryRestrictYMotionSrv, "SickLocOdometryRestrictYMotion", &sick_lidar_localization::ColaServices::serviceCbLocOdometryRestrictYMotion, this); + m_SickLocSetAutoStartActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetAutoStartActiveSrv, "SickLocSetAutoStartActive", &sick_lidar_localization::ColaServices::serviceCbLocSetAutoStartActive, this); + m_SickLocAutoStartActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocAutoStartActiveSrv, "SickLocAutoStartActive", &sick_lidar_localization::ColaServices::serviceCbLocAutoStartActive, this); + m_SickLocSetAutoStartSavePoseIntervalSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrv, "SickLocSetAutoStartSavePoseInterval", &sick_lidar_localization::ColaServices::serviceCbLocSetAutoStartSavePoseInterval, this); + m_SickLocAutoStartSavePoseIntervalSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocAutoStartSavePoseIntervalSrv, "SickLocAutoStartSavePoseInterval", &sick_lidar_localization::ColaServices::serviceCbLocAutoStartSavePoseInterval, this); + m_SickLocSetRingBufferRecordingActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrv, "SickLocSetRingBufferRecordingActive", &sick_lidar_localization::ColaServices::serviceCbLocSetRingBufferRecordingActive, this); + m_SickLocRingBufferRecordingActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocRingBufferRecordingActiveSrv, "SickLocRingBufferRecordingActive", &sick_lidar_localization::ColaServices::serviceCbLocRingBufferRecordingActive, this); + m_SickDevGetLidarIdentSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevGetLidarIdentSrv, "SickDevGetLidarIdent", &sick_lidar_localization::ColaServices::serviceCbDevGetLidarIdent, this); + m_SickDevGetLidarStateSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevGetLidarStateSrv, "SickDevGetLidarState", &sick_lidar_localization::ColaServices::serviceCbDevGetLidarState, this); + m_SickGetSoftwareVersionSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickGetSoftwareVersionSrv, "SickGetSoftwareVersion", &sick_lidar_localization::ColaServices::serviceCbGetSoftwareVersion, this); + m_SickLocAutoStartSavePoseSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocAutoStartSavePoseSrv, "SickLocAutoStartSavePose", &sick_lidar_localization::ColaServices::serviceCbLocAutoStartSavePose, this); + m_SickLocForceUpdateSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocForceUpdateSrv, "SickLocForceUpdate", &sick_lidar_localization::ColaServices::serviceCbLocForceUpdate, this); + m_SickLocSaveRingBufferRecordingSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSaveRingBufferRecordingSrv, "SickLocSaveRingBufferRecording", &sick_lidar_localization::ColaServices::serviceCbLocSaveRingBufferRecording, this); + m_SickLocStartDemoMappingSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocStartDemoMappingSrv, "SickLocStartDemoMapping", &sick_lidar_localization::ColaServices::serviceCbLocStartDemoMapping, this); + m_SickReportUserMessageSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickReportUserMessageSrv, "SickReportUserMessage", &sick_lidar_localization::ColaServices::serviceCbReportUserMessage, this); + m_SickSavePermanentSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickSavePermanentSrv, "SickSavePermanent", &sick_lidar_localization::ColaServices::serviceCbSavePermanent, this); + m_SickLocResultPortSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocResultPortSrv, "SickLocResultPort", &sick_lidar_localization::ColaServices::serviceCbLocResultPort, this); + m_SickLocResultModeSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocResultModeSrv, "SickLocResultMode", &sick_lidar_localization::ColaServices::serviceCbLocResultMode, this); + m_SickLocResultEndiannessSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocResultEndiannessSrv, "SickLocResultEndianness", &sick_lidar_localization::ColaServices::serviceCbLocResultEndianness, this); + m_SickLocResultStateSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocResultStateSrv, "SickLocResultState", &sick_lidar_localization::ColaServices::serviceCbLocResultState, this); + m_SickLocResultPoseIntervalSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocResultPoseIntervalSrv, "SickLocResultPoseInterval", &sick_lidar_localization::ColaServices::serviceCbLocResultPoseInterval, this); + m_SickDevSetIMUActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevSetIMUActiveSrv, "SickDevSetIMUActive", &sick_lidar_localization::ColaServices::serviceCbDevSetIMUActive, this); + m_SickDevIMUActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevIMUActiveSrv, "SickDevIMUActive", &sick_lidar_localization::ColaServices::serviceCbDevIMUActive, this); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + m_SickDevSetLidarConfigSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevSetLidarConfigSrv, "SickDevSetLidarConfig", &sick_lidar_localization::ColaServices::serviceCbDevSetLidarConfigROS2, this); + m_SickDevGetLidarConfigSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevGetLidarConfigSrv, "SickDevGetLidarConfig", &sick_lidar_localization::ColaServices::serviceCbDevGetLidarConfigROS2, this); + m_SickLocSetMapSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetMapSrv, "SickLocSetMap", &sick_lidar_localization::ColaServices::serviceCbLocSetMapROS2, this); + m_SickLocMapSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocMapSrv, "SickLocMap", &sick_lidar_localization::ColaServices::serviceCbLocMapROS2, this); + m_SickLocMapStateSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocMapStateSrv, "SickLocMapState", &sick_lidar_localization::ColaServices::serviceCbLocMapStateROS2, this); + m_SickLocInitializePoseSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocInitializePoseSrv, "SickLocInitializePose", &sick_lidar_localization::ColaServices::serviceCbLocInitializePoseROS2, this); + m_SickLocInitialPoseSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocInitialPoseSrv, "SickLocInitialPose", &sick_lidar_localization::ColaServices::serviceCbLocInitialPoseROS2, this); + m_SickLocSetReflectorsForSupportActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrv, "SickLocSetReflectorsForSupportActive", &sick_lidar_localization::ColaServices::serviceCbLocSetReflectorsForSupportActiveROS2, this); + m_SickLocReflectorsForSupportActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocReflectorsForSupportActiveSrv, "SickLocReflectorsForSupportActive", &sick_lidar_localization::ColaServices::serviceCbLocReflectorsForSupportActiveROS2, this); + m_SickLocSetOdometryActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetOdometryActiveSrv, "SickLocSetOdometryActive", &sick_lidar_localization::ColaServices::serviceCbLocSetOdometryActiveROS2, this); + m_SickLocOdometryActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocOdometryActiveSrv, "SickLocOdometryActive", &sick_lidar_localization::ColaServices::serviceCbLocOdometryActiveROS2, this); + m_SickLocSetOdometryPortSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetOdometryPortSrv, "SickLocSetOdometryPort", &sick_lidar_localization::ColaServices::serviceCbLocSetOdometryPortROS2, this); + m_SickLocOdometryPortSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocOdometryPortSrv, "SickLocOdometryPort", &sick_lidar_localization::ColaServices::serviceCbLocOdometryPortROS2, this); + m_SickLocSetOdometryRestrictYMotionSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrv, "SickLocSetOdometryRestrictYMotion", &sick_lidar_localization::ColaServices::serviceCbLocSetOdometryRestrictYMotionROS2, this); + m_SickLocOdometryRestrictYMotionSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocOdometryRestrictYMotionSrv, "SickLocOdometryRestrictYMotion", &sick_lidar_localization::ColaServices::serviceCbLocOdometryRestrictYMotionROS2, this); + m_SickLocSetAutoStartActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetAutoStartActiveSrv, "SickLocSetAutoStartActive", &sick_lidar_localization::ColaServices::serviceCbLocSetAutoStartActiveROS2, this); + m_SickLocAutoStartActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocAutoStartActiveSrv, "SickLocAutoStartActive", &sick_lidar_localization::ColaServices::serviceCbLocAutoStartActiveROS2, this); + m_SickLocSetAutoStartSavePoseIntervalSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrv, "SickLocSetAutoStartSavePoseInterval", &sick_lidar_localization::ColaServices::serviceCbLocSetAutoStartSavePoseIntervalROS2, this); + m_SickLocAutoStartSavePoseIntervalSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocAutoStartSavePoseIntervalSrv, "SickLocAutoStartSavePoseInterval", &sick_lidar_localization::ColaServices::serviceCbLocAutoStartSavePoseIntervalROS2, this); + m_SickLocSetRingBufferRecordingActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrv, "SickLocSetRingBufferRecordingActive", &sick_lidar_localization::ColaServices::serviceCbLocSetRingBufferRecordingActiveROS2, this); + m_SickLocRingBufferRecordingActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocRingBufferRecordingActiveSrv, "SickLocRingBufferRecordingActive", &sick_lidar_localization::ColaServices::serviceCbLocRingBufferRecordingActiveROS2, this); + m_SickDevGetLidarIdentSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevGetLidarIdentSrv, "SickDevGetLidarIdent", &sick_lidar_localization::ColaServices::serviceCbDevGetLidarIdentROS2, this); + m_SickDevGetLidarStateSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevGetLidarStateSrv, "SickDevGetLidarState", &sick_lidar_localization::ColaServices::serviceCbDevGetLidarStateROS2, this); + m_SickGetSoftwareVersionSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickGetSoftwareVersionSrv, "SickGetSoftwareVersion", &sick_lidar_localization::ColaServices::serviceCbGetSoftwareVersionROS2, this); + m_SickLocAutoStartSavePoseSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocAutoStartSavePoseSrv, "SickLocAutoStartSavePose", &sick_lidar_localization::ColaServices::serviceCbLocAutoStartSavePoseROS2, this); + m_SickLocForceUpdateSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocForceUpdateSrv, "SickLocForceUpdate", &sick_lidar_localization::ColaServices::serviceCbLocForceUpdateROS2, this); + m_SickLocSaveRingBufferRecordingSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocSaveRingBufferRecordingSrv, "SickLocSaveRingBufferRecording", &sick_lidar_localization::ColaServices::serviceCbLocSaveRingBufferRecordingROS2, this); + m_SickLocStartDemoMappingSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocStartDemoMappingSrv, "SickLocStartDemoMapping", &sick_lidar_localization::ColaServices::serviceCbLocStartDemoMappingROS2, this); + m_SickReportUserMessageSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickReportUserMessageSrv, "SickReportUserMessage", &sick_lidar_localization::ColaServices::serviceCbReportUserMessageROS2, this); + m_SickSavePermanentSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickSavePermanentSrv, "SickSavePermanent", &sick_lidar_localization::ColaServices::serviceCbSavePermanentROS2, this); + m_SickLocResultPortSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocResultPortSrv, "SickLocResultPort", &sick_lidar_localization::ColaServices::serviceCbLocResultPortROS2, this); + m_SickLocResultModeSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocResultModeSrv, "SickLocResultMode", &sick_lidar_localization::ColaServices::serviceCbLocResultModeROS2, this); + m_SickLocResultEndiannessSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocResultEndiannessSrv, "SickLocResultEndianness", &sick_lidar_localization::ColaServices::serviceCbLocResultEndiannessROS2, this); + m_SickLocResultStateSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocResultStateSrv, "SickLocResultState", &sick_lidar_localization::ColaServices::serviceCbLocResultStateROS2, this); + m_SickLocResultPoseIntervalSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocResultPoseIntervalSrv, "SickLocResultPoseInterval", &sick_lidar_localization::ColaServices::serviceCbLocResultPoseIntervalROS2, this); + m_SickDevSetIMUActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevSetIMUActiveSrv, "SickDevSetIMUActive", &sick_lidar_localization::ColaServices::serviceCbDevSetIMUActiveROS2, this); + m_SickDevIMUActiveSrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickDevIMUActiveSrv, "SickDevIMUActive", &sick_lidar_localization::ColaServices::serviceCbDevIMUActiveROS2, this); +#endif // __ROS_VERSION // Clients for ros services "SickLocColaTelegram" - m_service_client = nh->serviceClient("SickLocColaTelegram"); + m_service_client = ROS_CREATE_SRV_CLIENT(nh, sick_lidar_localization::SickLocColaTelegramSrv, "SickLocColaTelegram"); } } @@ -113,62 +204,43 @@ sick_lidar_localization::ColaServices::~ColaServices() */ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::ColaServices::sendColaTelegram(const std::string & cola_ascii_request) { + boost::lock_guard service_cb_lockguard(m_service_cb_mutex); // one service request at a time +#if defined __ROS_VERSION && __ROS_VERSION == 1 sick_lidar_localization::SickLocColaTelegramSrv cola_telegram; - sick_lidar_localization::SickLocColaTelegramMsg cola_response; - cola_telegram.request.cola_ascii_request = cola_ascii_request; - cola_telegram.request.wait_response_timeout = m_cola_response_timeout; + sick_lidar_localization::SickLocColaTelegramSrv::Request* cola_telegram_request = &cola_telegram.request; + sick_lidar_localization::SickLocColaTelegramSrv::Response* cola_telegram_response = &cola_telegram.response; +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + std::shared_ptr cola_telegram_request = std::make_shared(); + std::shared_ptr cola_telegram_response = std::make_shared(); +#endif + cola_telegram_request->cola_ascii_request = cola_ascii_request; + cola_telegram_request->wait_response_timeout = m_cola_response_timeout; try { - // Send cola telegram using ros service "SickLocColaTelegram", receive response from localization server - if (!m_service_client.call(cola_telegram) || cola_telegram.response.cola_ascii_response.empty()) + ROS::Time start_request_timestamp = ROS::now(); + if(m_driver_monitor == 0) + { + ROS_ERROR_STREAM("## ERROR ColaServices::sendColaTelegram(): ColaServices not initialized (driver_monitor == 0, " << __FILE__ << ":" << __LINE__ << ")"); + return sick_lidar_localization::SickLocColaTelegramMsg(); + } + bool service_call_ok = m_driver_monitor->serviceCbColaTelegram(*cola_telegram_request, *cola_telegram_response); + if (!service_call_ok || cola_telegram_response->cola_ascii_response.empty()) { - ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(): calling ros service \"SickLocColaTelegram\" failed with request: " - << sick_lidar_localization::Utils::flattenToString(cola_telegram.request) << " response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram.response)); - return cola_response; + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(): calling ros service \"SickLocColaTelegram\" failed with request: \"" + << cola_telegram_request->cola_ascii_request << "\", response: \"" << cola_telegram_response->cola_ascii_response << "\"" << " after " + << ROS::seconds(ROS::now() - start_request_timestamp) << " sec (timeout: " << m_cola_response_timeout << " sec, " << __FILE__ << ":" << __LINE__ << ")"); + return sick_lidar_localization::SickLocColaTelegramMsg(); } - ROS_DEBUG_STREAM("ColaServices::sendColaTelegram(): request " << sick_lidar_localization::Utils::flattenToString(cola_telegram.request) - << " response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram.response) << " succesfull."); + ROS_DEBUG_STREAM("ColaServices::sendColaTelegram(): request \"" << cola_telegram_request->cola_ascii_request + << "\", response: \"" << cola_telegram_response->cola_ascii_response << "\" succesfull."); // Decode and return response - return sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram.response.cola_ascii_response); - } - catch(const std::exception & exc) - { - ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << sick_lidar_localization::Utils::flattenToString(cola_telegram.request) - << "): cola response " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", exception " << exc.what()); - } - return cola_response; -} - -/*! - * Converts and returns the parameter of a cola ascii telegram into a numeric value. - * @param[in] cola_arg parameter of a cola ascii telegram - * @param[in] base numeric base (10 for decimal values or 16 for hex strings) - * @param[in] default_value default value returned in case of parse errors - * @return parameter converted to integer value - */ -int32_t sick_lidar_localization::ColaServices::convertColaArg(const std::string & cola_arg, int base, int32_t default_value) -{ - try - { - return std::stoi(cola_arg, 0, base); + return sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram_response->cola_ascii_response); } catch(const std::exception & exc) { - ROS_WARN_STREAM("## ERROR ColaServices::convertColaArg(" << cola_arg << ") failed, exception " << exc.what()); + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_telegram_request->cola_ascii_request << ") failed, exception " << exc.what()); } - return default_value; - -} - -/*! - * Converts and returns the parameter of a cola ascii response into a boolean value. - * @param[in] cola_response_arg parameter of a cola ascii response - * @param[in] default_value default value returned in case of parse errors - * @return parameter converted to boolean value - */ -bool sick_lidar_localization::ColaServices::convertColaResponseBool(const std::string & cola_response_arg, bool default_value) -{ - return ((convertColaArg(cola_response_arg, 10, (default_value ? 1 : 0)) > 0) ? true : false); + return sick_lidar_localization::SickLocColaTelegramMsg(); } /*! @@ -203,7 +275,7 @@ bool sick_lidar_localization::ColaServices::serviceCbLocState( sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); if(cola_response.command_name == "LocState" && cola_response.parameter.size() == 1) { - service_response.state = convertColaArg(cola_response.parameter[0], 10, -1); + service_response.state = sick_lidar_localization::ColaParser::convertColaArg(cola_response.parameter[0], 10, -1); service_response.success = (service_response.state != -1); return true; } @@ -241,21 +313,6 @@ bool sick_lidar_localization::ColaServices::serviceCbLocStop( return serviceCbWithSuccessResponse("sMN", "LocStop", "", service_response); } -/*! - * Callback for service messages (SickLocStopAndSaveSrv, Stop localization, save settings). - * Sends a cola telegram "sMN LocStopAndSave" and receives the response from localization controller - * using ros service "SickLocColaTelegramSrv". - * @param[in] service_request ros service request to localization controller - * @param[out] service_response service response from localization controller - * @return true on success, false in case of errors. - */ -bool sick_lidar_localization::ColaServices::serviceCbLocStopAndSave( - sick_lidar_localization::SickLocStopAndSaveSrv::Request &service_request, - sick_lidar_localization::SickLocStopAndSaveSrv::Response &service_response) -{ - return serviceCbWithSuccessResponse("sMN", "LocStopAndSave", "", service_response); -} - /*! * Callback for service messages (SickLocSetResultPortSrv, Set TCP-port for result output, default: 2201). * Sends a cola telegram "Set mode of result output (default: stream" and receives the response from localization controller @@ -274,12 +331,11 @@ bool sick_lidar_localization::ColaServices::serviceCbLocSetResultPort( sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); if(cola_response.command_name == "LocSetResultPort" && cola_response.parameter.size() > 0) // parameter[0]: Set Result (1: success) { - service_response.success = convertColaResponseBool(cola_response.parameter[0], false); + service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false); return true; } ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(\"" << cola_ascii << "\") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); return false; - } /*! @@ -373,3 +429,964 @@ bool sick_lidar_localization::ColaServices::serviceCbLocSetPose( cola_args << std::showpos << service_request.posex << " " << service_request.posey << " " << service_request.yaw << " " << service_request.uncertainty; return serviceCbWithSuccessResponse("sMN", "LocSetPose", cola_args.str(), service_response); } + +/*! + * Callback for service "SickDevSetLidarConfigSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbDevSetLidarConfig(sick_lidar_localization::SickDevSetLidarConfigSrv::Request& service_request, sick_lidar_localization::SickDevSetLidarConfigSrv::Response& service_response) +{ + service_response.set = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "DevSetLidarConfig" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.set) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickDevGetLidarConfigSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbDevGetLidarConfig(sick_lidar_localization::SickDevGetLidarConfigSrv::Request& service_request, sick_lidar_localization::SickDevGetLidarConfigSrv::Response& service_response) +{ + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "DevGetLidarConfig" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response)) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocSetMapSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetMap(sick_lidar_localization::SickLocSetMapSrv::Request& service_request, sick_lidar_localization::SickLocSetMapSrv::Response& service_response) +{ + service_response.set = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocSetMap" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.set) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocMapSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocMap(sick_lidar_localization::SickLocMapSrv::Request& service_request, sick_lidar_localization::SickLocMapSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocMap" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocMapStateSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocMapState(sick_lidar_localization::SickLocMapStateSrv::Request& service_request, sick_lidar_localization::SickLocMapStateSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocMapState" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocInitializePoseSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocInitializePose(sick_lidar_localization::SickLocInitializePoseSrv::Request& service_request, sick_lidar_localization::SickLocInitializePoseSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocInitializePose" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocInitialPoseSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocInitialPose(sick_lidar_localization::SickLocInitialPoseSrv::Request& service_request, sick_lidar_localization::SickLocInitialPoseSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocInitialPose") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocSetReflectorsForSupportActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetReflectorsForSupportActive(sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrv::Request& service_request, sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocSetReflectorsForSupportActive" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocReflectorsForSupportActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocReflectorsForSupportActive(sick_lidar_localization::SickLocReflectorsForSupportActiveSrv::Request& service_request, sick_lidar_localization::SickLocReflectorsForSupportActiveSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocReflectorsForSupportActive" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocSetOdometryActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetOdometryActive(sick_lidar_localization::SickLocSetOdometryActiveSrv::Request& service_request, sick_lidar_localization::SickLocSetOdometryActiveSrv::Response& service_response) +{ + service_response.set = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocSetOdometryActive" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.set) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocOdometryActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocOdometryActive(sick_lidar_localization::SickLocOdometryActiveSrv::Request& service_request, sick_lidar_localization::SickLocOdometryActiveSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocOdometryActive" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocSetOdometryPortSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetOdometryPort(sick_lidar_localization::SickLocSetOdometryPortSrv::Request& service_request, sick_lidar_localization::SickLocSetOdometryPortSrv::Response& service_response) +{ + service_response.set = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocSetOdometryPort" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.set) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocOdometryPortSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocOdometryPort(sick_lidar_localization::SickLocOdometryPortSrv::Request& service_request, sick_lidar_localization::SickLocOdometryPortSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocOdometryPort" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocSetOdometryRestrictYMotionSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetOdometryRestrictYMotion(sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrv::Request& service_request, sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocSetOdometryRestrictYMotion" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocOdometryRestrictYMotionSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocOdometryRestrictYMotion(sick_lidar_localization::SickLocOdometryRestrictYMotionSrv::Request& service_request, sick_lidar_localization::SickLocOdometryRestrictYMotionSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocOdometryRestrictYMotion" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocSetAutoStartActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetAutoStartActive(sick_lidar_localization::SickLocSetAutoStartActiveSrv::Request& service_request, sick_lidar_localization::SickLocSetAutoStartActiveSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocSetAutoStartActive" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocAutoStartActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocAutoStartActive(sick_lidar_localization::SickLocAutoStartActiveSrv::Request& service_request, sick_lidar_localization::SickLocAutoStartActiveSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocAutoStartActive" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocSetAutoStartSavePoseIntervalSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetAutoStartSavePoseInterval(sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrv::Request& service_request, sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocSetAutoStartSavePoseInterval" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocAutoStartSavePoseIntervalSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocAutoStartSavePoseInterval(sick_lidar_localization::SickLocAutoStartSavePoseIntervalSrv::Request& service_request, sick_lidar_localization::SickLocAutoStartSavePoseIntervalSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocAutoStartSavePoseInterval" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocSetRingBufferRecordingActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSetRingBufferRecordingActive(sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrv::Request& service_request, sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocSetRingBufferRecordingActive" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocRingBufferRecordingActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocRingBufferRecordingActive(sick_lidar_localization::SickLocRingBufferRecordingActiveSrv::Request& service_request, sick_lidar_localization::SickLocRingBufferRecordingActiveSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocRingBufferRecordingActive" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickDevGetLidarIdentSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbDevGetLidarIdent(sick_lidar_localization::SickDevGetLidarIdentSrv::Request& service_request, sick_lidar_localization::SickDevGetLidarIdentSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "DevGetLidarIdent" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickDevGetLidarStateSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbDevGetLidarState(sick_lidar_localization::SickDevGetLidarStateSrv::Request& service_request, sick_lidar_localization::SickDevGetLidarStateSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "DevGetLidarState" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickGetSoftwareVersionSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbGetSoftwareVersion(sick_lidar_localization::SickGetSoftwareVersionSrv::Request& service_request, sick_lidar_localization::SickGetSoftwareVersionSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "GetSoftwareVersion") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocAutoStartSavePoseSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocAutoStartSavePose(sick_lidar_localization::SickLocAutoStartSavePoseSrv::Request& service_request, sick_lidar_localization::SickLocAutoStartSavePoseSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocAutoStartSavePose") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocForceUpdateSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocForceUpdate(sick_lidar_localization::SickLocForceUpdateSrv::Request& service_request, sick_lidar_localization::SickLocForceUpdateSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocForceUpdate") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocSaveRingBufferRecordingSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocSaveRingBufferRecording(sick_lidar_localization::SickLocSaveRingBufferRecordingSrv::Request& service_request, sick_lidar_localization::SickLocSaveRingBufferRecordingSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocSaveRingBufferRecording" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocStartDemoMappingSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocStartDemoMapping(sick_lidar_localization::SickLocStartDemoMappingSrv::Request& service_request, sick_lidar_localization::SickLocStartDemoMappingSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocStartDemoMapping") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickReportUserMessageSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbReportUserMessage(sick_lidar_localization::SickReportUserMessageSrv::Request& service_request, sick_lidar_localization::SickReportUserMessageSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "ReportUserMessage" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickSavePermanentSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbSavePermanent(sick_lidar_localization::SickSavePermanentSrv::Request& service_request, sick_lidar_localization::SickSavePermanentSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "SavePermanent") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocResultPortSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocResultPort(sick_lidar_localization::SickLocResultPortSrv::Request& service_request, sick_lidar_localization::SickLocResultPortSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocResultPort") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocResultModeSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocResultMode(sick_lidar_localization::SickLocResultModeSrv::Request& service_request, sick_lidar_localization::SickLocResultModeSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocResultMode") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocResultEndiannessSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocResultEndianness(sick_lidar_localization::SickLocResultEndiannessSrv::Request& service_request, sick_lidar_localization::SickLocResultEndiannessSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocResultEndianness") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocResultStateSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocResultState(sick_lidar_localization::SickLocResultStateSrv::Request& service_request, sick_lidar_localization::SickLocResultStateSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocResultState") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickLocResultPoseIntervalSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbLocResultPoseInterval(sick_lidar_localization::SickLocResultPoseIntervalSrv::Request& service_request, sick_lidar_localization::SickLocResultPoseIntervalSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "LocResultPoseInterval") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickDevSetIMUActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbDevSetIMUActive(sick_lidar_localization::SickDevSetIMUActiveSrv::Request& service_request, sick_lidar_localization::SickDevSetIMUActiveSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "DevSetIMUActive" && cola_response.parameter.size() > 0) + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} + +/*! + * Callback for service "SickDevIMUActiveSrv" + * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response + * Uses ros service "SickLocColaTelegramSrv" + * @param[in] service_request ros service request to localization controller + * @param[out] service_response service response from localization controller + * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error). + */ +bool sick_lidar_localization::ColaServices::serviceCbDevIMUActive(sick_lidar_localization::SickDevIMUActiveSrv::Request& service_request, sick_lidar_localization::SickDevIMUActiveSrv::Response& service_response) +{ + service_response.success = false; + std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii); + if (cola_response.command_name == "DevIMUActive") + { + if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success) + { + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response) << ", ColaConverter::parseServiceResponse() failed."); + return false; + } + return true; + } + ROS_WARN_STREAM("## ERROR ColaServices::sendColaTelegram(" << cola_ascii << ") failed, invalid response: " << sick_lidar_localization::Utils::flattenToString(cola_response)); + return false; +} diff --git a/src/cola_transmitter.cpp b/src/cola_transmitter.cpp index 82a7344..3f6081d 100644 --- a/src/cola_transmitter.cpp +++ b/src/cola_transmitter.cpp @@ -54,7 +54,7 @@ * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/cola_parser.h" #include "sick_lidar_localization/cola_transmitter.h" @@ -117,7 +117,7 @@ bool sick_lidar_localization::ColaTransmitter::close(void) * @param[out] send_timestamp send timestamp in seconds (ros timestamp immediately before tcp send) * @return true on success, false on failure */ -bool sick_lidar_localization::ColaTransmitter::send(const std::vector & data, ros::Time & send_timestamp) +bool sick_lidar_localization::ColaTransmitter::send(const std::vector & data, ROS::Time & send_timestamp) { return send(m_tcp_socket.socket(), data, send_timestamp); } @@ -129,14 +129,14 @@ bool sick_lidar_localization::ColaTransmitter::send(const std::vector & * @param[out] send_timestamp send timestamp in seconds (ros timestamp immediately before tcp send) * @return true on success, false on failure */ -bool sick_lidar_localization::ColaTransmitter::send(boost::asio::ip::tcp::socket & socket, const std::vector & data, ros::Time & send_timestamp) +bool sick_lidar_localization::ColaTransmitter::send(boost::asio::ip::tcp::socket & socket, const std::vector & data, ROS::Time & send_timestamp) { try { - if (ros::ok() && socket.is_open()) + if (ROS::ok() && socket.is_open()) { boost::system::error_code errorcode; - send_timestamp = ros::Time::now(); + send_timestamp = ROS::now(); size_t bytes_written = boost::asio::write(socket, boost::asio::buffer(data.data(), data.size()), boost::asio::transfer_exactly(data.size()), errorcode); if (!errorcode && bytes_written == data.size()) return true; @@ -157,7 +157,7 @@ bool sick_lidar_localization::ColaTransmitter::send(boost::asio::ip::tcp::socket * @param[out] receive_timestamp receive timestamp in seconds (ros timestamp immediately after first response byte received) * @return true on success, false on failure */ -bool sick_lidar_localization::ColaTransmitter::receive(std::vector & telegram, double timeout, ros::Time & receive_timestamp) +bool sick_lidar_localization::ColaTransmitter::receive(std::vector & telegram, double timeout, ROS::Time & receive_timestamp) { return receive(m_tcp_socket.socket(), telegram, timeout, receive_timestamp); } @@ -170,15 +170,15 @@ bool sick_lidar_localization::ColaTransmitter::receive(std::vector & te * @param[out] receive_timestamp receive timestamp in seconds (ros timestamp immediately after first response byte received) * @return true on success, false on failure */ -bool sick_lidar_localization::ColaTransmitter::receive(boost::asio::ip::tcp::socket & socket, std::vector & telegram, double timeout, ros::Time & receive_timestamp) +bool sick_lidar_localization::ColaTransmitter::receive(boost::asio::ip::tcp::socket & socket, std::vector & telegram, double timeout, ROS::Time & receive_timestamp) { telegram.clear(); telegram.reserve(1024); try { std::vector binETX = sick_lidar_localization::ColaParser::binaryETX(); - ros::Time start_time = ros::Time::now(); - while (ros::ok() && socket.is_open()) + ROS::Time start_time = ROS::now(); + while (ROS::ok() && socket.is_open()) { // Read 1 byte uint8_t byte_received = 0; @@ -187,11 +187,11 @@ bool sick_lidar_localization::ColaTransmitter::receive(boost::asio::ip::tcp::soc if (socket.available() > 0 && boost::asio::read(socket, boost::asio::buffer(&byte_received, 1), boost::asio::transfer_exactly(1), errorcode) > 0 && !errorcode) { if (telegram.empty()) - receive_timestamp = ros::Time::now(); // timestamp after first byte received + receive_timestamp = ROS::now(); // timestamp after first byte received telegram.push_back(byte_received); } else - ros::Duration(0.0001).sleep(); + ROS::sleep(0.0001); // Check for "" (message completed) and return if received data ends with "" bool is_binary_cola = sick_lidar_localization::ColaAsciiBinaryConverter::IsColaBinary(telegram); if(is_binary_cola) @@ -208,7 +208,7 @@ bool sick_lidar_localization::ColaTransmitter::receive(boost::asio::ip::tcp::soc return true; // received, telegram completed } // Check for timeout - if ((ros::Time::now() - start_time).toSec() >= timeout) + if (ROS::seconds(ROS::now() - start_time) >= timeout) { // ROS_DEBUG_STREAM("ColaTransmitter::receive(): timeout, " << telegram.size() << " byte received: " << sick_lidar_localization::Utils::toHexString(telegram)); break; @@ -279,14 +279,14 @@ bool sick_lidar_localization::ColaTransmitter::stopReceiverThread(void) * @param[out] receive_timestamp receive timestamp in seconds (ros timestamp immediately after first response byte received) * @return true on success, false on failure (connection error or timeout) */ -bool sick_lidar_localization::ColaTransmitter::waitPopResponse(std::vector & telegram, double timeout, ros::Time & receive_timestamp) +bool sick_lidar_localization::ColaTransmitter::waitPopResponse(std::vector & telegram, double timeout, ROS::Time & receive_timestamp) { - ros::Time start_time = ros::Time::now(); - while(ros::ok() && m_receiver_thread_running && m_response_fifo.empty()) + ROS::Time start_time = ROS::now(); + while(ROS::ok() && m_receiver_thread_running && m_response_fifo.empty()) { - ros::Duration(0.0001).sleep(); + ROS::sleep(0.0001); m_response_fifo.waitOnceForElement(); - if((ros::Time::now() - start_time).toSec() >= timeout) + if(ROS::seconds(ROS::now() - start_time) >= timeout) break; } if(!m_response_fifo.empty()) @@ -304,7 +304,7 @@ bool sick_lidar_localization::ColaTransmitter::waitPopResponse(std::vector +#include "sick_lidar_localization/ros_wrapper.h" #include #include #include "sick_lidar_localization/driver_monitor.h" +#include "sick_lidar_localization/cola_configuration.h" +#include "sick_lidar_localization/cola_services.h" +#include "sick_lidar_localization/odom_converter.h" +#include "sick_lidar_localization/time_sync_service.h" int main(int argc, char** argv) { // Ros configuration and initialization - ros::init(argc, argv, "sim_loc_driver"); - ros::NodeHandle nh; + ROS::init(argc, argv, "sim_loc_driver"); + ROS::NodePtr nh = ROS::createNode("sim_loc_driver"); ROS_INFO_STREAM("sim_loc_driver started."); // Configuration and parameter for sim_loc_driver - std::string server_adress("192.168.0.1"), server_default_adress("192.168.0.1"); // IP adress of the localization controller, default: "192.168.0.1" + std::string server_adress("192.168.0.1"), server_default_adress("192.168.0.1"), system_server_adress(""); // IP adress of the localization controller, default: "192.168.0.1" + // Default tcp ports: see Operation-Instruction-v1.1.0.241R.pdf, page 51, "IP port number and protocol": // For requests and to transmit settings to the localization controller: // * IP port number 2111 and 2112 to send telegrams and to request data. @@ -79,10 +84,15 @@ int main(int argc, char** argv) // * Binary result port protocol TCP/IP int tcp_port_results = 2201; // Default: The localization controller uses IP port number 2201 to send localization results int tcp_port_cola = 2111; // For requests and to transmit settings to the localization controller: IP port number 2111 and 2112 to send telegrams and to request data, SOPAS CoLa-A or CoLa-B protocols - ros::param::param("/sim_loc_driver/localization_controller_ip_address" , server_adress, server_adress); - ros::param::param("/sick_lidar_localization/driver/localization_controller_default_ip_address", server_default_adress, server_default_adress); - ros::param::param("/sick_lidar_localization/driver/result_telegrams_tcp_port", tcp_port_results, tcp_port_results); - ros::param::param("/sick_lidar_localization/driver/cola_telegrams_tcp_port", tcp_port_cola, tcp_port_cola); + int udp_port_odom = 3000; // Default udp port to send odom packages to the localization controller + ROS::param(nh, "/sim_loc_driver/localization_controller_ip_address" , server_adress, server_adress); + ROS::param(nh, "/sick_lidar_localization/driver/localization_controller_default_ip_address", server_default_adress, server_default_adress); + ROS::param(nh, "/sick_lidar_localization/driver/result_telegrams_tcp_port", tcp_port_results, tcp_port_results); + ROS::param(nh, "/sick_lidar_localization/driver/cola_telegrams_tcp_port", tcp_port_cola, tcp_port_cola); + ROS::param(nh, "/sick_lidar_localization/driver/odom_telegrams_udp_port", udp_port_odom, udp_port_odom); + ROS::param(nh, "/system/localization_controller_ip_address" , system_server_adress, system_server_adress); // optional system setting + if(!system_server_adress.empty()) + server_adress = system_server_adress; server_adress = (server_adress.empty()) ? server_default_adress : server_adress; // Initialize driver threads to connect to localization controller and to monitor driver messages @@ -91,15 +101,38 @@ int main(int argc, char** argv) // - receives binary result port telegrams, // - converts them to SickLocResultPortTelegramMsg // - publishes the sim location data - sick_lidar_localization::DriverMonitor driver_monitor(&nh, server_adress, tcp_port_results, tcp_port_cola); + sick_lidar_localization::DriverMonitor driver_monitor(nh, server_adress, tcp_port_results, tcp_port_cola); + + // Initialize cola services + sick_lidar_localization::ColaServices cola_services(nh, &driver_monitor); + + // Initialize TimeSyncService + sick_lidar_localization::TimeSyncService time_sync_service(nh, &driver_monitor); + + // Initialize odometry converter + sick_lidar_localization::OdomConverter odom_converter(nh, server_adress, udp_port_odom, true); + // sim_loc_driver messages and services +#if defined __ROS_VERSION && __ROS_VERSION == 1 + auto messageCbResultPortTelegrams = &sick_lidar_localization::DriverMonitor::messageCbResultPortTelegrams; + auto serviceCbColaTelegram = &sick_lidar_localization::DriverMonitor::serviceCbColaTelegram; + auto messageCbOdometry = &sick_lidar_localization::OdomConverter::messageCbOdometry; +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + auto messageCbResultPortTelegrams = &sick_lidar_localization::DriverMonitor::messageCbResultPortTelegramsROS2; + auto serviceCbColaTelegram = &sick_lidar_localization::DriverMonitor::serviceCbColaTelegramROS2; + auto messageCbOdometry = &sick_lidar_localization::OdomConverter::messageCbOdometryROS2; +#endif + // Subscribe to sim_loc_driver messages std::string result_telegrams_topic = "/sick_lidar_localization/driver/result_telegrams"; // default topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) - ros::param::param("/sick_lidar_localization/driver/result_telegrams_topic", result_telegrams_topic, result_telegrams_topic); - ros::Subscriber result_telegram_subscriber = nh.subscribe(result_telegrams_topic, 1, &sick_lidar_localization::DriverMonitor::messageCbResultPortTelegrams, &driver_monitor); - + ROS::param(nh, "/sick_lidar_localization/driver/result_telegrams_topic", result_telegrams_topic, result_telegrams_topic); + sick_lidar_localization::SickLocResultPortTelegramMsgSubscriber result_telegram_subscriber = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::SickLocResultPortTelegramMsg, result_telegrams_topic, messageCbResultPortTelegrams, &driver_monitor); + std::string odom_topic = "/odom"; // ros topic for odometry messages + ROS::param(nh, "/sick_lidar_localization/driver/odom_topic", odom_topic, odom_topic); + sick_lidar_localization::OdomMsgSubscriber odom_subscriber = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::OdomMsg, odom_topic, messageCbOdometry, &odom_converter); + // Advertise service "SickLocColaTelegram" to send and receive Cola-ASCII telegrams to resp. from the localization server (request and response) - ros::ServiceServer service = nh.advertiseService("SickLocColaTelegram", &sick_lidar_localization::DriverMonitor::serviceCbColaTelegram, &driver_monitor); + sick_lidar_localization::SickLocColaTelegramSrvServer srv_server = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocColaTelegramSrv, "SickLocColaTelegram", serviceCbColaTelegram, &driver_monitor); ROS_INFO_STREAM("sim_loc_driver advertising service \"SickLocColaTelegram\" for Cola commands, message type SickLocColaTelegramSrv"); // Start driver threads to connect to localization controller and to monitor driver messages @@ -108,9 +141,20 @@ int main(int argc, char** argv) ROS_ERROR_STREAM("## ERROR sim_loc_driver: could not start driver monitor thread, exiting"); return EXIT_FAILURE; } + + // Initial configuration from launch file + sick_lidar_localization::ColaConfiguration cola_configuration(nh, &cola_services); + cola_configuration.start(); + + // Start time synchronization thread to run the software pll + if(!time_sync_service.start()) + { + ROS_ERROR_STREAM("## ERROR time_sync: could not start synchronization thread, exiting"); + return EXIT_FAILURE; + } // Run ros event loop - ros::spin(); + ROS::spin(nh); // Cleanup and exit std::cout << "sim_loc_driver finished." << std::endl; @@ -118,5 +162,6 @@ int main(int argc, char** argv) driver_monitor.stop(); std::cout << "sim_loc_driver exits." << std::endl; ROS_INFO_STREAM("sim_loc_driver exits."); + ROS::deleteNode(nh); return EXIT_SUCCESS; } diff --git a/src/driver_check.cpp b/src/driver_check.cpp index cd9da32..a707dea 100644 --- a/src/driver_check.cpp +++ b/src/driver_check.cpp @@ -58,36 +58,43 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/driver_check_thread.h" int main(int argc, char** argv) { // Ros configuration and initialization - ros::init(argc, argv, "sim_loc_driver_check"); - ros::NodeHandle nh; + ROS::init(argc, argv, "sim_loc_driver_check"); + ROS::NodePtr nh = ROS::createNode("sim_loc_driver_check"); ROS_INFO_STREAM("sim_loc_driver_check started."); std::string result_telegrams_topic = "/sick_lidar_localization/driver/result_telegrams"; // default topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) - ros::param::param("/sick_lidar_localization/sim_loc_driver_check/result_telegrams_topic", result_telegrams_topic, result_telegrams_topic); + ROS::param(nh, "/sick_lidar_localization/sim_loc_driver_check/result_telegrams_topic", result_telegrams_topic, result_telegrams_topic); // Init thread to check sim_loc_driver messages against configured min and max values - sick_lidar_localization::MessageCheckThread check_thread; + sick_lidar_localization::MessageCheckThread check_thread(nh); // Subscribe to sim_loc_driver messages - ros::Subscriber result_telegram_subscriber = nh.subscribe(result_telegrams_topic, 1, &sick_lidar_localization::MessageCheckThread::messageCbResultPortTelegrams, &check_thread); +#if defined __ROS_VERSION && __ROS_VERSION == 1 + sick_lidar_localization::SickLocResultPortTelegramMsgSubscriber result_telegram_subscriber + = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::SickLocResultPortTelegramMsg, result_telegrams_topic, &sick_lidar_localization::MessageCheckThread::messageCbResultPortTelegrams, &check_thread); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + sick_lidar_localization::SickLocResultPortTelegramMsgSubscriber result_telegram_subscriber + = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::SickLocResultPortTelegramMsg, result_telegrams_topic, &sick_lidar_localization::MessageCheckThread::messageCbResultPortTelegramsROS2, &check_thread); +#endif // Start checking thread check_thread.start(); // Run ros event loop - ros::spin(); + ROS::spin(nh); std::cout << "sim_loc_driver_check finished." << std::endl; ROS_INFO_STREAM("sim_loc_driver_check finished."); check_thread.stop(); std::cout << "sim_loc_driver_check exits." << std::endl; ROS_INFO_STREAM("sim_loc_driver_check exits."); + ROS::deleteNode(nh); return 0; } diff --git a/src/driver_check_thread.cpp b/src/driver_check_thread.cpp index 554cafe..a70bdb2 100644 --- a/src/driver_check_thread.cpp +++ b/src/driver_check_thread.cpp @@ -59,7 +59,7 @@ * */ #include -#include +#include "sick_lidar_localization/ros_wrapper.h" #include #include "sick_lidar_localization/driver_check_thread.h" @@ -69,35 +69,38 @@ * Constructor, reads the configuration parameter incl. the * min and max allowed values in sim_loc_driver messages. */ -sick_lidar_localization::MessageCheckThread::MessageCheckThread() -: m_message_check_thread_running(false), m_message_check_thread(0), m_message_check_frequency(100) +sick_lidar_localization::MessageCheckThread::MessageCheckThread(ROS::NodePtr nh) +: m_message_check_thread_running(false), m_message_check_thread(0), m_message_check_frequency(100), m_timestamp_valid_telegram(0) { - // Read configuation - int software_pll_fifo_length = 7, time_sync_initial_length = 10; - double time_sync_rate = 0.1, time_sync_initial_rate = 1.0; - ros::param::param("/sick_lidar_localization/sim_loc_driver_check/message_check_frequency", m_message_check_frequency, m_message_check_frequency); - ros::param::param("/sick_lidar_localization/time_sync/software_pll_fifo_length", software_pll_fifo_length, software_pll_fifo_length); - ros::param::param("/sick_lidar_localization/time_sync/time_sync_rate", time_sync_rate, time_sync_rate); - ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_rate", time_sync_initial_rate, time_sync_initial_rate); - ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_length", time_sync_initial_length, time_sync_initial_length); - assert(software_pll_fifo_length > 0 && time_sync_rate > FLT_EPSILON); - m_software_pll_expected_initialization_duration = (software_pll_fifo_length / time_sync_initial_rate + 1); // expected initialization time for software pll (system time from lidar ticks not yet available) - // Read min allowed values in a result port telegrams - m_result_port_telegram_min_values = readYamlResultPortTelegram("/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values"); - m_result_port_telegram_min_values.vehicle_time_valid = false; - m_result_port_telegram_min_values.vehicle_time_sec = 0; - m_result_port_telegram_min_values.vehicle_time_nsec = 0; - ros::param::param("/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/vehicle_time_delta", m_vehicle_time_delta_min, -1.0); - ros::param::param("/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/check_vehicle_time", m_vehicle_time_check_enabled, true); - // Read max allowed values in a result port telegrams - m_result_port_telegram_max_values = readYamlResultPortTelegram("/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values"); - m_result_port_telegram_max_values.vehicle_time_valid = true; - m_result_port_telegram_max_values.vehicle_time_sec = UINT32_MAX; - m_result_port_telegram_max_values.vehicle_time_nsec = UINT32_MAX; - ros::param::param("/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/vehicle_time_delta", m_vehicle_time_delta_max, 1.0); - ros::param::param("/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/check_vehicle_time", m_vehicle_time_check_enabled, true); - ROS_INFO_STREAM("MessageCheckThread: min allowed values in result port telegrams: " << sick_lidar_localization::Utils::flattenToString(m_result_port_telegram_min_values)); - ROS_INFO_STREAM("MessageCheckThread: max allowed values in result port telegrams: " << sick_lidar_localization::Utils::flattenToString(m_result_port_telegram_max_values)); + if(nh) // Read configuation + { + int software_pll_fifo_length = 7, time_sync_initial_length = 10; + double time_sync_rate = 0.1, time_sync_initial_rate = 1.0; + ROS::param(nh, "/sick_lidar_localization/sim_loc_driver_check/message_check_frequency", m_message_check_frequency, m_message_check_frequency); + ROS::param(nh, "/sick_lidar_localization/time_sync/software_pll_fifo_length", software_pll_fifo_length, software_pll_fifo_length); + ROS::param(nh, "/sick_lidar_localization/time_sync/time_sync_rate", time_sync_rate, time_sync_rate); + ROS::param(nh, "/sick_lidar_localization/time_sync/time_sync_initial_rate", time_sync_initial_rate, time_sync_initial_rate); + ROS::param(nh, "/sick_lidar_localization/time_sync/time_sync_initial_length", time_sync_initial_length, time_sync_initial_length); + assert(software_pll_fifo_length > 0 && time_sync_rate > FLT_EPSILON); + m_software_pll_expected_initialization_duration = (software_pll_fifo_length / time_sync_initial_rate + 1); // expected initialization time for software pll (system time from lidar ticks not yet available) + // Read min allowed values in a result port telegrams + m_result_port_telegram_min_values = readYamlResultPortTelegram(nh, "/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values"); + m_result_port_telegram_min_values.vehicle_time_valid = false; + m_result_port_telegram_min_values.vehicle_time_sec = 0; + m_result_port_telegram_min_values.vehicle_time_nsec = 0; + ROS::param(nh, "/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/vehicle_time_delta", m_vehicle_time_delta_min, -1.0); + ROS::param(nh, "/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values/check_vehicle_time", m_vehicle_time_check_enabled, true); + // Read max allowed values in a result port telegrams + m_result_port_telegram_max_values = readYamlResultPortTelegram(nh, "/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values"); + m_result_port_telegram_max_values.vehicle_time_valid = true; + m_result_port_telegram_max_values.vehicle_time_sec = UINT32_MAX; + m_result_port_telegram_max_values.vehicle_time_nsec = UINT32_MAX; + m_result_port_telegram_max_values.telegram_header.fw_version = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; + ROS::param(nh, "/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/vehicle_time_delta", m_vehicle_time_delta_max, 1.0); + ROS::param(nh, "/sick_lidar_localization/sim_loc_driver_check/result_telegram_max_values/check_vehicle_time", m_vehicle_time_check_enabled, true); + ROS_INFO_STREAM("MessageCheckThread: min allowed values in result port telegrams: " << sick_lidar_localization::Utils::flattenToString(m_result_port_telegram_min_values)); + ROS_INFO_STREAM("MessageCheckThread: max allowed values in result port telegrams: " << sick_lidar_localization::Utils::flattenToString(m_result_port_telegram_max_values)); + } } /* @@ -152,13 +155,13 @@ void sick_lidar_localization::MessageCheckThread::runMessageCheckThreadCb(void) { ROS_INFO_STREAM("MessageCheckThread: thread to check sim_loc_driver messages started"); size_t total_message_check_cnt = 0, total_message_check_failed_cnt = 0; - while(ros::ok() && m_message_check_thread_running) + while(ROS::ok() && m_message_check_thread_running) { - while(ros::ok() && m_message_check_thread_running && m_result_port_telegram_fifo.empty()) + while(ROS::ok() && m_message_check_thread_running && m_result_port_telegram_fifo.empty()) { - ros::Duration(1.0 / m_message_check_frequency).sleep(); + ROS::sleep(1.0 / m_message_check_frequency); } - if(ros::ok() && m_message_check_thread_running && !m_result_port_telegram_fifo.empty()) + if(ROS::ok() && m_message_check_thread_running && !m_result_port_telegram_fifo.empty()) { // Check sim_loc_driver message sick_lidar_localization::SickLocResultPortTelegramMsg result_port_telegram = m_result_port_telegram_fifo.pop(); @@ -181,7 +184,7 @@ void sick_lidar_localization::MessageCheckThread::runMessageCheckThreadCb(void) ROS_INFO_STREAM("MessageCheckThread: thread to check sim_loc_driver messages finished"); std::stringstream info_msg; info_msg << "MessageCheckThread: check messages thread summary: " << total_message_check_cnt << " messages checked, " << total_message_check_failed_cnt << " failures."; - if(ros::ok()) + if(ROS::ok()) ROS_INFO_STREAM(info_msg.str()); else std::cout << info_msg.str() << std::endl; @@ -192,7 +195,7 @@ void sick_lidar_localization::MessageCheckThread::runMessageCheckThreadCb(void) * @param[in] param_section section name in yaml configuration file, f.e. "/sick_lidar_localization/sim_loc_driver_check/result_telegram_min_values" * @return result port telegram with values initialized from yaml file. */ -sick_lidar_localization::SickLocResultPortTelegramMsg sick_lidar_localization::MessageCheckThread::readYamlResultPortTelegram(const std::string param_section) +sick_lidar_localization::SickLocResultPortTelegramMsg sick_lidar_localization::MessageCheckThread::readYamlResultPortTelegram(ROS::NodePtr nh, const std::string param_section) { int i_value = 0; double d_value = 0; @@ -206,61 +209,64 @@ sick_lidar_localization::SickLocResultPortTelegramMsg sick_lidar_localization::M telegram.vehicle_time_nsec = 0; // Read ros header - ros::param::param(param_section+"/header/seq", d_value, 0); +#if defined __ROS_VERSION && __ROS_VERSION == 1 // telegram.header.seq does not exist anymore in ROS2 + ROS::param(nh, param_section+"/header/seq", d_value, 0); telegram.header.seq = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // sequence ID, consecutively increasing ID, uint32, size:= 4 byte - ros::param::param(param_section+"/header/stamp", d_value, 0); - telegram.header.stamp = ros::Time(d_value); // time stamp - ros::param::param(param_section+"/header/frame_id", telegram.header.frame_id, ""); +#endif + ROS::param(nh, param_section+"/header/stamp", d_value, 0); + telegram.header.stamp = ROS::timeFromSec(d_value); // time stamp + ROS::param(nh, param_section+"/header/frame_id", telegram.header.frame_id, ""); // Read telegram header, 52 byte - ros::param::param(param_section+"/telegram_header/MagicWord", i_value, 0x5349434B); - telegram.telegram_header.MagicWord = (uint32_t)i_value; // Magic word SICK (0x53 0x49 0x43 0x4B). uint32, size:= 4 × UInt8 = 4 byte - ros::param::param(param_section+"/telegram_header/Length", i_value, 106); - telegram.telegram_header.Length = (uint32_t)i_value; // Length of telegram incl. header, payload, and trailer. uint32, size:= UInt32 = 4 byte - ros::param::param(param_section+"/telegram_header/PayloadType", i_value, 0x0642); - telegram.telegram_header.PayloadType = (uint16_t)i_value; // Payload type, 0x06c2 = Little Endian, 0x0642 = Big Endian. uint16, size:= UInt16 = 2 byte - ros::param::param(param_section+"/telegram_header/PayloadVersion", i_value, 1); - telegram.telegram_header.PayloadVersion = (uint16_t)i_value; // Version of PayloadType structure. uint16, size:= UInt16 = 2 byte - ros::param::param(param_section+"/telegram_header/OrderNumber", d_value, 0); - telegram.telegram_header.OrderNumber = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Order number of the localization controller. uint32, size:= UInt32 = 4 byte - ros::param::param(param_section+"/telegram_header/SerialNumber", d_value, 0); - telegram.telegram_header.SerialNumber = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Serial number of the localization controller. uint32, size:= UInt32 = 4 byte - ros::param::param>(param_section+"/telegram_header/FW_Version", i_vec, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}); - u8_vec.reserve(i_vec.size()); - for(size_t n = 0; n < i_vec.size(); n++) - u8_vec.push_back(i_vec[n]); - telegram.telegram_header.FW_Version = u8_vec; // Software version of the localization controller. uint8[], size:= 20 × UInt8 = 20 byte - ros::param::param(param_section+"/telegram_header/TelegramCounter", d_value, 0); - telegram.telegram_header.TelegramCounter = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Telegram counter since last start-up. uint32, size:= UInt32 = 4 byte - ros::param::param(param_section+"/telegram_header/SystemTime", d_value, 0); - telegram.telegram_header.SystemTime = ((d_value <= 0)?0:((d_value >= 0xFFFFFFFFFFFFFFFF)?0xFFFFFFFFFFFFFFFF:(uint64_t)(d_value))); // Not used. uint64, size:= NTP = 8 byte + ROS::param(nh, param_section+"/telegram_header/MagicWord", i_value, 0x5349434B); + telegram.telegram_header.magicword = (uint32_t)i_value; // Magic word SICK (0x53 0x49 0x43 0x4B). uint32, size:= 4 × UInt8 = 4 byte + ROS::param(nh, param_section+"/telegram_header/Length", i_value, 106); + telegram.telegram_header.length = (uint32_t)i_value; // Length of telegram incl. header, payload, and trailer. uint32, size:= UInt32 = 4 byte + ROS::param(nh, param_section+"/telegram_header/PayloadType", i_value, 0x0642); + telegram.telegram_header.payloadtype = (uint16_t)i_value; // Payload type, 0x06c2 = Little Endian, 0x0642 = Big Endian. uint16, size:= UInt16 = 2 byte + ROS::param(nh, param_section+"/telegram_header/PayloadVersion", i_value, 1); + telegram.telegram_header.payloadversion = (uint16_t)i_value; // Version of PayloadType structure. uint16, size:= UInt16 = 2 byte + ROS::param(nh, param_section+"/telegram_header/OrderNumber", d_value, 0); + telegram.telegram_header.ordernumber = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Order number of the localization controller. uint32, size:= UInt32 = 4 byte + ROS::param(nh, param_section+"/telegram_header/SerialNumber", d_value, 0); + telegram.telegram_header.serialnumber = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Serial number of the localization controller. uint32, size:= UInt32 = 4 byte + // ROS::param>(nh, param_section+"/telegram_header/FW_Version", i_vec, {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}); + // u8_vec.reserve(i_vec.size()); + // for(size_t n = 0; n < i_vec.size(); n++) + // u8_vec.push_back(i_vec[n]); + u8_vec = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + telegram.telegram_header.fw_version = u8_vec; // Software version of the localization controller. uint8[], size:= 20 × UInt8 = 20 byte + ROS::param(nh, param_section+"/telegram_header/TelegramCounter", d_value, 0); + telegram.telegram_header.telegramcounter = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Telegram counter since last start-up. uint32, size:= UInt32 = 4 byte + ROS::param(nh, param_section+"/telegram_header/SystemTime", d_value, 0); + telegram.telegram_header.systemtime = ((d_value <= 0)?0:((d_value >= 0xFFFFFFFFFFFFFFFF)?0xFFFFFFFFFFFFFFFF:(uint64_t)(d_value))); // Not used. uint64, size:= NTP = 8 byte // Read telegram payload, 52 byte - ros::param::param(param_section+"/telegram_payload/ErrorCode", i_value, 0); - telegram.telegram_payload.ErrorCode = (uint16_t)i_value; // ErrorCode 0:= OK, ErrorCode 1:= UNKNOWNERROR. uint16, size:= UInt16 = 2 byte - ros::param::param(param_section+"/telegram_payload/ScanCounter", d_value, 0); - telegram.telegram_payload.ScanCounter = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Counter of related scan data. uint32, size:= UInt32 = 4 byte - ros::param::param(param_section+"/telegram_payload/Timestamp", d_value, 0); - telegram.telegram_payload.Timestamp = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. uint32, size:= UInt32 = 4 byte - ros::param::param(param_section+"/telegram_payload/PoseX", telegram.telegram_payload.PoseX, 0); // Position X of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte - ros::param::param(param_section+"/telegram_payload/PoseY", telegram.telegram_payload.PoseY, 0); // Position Y of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte - ros::param::param(param_section+"/telegram_payload/PoseYaw", telegram.telegram_payload.PoseYaw, 0); // Orientation (yaw) of the vehicle on the map [mdeg]. int32, size:= Int32 = 4 byte - ros::param::param(param_section+"/telegram_payload/Reserved1", d_value, 0); - telegram.telegram_payload.Reserved1 = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Reserved. uint32, size:= UInt32 = 4 byte - ros::param::param(param_section+"/telegram_payload/Reserved2", telegram.telegram_payload.Reserved2, 0); // Reserved. int32, size:= Int32 = 4 byte - ros::param::param(param_section+"/telegram_payload/Quality", i_value, 0); - telegram.telegram_payload.Quality = (uint8_t)i_value; // Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality. uint8, size:= UInt8 = 1 byte - ros::param::param(param_section+"/telegram_payload/OutliersRatio", i_value, 0); - telegram.telegram_payload.OutliersRatio = (uint8_t)i_value; // Ratio of beams that cannot be assigned to the current reference map [%]. uint8, size:= UInt8 = 1 byte - ros::param::param(param_section+"/telegram_payload/CovarianceX", telegram.telegram_payload.CovarianceX, 0); // Covariance c1 of the pose X [mm^2]. int32, size:= Int32 = 4 byte - ros::param::param(param_section+"/telegram_payload/CovarianceY", telegram.telegram_payload.CovarianceY, 0); // Covariance c5 of the pose X [mm^2]. int32, size:= Int32 = 4 byte - ros::param::param(param_section+"/telegram_payload/CovarianceYaw", telegram.telegram_payload.CovarianceYaw, 0); // Covariance c9 of the pose Yaw [mdeg^2]. int32, size:= Int32 = 4 byte - ros::param::param(param_section+"/telegram_payload/Reserved3", d_value, 0); - telegram.telegram_payload.Reserved3 = ((d_value <= 0)?0:((d_value >= 0xFFFFFFFFFFFFFFFF)?0xFFFFFFFFFFFFFFFF:(uint64_t)(d_value))); // Reserved. uint64, size:= UInt64 = 8 byte + ROS::param(nh, param_section+"/telegram_payload/ErrorCode", i_value, 0); + telegram.telegram_payload.errorcode = (uint16_t)i_value; // ErrorCode 0:= OK, ErrorCode 1:= UNKNOWNERROR. uint16, size:= UInt16 = 2 byte + ROS::param(nh, param_section+"/telegram_payload/ScanCounter", d_value, 0); + telegram.telegram_payload.scancounter = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Counter of related scan data. uint32, size:= UInt32 = 4 byte + ROS::param(nh, param_section+"/telegram_payload/Timestamp", d_value, 0); + telegram.telegram_payload.timestamp = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. uint32, size:= UInt32 = 4 byte + ROS::param(nh, param_section+"/telegram_payload/PoseX", telegram.telegram_payload.posex, 0); // Position X of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte + ROS::param(nh, param_section+"/telegram_payload/PoseY", telegram.telegram_payload.posey, 0); // Position Y of the vehicle on the map in cartesian global coordinates [mm]. int32, size:= Int32 = 4 byte + ROS::param(nh, param_section+"/telegram_payload/PoseYaw", telegram.telegram_payload.poseyaw, 0); // Orientation (yaw) of the vehicle on the map [mdeg]. int32, size:= Int32 = 4 byte + ROS::param(nh, param_section+"/telegram_payload/Reserved1", d_value, 0); + telegram.telegram_payload.reserved1 = (uint32_t)boost::algorithm::clamp(d_value, 0, 0xFFFFFFFF); // Reserved. uint32, size:= UInt32 = 4 byte + ROS::param(nh, param_section+"/telegram_payload/Reserved2", telegram.telegram_payload.reserved2, 0); // Reserved. int32, size:= Int32 = 4 byte + ROS::param(nh, param_section+"/telegram_payload/Quality", i_value, 0); + telegram.telegram_payload.quality = (uint8_t)i_value; // Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality. uint8, size:= UInt8 = 1 byte + ROS::param(nh, param_section+"/telegram_payload/OutliersRatio", i_value, 0); + telegram.telegram_payload.outliersratio = (uint8_t)i_value; // Ratio of beams that cannot be assigned to the current reference map [%]. uint8, size:= UInt8 = 1 byte + ROS::param(nh, param_section+"/telegram_payload/CovarianceX", telegram.telegram_payload.covariancex, 0); // Covariance c1 of the pose X [mm^2]. int32, size:= Int32 = 4 byte + ROS::param(nh, param_section+"/telegram_payload/CovarianceY", telegram.telegram_payload.covariancey, 0); // Covariance c5 of the pose X [mm^2]. int32, size:= Int32 = 4 byte + ROS::param(nh, param_section+"/telegram_payload/CovarianceYaw", telegram.telegram_payload.covarianceyaw, 0); // Covariance c9 of the pose Yaw [mdeg^2]. int32, size:= Int32 = 4 byte + ROS::param(nh, param_section+"/telegram_payload/Reserved3", d_value, 0); + telegram.telegram_payload.reserved3 = ((d_value <= 0)?0:((d_value >= 0xFFFFFFFFFFFFFFFF)?0xFFFFFFFFFFFFFFFF:(uint64_t)(d_value))); // Reserved. uint64, size:= UInt64 = 8 byte // Read telegram trailer, 2 byte - ros::param::param(param_section+"/telegram_trailer/Checksum", i_value, 0); - telegram.telegram_trailer.Checksum = (uint16_t)i_value; // CRC16-CCITT checksum + ROS::param(nh, param_section+"/telegram_trailer/Checksum", i_value, 0); + telegram.telegram_trailer.checksum = (uint16_t)i_value; // CRC16-CCITT checksum return telegram; } @@ -292,36 +298,37 @@ template bool checkTelegramArray(const T & value, const T & min_valu */ bool sick_lidar_localization::MessageCheckThread::checkTelegram(sick_lidar_localization::SickLocResultPortTelegramMsg & telegram) { + //telegram.header.stamp.sec return // Check ros header - CHECK_TELEGRAM_VALUE(header.seq, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(header.stamp, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + // CHECK_TELEGRAM_VALUE(header.seq, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && // telegram.header.seq does not exist anymore in ROS2 + // CHECK_TELEGRAM_VALUE(header.stamp.sec, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && CHECK_TELEGRAM_ARRAY(header.frame_id, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && // Check telegram header, 52 byte - CHECK_TELEGRAM_VALUE(telegram_header.MagicWord, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_header.Length, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_header.PayloadType, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_header.PayloadVersion, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_header.OrderNumber, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_header.SerialNumber, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_ARRAY(telegram_header.FW_Version, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_header.TelegramCounter, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_header.SystemTime, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_header.magicword, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_header.length, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_header.payloadtype, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_header.payloadversion, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_header.ordernumber, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_header.serialnumber, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_ARRAY(telegram_header.fw_version, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_header.telegramcounter, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_header.systemtime, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && // Check telegram payload, 52 byte - CHECK_TELEGRAM_VALUE(telegram_payload.ErrorCode, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.ScanCounter, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.Timestamp, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.PoseX, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.PoseY, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.PoseYaw, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.Reserved1, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.Reserved2, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.Quality, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.OutliersRatio, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.CovarianceX, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.CovarianceY, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.CovarianceYaw, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && - CHECK_TELEGRAM_VALUE(telegram_payload.Reserved3, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.errorcode, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.scancounter, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.timestamp, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.posex, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.posey, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.poseyaw, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.reserved1, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.reserved2, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.quality, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.outliersratio, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.covariancex, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.covariancey, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.covarianceyaw, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && + CHECK_TELEGRAM_VALUE(telegram_payload.reserved3, telegram, m_result_port_telegram_min_values, m_result_port_telegram_max_values) && (!m_vehicle_time_check_enabled || checkVehicleTime(telegram)); // Check vehicle time (system time from ticks by software pll), if enabled by default } @@ -334,17 +341,18 @@ bool sick_lidar_localization::MessageCheckThread::checkTelegram(sick_lidar_local bool sick_lidar_localization::MessageCheckThread::checkVehicleTime(sick_lidar_localization::SickLocResultPortTelegramMsg & telegram) { - if(m_timestamp_valid_telegram.sec <= 0) - m_timestamp_valid_telegram = ros::Time::now(); + if(ROS::secondsSinceStart(m_timestamp_valid_telegram) < FLT_EPSILON) + m_timestamp_valid_telegram = ROS::now(); if(telegram.vehicle_time_valid) { - m_timestamp_valid_telegram = ros::Time::now(); - ros::Time message_time(telegram.header.stamp.sec, telegram.header.stamp.nsec); - ros::Time vehicle_time(telegram.vehicle_time_sec,telegram.vehicle_time_nsec); - ros::Duration delta_time = message_time - vehicle_time; - return delta_time.toSec() >= m_vehicle_time_delta_min && delta_time.toSec() <= m_vehicle_time_delta_max; + m_timestamp_valid_telegram = ROS::now(); + ROS::Time message_time = ROS::timeFromHeader(&telegram.header); + //ROS::Time message_time(telegram.header.stamp.sec, telegram.header.stamp.nsec); + ROS::Time vehicle_time(telegram.vehicle_time_sec,telegram.vehicle_time_nsec); + ROS::Duration delta_time = message_time - vehicle_time; + return ROS::seconds(delta_time) >= m_vehicle_time_delta_min && ROS::seconds(delta_time) <= m_vehicle_time_delta_max; } - else if((ros::Time::now() - m_timestamp_valid_telegram).toSec() <= m_software_pll_expected_initialization_duration // software pll is initializing + else if(ROS::seconds(ROS::now() - m_timestamp_valid_telegram) <= m_software_pll_expected_initialization_duration // software pll is initializing || (std::abs(m_vehicle_time_delta_min) >= FLT_MAX && std::abs(m_vehicle_time_delta_max) >= FLT_MAX)) // checkVehicleTime disabled for error simulation (unreachable controller etc.) { return true; // software pll initializing, system time from lidar ticks not yet available diff --git a/src/driver_monitor.cpp b/src/driver_monitor.cpp index c179a1d..7153a3e 100644 --- a/src/driver_monitor.cpp +++ b/src/driver_monitor.cpp @@ -53,7 +53,7 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/cola_parser.h" #include "sick_lidar_localization/driver_monitor.h" @@ -74,19 +74,19 @@ * @param[in] ip_port_results ip port for result telegrams, default: 2201 * @param[in] ip_port_cola ip port for command requests and responses, default: 2111 */ -sick_lidar_localization::DriverMonitor::DriverMonitor(ros::NodeHandle * nh, const std::string & server_adress, int ip_port_results, int ip_port_cola) +sick_lidar_localization::DriverMonitor::DriverMonitor(ROS::NodePtr nh, const std::string & server_adress, int ip_port_results, int ip_port_cola) : m_initialized(false), m_nh(nh), m_server_adress(server_adress), m_ip_port_results(ip_port_results), m_ip_port_cola(ip_port_cola), m_cola_binary(false), - m_monitoring_thread_running(false), m_monitoring_thread(0), m_monitoring_rate(1.0), m_receive_telegrams_timeout(1.0), m_cola_transmitter(0) + m_monitoring_thread_running(false), m_monitoring_thread(0), m_monitoring_rate(1.0), m_receive_telegrams_timeout(1.0), m_cola_response_timeout(1.0), m_cola_transmitter(0) { if(m_nh) { // Query configuration int cola_binary_mode = 0; - ros::param::param("/sick_lidar_localization/driver/cola_binary", cola_binary_mode, cola_binary_mode); + ROS::param(nh, "/sick_lidar_localization/driver/cola_binary", cola_binary_mode, cola_binary_mode); m_cola_binary = (cola_binary_mode == 1) ? true : false; // 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) - ros::param::param("/sick_lidar_localization/driver/monitoring_rate", m_monitoring_rate, m_monitoring_rate); // frequency to monitor driver messages, default: once per second - ros::param::param("/sick_lidar_localization/driver/monitoring_message_timeout", m_receive_telegrams_timeout, m_receive_telegrams_timeout); // timeout for driver messages, shutdown tcp-sockets and reconnect after message timeout, default: 1 second - ros::param::param("/sick_lidar_localization/time_sync/cola_response_timeout", m_cola_response_timeout, m_cola_response_timeout); + ROS::param(nh, "/sick_lidar_localization/driver/monitoring_rate", m_monitoring_rate, m_monitoring_rate); // frequency to monitor driver messages, default: once per second + ROS::param(nh, "/sick_lidar_localization/driver/monitoring_message_timeout", m_receive_telegrams_timeout, m_receive_telegrams_timeout); // timeout for driver messages, shutdown tcp-sockets and reconnect after message timeout, default: 1 second + ROS::param(nh, "/sick_lidar_localization/time_sync/cola_response_timeout", m_cola_response_timeout, m_cola_response_timeout); m_initialized = true; } } @@ -177,7 +177,7 @@ void sick_lidar_localization::DriverMonitor::stopColaTransmitter(void) */ void sick_lidar_localization::DriverMonitor::messageCbResultPortTelegrams(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg) { - m_driver_message_recv_timestamp.set(ros::Time::now()); + m_driver_message_recv_timestamp.set(ROS::now()); } /*! @@ -188,8 +188,8 @@ void sick_lidar_localization::DriverMonitor::messageCbResultPortTelegrams(const */ bool sick_lidar_localization::DriverMonitor::serviceCbColaTelegram(sick_lidar_localization::SickLocColaTelegramSrv::Request & cola_request, sick_lidar_localization::SickLocColaTelegramSrv::Response & cola_response) { - ROS_INFO_STREAM("DriverMonitor::serviceCbColaTelegram: starting Cola request { " << sick_lidar_localization::Utils::flattenToString(cola_request) << " }"); boost::lock_guard service_cb_lockguard(m_service_cb_mutex); // one service request at a time + ROS_INFO_STREAM("DriverMonitor::serviceCbColaTelegram: starting Cola request { " << cola_request.cola_ascii_request << " }, timeout " << cola_request.wait_response_timeout << " sec"); // initialize cola_response with default values cola_response.cola_ascii_response = ""; const std::string & asciiSTX = sick_lidar_localization::ColaParser::asciiSTX(); @@ -208,16 +208,19 @@ bool sick_lidar_localization::DriverMonitor::serviceCbColaTelegram(sick_lidar_lo } // Send request and wait for response with timeout std::vector binary_response; - ros::Time send_timestamp, receive_timestamp; + ROS::Time send_timestamp, receive_timestamp; if(!m_cola_transmitter->send(binary_request, send_timestamp)) { ROS_WARN_STREAM("## ERROR DriverMonitor::serviceCbColaTelegram: send() failed to localization server " << m_server_adress << ":" << m_ip_port_cola); stopColaTransmitter(); return false; } + ROS::Time waitResponseStartTime = ROS::now(); if(!m_cola_transmitter->waitPopResponse(binary_response, cola_request.wait_response_timeout, receive_timestamp) || binary_response.size() < 2) // at least 2 byte stx and etx { - ROS_WARN_STREAM("## ERROR DriverMonitor::serviceCbColaTelegram: receive() failed by localization server " << m_server_adress << ":" << m_ip_port_cola); + ROS_WARN_STREAM("## ERROR DriverMonitor::serviceCbColaTelegram: receive() failed by localization server " << m_server_adress << ":" << m_ip_port_cola + << "(" << binary_response.size() << " bytes received, configured timeout: " << cola_request.wait_response_timeout << " sec, receive failed after " + << ROS::seconds(ROS::now() - waitResponseStartTime) << " sec)"); stopColaTransmitter(); return false; } @@ -233,11 +236,9 @@ bool sick_lidar_localization::DriverMonitor::serviceCbColaTelegram(sick_lidar_lo { // Remove "" and "" start and end tags cola_response.cola_ascii_response = cola_response.cola_ascii_response.substr(asciiSTX.size(), cola_response.cola_ascii_response.size() - asciiSTX.size() - asciiETX.size()); // Cola-ASCII response - cola_response.send_timestamp_sec = send_timestamp.sec; // Send timestamp (seconds part of ros timestamp immediately before tcp send) - cola_response.send_timestamp_nsec = send_timestamp.nsec; // Send timestamp (nano seconds part of ros timestamp immediately before tcp send) - cola_response.receive_timestamp_sec = receive_timestamp.sec; // Receive timestamp (seconds part of ros timestamp immediately after first response byte received) - cola_response.receive_timestamp_nsec = receive_timestamp.nsec; // Receive timestamp (nano seconds part of ros timestamp immediately after first response byte received) - ROS_INFO_STREAM("DriverMonitor::serviceCbColaTelegram: finished Cola request { " << sick_lidar_localization::Utils::flattenToString(cola_request) << " } with response { " << sick_lidar_localization::Utils::flattenToString(cola_response) << " }"); + ROS::splitTime(send_timestamp, cola_response.send_timestamp_sec, cola_response.send_timestamp_nsec); // Send timestamp (seconds and nano seconds part of ros timestamp immediately before tcp send) + ROS::splitTime(receive_timestamp, cola_response.receive_timestamp_sec, cola_response.receive_timestamp_nsec); // Receive timestamp (seconds and nano seconds part of ros timestamp immediately after first response byte received) + ROS_INFO_STREAM("DriverMonitor::serviceCbColaTelegram: finished Cola request { " << cola_request.cola_ascii_request << " } with response { " << cola_response.cola_ascii_response << " }"); return true; } else @@ -257,7 +258,7 @@ bool sick_lidar_localization::DriverMonitor::serviceCbColaTelegram(sick_lidar_lo bool sick_lidar_localization::DriverMonitor::resultTelegramsReceiveStatusIsOk(void) { // Check timestamp of last result telegram - if((ros::Time::now() - m_driver_message_recv_timestamp.get()).toSec() <= m_receive_telegrams_timeout) + if(ROS::seconds(ROS::now() - m_driver_message_recv_timestamp.get()) <= m_receive_telegrams_timeout) return true; // OK: result telegram received within timeout // Call "sRN LocState" and check state of localization (no result telegrams when localization deactivated) @@ -267,13 +268,13 @@ bool sick_lidar_localization::DriverMonitor::resultTelegramsReceiveStatusIsOk(vo cola_telegram_request.wait_response_timeout = m_cola_response_timeout; if(!serviceCbColaTelegram(cola_telegram_request, cola_telegram_response)) { - ROS_WARN_STREAM("## ERROR DriverMonitor: serviceCbColaTelegram failed, cola request: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_request) << ", cola response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_response)); + ROS_WARN_STREAM("## ERROR DriverMonitor: serviceCbColaTelegram failed, cola request: " << cola_telegram_request.cola_ascii_request << ", cola response: " << cola_telegram_response.cola_ascii_response); return false; // Error: localization controller not responding } sick_lidar_localization::SickLocColaTelegramMsg cola_response = sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram_response.cola_ascii_response); if(cola_response.command_name != "LocState" || cola_response.parameter.size() != 1) { - ROS_WARN_STREAM("## ERROR DriverMonitor: unexpected cola response from localization controller, cola request: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_request) << ", cola response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_response)); + ROS_WARN_STREAM("## ERROR DriverMonitor: unexpected cola response from localization controller, cola request: " << cola_telegram_request.cola_ascii_request << ", cola response: " << cola_telegram_response.cola_ascii_response); return false; // Error decoding response from localization } if(cola_response.parameter[0] != "2") @@ -287,13 +288,13 @@ bool sick_lidar_localization::DriverMonitor::resultTelegramsReceiveStatusIsOk(vo cola_telegram_request.wait_response_timeout = m_cola_response_timeout; if(!serviceCbColaTelegram(cola_telegram_request, cola_telegram_response)) { - ROS_WARN_STREAM("## ERROR DriverMonitor: serviceCbColaTelegram failed, cola request: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_request) << ", cola response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_response)); + ROS_WARN_STREAM("## ERROR DriverMonitor: serviceCbColaTelegram failed, cola request: " << cola_telegram_request.cola_ascii_request << ", cola response: " << cola_telegram_response.cola_ascii_response); return false; // Error: localization controller not responding } cola_response = sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram_response.cola_ascii_response); if(cola_response.command_name != "LocResultState" || cola_response.parameter.size() != 1) { - ROS_WARN_STREAM("## ERROR DriverMonitor: unexpected cola response from localization controller, cola request: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_request) << ", cola response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_response)); + ROS_WARN_STREAM("## ERROR DriverMonitor: unexpected cola response from localization controller, cola request: " << cola_telegram_request.cola_ascii_request << ", cola response: " << cola_telegram_response.cola_ascii_response); return false; // Error decoding response from localization } if(cola_response.parameter[0] == "0") @@ -302,8 +303,15 @@ bool sick_lidar_localization::DriverMonitor::resultTelegramsReceiveStatusIsOk(vo return true; // OK: LocResultState == "0": Result telegrams deactivated } + // Check timestamp of last result telegram + ROS::Time driver_message_recv_timestamp = m_driver_message_recv_timestamp.get(); + ROS::Time current_time = ROS::now(); + if(ROS::seconds(current_time - driver_message_recv_timestamp) <= m_receive_telegrams_timeout) + return true; // OK: result telegram received within timeout // Timeout error: Localization and result telegrams activated, but no result telegrams received - ROS_WARN_STREAM("## ERROR DriverMonitor: Localization and result telegrams activated, timeout while waiting for result telegrams"); + ROS_WARN_STREAM("## ERROR DriverMonitor: Localization and result telegrams activated, timeout while waiting for result telegrams, current time: " + << ROS::secondsSinceStart(current_time) << " last message received: " << ROS::secondsSinceStart(driver_message_recv_timestamp) << ", delta_time: " + << ROS::seconds(current_time - driver_message_recv_timestamp) << ", configured timeout: " << m_receive_telegrams_timeout << " sec"); return false; } @@ -313,7 +321,7 @@ bool sick_lidar_localization::DriverMonitor::resultTelegramsReceiveStatusIsOk(vo void sick_lidar_localization::DriverMonitor::runMonitorThreadCb(void) { ROS_INFO_STREAM("DriverMonitor: monitoring thread started"); - while(ros::ok() && m_monitoring_thread_running) + while(ROS::ok() && m_monitoring_thread_running) { ROS_INFO_STREAM("DriverMonitor: starting connection thread"); sick_lidar_localization::DriverThread* driver_thread = new sick_lidar_localization::DriverThread(m_nh, m_server_adress, m_ip_port_results); @@ -322,28 +330,28 @@ void sick_lidar_localization::DriverMonitor::runMonitorThreadCb(void) ROS_ERROR_STREAM("## ERROR DriverMonitor: could not start tcp client thread"); } // initial wait until tcp connection established - while(ros::ok() && m_monitoring_thread_running && driver_thread->isRunning() && !driver_thread->isConnected()) + while(ROS::ok() && m_monitoring_thread_running && driver_thread->isRunning() && !driver_thread->isConnected()) { ROS_INFO_STREAM("DriverMonitor: waiting for connection to localization controller"); - ros::Duration(1).sleep(); // wait for initial tcp connection + ROS::sleep(1); // wait for initial tcp connection } // initial wait until monitoring starts - ros::Time initial_wait_end = ros::Time::now() + ros::Duration(std::max(m_receive_telegrams_timeout, 1.0/m_monitoring_rate)); - while(ros::ok() && m_monitoring_thread_running && driver_thread->isRunning() && driver_thread->isConnected() && ros::Time::now() < initial_wait_end) + ROS::Time initial_wait_end = ROS::now() + ROS::durationFromSec(std::max(m_receive_telegrams_timeout, 1.0/m_monitoring_rate)); + while(ROS::ok() && m_monitoring_thread_running && driver_thread->isRunning() && driver_thread->isConnected() && ROS::now() < initial_wait_end) { - ros::Duration(1).sleep(); // wait for monitoring start + ROS::sleep(1); // wait for monitoring start } // Monitor driver messages - ros::Duration monitoring_delay(1.0/m_monitoring_rate); - while(ros::ok() + double monitoring_delay = 1.0 / m_monitoring_rate; + while(ROS::ok() && m_monitoring_thread_running && driver_thread->isRunning() && driver_thread->isConnected() && resultTelegramsReceiveStatusIsOk()) { - monitoring_delay.sleep(); + ROS::sleep(monitoring_delay); } - if(ros::ok() && m_monitoring_thread_running) // timeout, telegram messages from driver missing + if(ROS::ok() && m_monitoring_thread_running) // timeout, telegram messages from driver missing { ROS_WARN_STREAM("DriverMonitor: tcp client thread timeout, closing tcp connections and restarting tcp thread."); driver_thread->shutdown(); diff --git a/src/driver_thread.cpp b/src/driver_thread.cpp index 94a98ce..8e63f6d 100644 --- a/src/driver_thread.cpp +++ b/src/driver_thread.cpp @@ -59,11 +59,10 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include #include -#include "sick_lidar_localization/SickLocDiagnosticMsg.h" #include "sick_lidar_localization/driver_thread.h" #include "sick_lidar_localization/testcase_generator.h" #include "sick_lidar_localization/utils.h" @@ -75,7 +74,7 @@ * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 * @param[in] tcp_port tcp port of the localization controller, default: The localization controller uses IP port number 2201 to send localization results */ -sick_lidar_localization::DriverThread::DriverThread(ros::NodeHandle * nh, const std::string & server_adress, int tcp_port) +sick_lidar_localization::DriverThread::DriverThread(ROS::NodePtr nh, const std::string & server_adress, int tcp_port) : m_initialized(false), m_tcp_connected(false), m_server_adress(server_adress), m_tcp_port(tcp_port), m_tcp_connection_retry_delay(1.0), m_tcp_receiver_thread(0), m_tcp_receiver_thread_running(false), m_converter_thread(0), m_converter_thread_running(false), @@ -86,24 +85,25 @@ sick_lidar_localization::DriverThread::DriverThread(ros::NodeHandle * nh, const // get config parameter std::string result_telegrams_topic = "/sick_lidar_localization/driver/result_telegrams"; // default topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) std::string diagnostic_topic = "/sick_lidar_localization/driver/diagnostic"; // default topic to publish diagnostic messages (type SickLocDiagnosticMsg) - ros::param::param("/sick_lidar_localization/driver/tcp_connection_retry_delay", m_tcp_connection_retry_delay, m_tcp_connection_retry_delay); - ros::param::param("/sick_lidar_localization/driver/result_telegrams_topic", result_telegrams_topic, result_telegrams_topic); - ros::param::param("/sick_lidar_localization/driver/result_telegrams_frame_id", m_result_telegrams_frame_id, "sick_lidar_localization"); - ros::param::param("/sick_lidar_localization/driver/diagnostic_topic", diagnostic_topic, diagnostic_topic); - ros::param::param("/sick_lidar_localization/driver/diagnostic_frame_id", m_diagnostic_frame_id, "sick_lidar_localization"); + ROS::param(nh, "/sick_lidar_localization/driver/tcp_connection_retry_delay", m_tcp_connection_retry_delay, m_tcp_connection_retry_delay); + ROS::param(nh, "/sick_lidar_localization/driver/result_telegrams_topic", result_telegrams_topic, result_telegrams_topic); + ROS::param(nh, "/sick_lidar_localization/driver/result_telegrams_frame_id", m_result_telegrams_frame_id, "sick_lidar_localization"); + ROS::param(nh, "/sick_lidar_localization/driver/diagnostic_topic", diagnostic_topic, diagnostic_topic); + ROS::param(nh, "/sick_lidar_localization/driver/diagnostic_frame_id", m_diagnostic_frame_id, "sick_lidar_localization"); int software_pll_fifo_length = 7, time_sync_initial_length = 10; double time_sync_rate = 0.1, time_sync_initial_rate = 1.0; - ros::param::param("/sick_lidar_localization/time_sync/software_pll_fifo_length", software_pll_fifo_length, software_pll_fifo_length); - ros::param::param("/sick_lidar_localization/time_sync/time_sync_rate", time_sync_rate, time_sync_rate); - ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_rate", time_sync_initial_rate, time_sync_initial_rate); - ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_length", time_sync_initial_length, time_sync_initial_length); + ROS::param(nh, "/sick_lidar_localization/time_sync/software_pll_fifo_length", software_pll_fifo_length, software_pll_fifo_length); + ROS::param(nh, "/sick_lidar_localization/time_sync/time_sync_rate", time_sync_rate, time_sync_rate); + ROS::param(nh, "/sick_lidar_localization/time_sync/time_sync_initial_rate", time_sync_initial_rate, time_sync_initial_rate); + ROS::param(nh, "/sick_lidar_localization/time_sync/time_sync_initial_length", time_sync_initial_length, time_sync_initial_length); assert(software_pll_fifo_length > 0 && time_sync_rate > FLT_EPSILON); m_software_pll_expected_initialization_duration = (software_pll_fifo_length / time_sync_initial_rate + 1); // expected initialization time for software pll (system time from lidar ticks not yet available) // ros publisher for result port telegram messages (type SickLocResultPortTelegramMsg) - m_result_telegrams_publisher = nh->advertise(result_telegrams_topic, 1); + m_result_telegrams_publisher = ROS_CREATE_PUBLISHER(nh, sick_lidar_localization::SickLocResultPortTelegramMsg, result_telegrams_topic); // ros publisher for diagnostic messages (type SickLocDiagnosticMsg) - m_diagnostic_publisher = nh->advertise(diagnostic_topic, 1); - m_timesync_service_client = nh->serviceClient("SickLocTimeSync"); + m_diagnostic_publisher = ROS_CREATE_PUBLISHER(nh, sick_lidar_localization::SickLocDiagnosticMsg, diagnostic_topic); + // ros service client for SickLocTimeSync + m_timesync_service_client = ROS_CREATE_SRV_CLIENT(nh, sick_lidar_localization::SickLocTimeSyncSrv, "SickLocTimeSync"); m_initialized = true; } } @@ -187,11 +187,11 @@ bool sick_lidar_localization::DriverThread::stop(bool force_shutdown) void sick_lidar_localization::DriverThread::publishDiagnosticMessage(const DRIVER_ERROR_CODES & error_code, const std::string & message) { sick_lidar_localization::SickLocDiagnosticMsg msg; - msg.header.stamp = ros::Time::now(); + msg.header.stamp = ROS::now(); msg.header.frame_id = m_diagnostic_frame_id; msg.error_code = (int32_t)error_code; msg.message = message; - m_diagnostic_publisher.publish(msg); + ROS_PUBLISH(m_diagnostic_publisher, msg); } /* @@ -231,9 +231,9 @@ void sick_lidar_localization::DriverThread::runReceiverThreadCb(void) ROS_INFO_STREAM("DriverThread: receiver thread started"); try { - ros::Time diagnostic_msg_published; + ROS::Time diagnostic_msg_published; // Connect to localization controller - while(ros::ok() && m_tcp_receiver_thread_running && !m_tcp_connected) + while(ROS::ok() && m_tcp_receiver_thread_running && !m_tcp_connected) { if(m_tcp_socket.connect(m_ioservice, m_server_adress, m_tcp_port)) { @@ -245,18 +245,18 @@ void sick_lidar_localization::DriverThread::runReceiverThreadCb(void) { publishDiagnosticMessage(NO_TCP_CONNECTION, std::string("sim_loc_driver: no tcp connection to localization controller ") + m_server_adress + ":" + std::to_string(m_tcp_port)); ROS_WARN_STREAM("DriverThread: no connection to localization controller " << m_server_adress << ":" << m_tcp_port << ", retry in " << m_tcp_connection_retry_delay << " seconds"); - ros::Duration(m_tcp_connection_retry_delay).sleep(); + ROS::sleep(m_tcp_connection_retry_delay); } } // Receive binary telegrams from localization controller size_t telegram_size = sick_lidar_localization::TestcaseGenerator::createDefaultResultPortTestcase().binary_data.size(); // 106 byte result port telegrams - while(ros::ok() && m_tcp_receiver_thread_running && m_tcp_socket.socket().is_open()) + while(ROS::ok() && m_tcp_receiver_thread_running && m_tcp_socket.socket().is_open()) { size_t bytes_received = 0, bytes_required = telegram_size; std::vector receive_buffer(bytes_required, 0); boost::system::error_code errorcode; std::string error_info(""); - while(ros::ok() && m_tcp_receiver_thread_running && m_tcp_socket.socket().is_open() && bytes_received < bytes_required) + while(ROS::ok() && m_tcp_receiver_thread_running && m_tcp_socket.socket().is_open() && bytes_received < bytes_required) { size_t bytes_to_read = bytes_required - bytes_received; bytes_received += boost::asio::read(m_tcp_socket.socket(), boost::asio::buffer(&receive_buffer[bytes_received],bytes_to_read), boost::asio::transfer_exactly(bytes_to_read), errorcode); @@ -271,20 +271,20 @@ void sick_lidar_localization::DriverThread::runReceiverThreadCb(void) } error_info = error_info_stream.str(); } - else if( (ros::Time::now() - diagnostic_msg_published).toSec() >= 60) + else if( ROS::seconds(ROS::now() - diagnostic_msg_published) >= 60) { publishDiagnosticMessage(NO_ERROR, std::string("sim_loc_driver: tcp connection established to localization controller ") + m_server_adress + ":" + std::to_string(m_tcp_port)); - diagnostic_msg_published = ros::Time::now(); + diagnostic_msg_published = ROS::now(); } if(bytes_received < bytes_required) - ros::Duration(0.0001).sleep(); + ROS::sleep(0.0001); } // Copy received telegram to fifo if(bytes_received >= bytes_required) { m_fifo_buffer.push(receive_buffer); ROS_DEBUG_STREAM("DriverThread: received " << bytes_received << " byte telegram (hex): " << sick_lidar_localization::Utils::toHexString(receive_buffer)); - ros::Duration(0.0001).sleep(); + ROS::sleep(0.0001); } } } @@ -306,10 +306,10 @@ void sick_lidar_localization::DriverThread::runConverterThreadCb(void) publishDiagnosticMessage(NO_ERROR, "sim_loc_driver: converter thread started"); ROS_INFO_STREAM("DriverThread: converter thread started"); // Decode and publish result port telegrams - ros::Time diagnostic_msg_published; - ros::Time timestamp_first_telegram; + ROS::Time diagnostic_msg_published; + ROS::Time timestamp_first_telegram; sick_lidar_localization::ResultPortParser result_port_parser(m_result_telegrams_frame_id); - while(ros::ok() && m_converter_thread_running) + while(ROS::ok() && m_converter_thread_running) { m_fifo_buffer.waitForElement(); // Wait for at least one element in the fifo buffer if(!m_fifo_buffer.empty()) @@ -333,35 +333,50 @@ void sick_lidar_localization::DriverThread::runConverterThreadCb(void) result_telegram.vehicle_time_valid = false; result_telegram.vehicle_time_sec = 0; result_telegram.vehicle_time_nsec = 0; - if(timestamp_first_telegram.sec <= 0) - timestamp_first_telegram = ros::Time::now(); + if(ROS::secondsSinceStart(timestamp_first_telegram) < FLT_EPSILON) + timestamp_first_telegram = ROS::now(); + +#if defined __ROS_VERSION && __ROS_VERSION == 1 sick_lidar_localization::SickLocTimeSyncSrv time_sync_msg; - time_sync_msg.request.timestamp_lidar_ms = result_telegram.telegram_payload.Timestamp; - if (m_timesync_service_client.call(time_sync_msg) && time_sync_msg.response.vehicle_time_valid) + sick_lidar_localization::SickLocTimeSyncSrv::Request* time_sync_msg_request = &time_sync_msg.request; + sick_lidar_localization::SickLocTimeSyncSrv::Response* time_sync_msg_response = &time_sync_msg.response; +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + std::shared_ptr time_sync_msg_request = std::make_shared(); +#endif + time_sync_msg_request->timestamp_lidar_ms = result_telegram.telegram_payload.timestamp; +#if defined __ROS_VERSION && __ROS_VERSION == 1 + bool service_call_ok = m_timesync_service_client.call(time_sync_msg); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + auto service_future = m_timesync_service_client->async_send_request(time_sync_msg_request); + auto future_status = service_future.wait_for(std::chrono::seconds(1)); // wait for the response + bool service_call_ok = (future_status == std::future_status::ready); // service successfully completed + std::shared_ptr time_sync_msg_response = (service_call_ok ? (service_future.get()) : (std::make_shared())); +#endif + if (service_call_ok && time_sync_msg_response->vehicle_time_valid) { - result_telegram.vehicle_time_valid = time_sync_msg.response.vehicle_time_valid; - result_telegram.vehicle_time_sec = time_sync_msg.response.vehicle_time_sec; - result_telegram.vehicle_time_nsec = time_sync_msg.response.vehicle_time_nsec; - ROS_DEBUG_STREAM("sim_loc_driver: Lidar ticks: " << result_telegram.telegram_payload.Timestamp << ", Systemtime by pll: " << result_telegram.vehicle_time_sec << "." << result_telegram.vehicle_time_sec); + result_telegram.vehicle_time_valid = time_sync_msg_response->vehicle_time_valid; + result_telegram.vehicle_time_sec = time_sync_msg_response->vehicle_time_sec; + result_telegram.vehicle_time_nsec = time_sync_msg_response->vehicle_time_nsec; + ROS_DEBUG_STREAM("sim_loc_driver: Lidar ticks: " << result_telegram.telegram_payload.timestamp << ", Systemtime by pll: " << result_telegram.vehicle_time_sec << "." << result_telegram.vehicle_time_sec); } - else if((ros::Time::now() - timestamp_first_telegram).toSec() <= m_software_pll_expected_initialization_duration) // software pll still initializing + else if(ROS::seconds(ROS::now() - timestamp_first_telegram) <= m_software_pll_expected_initialization_duration) // software pll still initializing ROS_DEBUG_STREAM("sim_loc_driver: no system time from ticks, software pll still initializing"); else // time sync error - ROS_WARN_STREAM("## ERROR sim_loc_driver: service \"SickLocTimeSync\" failed, could not get system time from ticks"); + ROS_WARN_STREAM("## ERROR sim_loc_driver: service \"SickLocTimeSync\" failed, could not get system time from ticks (" << __FILE__ << ":" << __LINE__ << ")"); // Publish the decoded result port telegram (type SickLocResultPortTelegramMsg) - m_result_telegrams_publisher.publish(result_telegram); + ROS_PUBLISH(m_result_telegrams_publisher, result_telegram); ROS_INFO_STREAM("DriverThread: result telegram received " << sick_lidar_localization::Utils::toHexString(binary_telegram) << ", published " << sick_lidar_localization::Utils::flattenToString(result_telegram)); - if( (ros::Time::now() - diagnostic_msg_published).toSec() >= 60) + if( ROS::seconds(ROS::now() - diagnostic_msg_published) >= 60) { publishDiagnosticMessage(NO_ERROR, "sim_loc_driver: status okay, receiving and publishing result telegrams"); - diagnostic_msg_published = ros::Time::now(); + diagnostic_msg_published = ROS::now(); } } } } else { - ros::Duration(0.001).sleep(); + ROS::sleep(0.001); } } m_converter_thread_running = false; diff --git a/src/odom_converter.cpp b/src/odom_converter.cpp new file mode 100755 index 0000000..7bfd38a --- /dev/null +++ b/src/odom_converter.cpp @@ -0,0 +1,377 @@ +/* + * @brief odom_converter subscribes to odometry messages, converts (vx, vy, omega) to binary odometry telegrams + * and sends the telegrams to the localization server by udp. + * See Operating-Instructions-EN-LLS-1.2.0.300R.pdf, chapter 5.9 "About Odometry for support", page 44-48 + * and Odometry_LLS.py for further details. + * + * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2020 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2020 SICK AG + * Copyright 2020 Ing.-Buero Dr. Michael Lehning + * + */ +#include +#include + +#include "sick_lidar_localization/odom_converter.h" + +/** static odometry telegram counter */ +sick_lidar_localization::SetGet32 sick_lidar_localization::OdomConverter::s_odom_telegram_count(0); + +namespace sick_lidar_localization +{ + /*! + * @brief class UdpSenderSocketImpl implements the udp socket for sending udp packages, uses boost::asio::ip::udp + */ + class UdpSenderSocketImpl + { + public: + /*! + * Constructor, opens an udp socket. + * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 + * @param[in] udp_port_odom udp port for odometry telegrams, default: 3000 + */ + UdpSenderSocketImpl(const std::string & server_adress = "192.168.0.1", int udp_port_odom = 3000) + : m_io_service(), m_udp_socket(m_io_service), m_socket_opened(false) + { + try + { + m_udp_socket.open(boost::asio::ip::udp::v4()); + m_socket_opened = m_udp_socket.is_open(); + if (!m_socket_opened) + { + ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl(" << server_adress << ":" << udp_port_odom << "): can't open udp socket."); + } + // m_udp_socket.set_option(boost::asio::socket_base::reuse_address(true)); + m_udp_socket.set_option(boost::asio::socket_base::broadcast(true)); + m_udp_endpoint = boost::asio::ip::udp::endpoint(boost::asio::ip::address::from_string(server_adress), udp_port_odom); + } + catch(const std::exception& e) + { + m_socket_opened = false; + ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl(): socket initialization failed, exception " << e.what()); + } + } + + /*! + * Returns true if the udp socket is opened and ready to send, or false otherwise. + */ + bool IsOpen(void) const { return m_socket_opened; } + + /*! + * Sends a binary telegram. + */ + bool Send(std::vector & binary_telegram) + { + size_t bytes_sent = 0; + if(m_socket_opened) + { + try + { + bytes_sent = m_udp_socket.send_to(boost::asio::buffer(binary_telegram.data(), binary_telegram.size()), m_udp_endpoint); + if(bytes_sent != binary_telegram.size()) + { + ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl()::Send(): " << bytes_sent << " of " << binary_telegram.size() << " bytes sent, boost::asio::ip::udp::socket::send_to() failed."); + } + } + catch(const std::exception& e) + { + ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl()::Send(): boost::asio::ip::udp::socket::send_to() failed, exception " << e.what()); + } + } + else + { + ROS_ERROR_STREAM("## ERROR UdpSenderSocketImpl()::Send(): udp socket not initialized"); + } + return (bytes_sent == binary_telegram.size()); + } + + protected: + + bool m_socket_opened; ///< true if the udp socket is opened and ready to send, or false otherwise + boost::asio::io_service m_io_service; ///< boost io for udp socket + boost::asio::ip::udp::socket m_udp_socket; ///< udp socket for binary odom telegrams + boost::asio::ip::udp::endpoint m_udp_endpoint; ///< udp receiver (i.e. the localizaton server) + }; +} // namespace sick_lidar_localization + +/*! + * Constructor. + * @param[in] nh ros node handle + * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 + * @param[in] udp_port_odom udp port for odometry telegrams, default: 3000 + * @param[in] run_unittest run a short unittest, default: true + */ +sick_lidar_localization::OdomConverter::OdomConverter(ROS::NodePtr nh, const std::string & server_adress, int udp_port_odom, bool run_unittest) +: m_nh(nh), m_server_adress(server_adress), m_udp_port_odom(udp_port_odom), m_odom_telegrams_bigendian(1), m_odom_telegrams_source_id(1), m_udp_sender_impl(0) + +{ + if(nh) + { + ROS::param(nh, "/sick_lidar_localization/driver/odom_telegrams_bigendian", m_odom_telegrams_bigendian, m_odom_telegrams_bigendian); // Send udp odometry telegrams big endian (1) or little endian (0) + ROS::param(nh, "/sick_lidar_localization/driver/odom_telegrams_source_id", m_odom_telegrams_source_id, m_odom_telegrams_source_id); // SourceID of udp odometry telegrams, e.g. vehicle controller 1 + // Initialize upd sender + m_udp_sender_impl = new sick_lidar_localization::UdpSenderSocketImpl(m_server_adress, m_udp_port_odom); + if(!m_udp_sender_impl->IsOpen()) + { + ROS_ERROR_STREAM("## ERROR OdomConverter(" << server_adress << ":" << udp_port_odom << "): can't initialize udp socket."); + } + } + // Run unittest + if(run_unittest && !Unittest(false)) + { + ROS_ERROR_STREAM("## ERROR OdomConverter::Unittest() failed."); + Unittest(true); // repeat unittest with verbose=true + } +} + +/*! + * Callback for ros odometry messages. Converts the message to a binary odom telegram and sends the telegram to the localization server by udp. + * @param[in] msg odometry message + */ +void sick_lidar_localization::OdomConverter::messageCbOdometry(const sick_lidar_localization::OdomMsg & msg) +{ + // Get timestamp, linear and angular velocity from odom message + ROS::Time timestamp_msg = ROS::timeFromHeader(&msg.header); + double vx_ms = msg.twist.twist.linear.x; // vx in m/s + double vy_ms = msg.twist.twist.linear.y; // vy in m/s + double omega_rs = msg.twist.twist.angular.z; // omega in rad/s + + // Convert to binary telegram + int16_t vx_mms = (int16_t)std::round(1000.0 * vx_ms); // x-component of velocity in mm/s + int16_t vy_mms = (int16_t)std::round(1000.0 * vy_ms); // y-component of velocity in mm/s + int32_t omega_mdegs = (int32_t)std::round(1000.0 * 180.0 * omega_rs / M_PI); // angular velocity in mdeg/s + uint32_t timestamp_msec = (uint32_t)ROS::timestampMilliseconds(timestamp_msg); // message timestamp in ms + bool bigendian = (m_odom_telegrams_bigendian > 0); // encode telegram payload big or little endian + uint16_t source_id = (uint16_t)(m_odom_telegrams_source_id & 0xFFFF); // configurable source id (sender id) + uint32_t telegram_count = s_odom_telegram_count.inc(); // increasing odom telegram counter + std::vector binary_telegram = Encode(bigendian, source_id, telegram_count, timestamp_msec, vx_mms, vy_mms, omega_mdegs); // binary telegram + + // Send binary telegram + if(m_udp_sender_impl == 0 || !m_udp_sender_impl->IsOpen()) // Reopen socket in case of network errors + { + delete(m_udp_sender_impl); + m_udp_sender_impl = new sick_lidar_localization::UdpSenderSocketImpl(m_server_adress, m_udp_port_odom); + } + if(m_udp_sender_impl == 0 || !m_udp_sender_impl->IsOpen()) + { + ROS_ERROR_STREAM("## ERROR OdomConverter(" << m_server_adress << ":" << m_udp_port_odom << "): can't initialize udp socket."); + } + else + { + if(!m_udp_sender_impl->Send(binary_telegram)) + { + // upd send failed, reopen socket on next telegram + ROS_ERROR_STREAM("OdomConverter::messageCbOdometry(vx=" << vx_ms << ",vy=" << vy_ms << ",vth_rs=" << omega_rs << ",timestamp_sec=" << ROS::secondsSinceStart(timestamp_msg) + << "): failed to send udp telegram " << ToHexString(binary_telegram)); + delete(m_udp_sender_impl); + m_udp_sender_impl = 0; + } + else + { + ROS_INFO_STREAM("OdomConverter::messageCbOdometry(vx=" << vx_ms << ",vy=" << vy_ms << ",vth_rs=" << omega_rs << ",timestamp_sec=" << ROS::secondsSinceStart(timestamp_msg) << "): udp telegram " + << ToHexString(binary_telegram) << " sent (vx_mms=" << vx_mms << ",vy_mms=" << vy_mms << ",vth_mdegs=" << omega_mdegs << ",timestamp_msec=" << timestamp_msec << ")"); + } + } +} + +/*! + * @brief Runs a unit test by converting some odometry messages and checking the result. + * @param[in] verbose true: print info messages, false: print error messages only + * @return unit test passed (true) or failed (false) + */ +bool sick_lidar_localization::OdomConverter::Unittest(bool verbose) +{ + return Unittest(true, 1, 1000, 50000, -500, 500, -1000, "534D4F5000100642000100010001000003E80000C350FE0C01F4FFFFFC18") + && Unittest(true, 100, 126, (uint32_t)1599489146050U, 200, 0, 10000, "534D4F50001006420001000100640000007E68FB7CC200C8000000002710") + && Unittest(true, 100, 481, (uint32_t)1599501152450U, -200, -300, -5000, "534D4F5000100642000100010064000001E169B2B0C2FF38FED4FFFFEC78") + && Unittest(true, 100, 1365, (uint32_t)1599502220425U, 200, 300, 5000, "534D4F50001006420001000100640000055569C2FC8900C8012C00001388"); +} + +/*! + * @brief Runs a single test by converting a odometry messages and checking the result. + * @param[in] bigendian send payload big endian (true) or little endian (false) + * @param[in] SourceID: 1 (e.g. vehicle controller 1) + * @param[in] TelegramCount telegram counter + * @param[in] Timestamp timestamp in ms + * @param[in] vx x-component of velocity in mm/s + * @param[in] vy y-component of velocity in mm/s + * @param[in] omega angular velocity in mdeg/s + * @param[in] hex_result expected result (binary odometry telegram as hex string) + * @param[in] verbose true: print info messages, false: print error messages only + * @return unit test passed (true) or failed (false) + */ +bool sick_lidar_localization::OdomConverter::Unittest(bool bigendian, uint16_t SourceID, uint32_t TelegramCount, uint32_t Timestamp, int16_t vx, int16_t vy, int32_t omega, const std::string & hex_result, bool verbose) +{ + std::vector binary_telegram = Encode(bigendian, SourceID, TelegramCount, Timestamp, vx, vy, omega); + std::string hex_encoded = ToHexString(binary_telegram); + if(hex_result != hex_encoded) + { + ROS_ERROR_STREAM("## ERROR OdomConverter::Unittest(" << bigendian << ", " << SourceID << ", " << TelegramCount << ", " << Timestamp << ", " << vx << ", " << vy << ", " << omega + << ") failed: encoded: \"" << hex_encoded << "\", expected: \"" << hex_result << "\""); + return false; + } + if(verbose) + { + ROS_INFO_STREAM("OdomConverter::Unittest(" << bigendian << ", " << SourceID << ", " << TelegramCount << ", " << Timestamp << ", " << vx << ", " << vy << ", " << omega + << ") passed: encoded: \"" << hex_encoded << "\", expected: \"" << hex_result << "\""); + } + else + { + ROS_DEBUG_STREAM("OdomConverter::Unittest(" << bigendian << ", " << SourceID << ", " << TelegramCount << ", " << Timestamp << ", " << vx << ", " << vy << ", " << omega + << ") passed: encoded: \"" << hex_encoded << "\", expected: \"" << hex_result << "\""); + } + return true; +} + +/*! + * Converts and returns a byte vector to hex string + */ +std::string sick_lidar_localization::OdomConverter::ToHexString(const std::vector & binary_telegram) +{ + std::stringstream str; + for(size_t n = 0; n < binary_telegram.size(); n++) + { + str << std::uppercase << std::hex << std::setfill('0') << std::setw(2) << (uint32_t)binary_telegram[n]; + } + return str.str(); +} + +/*! + * @brief Converts (timestamp, vx, vy, omega) to binary odometry telegrams. + * Telegram format: + * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + * | H E A D E R | P A Y L O A D | + * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + * | MagicWord PayloadLength payloadType(endianess) MsgType MsgTypeVersion SourceID | telegramCounter timestamp Vx Vy Omega | + * | 534d4f50 000C 0642 0000 0000 0000 | 00000001 00000000 0001 FFFF 00000001 | + * - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + * Note: The header always has Big Endian format + * @param[in] bigendian send payload big endian (true) or little endian (false) + * @param[in] SourceID: 1 (e.g. vehicle controller 1) + * @param[in] TelegramCount telegram counter + * @param[in] Timestamp timestamp in ms + * @param[in] vx x-component of velocity in mm/s + * @param[in] vy y-component of velocity in mm/s + * @param[in] omega angular velocity in mdeg/s + * @return binary odometry telegram + */ +std::vector sick_lidar_localization::OdomConverter::Encode(bool bigendian, uint16_t SourceID, uint32_t TelegramCount, uint32_t Timestamp, int16_t vx, int16_t vy, int32_t omega) +{ + std::vector binary_telegram; + // MagicWord Char32 SMOP 4 byte (SMOP: Sick Mobile Platforms UDP telegram) + binary_telegram.push_back('S'); + binary_telegram.push_back('M'); + binary_telegram.push_back('O'); + binary_telegram.push_back('P'); + // Header: 2 byte PayloadLength UInt16: Number of bytes per payload, here always 4+4+2+2+4 = 16 + EncodeAndPush(binary_telegram, (uint16_t)(16), true); + // Header: 2 byte PayloadType (endianness) UInt16 (BE: 0x0642, LE: 0x06c2) + EncodeAndPush(binary_telegram, (uint16_t)(bigendian ? 0x0642 : 0x06C2), true); + // Header: 2 byte MsgType UInt16: For odometry messages, MsgType = 1 is used + EncodeAndPush(binary_telegram, (uint16_t)(1), true); + // Header: 2 byte MsgTypeVersion UInt16: Version of the MsgType payload (previously only MsgTypeVersion = 1) + EncodeAndPush(binary_telegram, (uint16_t)(1), true); + // Header: 2 byte SourceID UInt16: e.g. vehicle controller 1 + EncodeAndPush(binary_telegram, (uint16_t)(SourceID), true); + // Payload: 4 byte TelegramCount UInt32 + EncodeAndPush(binary_telegram, (uint32_t)(TelegramCount), bigendian); + // Payload: 4 byte Timestamp UInt32 in ms + EncodeAndPush(binary_telegram, (uint32_t)(Timestamp), bigendian); + // Payload: 2 byte vx Int16 in mm/s + EncodeAndPush(binary_telegram, (uint16_t)(vx), bigendian); + // Payload: 2 byte vy Int16 in mm/s + EncodeAndPush(binary_telegram, (uint16_t)(vy), bigendian); + // Payload: 4 byte vx Int16 in mm/s + EncodeAndPush(binary_telegram, (uint32_t)(omega), bigendian); + return binary_telegram; +} + +/*! + * @brief Converts and pushes 2 byte to binary odometry telegram. + * @param[out] binary_telegram byte vetor to append 2 bytes + * @param[in] value: bytes to encode in big or little endian order + * @param[in] bigendian send payload big endian (true) or little endian (false) + */ +void sick_lidar_localization::OdomConverter::EncodeAndPush(std::vector & binary_telegram, uint16_t value, bool bigendian) +{ + if(bigendian) + { + binary_telegram.push_back((value >> 8) & 0xFF); + binary_telegram.push_back((value) & 0xFF); + } + else + { + binary_telegram.push_back((value) & 0xFF); + binary_telegram.push_back((value >> 8) & 0xFF); + } +} + +/*! + * @brief Converts and pushes 4 byte to binary odometry telegram. + * @param[out] binary_telegram byte vetor to append 4 bytes + * @param[in] value: bytes to encode in big or little endian order + * @param[in] bigendian send payload big endian (true) or little endian (false) + */ +void sick_lidar_localization::OdomConverter::EncodeAndPush(std::vector & binary_telegram, uint32_t value, bool bigendian) +{ + if(bigendian) + { + binary_telegram.push_back((value >> 24) & 0xFF); + binary_telegram.push_back((value >> 16) & 0xFF); + binary_telegram.push_back((value >> 8) & 0xFF); + binary_telegram.push_back((value) & 0xFF); + } + else + { + binary_telegram.push_back((value) & 0xFF); + binary_telegram.push_back((value >> 8) & 0xFF); + binary_telegram.push_back((value >> 16) & 0xFF); + binary_telegram.push_back((value >> 24) & 0xFF); + } +} \ No newline at end of file diff --git a/src/pointcloud_converter.cpp b/src/pointcloud_converter.cpp index e9172c9..0e73eb2 100644 --- a/src/pointcloud_converter.cpp +++ b/src/pointcloud_converter.cpp @@ -56,36 +56,43 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/pointcloud_converter.h" int main(int argc, char** argv) { // Ros configuration and initialization - ros::init(argc, argv, "pointcloud_converter"); - ros::NodeHandle nh; + ROS::init(argc, argv, "pointcloud_converter"); + ROS::NodePtr nh = ROS::createNode("pointcloud_converter"); ROS_INFO_STREAM("pointcloud_converter started."); std::string result_telegrams_topic = "/sick_lidar_localization/driver/result_telegrams"; // default topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) - ros::param::param("/sick_lidar_localization/driver/result_telegrams_topic", result_telegrams_topic, result_telegrams_topic); + ROS::param(nh, "/sick_lidar_localization/driver/result_telegrams_topic", 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); + sick_lidar_localization::PointCloudConverter pointcloud_converter(nh); // Subscribe to sim_loc_driver messages - ros::Subscriber result_telegram_subscriber = nh.subscribe(result_telegrams_topic, 1, &sick_lidar_localization::PointCloudConverter::messageCbResultPortTelegrams, &pointcloud_converter); +#if defined __ROS_VERSION && __ROS_VERSION == 1 + sick_lidar_localization::SickLocResultPortTelegramMsgSubscriber result_telegram_subscriber + = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::SickLocResultPortTelegramMsg, result_telegrams_topic, &sick_lidar_localization::PointCloudConverter::messageCbResultPortTelegrams, &pointcloud_converter); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + sick_lidar_localization::SickLocResultPortTelegramMsgSubscriber result_telegram_subscriber + = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::SickLocResultPortTelegramMsg, result_telegrams_topic, &sick_lidar_localization::PointCloudConverter::messageCbResultPortTelegramsROS2, &pointcloud_converter); +#endif // Start pointcloud converter thread pointcloud_converter.start(); // Run ros event loop - ros::spin(); + ROS::spin(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."); + ROS::deleteNode(nh); return 0; } diff --git a/src/pointcloud_converter_thread.cpp b/src/pointcloud_converter_thread.cpp index d955585..d747d35 100644 --- a/src/pointcloud_converter_thread.cpp +++ b/src/pointcloud_converter_thread.cpp @@ -68,17 +68,17 @@ * Constructor * @param[in] nh ros node handle */ -sick_lidar_localization::PointCloudConverter::PointCloudConverter(ros::NodeHandle* nh) -: m_point_cloud_frame_id("sick_lidar_localization"), m_converter_thread_running(false), m_converter_thread(0) +sick_lidar_localization::PointCloudConverter::PointCloudConverter(ROS::NodePtr 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 SickLocResultPortTelegramMsg) - ros::param::param("/sick_lidar_localization/driver/point_cloud_topic", point_cloud_topic, point_cloud_topic); - ros::param::param("/sick_lidar_localization/driver/point_cloud_frame_id", m_point_cloud_frame_id, m_point_cloud_frame_id); - ros::param::param("/sick_lidar_localization/driver/tf_parent_frame_id", m_tf_parent_frame_id, m_tf_parent_frame_id); - ros::param::param("/sick_lidar_localization/driver/tf_child_frame_id", m_tf_child_frame_id, m_tf_child_frame_id); - m_point_cloud_publisher = nh->advertise(point_cloud_topic, 1); + ROS::param(nh, "/sick_lidar_localization/driver/point_cloud_topic", point_cloud_topic, point_cloud_topic); + ROS::param(nh, "/sick_lidar_localization/driver/point_cloud_frame_id", m_point_cloud_frame_id, m_point_cloud_frame_id); + ROS::param(nh, "/sick_lidar_localization/driver/tf_parent_frame_id", m_tf_parent_frame_id, m_tf_parent_frame_id); + ROS::param(nh, "/sick_lidar_localization/driver/tf_child_frame_id", m_tf_child_frame_id, m_tf_child_frame_id); + m_point_cloud_publisher = ROS_CREATE_PUBLISHER(nh, sick_lidar_localization::PointCloud2Msg, point_cloud_topic); } } @@ -175,19 +175,19 @@ sensor_msgs::PointCloud2 sick_lidar_localization::PointCloudConverter::convertTo // 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.telegram_payload.PoseX, // x-position in meter - 1.0e-3 * msg.telegram_payload.PoseY, // y-position in meter - 1.0e-3 * msg.telegram_payload.PoseYaw * M_PI / 180.0, // yaw angle in radians + 1.0e-3 * msg.telegram_payload.posex, // x-position in meter + 1.0e-3 * msg.telegram_payload.posey, // y-position in meter + 1.0e-3 * msg.telegram_payload.poseyaw * 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.vehicle_time_valid) // software pll initialized: use system time of vehicle pose calculated from lidar ticks by software pll { pointcloud_msg.header.stamp.sec = msg.vehicle_time_sec; - pointcloud_msg.header.stamp.nsec = msg.vehicle_time_nsec; + pointcloud_msg.header.stamp.NSEC = msg.vehicle_time_nsec; } pointcloud_msg.header.frame_id = m_point_cloud_frame_id; - pointcloud_msg.header.seq = 0; + // pointcloud_msg.header.seq = 0; // no seq field in ROS2 // clear cloud data pointcloud_msg.height = 0; pointcloud_msg.width = 0; @@ -228,15 +228,15 @@ sensor_msgs::PointCloud2 sick_lidar_localization::PointCloudConverter::convertTo */ geometry_msgs::TransformStamped sick_lidar_localization::PointCloudConverter::convertToTransform(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg) { - double posx = 1.0e-3 * msg.telegram_payload.PoseX; // x-position in meter - double posy = 1.0e-3 * msg.telegram_payload.PoseY; // y-position in meter - double yaw = 1.0e-3 * msg.telegram_payload.PoseYaw * M_PI / 180.0; // yaw angle in radians + double posx = 1.0e-3 * msg.telegram_payload.posex; // x-position in meter + double posy = 1.0e-3 * msg.telegram_payload.posey; // y-position in meter + double yaw = 1.0e-3 * msg.telegram_payload.poseyaw * M_PI / 180.0; // yaw angle in radians geometry_msgs::TransformStamped vehicle_transform; vehicle_transform.header.stamp = msg.header.stamp; // telegram timestamp if(msg.vehicle_time_valid) // software pll initialized: use system time of vehicle pose calculated from lidar ticks by software pll { vehicle_transform.header.stamp.sec = msg.vehicle_time_sec; - vehicle_transform.header.stamp.nsec = msg.vehicle_time_nsec; + vehicle_transform.header.stamp.NSEC = msg.vehicle_time_nsec; } vehicle_transform.header.frame_id = m_tf_parent_frame_id; vehicle_transform.child_frame_id = m_tf_child_frame_id; @@ -260,21 +260,26 @@ geometry_msgs::TransformStamped sick_lidar_localization::PointCloudConverter::co 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; - while(ros::ok() && m_converter_thread_running) +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + tf2_ros::TransformBroadcaster tf_broadcaster(m_nh); +#endif + + while(ROS::ok() && m_converter_thread_running) { // Wait for next telegram - while(ros::ok() && m_converter_thread_running && m_result_port_telegram_fifo.empty()) + while(ROS::ok() && m_converter_thread_running && m_result_port_telegram_fifo.empty()) { - ros::Duration(0.0001).sleep(); + ROS::sleep(0.0001); m_result_port_telegram_fifo.waitForElement(); } - if(ros::ok() && m_converter_thread_running && !m_result_port_telegram_fifo.empty()) + if(ROS::ok() && m_converter_thread_running && !m_result_port_telegram_fifo.empty()) { sick_lidar_localization::SickLocResultPortTelegramMsg telegram = m_result_port_telegram_fifo.pop(); // Convert vehicle position from result telegram to PointCloud2 sensor_msgs::PointCloud2 pointcloud_msg = convertToPointCloud(telegram); - m_point_cloud_publisher.publish(pointcloud_msg); + ROS_PUBLISH(m_point_cloud_publisher, pointcloud_msg); // Convert vehicle pose from result telegram to tf transform geometry_msgs::TransformStamped tf2_vehicle_transform = convertToTransform(telegram); tf_broadcaster.sendTransform(tf2_vehicle_transform); diff --git a/src/result_port_parser.cpp b/src/result_port_parser.cpp index d33173f..611af1c 100644 --- a/src/result_port_parser.cpp +++ b/src/result_port_parser.cpp @@ -54,7 +54,7 @@ * */ #include -#include +#include "sick_lidar_localization/ros_wrapper.h" #include "crc16ccitt_false.h" #include "sick_lidar_localization/result_port_parser.h" @@ -191,37 +191,37 @@ size_t sick_lidar_localization::ResultPortParser::decodeResultPortHeader(const s size_t bytes_decoded = 0; // Decode MagicWord: Magic word SICK (0x53 0x49 0x43 0x4B). Size: 4 × UInt8 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.MagicWord, "Header.MagicWord"); - PARSE_ASSERT(telegram_header.MagicWord == 0x5349434B, std::string("ResultPortParser::decodeResultPortHeader(): invalid Header.MagicWord ") + std::to_string(telegram_header.MagicWord)); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.magicword, "Header.MagicWord"); + PARSE_ASSERT(telegram_header.magicword == 0x5349434B, std::string("ResultPortParser::decodeResultPortHeader(): invalid Header.MagicWord ") + std::to_string(telegram_header.magicword)); // Decode Length: Length of telegram incl. header, payload, and trailer. Size: UInt32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.Length, "Header.Length"); - PARSE_ASSERT(telegram_header.Length == 106, std::string("ResultPortParser::decodeResultPortHeader(): invalid Header.Length ") + std::to_string(telegram_header.Length)); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.length, "Header.Length"); + PARSE_ASSERT(telegram_header.length == 106, std::string("ResultPortParser::decodeResultPortHeader(): invalid Header.Length ") + std::to_string(telegram_header.length)); // Decode PayloadType: Payload type 0x06c2 = Little Endian, 0x0642 = Big Endian. Size: UInt16 = 2 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.PayloadType, "Header.PayloadType"); - PARSE_ASSERT(telegram_header.PayloadType == 0x06c2 || telegram_header.PayloadType == 0x0642, std::string("ResultPortParser::decodeResultPortHeader(): invalid PayloadType ") + std::to_string(telegram_header.PayloadType)); - m_little_endian_payload = isLittleEndianPayload(telegram_header.PayloadType); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.payloadtype, "Header.PayloadType"); + PARSE_ASSERT(telegram_header.payloadtype == 0x06c2 || telegram_header.payloadtype == 0x0642, std::string("ResultPortParser::decodeResultPortHeader(): invalid PayloadType ") + std::to_string(telegram_header.payloadtype)); + m_little_endian_payload = isLittleEndianPayload(telegram_header.payloadtype); // Decode PayloadVersion: Version of PayloadType structure. Size: UInt16 = 2 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.PayloadVersion, "Header.PayloadVersion"); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.payloadversion, "Header.PayloadVersion"); // Decode OrderNumber: Order number of the localization controller. Size: UInt32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.OrderNumber, "Header.OrderNumber"); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.ordernumber, "Header.OrderNumber"); // Decode SerialNumber: Serial number of the localization controller. Size: UInt32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.SerialNumber, "Header.SerialNumber"); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.serialnumber, "Header.SerialNumber"); // Decode FW_Version: Software version of the localization controller. Size: 20 × UInt8 = 20 byte - telegram_header.FW_Version = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - bytes_decoded += copyBytesToArray(binary_data, start_byte + bytes_decoded, telegram_header.FW_Version, "Header.FW_Version"); + telegram_header.fw_version = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + bytes_decoded += copyBytesToArray(binary_data, start_byte + bytes_decoded, telegram_header.fw_version, "Header.FW_Version"); // Decode TelegramCounter: Telegram counter since last start-up. Size: UInt32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.TelegramCounter, "Header.TelegramCounter"); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.telegramcounter, "Header.TelegramCounter"); // Decode SystemTime: Not used. Size: NTP = 8 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.SystemTime, "Header.SystemTime"); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_header.systemtime, "Header.SystemTime"); PARSE_ASSERT(bytes_decoded == 52, std::string("ResultPortParser::decodeResultPortHeader(): ") + std::to_string(bytes_decoded) + " bytes decoded, expected 52 byte"); return bytes_decoded; @@ -240,48 +240,48 @@ size_t sick_lidar_localization::ResultPortParser::decodeResultPortPayload(const size_t bytes_decoded = 0; // Decode ErrorCode: ErrorCode 0: OK, ErrorCode 1: UNKNOWNERROR. Size: UInt16 = 2 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.ErrorCode, "Payload.ErrorCode", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.errorcode, "Payload.ErrorCode", m_little_endian_payload); // Decode ScanCounter: Counter of related scan data. Size: UInt32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.ScanCounter, "Payload.ScanCounter", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.scancounter, "Payload.ScanCounter", m_little_endian_payload); // Decode Timestamp: Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.Timestamp, "Payload.Timestamp", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.timestamp, "Payload.Timestamp", m_little_endian_payload); // Decode PoseX: Position X of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.PoseX, "Payload.PoseX", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.posex, "Payload.PoseX", m_little_endian_payload); // Decode PoseY: Position Y of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.PoseY, "Payload.PoseY", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.posey, "Payload.PoseY", m_little_endian_payload); // Decode PoseYaw: Orientation (yaw) of the vehicle on the map [mdeg] Size: Int32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.PoseYaw, "Payload.PoseYaw", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.poseyaw, "Payload.PoseYaw", m_little_endian_payload); // Decode Reserved1: Reserved. Size: UInt32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.Reserved1, "Payload.Reserved1", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.reserved1, "Payload.Reserved1", m_little_endian_payload); // Decode Reserved2: Reserved. Size: Int32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.Reserved2, "Payload.Reserved2", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.reserved2, "Payload.Reserved2", m_little_endian_payload); // Decode Quality: Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality. Size: UInt8 = 1 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.Quality, "Payload.Quality", m_little_endian_payload); - PARSE_ASSERT(telegram_payload.Quality >= 0 && telegram_payload.Quality <= 100, std::string("ResultPortParser::decodeResultPortPayload(): invalid Payload.Quality ") + std::to_string(telegram_payload.Quality)); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.quality, "Payload.Quality", m_little_endian_payload); + PARSE_ASSERT(telegram_payload.quality >= 0 && telegram_payload.quality <= 100, std::string("ResultPortParser::decodeResultPortPayload(): invalid Payload.Quality ") + std::to_string(telegram_payload.quality)); // Decode OutliersRatio: Ratio of beams that cannot be assigned to the current reference map [%]. Size: UInt8 = 1 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.OutliersRatio, "Payload.OutliersRatio", m_little_endian_payload); - PARSE_ASSERT(telegram_payload.OutliersRatio >= 0 && telegram_payload.OutliersRatio <= 100, std::string("ResultPortParser::decodeResultPortPayload(): invalid Payload.OutliersRatio ") + std::to_string(telegram_payload.OutliersRatio)); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.outliersratio, "Payload.OutliersRatio", m_little_endian_payload); + PARSE_ASSERT(telegram_payload.outliersratio >= 0 && telegram_payload.outliersratio <= 100, std::string("ResultPortParser::decodeResultPortPayload(): invalid Payload.OutliersRatio ") + std::to_string(telegram_payload.outliersratio)); // Decode CovarianceX: Covariance c1 of the pose X [mm^2]. Size: Int32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.CovarianceX, "Payload.CovarianceX", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.covariancex, "Payload.CovarianceX", m_little_endian_payload); // Decode CovarianceY: Covariance c5 of the pose Y [mm^2]. Size: Int32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.CovarianceY, "Payload.", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.covariancey, "Payload.", m_little_endian_payload); // Decode CovarianceYaw: Covariance c9 of the pose Yaw [mdeg^2]. Size: Int32 = 4 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.CovarianceYaw, "Payload.CovarianceYaw", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.covarianceyaw, "Payload.CovarianceYaw", m_little_endian_payload); // Decode Reserved3: Reserved. Size: UInt64 = 8 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.Reserved3, "Payload.Reserved3", m_little_endian_payload); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_payload.reserved3, "Payload.Reserved3", m_little_endian_payload); PARSE_ASSERT(bytes_decoded == 52, std::string("ResultPortParser::decodeResultPortPayload(): ") + std::to_string(bytes_decoded) + " bytes decoded, expected 52 byte"); return bytes_decoded; @@ -300,7 +300,7 @@ size_t sick_lidar_localization::ResultPortParser::decodeResultPortTrailer(const size_t bytes_decoded = 0; // Decode Checksum: CRC16-CCITT over length of header (52 bytes) and payload (52 bytes) without 2 bytes of this trailer. Size: UInt16 = 2 byte - bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_trailer.Checksum, "Payload.Checksum"); + bytes_decoded += copyBytesToValue(binary_data, start_byte + bytes_decoded, telegram_trailer.checksum, "Payload.Checksum"); PARSE_ASSERT(bytes_decoded == 2, std::string("ResultPortParser::decodeResultPortTrailer(): ") + std::to_string(bytes_decoded) + " bytes decoded, expected 2 byte"); return bytes_decoded; @@ -317,7 +317,7 @@ bool sick_lidar_localization::ResultPortParser::decode(const std::vector= 106, std::string("ResultPortParser::decode(): ") + std::to_string(binary_data.size()) + " byte binary data, expected 106 byte result port telegram"); - m_result_port_telegram.header.stamp = ros::Time::now(); + m_result_port_telegram.header.stamp = ROS::now(); m_result_port_telegram.header.frame_id = m_publish_frame_id; // Decode result port header @@ -328,13 +328,13 @@ bool sick_lidar_localization::ResultPortParser::decode(const std::vector void sick_lidar_localization::ResultPortParser::encodePushV */ void sick_lidar_localization::ResultPortParser::encodeResultPortHeader(const sick_lidar_localization::SickLocResultPortHeaderMsg & telegram_header, std::vector & binary_data) { - encodePushValue(telegram_header.MagicWord, binary_data); - encodePushValue(telegram_header.Length, binary_data); - encodePushValue(telegram_header.PayloadType, binary_data); - encodePushValue(telegram_header.PayloadVersion, binary_data); - encodePushValue(telegram_header.OrderNumber, binary_data); - encodePushValue(telegram_header.SerialNumber, binary_data); - for (size_t n = 0; n < telegram_header.FW_Version.size(); n++) - binary_data.push_back(telegram_header.FW_Version[n]); - encodePushValue(telegram_header.TelegramCounter, binary_data); - encodePushValue(telegram_header.SystemTime, binary_data); + encodePushValue(telegram_header.magicword, binary_data); + encodePushValue(telegram_header.length, binary_data); + encodePushValue(telegram_header.payloadtype, binary_data); + encodePushValue(telegram_header.payloadversion, binary_data); + encodePushValue(telegram_header.ordernumber, binary_data); + encodePushValue(telegram_header.serialnumber, binary_data); + for (size_t n = 0; n < telegram_header.fw_version.size(); n++) + binary_data.push_back(telegram_header.fw_version[n]); + encodePushValue(telegram_header.telegramcounter, binary_data); + encodePushValue(telegram_header.systemtime, binary_data); } /* @@ -396,20 +396,20 @@ void sick_lidar_localization::ResultPortParser::encodeResultPortHeader(const sic */ void sick_lidar_localization::ResultPortParser::encodeResultPortPayload(const sick_lidar_localization::SickLocResultPortPayloadMsg & telegram_payload, std::vector & binary_data) { - encodePushValue(telegram_payload.ErrorCode, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.ScanCounter, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.Timestamp, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.PoseX, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.PoseY, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.PoseYaw, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.Reserved1, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.Reserved2, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.Quality, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.OutliersRatio, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.CovarianceX, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.CovarianceY, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.CovarianceYaw, binary_data, m_little_endian_payload); - encodePushValue(telegram_payload.Reserved3, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.errorcode, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.scancounter, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.timestamp, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.posex, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.posey, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.poseyaw, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.reserved1, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.reserved2, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.quality, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.outliersratio, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.covariancex, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.covariancey, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.covarianceyaw, binary_data, m_little_endian_payload); + encodePushValue(telegram_payload.reserved3, binary_data, m_little_endian_payload); } /* @@ -430,11 +430,11 @@ std::vector sick_lidar_localization::ResultPortParser::encode(void) { std::vector binary_data; binary_data.reserve(106); - m_little_endian_payload = isLittleEndianPayload(m_result_port_telegram.telegram_header.PayloadType); + m_little_endian_payload = isLittleEndianPayload(m_result_port_telegram.telegram_header.payloadtype); encodeResultPortHeader(m_result_port_telegram.telegram_header, binary_data); encodeResultPortPayload(m_result_port_telegram.telegram_payload, binary_data); uint16_t checksum = computeChecksum(binary_data, false); - m_result_port_telegram.telegram_trailer.Checksum = checksum; + m_result_port_telegram.telegram_trailer.checksum = checksum; encodeResultPortTrailer(checksum, binary_data); return binary_data; } diff --git a/src/ros_wrapper.cpp b/src/ros_wrapper.cpp new file mode 100755 index 0000000..e7cef0d --- /dev/null +++ b/src/ros_wrapper.cpp @@ -0,0 +1,206 @@ +/* + * @brief ros_wrapper encapsulates the ROS API and switches ROS specific implementation + * between ROS 1 and ROS 2. + * + * Copyright (C) 2020 Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2020 SICK AG, Waldkirch + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of SICK AG nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * * Neither the name of Ing.-Buero Dr. Michael Lehning nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: + * Michael Lehning + * + * Copyright 2020 SICK AG + * Copyright 2020 Ing.-Buero Dr. Michael Lehning + * + */ +#include + +#include "sick_lidar_localization/ros_wrapper.h" + +#if defined __ROS_VERSION && __ROS_VERSION == 2 +static rclcpp::Clock s_wrapper_clock; +#endif + +/** Creates a new ros node, shortcut for new ros::NodeHandle() on ROS1 resp. rclcpp::Node::make_shared(node_name) on ROS2 */ +ROS::NodePtr ROS::createNode(const std::string& node_name) +{ + ROS::NodePtr nh = 0; +#if defined __ROS_VERSION && __ROS_VERSION == 1 + nh = new ros::NodeHandle(); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true); + nh = rclcpp::Node::make_shared(node_name, "", node_options); +#endif + double timestamp_sec = ROS::secondsSinceStart(ROS::now()); + if ((timestamp_sec = ROS::secondsSinceStart(ROS::now())) < 0) // insure start timestamp after initialization of the first node + ROS_WARN_STREAM("## ERROR ROS::createNode(): time confusion, ROS::secondsSinceStart(ROS::now()) returned negative seconds " << timestamp_sec); + return nh; +} + +#if defined __ROS_VERSION && __ROS_VERSION == 2 +/** ROS1-/ROS2-compatible shortcut for rclcpp::init(argc, argv); */ +void ROS::init(int argc, char** argv, const std::string nodename) +{ + rclcpp::init(argc, argv); +} +#endif + +/** ROS1-/ROS2-compatible shortcut for ros::spin(); */ +#if defined __ROS_VERSION && __ROS_VERSION == 1 +void ROS::spin(ROS::NodePtr nh) +{ + ros::spin(); +} +#endif + +/** Deletes a ros node */ +void ROS::deleteNode(ROS::NodePtr & node) +{ +#if defined __ROS_VERSION && __ROS_VERSION == 1 + if(node) + { + delete(node); + node = 0; + } +#endif +} + +/** Shortcut to replace ros::Duration(seconds).sleep() by std::thread */ +void ROS::sleep(double seconds) +{ + std::this_thread::sleep_for(std::chrono::microseconds((int64_t)(1.0e6*seconds))); +} + +/** Shortcut to ros::Time::now() on ROS1 resp. rclcpp::Clock::now() on ROS2 */ +ROS::Time ROS::now(void) +{ +#if defined __ROS_VERSION && __ROS_VERSION == 1 + return ros::Time::now(); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + return s_wrapper_clock.now(); +#else + return 0; +#endif +} + +/** Splits a ROS::Duration into seconds and nanoseconds part */ +void ROS::splitTime(ROS::Duration time, uint32_t& seconds, uint32_t& nanoseconds) +{ +#if defined __ROS_VERSION && __ROS_VERSION == 1 + seconds = time.sec; + nanoseconds = time.nsec; +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + seconds = (uint32_t)(time.nanoseconds() / 1000000000); + nanoseconds = (uint32_t)(time.nanoseconds() - 1000000000 * seconds); +#endif +} + +/** Splits a ROS::Time into seconds and nanoseconds part */ +void ROS::splitTime(ROS::Time time, uint32_t& seconds, uint32_t& nanoseconds) +{ +#if defined __ROS_VERSION && __ROS_VERSION == 1 + seconds = time.sec; + nanoseconds = time.nsec; +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + seconds = (uint32_t)(time.nanoseconds() / 1000000000); + nanoseconds = (uint32_t)(time.nanoseconds() - 1000000000 * seconds); +#endif +} + +/** Shortcut to return ros::Time(msg_header->stamp.sec,msg_header->stamp.nsec) on ROS1 resp. rclcpp::Time(msg_header->stamp.sec,msg_header->stamp.nanosec) on ROS2 */ +ROS::Time ROS::timeFromHeader(const std_msgs::Header* msg_header) +{ + return ROS::Time(std::max(0,(int32_t)msg_header->stamp.sec), msg_header->stamp.NSEC); +} + +/** Returns a time (type ros::Time on ROS1 resp. rclcpp::Time on ROS2) from a given amount of seconds. + ** Note: ros::Time(1) initializes 1 second, while rclcpp::Time(1) initializes 1 nanosecond. + ** Do not use the Time constructor with one parameter! Always use ROS::timeFromSec(seconds) + ** or ROS::Time(int32_t seconds, uint32_t nanoseconds). */ +ROS::Time ROS::timeFromSec(double seconds) +{ + int32_t i32seconds = (int32_t)(seconds); + int32_t i32nanoseconds = (int32_t)(1000000000 * (seconds - i32seconds)); + return ROS::Time(std::max(0,i32seconds), std::max(0,i32nanoseconds)); +} + +/** Returns a duration (type ros::Duration on ROS1 resp. rclcpp::Duration on ROS2) from a given amount of seconds. + ** Note: ros::Duration(1) initializes 1 second, while rclcpp::Duration(1) initializes 1 nanosecond. + ** Do not use the Duration constructor with one parameter! Always use ROS::durationFromSec(seconds) + ** or ROS::Duration(int32_t seconds, uint32_t nanoseconds). */ +ROS::Duration ROS::durationFromSec(double seconds) +{ + int32_t i32seconds = (int32_t)(seconds); + int32_t i32nanoseconds = (int32_t)(1000000000 * (seconds - i32seconds)); + return ROS::Duration(i32seconds, i32nanoseconds); +} + +/** Shortcut to return ros::Duration::toSec() on ROS1 resp. rclcpp::Duration::seconds() on ROS2 */ +double ROS::seconds(ROS::Duration duration) +{ +#if defined __ROS_VERSION && __ROS_VERSION == 1 + return duration.toSec(); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + return duration.seconds(); +#else + return 0; +#endif +} + +/** Shortcut to return the time in seconds since program start (first node started) */ +double ROS::secondsSinceStart(const ROS::Time& time) +{ + static ROS::Time s_wrapper_start_time = ROS::now(); + return ROS::seconds(time - s_wrapper_start_time); +} + +/** Shortcut to return the timestamp in milliseconds from ROS1 resp. ROS2 time */ +uint64_t ROS::timestampMilliseconds(const ROS::Time& time) +{ +#if defined __ROS_VERSION && __ROS_VERSION == 1 + return (uint64_t)(time.sec * 1000) + (uint64_t)(time.nsec / 1000000); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + return (uint64_t)(time.nanoseconds() / 1000000); +#endif +} diff --git a/src/testcase_generator.cpp b/src/testcase_generator.cpp index 0bb9f93..1ce0c0c 100644 --- a/src/testcase_generator.cpp +++ b/src/testcase_generator.cpp @@ -53,7 +53,7 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/cola_parser.h" #include "sick_lidar_localization/random_generator.h" @@ -67,7 +67,7 @@ uint32_t sick_lidar_localization::TestcaseGenerator::s_u32ResultPoseInterval = 1; /*! - * test server settings, set by sMN or sRN requests + * test server int32 settings, set by sMN or sRN requests */ std::map sick_lidar_localization::TestcaseGenerator::s_controller_settings = { {"IsSystemReady", 1}, // 0:false, 1:true (default) @@ -80,6 +80,11 @@ std::map sick_lidar_localization::TestcaseGenerator::s_con {"LocRequestResultData", 1} // in poll mode, trigger sending the localization result of the next processed scan via TCP interface. }; +/*! + * test server string settings, set by sMN or sRN requests + */ +std::map sick_lidar_localization::TestcaseGenerator::s_controller_settings_str; + /*! * Returns true, if localization is active (default), otherwise false (localization deactivated) * @return result telegrams are activated (true) or deactivated @@ -107,7 +112,7 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T sick_lidar_localization::SickLocResultPortTestcaseMsg testcase; // ROS Header with sequence id, timestamp and frame id - testcase.header.stamp = ros::Time::now(); + testcase.header.stamp = ROS::now(); testcase.header.frame_id = "sick_localization_testcase"; // binary encoded result port telegram (default example) @@ -150,33 +155,33 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T static sick_lidar_localization::UniformRandomInteger random_covariance_generator(0, INT32_MAX); // Create default SickLocResultPortTelegramMsg - static ros::Time start_time = ros::Time::now(); + static ROS::Time start_time = ROS::now(); static sick_lidar_localization::SickLocResultPortTestcaseMsg default_testcase = createDefaultResultPortTestcase(); sick_lidar_localization::SickLocResultPortTestcaseMsg testcase = default_testcase; sick_lidar_localization::SickLocResultPortTelegramMsg & telegram_msg = testcase.telegram_msg; // Modify SickLocResultPortTelegramMsg with random values - telegram_msg.telegram_header.PayloadType = ((random1_generator.generate() > 0) ? 0x06c2 : 0x0642); // Payload type: 0x06c2 = Little Endian, 0x0642 = Big Endian. Size: UInt16 = 2 byte - telegram_msg.telegram_header.OrderNumber = (uint32_t)random32_generator.generate(); // Order number of the localization controller. Size: UInt32 = 4 byte - telegram_msg.telegram_header.SerialNumber = (uint32_t)random32_generator.generate(); // Serial number of the localization controller. Size: UInt32 = 4 byte - for(size_t n = 0; n < telegram_msg.telegram_header.FW_Version.size(); n++) - telegram_msg.telegram_header.FW_Version[n] = (uint8_t)random8_generator.generate(); // Software version of the localization controller. Size: 20 × UInt8 = 20 byte - telegram_msg.telegram_payload.PoseX = random32_generator.generate(); // Position X of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte - telegram_msg.telegram_payload.PoseY = random32_generator.generate(); // Position Y of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte - telegram_msg.telegram_payload.PoseYaw = random_yaw_generator.generate(); // Orientation (yaw) of the vehicle on the map [mdeg], range -180 to +180 deg assumed. Size: Int32 = 4 byte - telegram_msg.telegram_payload.Reserved1 = (uint32_t)random32_generator.generate(); // Reserved. Size: UInt32 = 4 byte - telegram_msg.telegram_payload.Reserved2 = random32_generator.generate(); // Reserved. Size: Int32 = 4 byte - telegram_msg.telegram_payload.Quality = (uint8_t)random_quality_generator.generate(); // Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality. Size: UInt8 = 1 byte - telegram_msg.telegram_payload.OutliersRatio = (uint8_t)random_quality_generator.generate(); // Ratio of beams that cannot be assigned to the current reference map [%]. Size: UInt8 = 1 byte - telegram_msg.telegram_payload.CovarianceX = random_covariance_generator.generate(); // Covariance c1 of the pose X [mm^2]. Size: Int32 = 4 byte - telegram_msg.telegram_payload.CovarianceY = random_covariance_generator.generate(); // Covariance c5 of the pose Y [mm^2]. Size: Int32 = 4 byte - telegram_msg.telegram_payload.CovarianceYaw = random_covariance_generator.generate(); // Covariance c9 of the pose Yaw [mdeg^2]. Size: Int32 = 4 byte - telegram_msg.telegram_payload.Reserved3 = (((uint64_t)random32_generator.generate() << 32) | (uint64_t)random32_generator.generate()); // Reserved. Size: UInt64 = 8 byte + telegram_msg.telegram_header.payloadtype = ((random1_generator.generate() > 0) ? 0x06c2 : 0x0642); // Payload type: 0x06c2 = Little Endian, 0x0642 = Big Endian. Size: UInt16 = 2 byte + telegram_msg.telegram_header.ordernumber = (uint32_t)random32_generator.generate(); // Order number of the localization controller. Size: UInt32 = 4 byte + telegram_msg.telegram_header.serialnumber = (uint32_t)random32_generator.generate(); // Serial number of the localization controller. Size: UInt32 = 4 byte + for(size_t n = 0; n < telegram_msg.telegram_header.fw_version.size(); n++) + telegram_msg.telegram_header.fw_version[n] = (uint8_t)random8_generator.generate(); // Software version of the localization controller. Size: 20 × UInt8 = 20 byte + telegram_msg.telegram_payload.posex = random32_generator.generate(); // Position X of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte + telegram_msg.telegram_payload.posey = random32_generator.generate(); // Position Y of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte + telegram_msg.telegram_payload.poseyaw = random_yaw_generator.generate(); // Orientation (yaw) of the vehicle on the map [mdeg], range -180 to +180 deg assumed. Size: Int32 = 4 byte + telegram_msg.telegram_payload.reserved1 = (uint32_t)random32_generator.generate(); // Reserved. Size: UInt32 = 4 byte + telegram_msg.telegram_payload.reserved2 = random32_generator.generate(); // Reserved. Size: Int32 = 4 byte + telegram_msg.telegram_payload.quality = (uint8_t)random_quality_generator.generate(); // Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality. Size: UInt8 = 1 byte + telegram_msg.telegram_payload.outliersratio = (uint8_t)random_quality_generator.generate(); // Ratio of beams that cannot be assigned to the current reference map [%]. Size: UInt8 = 1 byte + telegram_msg.telegram_payload.covariancex = random_covariance_generator.generate(); // Covariance c1 of the pose X [mm^2]. Size: Int32 = 4 byte + telegram_msg.telegram_payload.covariancey = random_covariance_generator.generate(); // Covariance c5 of the pose Y [mm^2]. Size: Int32 = 4 byte + telegram_msg.telegram_payload.covarianceyaw = random_covariance_generator.generate(); // Covariance c9 of the pose Yaw [mdeg^2]. Size: Int32 = 4 byte + telegram_msg.telegram_payload.reserved3 = (((uint64_t)random32_generator.generate() << 32) | (uint64_t)random32_generator.generate()); // Reserved. Size: UInt64 = 8 byte // Update telegram timestamps - double delta_time_seconds = (ros::Time::now() - start_time).toSec(); - telegram_msg.telegram_payload.Timestamp = createTimestampTicksMilliSec(); // Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte - telegram_msg.telegram_header.SystemTime += (uint64_t)(delta_time_seconds); // SystemTime not used. Size: NTP = 8 byte + double delta_time_seconds = ROS::seconds(ROS::now() - start_time); + telegram_msg.telegram_payload.timestamp = createTimestampTicksMilliSec(); // Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte + telegram_msg.telegram_header.systemtime += (uint64_t)(delta_time_seconds); // SystemTime not used. Size: NTP = 8 byte // Re-encode the modified result port telegram (SickLocResultPortTelegramMsg) sick_lidar_localization::ResultPortParser result_port_parser(testcase.header.frame_id); @@ -185,11 +190,11 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T testcase.telegram_msg = result_port_parser.getTelegramMsg(); // Increment telegram counter for next testcase - default_testcase.telegram_msg.telegram_header.TelegramCounter += 1; // Telegram counter since last start-up. Size: UInt32 = 4 byte - default_testcase.telegram_msg.telegram_payload.ScanCounter += 1; // Counter of related scan data. Size: UInt32 = 4 byte + default_testcase.telegram_msg.telegram_header.telegramcounter += 1; // Telegram counter since last start-up. Size: UInt32 = 4 byte + default_testcase.telegram_msg.telegram_payload.scancounter += 1; // Counter of related scan data. Size: UInt32 = 4 byte // Update testcase timestamp - testcase.header.stamp = ros::Time::now(); + testcase.header.stamp = ROS::now(); return testcase; } @@ -204,21 +209,21 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::TestcaseGenerator::createResultPortCircles(double circle_radius, double circle_yaw) { // Create default SickLocResultPortTelegramMsg - static ros::Time start_time = ros::Time::now(); + static ROS::Time start_time = ROS::now(); static sick_lidar_localization::SickLocResultPortTestcaseMsg default_testcase = createDefaultResultPortTestcase(); sick_lidar_localization::SickLocResultPortTestcaseMsg testcase = default_testcase; sick_lidar_localization::SickLocResultPortTelegramMsg & telegram_msg = testcase.telegram_msg; // Set current position and orientation - telegram_msg.telegram_payload.PoseX = (int32_t)(1000.0 * circle_radius * std::cos(circle_yaw)); // Position X of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte - telegram_msg.telegram_payload.PoseY = (int32_t)(1000.0 * circle_radius * std::sin(circle_yaw)); // Position Y of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte + telegram_msg.telegram_payload.posex = (int32_t)(1000.0 * circle_radius * std::cos(circle_yaw)); // Position X of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte + telegram_msg.telegram_payload.posey = (int32_t)(1000.0 * circle_radius * std::sin(circle_yaw)); // Position Y of the vehicle on the map in cartesian global coordinates [mm]. Size: Int32 = 4 byte double orientation = sick_lidar_localization::Utils::normalizeAngle(circle_yaw + M_PI_2); // Orienation := circle_yaw + 90 degree - telegram_msg.telegram_payload.PoseYaw = (int32_t)(1000.0 * orientation * 180.0 / M_PI); // Orientation (yaw) of the vehicle on the map [mdeg], range -180 to +180 deg assumed. Size: Int32 = 4 byte + telegram_msg.telegram_payload.poseyaw = (int32_t)(1000.0 * orientation * 180.0 / M_PI); // Orientation (yaw) of the vehicle on the map [mdeg], range -180 to +180 deg assumed. Size: Int32 = 4 byte // Update telegram timestamps - double delta_time_seconds = (ros::Time::now() - start_time).toSec(); - telegram_msg.telegram_payload.Timestamp = createTimestampTicksMilliSec(); // Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte - telegram_msg.telegram_header.SystemTime += (uint64_t)(delta_time_seconds); // SystemTime not used. Size: NTP = 8 byte + double delta_time_seconds = ROS::seconds(ROS::now() - start_time); + telegram_msg.telegram_payload.timestamp = createTimestampTicksMilliSec(); // Time stamp of the pose [ms]. The time stamp indicates the time at which the pose is calculated. Size: UInt32 = 4 byte + telegram_msg.telegram_header.systemtime += (uint64_t)(delta_time_seconds); // SystemTime not used. Size: NTP = 8 byte // Re-encode the modified result port telegram (SickLocResultPortTelegramMsg) sick_lidar_localization::ResultPortParser result_port_parser(testcase.header.frame_id); @@ -227,11 +232,11 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T testcase.telegram_msg = result_port_parser.getTelegramMsg(); // Increment telegram counter for next testcase - default_testcase.telegram_msg.telegram_header.TelegramCounter += 1; // Telegram counter since last start-up. Size: UInt32 = 4 byte - default_testcase.telegram_msg.telegram_payload.ScanCounter += 1; // Counter of related scan data. Size: UInt32 = 4 byte + default_testcase.telegram_msg.telegram_header.telegramcounter += 1; // Telegram counter since last start-up. Size: UInt32 = 4 byte + default_testcase.telegram_msg.telegram_payload.scancounter += 1; // Counter of related scan data. Size: UInt32 = 4 byte // Update testcase timestamp - testcase.header.stamp = ros::Time::now(); + testcase.header.stamp = ROS::now(); return testcase; } @@ -249,11 +254,11 @@ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::Testcas { static sick_lidar_localization::UniformRandomInteger time_jitter_network_ms(0, 2); // Simulate some network latency - ros::Duration(0.001 * time_jitter_network_ms.generate()).sleep(); + ROS::sleep(0.001 * time_jitter_network_ms.generate()); // Create current timestamp in ticks uint32_t ticks_ms = createTimestampTicksMilliSec(); // Simulate some network latency - ros::Duration(0.001 * time_jitter_network_ms.generate()).sleep(); + ROS::sleep(0.001 * time_jitter_network_ms.generate()); return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {hexstr(ticks_ms)}); } @@ -305,13 +310,244 @@ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::Testcas return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(success?1:0)}); } - if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetResultPoseInterval" && cola_request.parameter.size() == 1) { s_u32ResultPoseInterval = std::strtoul(cola_request.parameter[0].c_str(), 0, 0); return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); } + + /* Start of test server responses for new service requests (release 4 or later) */ + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "DevSetLidarConfig" && cola_request.parameter.size() == 15) + { + for(size_t n = 0; n < cola_request.parameter.size(); n++) + s_controller_settings_str["DevSetLidarConfig_"+std::to_string(n)] = cola_request.parameter[n]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "DevGetLidarConfig" && cola_request.parameter.size() == 1) + { + std::vector config_parameter; + for(size_t n = 1; n < 15; n++) + // if(n != 9) + config_parameter.push_back(s_controller_settings_str["DevSetLidarConfig_"+std::to_string(n)]); + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, config_parameter); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetMap" && cola_request.parameter.size() == 2) + { + s_controller_settings_str["LocSetMap"] = cola_request.parameter[1]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)}); + } + + if(cola_request.command_name == "LocMap")//if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocMap" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sRA, cola_request.command_name, {s_controller_settings_str["LocSetMap"]}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocMapState" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocInitializePose" && cola_request.parameter.size() == 4) + { + for(size_t n = 0; n < cola_request.parameter.size(); n++) + s_controller_settings_str["LocInitializePose_"+std::to_string(n)] = cola_request.parameter[n]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocInitialPose" && cola_request.parameter.size() == 0) + { + std::vector parameter; + for(size_t n = 0; n < 4; n++) + parameter.push_back(s_controller_settings_str["LocInitializePose_"+std::to_string(n)]); + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, parameter); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetPoseQualityCovWeight" && cola_request.parameter.size() == 1) + { + s_controller_settings_str["LocSetPoseQualityCovWeight"] = cola_request.parameter[0]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocPoseQualityCovWeight" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetPoseQualityCovWeight"]}); + } + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetPoseQualityMeanDistWeight" && cola_request.parameter.size() == 1) + { + s_controller_settings_str["LocSetPoseQualityMeanDistWeight"] = cola_request.parameter[0]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocPoseQualityMeanDistWeight" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetPoseQualityMeanDistWeight"]}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetReflectorsForSupportActive" && cola_request.parameter.size() == 1) + { + s_controller_settings_str["LocSetReflectorsForSupportActive"] = cola_request.parameter[0]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocReflectorsForSupportActive" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetReflectorsForSupportActive"]}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetOdometryActive" && cola_request.parameter.size() == 1) + { + s_controller_settings_str["LocSetOdometryActive"] = cola_request.parameter[0]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocOdometryActive" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetOdometryActive"]}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetOdometryPort" && cola_request.parameter.size() == 1) + { + s_controller_settings_str["LocSetOdometryPort"] = cola_request.parameter[0]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1), decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocOdometryPort" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetOdometryPort"]}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetOdometryRestrictYMotion" && cola_request.parameter.size() == 1) + { + s_controller_settings_str["LocSetOdometryRestrictYMotion"] = cola_request.parameter[0]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocOdometryRestrictYMotion" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetOdometryRestrictYMotion"]}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetAutoStartActive" && cola_request.parameter.size() == 1) + { + s_controller_settings_str["LocSetAutoStartActive"] = cola_request.parameter[0]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocAutoStartActive" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetAutoStartActive"]}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetAutoStartSavePoseInterval" && cola_request.parameter.size() == 1) + { + s_controller_settings_str["LocSetAutoStartSavePoseInterval"] = cola_request.parameter[0]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocAutoStartSavePoseInterval" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetAutoStartSavePoseInterval"]}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSetRingBufferRecordingActive" && cola_request.parameter.size() == 1) + { + s_controller_settings_str["LocSetRingBufferRecordingActive"] = cola_request.parameter[0]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocRingBufferRecordingActive" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {s_controller_settings_str["LocSetRingBufferRecordingActive"]}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "DevGetLidarIdent" && cola_request.parameter.size() == 1) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {"TestcaseGenerator" + cola_request.parameter[0]}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "DevGetLidarState" && cola_request.parameter.size() == 1) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(2), decstr(2), decstr(2)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "GetSoftwareVersion" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {"1.0"}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocAutoStartSavePose" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocForceUpdate" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocSaveRingBufferRecording" && cola_request.parameter.size() == 2) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(2)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocStartDemoMapping" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "ReportUserMessage" && cola_request.parameter.size() == 2) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "SavePermanent" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocResultPort" && cola_request.parameter.size() == 0) + { + int32_t port = ((s_controller_settings["LocResultPort"]) > 0 ? s_controller_settings["LocResultPort"] : 2201); + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sRA, cola_request.command_name, {hexstr(port)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocResultMode" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sRA, cola_request.command_name, {decstr(s_controller_settings["LocResultMode"])}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocResultEndianness" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sRA, cola_request.command_name, {decstr(s_controller_settings["LocResultEndianness"])}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocResultState" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sRA, cola_request.command_name, {decstr(s_controller_settings["LocResultState"])}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocResultPoseInterval" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sRA, cola_request.command_name, {decstr(s_u32ResultPoseInterval)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "DevSetIMUActive" && cola_request.parameter.size() == 1) + { + s_controller_settings_str["DevSetIMUActive"] = cola_request.parameter[0]; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)}); + } + + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "DevIMUActive" && cola_request.parameter.size() == 0) + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sRA, cola_request.command_name, {s_controller_settings_str["DevSetIMUActive"]}); + } + + /* End of test server responses for new service requests (release 4 or later) */ + // Create sAN responses to sMN requests resp. sRA responses to sRN requests if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN || cola_request.command_type == sick_lidar_localization::ColaParser::sRN) { @@ -341,11 +577,13 @@ sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::Testcas */ uint32_t sick_lidar_localization::TestcaseGenerator::createTimestampTicksMilliSec(void) { - static ros::Time start = ros::Time::now(); + static ROS::Time start = ROS::now(); static sick_lidar_localization::UniformRandomInteger time_jitter_ticks_ms(-2, +2); // Create current timestamp in ticks - ros::Duration timestamp = (ros::Time::now() - start); - uint32_t ticks_ms = (((uint64_t)timestamp.sec * 1000 + (uint64_t)timestamp.nsec/1000000 + 1000) & 0xFFFFFFFF); + ROS::Duration timestamp = (ROS::now() - start); + uint32_t seconds = 0, nanoseconds = 0; + ROS::splitTime(timestamp, seconds, nanoseconds); + uint32_t ticks_ms = (((uint64_t)seconds * 1000 + (uint64_t)nanoseconds/1000000 + 1000) & 0xFFFFFFFF); // Create some jitter, simulation network latency and time drift ticks_ms += time_jitter_ticks_ms.generate(); return ticks_ms; diff --git a/src/time_sync.cpp b/src/time_sync.cpp deleted file mode 100644 index 815d962..0000000 --- a/src/time_sync.cpp +++ /dev/null @@ -1,104 +0,0 @@ -/* - * @brief time_sync advertises and runs ros services "SickLocRequestTimestamp" and "SickLocTimeSync" - * for time synchronization. - * - * ROS service SickLocRequestTimestamp requests a timestamp from the localization controller - * by sending cola command LocRequestTimestamp ("sMN LocRequestTimestamp"). - * - * The service receives and decodes the current timestamp (uint32 timestamp in milliseconds) - * and calculates the time offset with the following formular: - * - * delta_time_ms := mean_time_vehicle_ms - timestamp_lidar_ms - * mean_time_vehicle_ms := (send_time_vehicle + receive_time_vehicle) / 2 - * := vehicles mean timestamp in milliseconds - * send_time_vehicle := vehicles timestamp when sending LocRequestTimestamp - * receive_time_vehicle := vehicles timestamp when receiving the LocRequestTimestamp response - * timestamp_lidar_ms := lidar timestamp in milliseconds from LocRequestTimestamp response - * - * See Operation-Instruction-v1.1.0.241R.pdf for details about time synchronization and - * time offset calculation. See Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.PDF - * for further details about Cola telegram LocRequestTimestamp. - * - * 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/time_sync_service.h" - -int main(int argc, char** argv) -{ - // Ros configuration and initialization - ros::init(argc, argv, "time_sync"); - ros::NodeHandle nh; - ROS_INFO_STREAM("time_sync started."); - - // Initialize TimeSyncService - sick_lidar_localization::TimeSyncService time_sync_service(&nh); - - // Start time synchronization thread to run the software pll - if(!time_sync_service.start()) - { - ROS_ERROR_STREAM("## ERROR time_sync: could not start synchronization thread, exiting"); - return EXIT_FAILURE; - } - - // Run ros event loop - ros::spin(); - - // Cleanup and exit - std::cout << "time_sync finished." << std::endl; - ROS_INFO_STREAM("time_sync finished."); - time_sync_service.stop(); - std::cout << "time_sync exits." << std::endl; - ROS_INFO_STREAM("time_sync exits."); - return EXIT_SUCCESS; -} diff --git a/src/time_sync_service.cpp b/src/time_sync_service.cpp index 65f0b47..ca8f624 100644 --- a/src/time_sync_service.cpp +++ b/src/time_sync_service.cpp @@ -82,7 +82,7 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/cola_parser.h" #include "sick_lidar_localization/SoftwarePLL.h" @@ -91,32 +91,37 @@ /*! * Constructor */ -sick_lidar_localization::TimeSyncService::TimeSyncService(ros::NodeHandle* nh) -: m_time_sync_thread_running(false), m_time_sync_thread(0), m_cola_binary(false), m_cola_binary_mode(0), m_software_pll_fifo_length(7), +sick_lidar_localization::TimeSyncService::TimeSyncService(ROS::NodePtr nh, sick_lidar_localization::DriverMonitor* driver_monitor) +: m_nh(nh), m_driver_monitor(driver_monitor), m_time_sync_thread_running(false), m_time_sync_thread(0), m_cola_binary(false), m_cola_binary_mode(0), m_software_pll_fifo_length(7), m_time_sync_rate(0.1), m_time_sync_initial_rate(1.0), m_time_sync_initial_length(10), m_cola_response_timeout(1.0) { if(nh) { // Configuration and parameter - ros::param::param("/sick_lidar_localization/driver/cola_binary", m_cola_binary_mode, m_cola_binary_mode); + ROS::param(nh, "/sick_lidar_localization/driver/cola_binary", m_cola_binary_mode, m_cola_binary_mode); m_cola_binary = (m_cola_binary_mode == 1) ? true : false; // 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) - ros::param::param("/sick_lidar_localization/time_sync/software_pll_fifo_length", m_software_pll_fifo_length, m_software_pll_fifo_length); + ROS::param(nh, "/sick_lidar_localization/time_sync/software_pll_fifo_length", m_software_pll_fifo_length, m_software_pll_fifo_length); double time_sync_rate = 0.1, time_sync_initial_rate = 1.0; - ros::param::param("/sick_lidar_localization/time_sync/time_sync_rate", time_sync_rate, time_sync_rate); - ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_rate", time_sync_initial_rate, time_sync_initial_rate); - m_time_sync_rate = ros::Rate(time_sync_rate); - m_time_sync_initial_rate = ros::Rate(time_sync_initial_rate); - ros::param::param("/sick_lidar_localization/time_sync/time_sync_initial_length", m_time_sync_initial_length, m_time_sync_initial_length); - ros::param::param("/sick_lidar_localization/time_sync/cola_response_timeout", m_cola_response_timeout, m_cola_response_timeout); + ROS::param(nh, "/sick_lidar_localization/time_sync/time_sync_rate", time_sync_rate, time_sync_rate); + ROS::param(nh, "/sick_lidar_localization/time_sync/time_sync_initial_rate", time_sync_initial_rate, time_sync_initial_rate); + m_time_sync_rate = time_sync_rate; + m_time_sync_initial_rate = time_sync_initial_rate; + ROS::param(nh, "/sick_lidar_localization/time_sync/time_sync_initial_length", m_time_sync_initial_length, m_time_sync_initial_length); + ROS::param(nh, "/sick_lidar_localization/time_sync/cola_response_timeout", m_cola_response_timeout, m_cola_response_timeout); // Advertise service "SickLocRequestTimestamp" to send a LocRequestTimestamp, receive the response and to calculate the time offset - m_timestamp_service_server = nh->advertiseService("SickLocRequestTimestamp", &sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp, this); - ROS_INFO_STREAM("TimeSyncService: advertising ros service \"SickLocRequestTimestamp\" for LocRequestTimestamp commands, message type SickLocRequestTimestamp"); // Advertise service "SickLocTimeSync" to calculate system time from ticks by software pll - m_timesync_service_server = nh->advertiseService("SickLocTimeSync", &sick_lidar_localization::TimeSyncService::serviceCbTimeSync, this); +#if defined __ROS_VERSION && __ROS_VERSION == 1 + m_timestamp_service_server = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocRequestTimestampSrv, "SickLocRequestTimestamp", &sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp, this); + m_timesync_service_server = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocTimeSyncSrv, "SickLocTimeSync", &sick_lidar_localization::TimeSyncService::serviceCbTimeSync, this); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + m_timestamp_service_server = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocRequestTimestampSrv, "SickLocRequestTimestamp", &sick_lidar_localization::TimeSyncService::serviceCbRequestTimestampROS2, this); + m_timesync_service_server = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::SickLocTimeSyncSrv, "SickLocTimeSync", &sick_lidar_localization::TimeSyncService::serviceCbTimeSyncROS2, this); +#endif + ROS_INFO_STREAM("TimeSyncService: advertising ros service \"SickLocRequestTimestamp\" for LocRequestTimestamp commands, message type SickLocRequestTimestamp"); ROS_INFO_STREAM("TimeSyncService: advertising ros service \"SickLocTimeSync\" for time synchronization by software pll, message type SickLocTimeSync"); // Clients for ros services "SickLocColaTelegram", "SickLocRequestTimestamp" and "SickLocTimeSync", required for time synchronization using a software pll - m_cola_service_client = nh->serviceClient("SickLocColaTelegram"); - m_request_timestamp_client = nh->serviceClient("SickLocRequestTimestamp"); + m_cola_service_client = ROS_CREATE_SRV_CLIENT(nh, sick_lidar_localization::SickLocColaTelegramSrv, "SickLocColaTelegram"); + // m_request_timestamp_client = ROS_CREATE_SRV_CLIENT(nh, sick_lidar_localization::SickLocRequestTimestampSrv, "SickLocRequestTimestamp"); } } @@ -137,26 +142,50 @@ sick_lidar_localization::TimeSyncService::~TimeSyncService() */ bool sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp(sick_lidar_localization::SickLocRequestTimestampSrv::Request & service_request, sick_lidar_localization::SickLocRequestTimestampSrv::Response & service_response) { + boost::lock_guard service_cb_lockguard(m_service_cb_mutex); // one service request at a time // Sends cola command "sMN LocRequestTimestamp" and receive timestamp from localization controller using ros service "SickLocColaTelegram" - sick_lidar_localization::SickLocColaTelegramSrv cola_telegram; - cola_telegram.request.cola_ascii_request = "sMN LocRequestTimestamp"; - cola_telegram.request.wait_response_timeout = m_cola_response_timeout; - // cola_telegram.request.send_binary = m_cola_binary; m_cola_binary = ((m_cola_binary_mode == 2) ? (!m_cola_binary) : (m_cola_binary)); // m_cola_binary_mode == 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) - if (!m_cola_service_client.call(cola_telegram) || cola_telegram.response.cola_ascii_response.empty()) +#if defined __ROS_VERSION && __ROS_VERSION == 1 + sick_lidar_localization::SickLocColaTelegramSrv cola_telegram; + sick_lidar_localization::SickLocColaTelegramSrv::Request* cola_telegram_request = &cola_telegram.request; + sick_lidar_localization::SickLocColaTelegramSrv::Response* cola_telegram_response = &cola_telegram.response; +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + std::shared_ptr cola_telegram_request = std::make_shared(); + std::shared_ptr cola_telegram_response = std::make_shared(); +#endif + cola_telegram_request->cola_ascii_request = "sMN LocRequestTimestamp"; + cola_telegram_request->wait_response_timeout = m_cola_response_timeout; + // cola_telegram_request.send_binary = m_cola_binary; + try + { + ROS::Time start_request_timestamp = ROS::now(); + if(m_driver_monitor == 0) + { + ROS_ERROR_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): TimeSyncService not initialized (driver_monitor == 0, " << __FILE__ << ":" << __LINE__ << ")"); + return false; + } + bool service_call_ok = m_driver_monitor->serviceCbColaTelegram(*cola_telegram_request, *cola_telegram_response); + if (!service_call_ok || cola_telegram_response->cola_ascii_response.empty()) + { + ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): calling ros service \"SickLocColaTelegram\" failed with request: \"" + << cola_telegram_request->cola_ascii_request << "\" response: \"" << cola_telegram_response->cola_ascii_response << "\" after " + << ROS::seconds(ROS::now() - start_request_timestamp) << " sec (status: " << service_call_ok << ", timeout: " << m_cola_response_timeout + << " sec, " << __FILE__ << ":" << __LINE__ << ")"); + return false; + } + ROS_DEBUG_STREAM("TimeSyncService::serviceCbRequestTimestamp(): request \"" << cola_telegram_request->cola_ascii_request << "\" response: \"" << cola_telegram_response->cola_ascii_response << "\" succesfull."); + } + catch(const std::exception & exc) { - ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): calling ros service \"SickLocColaTelegram\" failed with request: " - << sick_lidar_localization::Utils::flattenToString(cola_telegram.request) << " response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram.response)); + ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): request \"" << cola_telegram_request->cola_ascii_request << "\" failed, exception " << exc.what()); return false; } - ROS_DEBUG_STREAM("TimeSyncService::serviceCbRequestTimestamp(): request " << sick_lidar_localization::Utils::flattenToString(cola_telegram.request) - << " response: " << sick_lidar_localization::Utils::flattenToString(cola_telegram.response) << " succesfull."); // Decode response, get timestamp_lidar_ms from parameter - sick_lidar_localization::SickLocColaTelegramMsg cola_response = sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram.response.cola_ascii_response); + sick_lidar_localization::SickLocColaTelegramMsg cola_response = sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram_response->cola_ascii_response); if(cola_response.command_name != "LocRequestTimestamp" || cola_response.parameter.size() != 1) { - ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): invalid or unexpected cola response: \"" << cola_telegram.response.cola_ascii_response + ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): invalid or unexpected cola response: \"" << cola_telegram_response->cola_ascii_response << "\" decoded to " << sick_lidar_localization::Utils::flattenToString(cola_response)); return false; } @@ -170,17 +199,17 @@ bool sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp(sick_li << ", exception " << exc.what()); return false; } - if(service_response.timestamp_lidar_ms <= 0 || cola_telegram.response.send_timestamp_sec <= 0 || cola_telegram.response.receive_timestamp_sec <= 0) + if(service_response.timestamp_lidar_ms <= 0 || cola_telegram_response->send_timestamp_sec <= 0 || cola_telegram_response->receive_timestamp_sec <= 0) { ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): invalid timestamps in cola response " << sick_lidar_localization::Utils::flattenToString(cola_response)); return false; } // Set timestamps in service_response - service_response.send_time_vehicle_sec = cola_telegram.response.send_timestamp_sec; // Vehicle timestamp when sending LocRequestTimestamp (seconds part of ros timestamp immediately before tcp send) - service_response.send_time_vehicle_nsec = cola_telegram.response.send_timestamp_nsec; // Vehicle timestamp when sending LocRequestTimestamp (nano seconds part of ros timestamp immediately before tcp send) - service_response.receive_time_vehicle_sec = cola_telegram.response.receive_timestamp_sec; // Vehicle timestamp when receiving the LocRequestTimestamp response (seconds part of ros timestamp immediately after first response byte received) - service_response.receive_time_vehicle_nsec = cola_telegram.response.receive_timestamp_nsec; // Vehicle timestamp when receiving the LocRequestTimestamp response (nano seconds part of ros timestamp immediately after first response byte received) + service_response.send_time_vehicle_sec = cola_telegram_response->send_timestamp_sec; // Vehicle timestamp when sending LocRequestTimestamp (seconds part of ros timestamp immediately before tcp send) + service_response.send_time_vehicle_nsec = cola_telegram_response->send_timestamp_nsec; // Vehicle timestamp when sending LocRequestTimestamp (nano seconds part of ros timestamp immediately before tcp send) + service_response.receive_time_vehicle_sec = cola_telegram_response->receive_timestamp_sec; // Vehicle timestamp when receiving the LocRequestTimestamp response (seconds part of ros timestamp immediately after first response byte received) + service_response.receive_time_vehicle_nsec = cola_telegram_response->receive_timestamp_nsec; // Vehicle timestamp when receiving the LocRequestTimestamp response (nano seconds part of ros timestamp immediately after first response byte received) // Calculate time offset uint64_t send_time_vehicle_nsec = service_response.send_time_vehicle_sec * 1000000000UL + service_response.send_time_vehicle_nsec; @@ -193,10 +222,11 @@ bool sick_lidar_localization::TimeSyncService::serviceCbRequestTimestamp(sick_li updateSoftwarePll(service_response); // Get system timestamp from ticks via ros service "SickLocTimeSync" - sick_lidar_localization::SickLocTimeSyncSrv time_sync_msg; - time_sync_msg.request.timestamp_lidar_ms = service_response.timestamp_lidar_ms; - if(serviceCbTimeSync(time_sync_msg.request, time_sync_msg.response) && time_sync_msg.response.vehicle_time_valid) - ROS_INFO_STREAM("TimeSyncService::serviceCbRequestTimestamp(): Lidar ticks: " << service_response.timestamp_lidar_ms << ", Systemtime: " << time_sync_msg.response.vehicle_time_sec << "." << time_sync_msg.response.vehicle_time_sec); + sick_lidar_localization::SickLocTimeSyncSrv::Request time_sync_msg_request; + sick_lidar_localization::SickLocTimeSyncSrv::Response time_sync_msg_response; + time_sync_msg_request.timestamp_lidar_ms = service_response.timestamp_lidar_ms; + if(serviceCbTimeSync(time_sync_msg_request, time_sync_msg_response) && time_sync_msg_response.vehicle_time_valid) + ROS_INFO_STREAM("TimeSyncService::serviceCbRequestTimestamp(): Lidar ticks: " << service_response.timestamp_lidar_ms << ", Systemtime: " << time_sync_msg_response.vehicle_time_sec << "." << time_sync_msg_response.vehicle_time_sec); else if(isSoftwarePllInitialized()) ROS_WARN_STREAM("## ERROR TimeSyncService::serviceCbRequestTimestamp(): service \"SickLocTimeSync\" failed, could not get system time from ticks"); else @@ -256,11 +286,10 @@ bool sick_lidar_localization::TimeSyncService::serviceCbTimeSync(sick_lidar_loca if(software_pll_send_time.GetCorrectedTimeStamp(system_timestamp_1_sec, system_timestamp_1_nsec, ticks) && software_pll_receive_time.GetCorrectedTimeStamp(system_timestamp_2_sec, system_timestamp_2_nsec, ticks)) { - ros::Time system_timestamp_1(system_timestamp_1_sec, system_timestamp_1_nsec); - ros::Time system_timestamp_2(system_timestamp_2_sec, system_timestamp_2_nsec); - ros::Time system_timestamp = system_timestamp_1 + ros::Duration(0.5 * (system_timestamp_2 - system_timestamp_1).toSec()); - time_sync_response.vehicle_time_sec = system_timestamp.sec; - time_sync_response.vehicle_time_nsec = system_timestamp.nsec; + ROS::Time system_timestamp_1(system_timestamp_1_sec, system_timestamp_1_nsec); + ROS::Time system_timestamp_2(system_timestamp_2_sec, system_timestamp_2_nsec); + ROS::Time system_timestamp = system_timestamp_1 + ROS::durationFromSec(0.5 * ROS::seconds(system_timestamp_2 - system_timestamp_1)); + ROS::splitTime(system_timestamp, time_sync_response.vehicle_time_sec, time_sync_response.vehicle_time_nsec); time_sync_response.vehicle_time_valid = true; ROS_INFO_STREAM("TimeSyncService::serviceCbTimeSync(): Lidar ticks: " << ticks << ", Systemtime: " << time_sync_response.vehicle_time_sec << "." << time_sync_response.vehicle_time_nsec); } @@ -312,25 +341,34 @@ bool sick_lidar_localization::TimeSyncService::stop(void) void sick_lidar_localization::TimeSyncService::runTimeSyncThreadCb(void) { int time_sync_cnt = 0; - while(ros::ok() && m_time_sync_thread_running) + while(ROS::ok() && m_time_sync_thread_running) { // Run LocRequestTimestamp every second during initialization of software pll, otherwise every 10 seconds - ros::Rate & time_sync_rate = ((time_sync_cnt < m_time_sync_initial_length) ? m_time_sync_initial_rate : m_time_sync_rate); + ROS::Rate time_sync_rate((time_sync_cnt < m_time_sync_initial_length) ? m_time_sync_initial_rate : m_time_sync_rate); time_sync_rate.sleep(); - if(ros::ok() && m_time_sync_thread_running) + if(ROS::ok() && m_time_sync_thread_running) { // Call ros service "SickLocRequestTimestamp" +#if defined __ROS_VERSION && __ROS_VERSION == 1 sick_lidar_localization::SickLocRequestTimestampSrv timestamp_service; - if (!m_request_timestamp_client.call(timestamp_service) || timestamp_service.response.timestamp_lidar_ms == 0) + sick_lidar_localization::SickLocRequestTimestampSrv::Request* timestamp_service_request = ×tamp_service.request; + sick_lidar_localization::SickLocRequestTimestampSrv::Response* timestamp_service_response = ×tamp_service.response; + // bool service_call_ok = m_request_timestamp_client.call(timestamp_service); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + std::shared_ptr timestamp_service_request = std::make_shared(); + std::shared_ptr timestamp_service_response = std::make_shared(); +#endif + // ROS_INFO_STREAM("TimeSyncService::runTimeSyncThreadCb(): calling serviceCbRequestTimestamp() ..."); + bool service_call_ok = serviceCbRequestTimestamp(*timestamp_service_request, *timestamp_service_response); + if (!service_call_ok || timestamp_service_response->timestamp_lidar_ms == 0) { - ROS_WARN_STREAM("## ERROR TimeSyncService::runTimeSyncThreadCb(): calling ros service \"SickLocRequestTimestamp\" failed, response: " - << sick_lidar_localization::Utils::flattenToString(timestamp_service.response)); + ROS_WARN_STREAM("## ERROR TimeSyncService::runTimeSyncThreadCb(): calling ros service \"SickLocRequestTimestamp\" failed, response: timestamp_lidar_ms=" << timestamp_service_response->timestamp_lidar_ms + << " (status: " << service_call_ok << ", timeout: 1 sec, " << __FILE__ << ":" << __LINE__ << ")"); } else { time_sync_cnt++; - ROS_INFO_STREAM("TimeSyncService::runTimeSyncThreadCb(): ros service \"SickLocRequestTimestamp\" successfull, response: " - << sick_lidar_localization::Utils::flattenToString(timestamp_service.response)); + ROS_INFO_STREAM("TimeSyncService::runTimeSyncThreadCb(): ros service \"SickLocRequestTimestamp\" successfull, response: timestamp_lidar_ms=" << timestamp_service_response->timestamp_lidar_ms); } } } diff --git a/src/utils.cpp b/src/utils.cpp index af59596..998cdab 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -53,7 +53,7 @@ * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include #include #include diff --git a/srv/SickDevGetLidarConfigSrv.srv b/srv/SickDevGetLidarConfigSrv.srv new file mode 100755 index 0000000..4901fb7 --- /dev/null +++ b/srv/SickDevGetLidarConfigSrv.srv @@ -0,0 +1,32 @@ +# Definition of ROS service SickDevGetLidarConfig for sick localization +# Example call (ROS1): +# rosservice call SickDevGetLidarConfig "{scannerindex: 0}" +# Example call (ROS2): +# ros2 service call SickDevGetLidarConfig sick_lidar_localization/srv/SickDevGetLidarConfigSrv "{scannerindex: 0}" +# + +# +# Request (input) +# + +uint32 scannerindex # The scanner index. [, 1] + +--- + +# +# Response (output) +# + +uint32 minrange # Beams with a range lower than this distance will be discarded. [, 250000] in [mm] +uint32 maxrange # Beams with a range greater than this distance will be discarded. [, 250000] in [mm] +int32 minangle # Beams with an angle lower than this threshold will be discarded. [-180000, 180000] in [mdeg] +int32 maxangle # Beams with an angle greater than this threshold will be discarded. [-180000, 180000] in [mdeg] +int32 x # X position relative to vehicle coordinate system. [-50000, 50000] in [mm] +int32 y # Y position relative to vehicle coordinate system. [-50000, 50000] in [mm] +int32 yaw # Yaw angle relative to vehicle coordinate system. [-180000, 180000] in [mdeg] +bool upsidedown # Whether the sensor is mounted upside down. {0 (false), 1 (true)} +string ip # IP address of the sensor. Must be in the same subnet as the port which it is connected to. Max length = 15 +uint32 port # COLA Port of the scanner used for communication. This is usually 2111 or 2122. [0, 65535] +int32 interfacetype # The type of the interface to this lidar. {0 (TCP), 1 (SERIAL)} +uint32 maplayer # The index of the map layer on which this LiDAR operates. If no map layers are used, set it to 0. NOTE: This feature is not implemented yet but is reserved for later releases. [, ] +bool active # Whether this scanner shall be used for localization. For the LiDAR with the index 0 this should be always 1 (true). {0 (false), 1 (true)} diff --git a/srv/SickDevGetLidarIdentSrv.srv b/srv/SickDevGetLidarIdentSrv.srv new file mode 100755 index 0000000..5542fa9 --- /dev/null +++ b/srv/SickDevGetLidarIdentSrv.srv @@ -0,0 +1,22 @@ +# Definition of ROS service SickDevGetLidarIdent for sick localization +# Returns the scanner ident of the specified lidar. +# Example call (ROS1): +# rosservice call SickDevGetLidarIdent "{index: 0}" +# Example call (ROS2): +# ros2 service call SickDevGetLidarIdent sick_lidar_localization/srv/SickDevGetLidarIdentSrv "{index: 0}" +# + +# +# Request (input) +# + +uint32 index # The scanner index. [, 1] + +--- + +# +# Response (output) +# + +string scannerident # The type name of the scanner available after configuration and successful connection. Max length = 255 +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickDevGetLidarStateSrv.srv b/srv/SickDevGetLidarStateSrv.srv new file mode 100755 index 0000000..8db3bf3 --- /dev/null +++ b/srv/SickDevGetLidarStateSrv.srv @@ -0,0 +1,24 @@ +# Definition of ROS service SickDevGetLidarState for sick localization +# Returns the lidar state of the given lidar. +# Example call (ROS1): +# rosservice call SickDevGetLidarState "{index: 0}" +# Example call (ROS2): +# ros2 service call SickDevGetLidarState sick_lidar_localization/srv/SickDevGetLidarStateSrv "{index: 0}" +# + +# +# Request (input) +# + +uint32 index # The scanner index. [, 1] + +--- + +# +# Response (output) +# + +int32 devicestatus # Status of the device. {0 (ERROR), 1 (UNDEF), 2 (OKAY)} +int32 deviceconnected # State of the connection to the scanner. {0 (ERROR), 1 (UNDEF), 2 (OKAY)} +int32 receivingdata # State of the data transmission from the scanner. {0 (ERROR), 1 (UNDEF), 2 (OKAY)} +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickDevIMUActiveSrv.srv b/srv/SickDevIMUActiveSrv.srv new file mode 100755 index 0000000..961e5c1 --- /dev/null +++ b/srv/SickDevIMUActiveSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickDevIMUActive for sick localization +# Read IMU Active status +# Example call (ROS1): +# rosservice call SickDevIMUActive "{}" +# Example call (ROS2): +# ros2 service call SickDevIMUActive sick_lidar_localization/srv/SickDevIMUActiveSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool active # IMU data used for Localization +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickDevSetIMUActiveSrv.srv b/srv/SickDevSetIMUActiveSrv.srv new file mode 100755 index 0000000..7918573 --- /dev/null +++ b/srv/SickDevSetIMUActiveSrv.srv @@ -0,0 +1,21 @@ +# Definition of ROS service SickDevSetIMUActive for sick localization +# Set IMU Active +# Example call (ROS1): +# rosservice call SickDevSetIMUActive "{active: 1}" +# Example call (ROS2): +# ros2 service call SickDevSetIMUActive sick_lidar_localization/srv/SickDevSetIMUActiveSrv "{active: 1}" +# + +# +# Request (input) +# + +bool active # Use IMU data for Localization + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickDevSetLidarConfigSrv.srv b/srv/SickDevSetLidarConfigSrv.srv new file mode 100755 index 0000000..fcff90a --- /dev/null +++ b/srv/SickDevSetLidarConfigSrv.srv @@ -0,0 +1,35 @@ +# Definition of ROS service SickDevSetLidarConfig for sick localization +# Sets the configuration for a lidar +# Example call (ROS1): +# rosservice call SickDevSetLidarConfig "{index: 0, minrange: 100, maxrange: 200000, minangle: -15000, maxangle: 15000, x: 1000, y: -1000, yaw: 2000, upsidedown: true, ip: 192.168.0.123, port: 2111, interfacetype: 0, maplayer: 0, active: true}" +# Example call (ROS2): +# ros2 service call SickDevSetLidarConfig sick_lidar_localization/srv/SickDevSetLidarConfigSrv "{index: 0, minrange: 100, maxrange: 200000, minangle: -15000, maxangle: 15000, x: 1000, y: -1000, yaw: 2000, upsidedown: true, ip: 192.168.0.123, port: 2111, interfacetype: 0, maplayer: 0, active: true}" +# + +# +# Request (input) +# + +uint32 index # Index of the lidar that shall be configured. [0, 1] +uint32 minrange # Beams with a range lower than this distance will be discarded. [, 250000] in [mm] +uint32 maxrange # Beams with a range greater than this distance will be discarded. [, 250000] in [mm] +int32 minangle # Beams with an angle lower than this threshold will be discarded. [-180000, 180000] in [mdeg] +int32 maxangle # Beams with an angle greater than this threshold will be discarded. [-180000, 180000] in [mdeg] +int32 x # X position relative to vehicle coordinate system. [-50000, 50000] in [mm] +int32 y # Y position relative to vehicle coordinate system. [-50000, 50000] in [mm] +int32 yaw # Yaw angle relative to vehicle coordinate system. [-180000, 180000] in [mdeg] +bool upsidedown # Whether the sensor is mounted upside down. {0 (false), 1 (true)} +string ip # IP address of the sensor. Must be in the same subnet as the port which it is connected to. Max length = 15 +uint32 port # COLA Port of the scanner used for communication. This is usually 2111 or 2122. [0, 65535] +int32 interfacetype # The type of the interface to this lidar. {0 (TCP), 1 (SERIAL)} +uint32 maplayer # The index of the map layer on which this LiDAR operates. If no map layers are used, set it to 0. NOTE: This feature is not implemented yet but is reserved for later releases. [, ] +bool active # Whether this scanner shall be used for localization. For the LiDAR with the index 0 this should be always 1 (true). {0 (false), 1 (true)} + +--- + +# +# Response (output) +# + +bool set # Whether the arguments were valid and have been configured. {0 (false), 1 (true)} +bool executed # Whether the lidar driver started and received data. {0 (false), 1 (true)} diff --git a/srv/SickGetSoftwareVersionSrv.srv b/srv/SickGetSoftwareVersionSrv.srv new file mode 100755 index 0000000..1383dcc --- /dev/null +++ b/srv/SickGetSoftwareVersionSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickGetSoftwareVersion for sick localization +# Returns the version string of the localization system. +# Example call (ROS1): +# rosservice call SickGetSoftwareVersion "{}" +# Example call (ROS2): +# ros2 service call SickGetSoftwareVersion sick_lidar_localization/srv/SickGetSoftwareVersionSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +string version # The software version. Max length = 255 +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocAutoStartActiveSrv.srv b/srv/SickLocAutoStartActiveSrv.srv new file mode 100755 index 0000000..4bef1b2 --- /dev/null +++ b/srv/SickLocAutoStartActiveSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocAutoStartActive for sick localization +# Returns whether autostart is active or not +# Example call (ROS1): +# rosservice call SickLocAutoStartActive "{}" +# Example call (ROS2): +# ros2 service call SickLocAutoStartActive sick_lidar_localization/srv/SickLocAutoStartActiveSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool active # Whether autostart should be used or not. {0 (false), 1 (true)} +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocAutoStartSavePoseIntervalSrv.srv b/srv/SickLocAutoStartSavePoseIntervalSrv.srv new file mode 100755 index 0000000..a05cef4 --- /dev/null +++ b/srv/SickLocAutoStartSavePoseIntervalSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocAutoStartSavePoseInterval for sick localization +# The interval in seconds for saving the pose automatically for auto start while localizing. +# Example call (ROS1): +# rosservice call SickLocAutoStartSavePoseInterval "{}" +# Example call (ROS2): +# ros2 service call SickLocAutoStartSavePoseInterval sick_lidar_localization/srv/SickLocAutoStartSavePoseIntervalSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +uint32 interval # Interval between consecutive pose saving in seconds. [, ] +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocAutoStartSavePoseSrv.srv b/srv/SickLocAutoStartSavePoseSrv.srv new file mode 100755 index 0000000..4e678ae --- /dev/null +++ b/srv/SickLocAutoStartSavePoseSrv.srv @@ -0,0 +1,19 @@ +# Definition of ROS service SickLocAutoStartSavePose for sick localization +# Saves the current pose. If no further pose writing occurs the system will reinitialize itself at this position on restart if LocAutoStart is enabled. +# Example call (ROS1): +# rosservice call SickLocAutoStartSavePose "{}" +# Example call (ROS2): +# ros2 service call SickLocAutoStartSavePose sick_lidar_localization/srv/SickLocAutoStartSavePoseSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocForceUpdateSrv.srv b/srv/SickLocForceUpdateSrv.srv new file mode 100755 index 0000000..bb3e079 --- /dev/null +++ b/srv/SickLocForceUpdateSrv.srv @@ -0,0 +1,19 @@ +# Definition of ROS service SickLocForceUpdate for sick localization +# Forces an update of the map localization with the next scan. This should be used with care because it is not garanteed that this converges to the correct pose. Moving the LiDAR instead should be preferred because it produces more robust updates. +# Example call (ROS1): +# rosservice call SickLocForceUpdate "{}" +# Example call (ROS2): +# ros2 service call SickLocForceUpdate sick_lidar_localization/srv/SickLocForceUpdateSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocInitialPoseSrv.srv b/srv/SickLocInitialPoseSrv.srv new file mode 100755 index 0000000..df49764 --- /dev/null +++ b/srv/SickLocInitialPoseSrv.srv @@ -0,0 +1,23 @@ +# Definition of ROS service SickLocInitialPose for sick localization +# Returns the initial pose +# Example call (ROS1): +# rosservice call SickLocInitialPose "{}" +# Example call (ROS2): +# ros2 service call SickLocInitialPose sick_lidar_localization/srv/SickLocInitialPoseSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +int32 x # Vehicle x position relative to map origin. [, ] in [mm] +int32 y # Vehicle y position relative to map origin. [, ] in [mm] +int32 yaw # Vehicle yaw angle relative to map origin. [-180000, 180000] in [mdeg] +uint32 sigmatranslation # Standard deviation of the initial position describing the uncertainty. [300, 5000] in [mm] +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocInitializePoseSrv.srv b/srv/SickLocInitializePoseSrv.srv new file mode 100755 index 0000000..faef5a1 --- /dev/null +++ b/srv/SickLocInitializePoseSrv.srv @@ -0,0 +1,24 @@ +# Definition of ROS service SickLocInitializePose for sick localization +# Automatically adjusts the given input pose according to the map of the environment and the current LiDAR measurements. +# Example call (ROS1): +# rosservice call SickLocInitializePose "{x: 100, y: -100, yaw: 2000, sigmatranslation: 1000}" +# Example call (ROS2): +# ros2 service call SickLocInitializePose sick_lidar_localization/srv/SickLocInitializePoseSrv "{x: 100, y: -100, yaw: 2000, sigmatranslation: 1000}" +# + +# +# Request (input) +# + +int32 x # Vehicle x position relative to map origin. [, ] in [mm] +int32 y # Vehicle y position relative to map origin. [, ] in [mm] +int32 yaw # Vehicle yaw angle relative to map origin. [-180000, 180000] in [mdeg] +uint32 sigmatranslation # Standard deviation of the initial position describing the uncertainty. [300, 5000] in [mm] + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocMapSrv.srv b/srv/SickLocMapSrv.srv new file mode 100755 index 0000000..e68f93a --- /dev/null +++ b/srv/SickLocMapSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocMap for sick localization +# Returns the filename of the configured map or an empty string if no map is defined. +# Example call (ROS1): +# rosservice call SickLocMap "}" +# Example call (ROS2): +# ros2 service call SickLocMap sick_lidar_localization/srv/SickLocMapSrv "}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +string map # filename of the configured map or an empty string if no map is defined. +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocMapStateSrv.srv b/srv/SickLocMapStateSrv.srv new file mode 100755 index 0000000..dca1cf4 --- /dev/null +++ b/srv/SickLocMapStateSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocMapState for sick localization +# Returns the current state of the map. 0: not active, 1: active. +# Example call (ROS1): +# rosservice call SickLocMapState "{}" +# Example call (ROS2): +# ros2 service call SickLocMapState sick_lidar_localization/srv/SickLocMapStateSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool mapstate # the current state of the map. 0: not active, 1: active. +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocOdometryActiveSrv.srv b/srv/SickLocOdometryActiveSrv.srv new file mode 100755 index 0000000..4b6a7ec --- /dev/null +++ b/srv/SickLocOdometryActiveSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocOdometryActive for sick localization +# Whether the Scan Matcher uses data from an odometry sensor +# Example call (ROS1): +# rosservice call SickLocOdometryActive "{}" +# Example call (ROS2): +# ros2 service call SickLocOdometryActive sick_lidar_localization/srv/SickLocOdometryActiveSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool active # Whether data from an odometry sensor is considered in Scan Matching {0 (false), 1 (true)} +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocOdometryPortSrv.srv b/srv/SickLocOdometryPortSrv.srv new file mode 100755 index 0000000..76c6ebf --- /dev/null +++ b/srv/SickLocOdometryPortSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocOdometryPort for sick localization +# Returns the port of the UDP socket for odometry measurement input. +# Example call (ROS1): +# rosservice call SickLocOdometryPort "{}" +# Example call (ROS2): +# ros2 service call SickLocOdometryPort sick_lidar_localization/srv/SickLocOdometryPortSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +uint32 port # The new UDP port of the UDP socket for odometry measurement input. [, ] +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocOdometryRestrictYMotionSrv.srv b/srv/SickLocOdometryRestrictYMotionSrv.srv new file mode 100755 index 0000000..8cae4a4 --- /dev/null +++ b/srv/SickLocOdometryRestrictYMotionSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocOdometryRestrictYMotion for sick localization +# Returns the variable that Indicates that the vehicle will be able to move in Y-Direction or not. If true the mounting pose of the sensor matters. For omnidirektional vehicles this must be set to false. +# Example call (ROS1): +# rosservice call SickLocOdometryRestrictYMotion "{}" +# Example call (ROS2): +# ros2 service call SickLocOdometryRestrictYMotion sick_lidar_localization/srv/SickLocOdometryRestrictYMotionSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool active # Indicates that the vehicle will be able to move in Y-Direction or not +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocReflectorsForSupportActiveSrv.srv b/srv/SickLocReflectorsForSupportActiveSrv.srv new file mode 100755 index 0000000..9a17298 --- /dev/null +++ b/srv/SickLocReflectorsForSupportActiveSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocReflectorsForSupportActive for sick localization +# Whether the map based localization uses mapped reflectors for robustification. +# Example call (ROS1): +# rosservice call SickLocReflectorsForSupportActive "{}" +# Example call (ROS2): +# ros2 service call SickLocReflectorsForSupportActive sick_lidar_localization/srv/SickLocReflectorsForSupportActiveSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool active # Whether the map based localization uses mapped reflectors for robustification. +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocResultEndiannessSrv.srv b/srv/SickLocResultEndiannessSrv.srv new file mode 100755 index 0000000..6419e76 --- /dev/null +++ b/srv/SickLocResultEndiannessSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocResultEndianness for sick localization +# Returns the endianness of the result port: 0 BIG_ENDIAN (default), 1 LITTLE_ENDIAN +# Example call (ROS1): +# rosservice call SickLocResultEndianness "{}" +# Example call (ROS2): +# ros2 service call SickLocResultEndianness sick_lidar_localization/srv/SickLocResultEndiannessSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +int32 endianness # The endianness of the result port. 0 BIG_ENDIAN (default), 1 LITTLE_ENDIAN +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocResultModeSrv.srv b/srv/SickLocResultModeSrv.srv new file mode 100755 index 0000000..04942d8 --- /dev/null +++ b/srv/SickLocResultModeSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocResultMode for sick localization +# Returns the current result mode: 0 STREAM (default), 1 POLL +# Example call (ROS1): +# rosservice call SickLocResultMode "{}" +# Example call (ROS2): +# ros2 service call SickLocResultMode sick_lidar_localization/srv/SickLocResultModeSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +int32 mode # The current result mode. 0 STREAM (default), 1 POLL +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocResultPortSrv.srv b/srv/SickLocResultPortSrv.srv new file mode 100755 index 0000000..e9ece30 --- /dev/null +++ b/srv/SickLocResultPortSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocResultPort for sick localization +# Read the TCP port of the localization controller for result output. +# Example call (ROS1): +# rosservice call SickLocResultPort "{}" +# Example call (ROS2): +# ros2 service call SickLocResultPort sick_lidar_localization/srv/SickLocResultPortSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +int32 port # TCP-port for result output telegrams (default: 2201) +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocResultPoseIntervalSrv.srv b/srv/SickLocResultPoseIntervalSrv.srv new file mode 100755 index 0000000..473ab90 --- /dev/null +++ b/srv/SickLocResultPoseIntervalSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocResultPoseInterval for sick localization +# Reads the interval of the pose result output. +# Example call (ROS1): +# rosservice call SickLocResultPoseInterval "{}" +# Example call (ROS2): +# ros2 service call SickLocResultPoseInterval sick_lidar_localization/srv/SickLocResultPoseIntervalSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +int32 interval # interval in number of scans, 0-255, 1 (default): result with each processed scan +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocResultStateSrv.srv b/srv/SickLocResultStateSrv.srv new file mode 100755 index 0000000..cb4a91f --- /dev/null +++ b/srv/SickLocResultStateSrv.srv @@ -0,0 +1,19 @@ +# Definition of ROS service SickLocResultState for sick localization +# Example call (ROS1): +# rosservice call SickLocResultState "{}" +# Example call (ROS2): +# ros2 service call SickLocResultState sick_lidar_localization/srv/SickLocResultStateSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +uint32 state # The output state of the result interface as bit field: From LSB to MSB: Bit 1: Pose output enabled Bit 8: Error flag +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocRingBufferRecordingActiveSrv.srv b/srv/SickLocRingBufferRecordingActiveSrv.srv new file mode 100755 index 0000000..2f7d48d --- /dev/null +++ b/srv/SickLocRingBufferRecordingActiveSrv.srv @@ -0,0 +1,20 @@ +# Definition of ROS service SickLocRingBufferRecordingActive for sick localization +# Returns whether rolling buffer recording on error reporting is active. {0 (false), 1 (true)} +# Example call (ROS1): +# rosservice call SickLocRingBufferRecordingActive "{}" +# Example call (ROS2): +# ros2 service call SickLocRingBufferRecordingActive sick_lidar_localization/srv/SickLocRingBufferRecordingActiveSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool active # Whether rolling buffer recording on error reporting is active. {0 (false), 1 (true)} +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocSaveRingBufferRecordingSrv.srv b/srv/SickLocSaveRingBufferRecordingSrv.srv new file mode 100755 index 0000000..c591969 --- /dev/null +++ b/srv/SickLocSaveRingBufferRecordingSrv.srv @@ -0,0 +1,21 @@ +# Definition of ROS service SickLocSaveRingBufferRecording for sick localization +# Saves the current sensor data ring buffer as log file. The log file can be downloaded via FTP. +# Example call (ROS1): +# rosservice call SickLocSaveRingBufferRecording "{reason: test}" +# Example call (ROS2): +# ros2 service call SickLocSaveRingBufferRecording sick_lidar_localization/srv/SickLocSaveRingBufferRecordingSrv "{reason: test}" +# + +# +# Request (input) +# + +string reason # Reason why the recording should be saved. Max length = 512 + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocSetAutoStartActiveSrv.srv b/srv/SickLocSetAutoStartActiveSrv.srv new file mode 100755 index 0000000..aa7f523 --- /dev/null +++ b/srv/SickLocSetAutoStartActiveSrv.srv @@ -0,0 +1,21 @@ +# Definition of ROS service SickLocSetAutoStartActive for sick localization +# Sets whether autostart should be used or not. +# Example call (ROS1): +# rosservice call SickLocSetAutoStartActive "{active: True}" +# Example call (ROS2): +# ros2 service call SickLocSetAutoStartActive sick_lidar_localization/srv/SickLocSetAutoStartActiveSrv "{active: True}" +# + +# +# Request (input) +# + +bool active # Whether autostart should be used or not. {0 (false), 1 (true)} + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocSetAutoStartSavePoseIntervalSrv.srv b/srv/SickLocSetAutoStartSavePoseIntervalSrv.srv new file mode 100755 index 0000000..9c2a1b0 --- /dev/null +++ b/srv/SickLocSetAutoStartSavePoseIntervalSrv.srv @@ -0,0 +1,21 @@ +# Definition of ROS service SickLocSetAutoStartSavePoseInterval for sick localization +# Set the interval in seconds for saving the pose automatically for auto start while localizing +# Example call (ROS1): +# rosservice call SickLocSetAutoStartSavePoseInterval "{interval: 1}" +# Example call (ROS2): +# ros2 service call SickLocSetAutoStartSavePoseInterval sick_lidar_localization/srv/SickLocSetAutoStartSavePoseIntervalSrv "{interval: 1}" +# + +# +# Request (input) +# + +uint32 interval # Interval between consecutive pose saving in seconds. [, ] + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocSetMapSrv.srv b/srv/SickLocSetMapSrv.srv new file mode 100755 index 0000000..fb5efe2 --- /dev/null +++ b/srv/SickLocSetMapSrv.srv @@ -0,0 +1,22 @@ +# Definition of ROS service SickLocSetMap for sick localization +# Loads the given map +# Example call (ROS1): +# rosservice call SickLocSetMap "{mapfilename: test.map}" +# Example call (ROS2): +# ros2 service call SickLocSetMap sick_lidar_localization/srv/SickLocSetMapSrv "{mapfilename: test.map}" +# + +# +# Request (input) +# + +string mapfilename # The filename of the map to load. Max length = 255 + +--- + +# +# Response (output) +# + +bool set # Whether the arguments were valid and have been configured. {0 (false), 1 (true)} +bool executed # Whether the lidar driver started and received data. {0 (false), 1 (true)} diff --git a/srv/SickLocSetOdometryActiveSrv.srv b/srv/SickLocSetOdometryActiveSrv.srv new file mode 100755 index 0000000..fb7a264 --- /dev/null +++ b/srv/SickLocSetOdometryActiveSrv.srv @@ -0,0 +1,22 @@ +# Definition of ROS service SickLocSetOdometryActive for sick localization +# Enables or disables usage of odometry data in Scan Matching +# Example call (ROS1): +# rosservice call SickLocSetOdometryActive "{active: 1}" +# Example call (ROS2): +# ros2 service call SickLocSetOdometryActive sick_lidar_localization/srv/SickLocSetOdometryActiveSrv "{active: 1}" +# + +# +# Request (input) +# + +bool active # Whether data from an odometry sensor is considered in Scan Matching {0 (false), 1 (true)} + +--- + +# +# Response (output) +# + +bool set # Whether the odometry configuration is set. {0 (false), 1 (true)} +bool executed # Whether the odometry configuration is fine and the UDP socket could be started. {0 (false), 1 (true)} diff --git a/srv/SickLocSetOdometryPortSrv.srv b/srv/SickLocSetOdometryPortSrv.srv new file mode 100755 index 0000000..3cd0549 --- /dev/null +++ b/srv/SickLocSetOdometryPortSrv.srv @@ -0,0 +1,22 @@ +# Definition of ROS service SickLocSetOdometryPort for sick localization +# Sets the UDP port of the UDP socket for odometry measurement input. +# Example call (ROS1): +# rosservice call SickLocSetOdometryPort "{port: 3000}" +# Example call (ROS2): +# ros2 service call SickLocSetOdometryPort sick_lidar_localization/srv/SickLocSetOdometryPortSrv "{port: 3000}" +# + +# +# Request (input) +# + +uint32 port # The new UDP port of the UDP socket for odometry measurement input. [, ] + +--- + +# +# Response (output) +# + +bool set # Whether the arguments were valid and have been configured. {0 (false), 1 (true)} +bool executed # Whether the lidar driver started and received data. {0 (false), 1 (true)} diff --git a/srv/SickLocSetOdometryRestrictYMotionSrv.srv b/srv/SickLocSetOdometryRestrictYMotionSrv.srv new file mode 100755 index 0000000..36ed596 --- /dev/null +++ b/srv/SickLocSetOdometryRestrictYMotionSrv.srv @@ -0,0 +1,21 @@ +# Definition of ROS service SickLocSetOdometryRestrictYMotion for sick localization +# Method to set the variable that Indicates that the vehicle will be able to move in Y-Direction or not. If true the mounting pose of the sensor matters. For omnidirektional vehicles this must be set to false. +# Example call (ROS1): +# rosservice call SickLocSetOdometryRestrictYMotion "{active: 1}" +# Example call (ROS2): +# ros2 service call SickLocSetOdometryRestrictYMotion sick_lidar_localization/srv/SickLocSetOdometryRestrictYMotionSrv "{active: 1}" +# + +# +# Request (input) +# + +bool active # Indicates that the vehicle will be able to move in Y-Direction or not + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocSetReflectorsForSupportActiveSrv.srv b/srv/SickLocSetReflectorsForSupportActiveSrv.srv new file mode 100755 index 0000000..51066b6 --- /dev/null +++ b/srv/SickLocSetReflectorsForSupportActiveSrv.srv @@ -0,0 +1,21 @@ +# Definition of ROS service SickLocSetReflectorsForSupportActive for sick localization +# Enables or disables usage of mapped reflectors for map based localization robustification +# Example call (ROS1): +# rosservice call SickLocSetReflectorsForSupportActive "{active: 1}" +# Example call (ROS2): +# ros2 service call SickLocSetReflectorsForSupportActive sick_lidar_localization/srv/SickLocSetReflectorsForSupportActiveSrv "{active: 1}" +# + +# +# Request (input) +# + +bool active # Whether the map based localization shall use mapped reflectors for robustification. {0 (false), 1 (true)} + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocSetResultPoseIntervalSrv.srv b/srv/SickLocSetResultPoseIntervalSrv.srv index 823888f..0e6276e 100644 --- a/srv/SickLocSetResultPoseIntervalSrv.srv +++ b/srv/SickLocSetResultPoseIntervalSrv.srv @@ -1,6 +1,6 @@ # Definition of ROS service SickLocSetResultPoseInterval for sick localization. # -# ROS service SickLocSetResultPoseInterval sets the interval of result output telegrams +# ROS service SickLocSetResultPoseInterval sets the interval of the pose result output. # by sending cola command ("sMN LocSetResultPoseInterval ") # with parameter # : 0-255, interval in number of scans, 1: result with each processed scan, default: 1 diff --git a/srv/SickLocSetRingBufferRecordingActiveSrv.srv b/srv/SickLocSetRingBufferRecordingActiveSrv.srv new file mode 100755 index 0000000..e11c150 --- /dev/null +++ b/srv/SickLocSetRingBufferRecordingActiveSrv.srv @@ -0,0 +1,21 @@ +# Definition of ROS service SickLocSetRingBufferRecordingActive for sick localization +# Activates or deactivates rolling buffer log file recording on error report +# Example call (ROS1): +# rosservice call SickLocSetRingBufferRecordingActive "{active: 1}" +# Example call (ROS2): +# ros2 service call SickLocSetRingBufferRecordingActive sick_lidar_localization/srv/SickLocSetRingBufferRecordingActiveSrv "{active: 1}" +# + +# +# Request (input) +# + +bool active # Whether rolling buffer recording on error reporting shall be active. {0 (false), 1 (true)} + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocStartDemoMappingSrv.srv b/srv/SickLocStartDemoMappingSrv.srv new file mode 100755 index 0000000..5512791 --- /dev/null +++ b/srv/SickLocStartDemoMappingSrv.srv @@ -0,0 +1,19 @@ +# Definition of ROS service SickLocStartDemoMapping for sick localization +# If all conditions are met, starts the demo mapping system. +# Example call (ROS1): +# rosservice call SickLocStartDemoMapping "{}" +# Example call (ROS2): +# ros2 service call SickLocStartDemoMapping sick_lidar_localization/srv/SickLocStartDemoMappingSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickLocStopAndSaveSrv.srv b/srv/SickLocStopAndSaveSrv.srv deleted file mode 100644 index f48a584..0000000 --- a/srv/SickLocStopAndSaveSrv.srv +++ /dev/null @@ -1,20 +0,0 @@ -# Definition of ROS service SickLocStopAndSave for sick localization. -# -# ROS service SickLocStopAndSave stops localization and saves settings -# by sending cola command ("sMN LocStopAndSave"). -# -# See Telegram-Listing-v1.1.0.241R.pdf for further details about -# Cola telegrams and this command. - -# -# Request (input) -# - ---- - -# -# Response (output) -# - -bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) - diff --git a/srv/SickLocStopSrv.srv b/srv/SickLocStopSrv.srv index 16531c4..30db733 100644 --- a/srv/SickLocStopSrv.srv +++ b/srv/SickLocStopSrv.srv @@ -1,6 +1,6 @@ # Definition of ROS service SickLocStop for sick localization. # -# ROS service SickLocStopAndSave stops localization +# ROS service SickLocStop stops localization # by sending cola command ("sMN LocStop"). # # See Telegram-Listing-v1.1.0.241R.pdf for further details about diff --git a/srv/SickReportUserMessageSrv.srv b/srv/SickReportUserMessageSrv.srv new file mode 100755 index 0000000..cc0db9b --- /dev/null +++ b/srv/SickReportUserMessageSrv.srv @@ -0,0 +1,21 @@ +# Definition of ROS service SickReportUserMessage for sick localization +# Method to report messages to the localization system. +# Example call (ROS1): +# rosservice call SickReportUserMessage "{usermessage: test_message}" +# Example call (ROS2): +# ros2 service call SickReportUserMessage sick_lidar_localization/srv/SickReportUserMessageSrv "{usermessage: test_message}" +# + +# +# Request (input) +# + +string usermessage # The message transmitted to the localization system. Max length = 400 + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/srv/SickSavePermanentSrv.srv b/srv/SickSavePermanentSrv.srv new file mode 100755 index 0000000..037d93b --- /dev/null +++ b/srv/SickSavePermanentSrv.srv @@ -0,0 +1,19 @@ +# Definition of ROS service SickSavePermanent for sick localization +# Saves the parameters permanently on the device. They will be reloaded on reboot. +# Example call (ROS1): +# rosservice call SickSavePermanent "{}" +# Example call (ROS2): +# ros2 service call SickSavePermanent sick_lidar_localization/srv/SickSavePermanentSrv "{}" +# + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller) diff --git a/test/config/rviz2_odom_msg_sender.rviz b/test/config/rviz2_odom_msg_sender.rviz new file mode 100755 index 0000000..fabcfd9 --- /dev/null +++ b/test/config/rviz2_odom_msg_sender.rviz @@ -0,0 +1,180 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Odometry1 + - /Odometry1/Shape1 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05000000074505806 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 9.802750587463379 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9403987526893616 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.198582172393799 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000354000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 1076 + Y: 94 diff --git a/test/config/rviz2_sick_lidar_localization_demo_pointcloud.rviz b/test/config/rviz2_sick_lidar_localization_demo_pointcloud.rviz new file mode 100755 index 0000000..4ebb499 --- /dev/null +++ b/test/config/rviz2_sick_lidar_localization_demo_pointcloud.rviz @@ -0,0 +1,157 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + 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 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: pointcloud_sick_lidar_localization + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 7.74399995803833 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9603978395462036 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7403980493545532 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000354000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 103 + Y: 42 diff --git a/test/config/rviz2_sick_lidar_localization_demo_tf.rviz b/test/config/rviz2_sick_lidar_localization_demo_tf.rviz new file mode 100755 index 0000000..aa88523 --- /dev/null +++ b/test/config/rviz2_sick_lidar_localization_demo_tf.rviz @@ -0,0 +1,148 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + map: + Value: true + pointcloud_sick_lidar_localization: + Value: true + tf_demo_map: + Value: true + tf_sick_lidar_localization: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + tf_demo_map: + tf_sick_lidar_localization: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: tf_demo_map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 7.74399995803833 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9603978395462036 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7403980493545532 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000354000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 1108 + Y: 42 diff --git a/test/config/rviz_odom_msg_sender.rviz b/test/config/rviz_odom_msg_sender.rviz new file mode 100755 index 0000000..a77c28a --- /dev/null +++ b/test/config/rviz_odom_msg_sender.rviz @@ -0,0 +1,171 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /Odometry1 + - /Odometry1/Shape1 + Splitter Ratio: 0.5 + Tree Height: 549 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.05000000074505806 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /odom + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 7.744000434875488 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.630398154258728 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.6703981161117554 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 67 + Y: 27 diff --git a/test/config/rviz_sick_lidar_localization_demo_pointcloud.rviz b/test/config/rviz_sick_lidar_localization_demo_pointcloud.rviz new file mode 100755 index 0000000..8dedb60 --- /dev/null +++ b/test/config/rviz_sick_lidar_localization_demo_pointcloud.rviz @@ -0,0 +1,151 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + Splitter Ratio: 0.5 + Tree Height: 549 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + 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): 5 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 6.814720153808594 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 67 + Y: 41 diff --git a/test/config/rviz_sick_lidar_localization_demo_tf.rviz b/test/config/rviz_sick_lidar_localization_demo_tf.rviz new file mode 100755 index 0000000..bb4bfd2 --- /dev/null +++ b/test/config/rviz_sick_lidar_localization_demo_tf.rviz @@ -0,0 +1,144 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 549 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + map: + Value: true + pointcloud_sick_lidar_localization: + Value: true + tf_demo_map: + Value: true + tf_sick_lidar_localization: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + tf_demo_map: + tf_sick_lidar_localization: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: tf_demo_map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 6.814720153808594 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 973 + Y: 41 diff --git a/test/include/sick_lidar_localization/test_server_thread.h b/test/include/sick_lidar_localization/test_server_thread.h index 116880f..1597a15 100644 --- a/test/include/sick_lidar_localization/test_server_thread.h +++ b/test/include/sick_lidar_localization/test_server_thread.h @@ -87,7 +87,7 @@ namespace sick_lidar_localization * @param[in] ip_port_results ip port for result telegrams, default: 2201 * @param[in] ip_port_cola ip port for command requests and responses, default: 2111 */ - TestServerThread(ros::NodeHandle* nh = 0, int ip_port_results = 2201, int ip_port_cola = 2111); + TestServerThread(ROS::NodePtr nh = 0, int ip_port_results = 2201, int ip_port_cola = 2111); /*! * Destructor. Stops the server thread and closes all tcp connections. @@ -112,6 +112,8 @@ namespace sick_lidar_localization * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) */ void messageCbResultPortTelegrams(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg); + /*! ROS2 version of function messageCbResultPortTelegrams */ + virtual void messageCbResultPortTelegramsROS2(const std::shared_ptr msg) { messageCbResultPortTelegrams(*msg); } protected: @@ -185,7 +187,7 @@ namespace sick_lidar_localization virtual void runErrorSimulationThreadCb(void); /*! - * Waits for a given time in seconds, as long as ros::ok() and m_error_simulation_thread_running == true. + * Waits for a given time in seconds, as long as ROS::ok() and m_error_simulation_thread_running == true. * @param[in] seconds delay in seconds */ void errorSimulationWait(double seconds); @@ -215,7 +217,7 @@ namespace sick_lidar_localization std::list m_tcp_worker_threads; ///< list of tcp worker thread (one thread for each tcp client, generating telegrams) boost::mutex m_tcp_worker_threads_mutex; ///< mutex to protect m_tcp_worker_threads bool m_worker_thread_running; ///< true: worker threads started, otherwise false - ros::Publisher m_result_testcases_publisher; ///< ros publisher for testcases with result port telegrams (type SickLocResultPortTestcaseMsg) + sick_lidar_localization::SickLocResultPortTestcaseMsgPublisher m_result_testcases_publisher; ///< ros publisher for testcases with result port telegrams (type SickLocResultPortTestcaseMsg) std::string m_result_testcases_frame_id; ///< ros frame id of testcase messages (type SickLocResultPortTestcaseMsg), default: "result_testcases" bool m_demo_move_in_circles; ///< true: simulate a sensor moving in circles, false (default): create random based result port telegrams diff --git a/test/include/sick_lidar_localization/verifier_thread.h b/test/include/sick_lidar_localization/verifier_thread.h index 3ebd5f1..5a78b5e 100644 --- a/test/include/sick_lidar_localization/verifier_thread.h +++ b/test/include/sick_lidar_localization/verifier_thread.h @@ -86,8 +86,9 @@ namespace sick_lidar_localization /*! * Constructor + * @param[in] nh ros node handle */ - VerifierThread(); + VerifierThread(ROS::NodePtr nh = 0); /*! * Destructor @@ -111,12 +112,16 @@ namespace sick_lidar_localization * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) */ virtual void messageCbResultPortTelegrams(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg); + /*! ROS2 version of function messageCbResultPortTelegrams */ + virtual void messageCbResultPortTelegramsROS2(const std::shared_ptr msg) { messageCbResultPortTelegrams(*msg); } /*! * Callback for testcase messages (SickLocResultPortTestcaseMsg) from sim_loc_test_server. * @param[in] msg testcase message (SickLocResultPortTestcaseMsg) */ virtual void messageCbResultPortTestcases(const sick_lidar_localization::SickLocResultPortTestcaseMsg & msg); + /*! ROS2 version of function messageCbResultPortTelegrams */ + virtual void messageCbResultPortTestcasesROS2(const std::shared_ptr msg) { messageCbResultPortTestcases(*msg); } protected: diff --git a/test/python/Odometry_LLS.py b/test/python/Odometry_LLS.py new file mode 100755 index 0000000..f8d8d20 --- /dev/null +++ b/test/python/Odometry_LLS.py @@ -0,0 +1,145 @@ +import socket +import math + +import time +starttime=time.time() + +# Network Socket +global sock + +# IP and Port for UDP Telegrams (IP = IP of SIM1000FX/SIM1012) +UDP_IP = "192.168.0.1" +UDP_PORT = 3000 + +# Telegram counter +global telegramCount +telegramCount = 0 + +def to_hex(data): # replaces data.hex() in python2 + return ''.join(hex(ord(c))[2:] for c in data) # return data.hex( + +# Function to ensure that number "decINT" is represented by "digits" bytes +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 encodeOdometryTelegram(sourceid, telegramcounter, vx, vy, omega, ts): + + # ------------------------------------------------------------------------------------------------------------------------------------------------------ + #| 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 = to_hex(magicWordInByte) # 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(telegramcounter, 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) + + return message + +def sendOdometryTelegram(vx, vy, omega, ts): + + # Import socket + global sock + #Telegram counter + global telegramCount + + message = encodeOdometryTelegram(100, telegramCount, vx, vy, omega, ts) + + print('########### DECIMAL ###################') + print("TelegramCount :",telegramCount) + print("Timestamp :", ts) + print("Vx :", vx) + print("Vy :", vy) + print("Vomega :", omega) + + print('########### HEX ###################') + print(toHexPad(telegramCount, 8)) + print(toHexPad(ts, 8)) + print(toHexPad(vx, 4)) + print(toHexPad(vy, 4)) + print(toHexPad(omega, 8)) + + # Convert to bytes object + MESSAGEHEX = bytes.fromhex(message) + print ("message (str):", MESSAGEHEX) + print ("message (hex):", MESSAGEHEX.hex()) + + # Send datagram + sock.sendto(MESSAGEHEX, (UDP_IP, UDP_PORT)) + + # Increment telegram counter + telegramCount = telegramCount + 1 + + +def startSendingTelegrams(): + + # Timestamp of current telegram and previous telegram + ts = 0 + prevts = 0 + # Linear and angular velocity from odometry sensor (all integer numbers !!!) + vel_x = 200 # [mm/s] + vel_y = 0 # [mm/s] + vel_omega = 10000 # [mdeg/s] + # Odometry measurements frequency + freq = 0.025 # send every 25ms + while True: + ts = int(time.time()*1000) # [ms] + sendOdometryTelegram(vel_x,vel_y,vel_omega, ts) + # Delta between telegram timestamps + #print("Delta ts: ", ts - prevts, " ms") + # Update previous telegram timestamps + #prevts = ts + # Sleep in order to reach odom telegram cycle time specified by "freq" + time.sleep(freq - (time.time() % freq)) + + + +def initilize(): + global sock + + # Target IP and Port for Odometry UDP telegrams + print ("UDP target IP:", UDP_IP) + print ("UDP target port:", UDP_PORT) + + # UDP socket + 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/python/Odometry_LLS.pyc b/test/python/Odometry_LLS.pyc new file mode 100755 index 0000000..003909c Binary files /dev/null and b/test/python/Odometry_LLS.pyc differ diff --git a/test/python/odom_msg_sender.py b/test/python/odom_msg_sender.py new file mode 100755 index 0000000..3c23192 --- /dev/null +++ b/test/python/odom_msg_sender.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python + +# odometry message sender simulating a vehicle moving in circles. +# Source: https://gist.github.com/atotto/f2754f75bedb6ea56e3e0264ec405dcf +# ROS1 usage: +# source /opt/ros/melodic/setup.bash +# source ./devel/setup.bash +# roslaunch sick_lidar_localization odom_msg_sender.launch +# ROS2 usage: +# source /opt/ros/eloquent/setup.bash +# pip3 install transformations +# python3 odom_msg_sender.py +# ros2 run rviz2 rviz2 -d ../config/rviz2_odom_msg_sender.rviz + +import math +from math import sin, cos, pi +import os +import socket +import select +import sys +import nav_msgs +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Point, Pose, Quaternion, TransformStamped, Twist, Vector3 +from Odometry_LLS import encodeOdometryTelegram + +# Determine ROS1 or ROS2. Unfortunately their APIs are different. +if "ROS_VERSION" in os.environ: + ROS_VERSION = int(os.environ["ROS_VERSION"]) +else: + ROS_VERSION = 2 + print("odom_msg_sender: environment ROS_VERSION not set, assuming ROS_VERSION=2") +print("odom_msg_sender: ROS_VERSION={}".format(ROS_VERSION)) + +if ROS_VERSION > 1: + import rclpy as rospy + import tf2_ros as tf2 + import transformations +else: + import rospy + import tf as tf2 + +def ros_ok(): + if ROS_VERSION > 1: + return rospy.ok() + else: + return not rospy.is_shutdown() + +def spin_once(node): + if ROS_VERSION > 1: + rospy.spin_once(node) + else: + pass + +def now(node): + if ROS_VERSION > 1: + return node.get_clock().now() + else: + return rospy.Time.now() + +def to_sec(dt): + if ROS_VERSION > 1: + return 1.0e-9 * dt.nanoseconds + else: + return dt.to_sec() + +def to_millisec(timestamp): + if ROS_VERSION > 1: + return int(current_time.nanoseconds / 1000000) + else: + return int(1000 * current_time.secs + current_time.nsecs / 1000000) + +def timestamp(current_time): + if ROS_VERSION > 1: + return current_time.to_msg() + else: + return current_time + +def to_hex(data): + if ROS_VERSION > 1: + return data.hex() + else: + return ''.join(hex(ord(c))[2:].zfill(2) for c in data) + +def quaternion_from_euler(roll,pitch,yaw): + if ROS_VERSION > 1: + # ROS2 does not include package tf2.transformations, therefore transformations from https://pypi.org/project/transformations/ are used. + # Install with "pip3 install transformations". Note from https://pypi.org/project/transformations/: + # Transformations.py is no longer actively developed and has a few known issues and numerical instabilities. + # Quaternions w+ix+jy+kz are represented as [w, x, y, z] + [w, x, y, z] = transformations.quaternion_from_euler(roll,pitch,yaw, "sxyz") + return [x, y, z, w] + else: + return tf2.transformations.quaternion_from_euler(roll,pitch,yaw) + +def sendTransform(odom_broadcaster, x, y, z, odom_quat, current_time, base_id, child_frame_id): + if ROS_VERSION > 1: + t = TransformStamped() + t.header.stamp = timestamp(current_time) + t.header.frame_id = base_id + t.child_frame_id = child_frame_id + t.transform.translation.x = x + t.transform.translation.y = y + t.transform.translation.z = 0.0 + t.transform.rotation.x = odom_quat[0] + t.transform.rotation.y = odom_quat[1] + t.transform.rotation.z = odom_quat[2] + t.transform.rotation.w = odom_quat[3] + odom_broadcaster.sendTransform(t) + else: + odom_broadcaster.sendTransform((x, y, 0.), odom_quat, current_time, base_id, child_frame_id) + + +# main entry point of this script, runs odom_msg_sender +if __name__ == "__main__": + + if ROS_VERSION > 1: + rospy.init(args=sys.argv) + node = rospy.create_node('odom_msg_sender') + odom_pub = node.create_publisher(Odometry, 'odom', 10) + odom_broadcaster = tf2.TransformBroadcaster(node) + per_millisecond_rate = node.create_rate(1000) + else: + rospy.init_node('odom_msg_sender') + node = None + odom_pub = rospy.Publisher("odom", Odometry, queue_size=50) + odom_broadcaster = tf2.TransformBroadcaster() + per_millisecond_rate = rospy.Rate(1000.0) + + # upd receiver to verify udp telegrams from sim_loc_driver + udp_receive_socket = socket.socket(family=socket.AF_INET, type=socket.SOCK_DGRAM) + udp_receive_socket.setblocking(False) + udp_receive_socket.settimeout(1.0) + udp_receive_socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + udp_receive_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + udp_receive_socket.bind(("127.0.0.1", 3000)) + # udp_receive_socket.bind(("0.0.0.0", 3000)) + + # position and velocity of odom messages + x = 0 + y = 0.0 + th = 0.0 + vx = 0.1 + vy = -0.1 + vth = 0.1 + + # configuration + send_rate = 20.0 # send at least 20 odometry messages per second + exit_after = 500 # exit after 500 odometry messages + + telegramcounter = 0 + udp_verified_counter = 0 + current_time = now(node) + last_time = now(node) + + while ros_ok() and telegramcounter < exit_after: + current_time = now(node) + + # compute odometry in a typical way given the velocities of the robot + dt = to_sec(current_time - last_time) + delta_x = (vx * cos(th) - vy * sin(th)) * dt + delta_y = (vx * sin(th) + vy * cos(th)) * dt + delta_th = vth * dt + + x += delta_x + y += delta_y + th += delta_th + + # since all odometry is 6DOF we'll need a quaternion created from yaw + odom_quat = quaternion_from_euler(0.0, 0.0, th) + + # first, we'll publish the transform over tf + sendTransform(odom_broadcaster, x, y, 0.0, odom_quat, current_time, "base_link", "odom") + + # next, we'll publish the odometry message over ROS + odom = Odometry() + odom.header.stamp = timestamp(current_time) + odom.header.frame_id = "odom" + + # set the position and orientation + # odom.pose.pose = Pose((Point(x, y, 0.), Quaternion(*odom_quat)) + odom.pose.pose.position.x = x + odom.pose.pose.position.y = y + odom.pose.pose.position.z = 0.0 + odom.pose.pose.orientation.x = odom_quat[0] + odom.pose.pose.orientation.y = odom_quat[1] + odom.pose.pose.orientation.z = odom_quat[2] + odom.pose.pose.orientation.w = odom_quat[3] + + # set the velocity + odom.child_frame_id = "base_link" + # odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth)) + odom.twist.twist.linear.x = vx + odom.twist.twist.linear.y = vy + odom.twist.twist.linear.z = 0.0 + odom.twist.twist.angular.x = 0.0 + odom.twist.twist.angular.y = 0.0 + odom.twist.twist.angular.z = vth + + # publish the message + udp_telegram_received = False + odom_pub.publish(odom) + + last_time = current_time + print("odom_msg_sender: x={}, y={}, th={}, vx={}, vy={}, vth={}".format(x, y, th, vx, vy, vth)) + spin_once(node) + + # receive upd telegram from sim_loc_driver and check against odom message sent + # (verify with encodeOdometryTelegram by Odometry_LLS.py) + while(to_sec(now(node) - last_time) < 1.0/send_rate): + spin_once(node) + # receive udp telegram from sim_loc_driver with timeout + if not udp_telegram_received and select.select([udp_receive_socket], [], [], 0.5/send_rate)[0]: + udp_received = udp_receive_socket.recvfrom(1024)[0] + udp_hex = to_hex(udp_received) + udp_telegram_received = True + # verify udp telegram + telegramcounter = telegramcounter + 1 + ts = to_millisec(current_time) + udp_expected = encodeOdometryTelegram(1, telegramcounter, int(round(vx * 1000)), int(round(vy * 1000)), int(round(vth * 1000 * 180 / pi)), ts) + print("odom_msg_sender: received udp telegram {}".format(udp_hex)) + print("odom_msg_sender: expected udp telegram {}".format(udp_expected)) + if udp_hex == udp_expected: + udp_verified_counter = udp_verified_counter + 1 + print("odom_msg_sender: ok, received and expected udp telegram are identical.") + else: + print("## ERROR odom_msg_sender: received and expected udp telegram NOT identical.") + per_millisecond_rate.sleep() + if not udp_telegram_received: + print("## ERROR odom_msg_sender: udp telegram NOT received.") + if telegramcounter == udp_verified_counter: + print("odom_msg_sender: {} odom messages sent, {} udp telegrams received and verified, {} errors".format(telegramcounter, udp_verified_counter, telegramcounter - udp_verified_counter)) + else: + print("## ERROR odom_msg_sender: {} odom messages sent, {} udp telegrams received and verified, {} errors".format(telegramcounter, udp_verified_counter, telegramcounter - udp_verified_counter)) + diff --git a/test/python/odom_udp_receiver.cmd b/test/python/odom_udp_receiver.cmd new file mode 100755 index 0000000..78c47d5 --- /dev/null +++ b/test/python/odom_udp_receiver.cmd @@ -0,0 +1,6 @@ +@if exist %ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64 ( + set PYTHON_DIR=%ProgramFiles(x86)%\Microsoft Visual Studio\Shared\Python37_64 + set PATH=%PYTHON_DIR%;%PYTHON_DIR%\Scripts;%PATH% +) + +python odom_udp_receiver.py diff --git a/test/python/odom_udp_receiver.py b/test/python/odom_udp_receiver.py new file mode 100755 index 0000000..9d67136 --- /dev/null +++ b/test/python/odom_udp_receiver.py @@ -0,0 +1,28 @@ +# udp receiver for odometry telegrams from sim_loc_driver +# python3 odom_udp_receiver.py + +import socket + +# configuration +exit_after = 100 # exit after 100 odometry messages + +# upd receiver to verify udp telegrams from sim_loc_driver +udp_receive_socket = socket.socket(family=socket.AF_INET, type=socket.SOCK_DGRAM) +udp_receive_socket.setblocking(True) +# udp_receive_socket.settimeout(1.0) +udp_receive_socket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) +udp_receive_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) +udp_receive_socket.bind(("0.0.0.0", 3000)) +# udp_receive_socket.bind(("127.0.0.1", 3000)) + +telegramcounter = 0 + +while telegramcounter < exit_after: + + # receive udp telegram from sim_loc_driver with timeout + #if select.select([udp_receive_socket], [], [], 0.1)[0]: + udp_received = udp_receive_socket.recvfrom(1024)[0] + udp_hex = udp_received.hex() + telegramcounter = telegramcounter + 1 + print("odom_msg_sender: received {}. udp telegram {}".format(telegramcounter, udp_hex)) + diff --git a/test/python/srv_helper.py b/test/python/srv_helper.py new file mode 100755 index 0000000..9ef7026 --- /dev/null +++ b/test/python/srv_helper.py @@ -0,0 +1,246 @@ +# utility functions to support the new lidar_loc services +# Usage: python3 srv_helper.py + +import os + +def createDefaultSrvFile(service_name): + print("createDefaultSrvFile({})".format(service_name)) + filename = "srv/Sick{}Srv.srv".format(service_name) + file = open(filename , "w") + file.write("# Definition of ROS service Sick{} for sick localization\n".format(service_name)) + file.write("# Example call (ROS1):\n") + file.write("# rosservice call Sick{} \"{{}}\"\n".format(service_name)) + file.write("# Example call (ROS2):\n") + file.write("# ros2 service call Sick{} sick_lidar_localization/srv/Sick{}Srv \"{{}}\"\n".format(service_name, service_name)) + file.write("# \n") + file.write("\n") + file.write("# \n") + file.write("# Request (input)\n") + file.write("# \n") + file.write("\n") + file.write("---\n") + file.write("\n") + file.write("# \n") + file.write("# Response (output)\n") + file.write("# \n") + file.write("\n") + file.write("bool success # true: success response received from localization controller, false: service failed (timeout or error status from controller)\n") + file.close() + +def createServerVariableDefinition(file,service_name): + file.write(" sick_lidar_localization::Sick{}SrvServer m_Sick{}SrvServer; ///< service \"{}\", callback &sick_lidar_localization::ColaServices::serviceCb{}\n".format(service_name,service_name,service_name,service_name)) + +def createServerVariableDeclarationROS1(file,service_name): + file.write(" m_Sick{}SrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::Sick{}Srv, \"Sick{}\", &sick_lidar_localization::ColaServices::serviceCb{}, this);\n".format(service_name,service_name,service_name,service_name)) + +def createServerVariableDeclarationROS2(file,service_name): + file.write(" m_Sick{}SrvServer = ROS_CREATE_SRV_SERVER(nh, sick_lidar_localization::Sick{}Srv, \"Sick{}\", &sick_lidar_localization::ColaServices::serviceCb{}ROS2, this);\n".format(service_name,service_name,service_name,service_name)) + +def createSrvCallbackDeclaration(file,service_name): + file.write(" /*!\n") + file.write(" * Callback for service \"Sick{}Srv\"\n".format(service_name)) + file.write(" * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response\n") + file.write(" * Uses ros service \"SickLocColaTelegramSrv\"\n") + file.write(" * @param[in] service_request ros service request to localization controller\n") + file.write(" * @param[out] service_response service response from localization controller\n") + file.write(" * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error).\n") + file.write(" */\n") + file.write(" virtual bool serviceCb{}(sick_lidar_localization::Sick{}Srv::Request& service_request, sick_lidar_localization::Sick{}Srv::Response& service_response);\n".format(service_name,service_name,service_name)) + file.write(" /*! ROS2 version of function serviceCb{} */\n".format(service_name)) + file.write(" virtual bool serviceCb{}ROS2(std::shared_ptr service_request, std::shared_ptr service_response)\n".format(service_name,service_name,service_name)) + file.write(" {\n") + file.write(" return serviceCb{}(*service_request, *service_response);\n".format(service_name)) + file.write(" }\n") + file.write("\n") + +def createSrvCallbackImplementation(file,service_name): + file.write("/*!\n") + file.write(" * Callback for service \"Sick{}Srv\"\n".format(service_name)) + file.write(" * Converts the service request to cola telegram, sends the telegram to the localization controller and receives the response\n") + file.write(" * Uses ros service \"SickLocColaTelegramSrv\"\n") + file.write(" * @param[in] service_request ros service request to localization controller\n") + file.write(" * @param[out] service_response service response from localization controller\n") + file.write(" * @return true on success, false in case of errors (negative response from localization controller, timeout, service or communication error).\n") + file.write(" */\n") + file.write("bool sick_lidar_localization::ColaServices::serviceCb{}(sick_lidar_localization::Sick{}Srv::Request& service_request, sick_lidar_localization::Sick{}Srv::Response& service_response)\n".format(service_name,service_name,service_name)) + file.write("{\n") + file.write(" service_response.success = false;\n") + file.write(" std::string cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request);\n") + file.write(" sick_lidar_localization::SickLocColaTelegramMsg cola_response = sendColaTelegram(cola_ascii);\n") + file.write(" if (cola_response.command_name == \"{}\" && cola_response.parameter.size() > 0)\n".format(service_name)) + file.write(" {\n") + file.write(" if (!sick_lidar_localization::ColaEncoder::parseServiceResponse(cola_response, service_response) || !service_response.success)\n") + file.write(" {\n") + file.write(" ROS_WARN_STREAM(\"## ERROR ColaServices::sendColaTelegram(\" << cola_ascii << \") failed, invalid response: \" << sick_lidar_localization::Utils::flattenToString(cola_response) << \", ColaConverter::parseServiceResponse() failed.\");\n") + file.write(" return false;\n") + file.write(" }\n") + file.write(" return true;\n") + file.write(" }\n") + file.write(" ROS_WARN_STREAM(\"## ERROR ColaServices::sendColaTelegram(\" << cola_ascii << \") failed, invalid response: \" << sick_lidar_localization::Utils::flattenToString(cola_response));\n") + file.write(" return false;\n") + file.write("}\n") + file.write("\n") + +def createColaEncoderDeclaration(file,service_name): + file.write(" /*!\n") + file.write(" * Converts the service request for service Sick{}Srv into a cola ascii telegram\n".format(service_name)) + file.write(" * @param[in] service_request ros request for service Sick{}Srv\n".format(service_name)) + file.write(" * @return cola ascii telegram\n") + file.write(" */\n") + file.write(" static std::string encodeServiceRequest(const sick_lidar_localization::Sick{}Srv::Request& service_request);\n".format(service_name)) + file.write(" \n") + file.write(" /*!\n") + file.write(" * Parses a cola response and converts the arguments to a service response for service Sick{}Srv\n".format(service_name)) + file.write(" * @param[in] cola_response cola ascii telegram (response from localization server)\n") + file.write(" * @param[out] service_response converted response for service Sick{}Srv\n".format(service_name)) + file.write(" * @return true on succes or false in case of parse errors\n") + file.write(" */\n") + file.write(" static bool parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::Sick{}Srv::Response& service_response);\n".format(service_name)) + file.write(" \n") + +def createColaEncoderImplementation(file,service_name): + file.write("/*!\n") + file.write(" * Converts the service request for service Sick{}Srv into a cola ascii telegram\n".format(service_name)) + file.write(" * @param[in] service_request ros request for service Sick{}Srv\n".format(service_name)) + file.write(" * @return cola ascii telegram\n") + file.write(" */\n") + file.write("std::string sick_lidar_localization::ColaEncoder::encodeServiceRequest(const sick_lidar_localization::Sick{}Srv::Request& service_request)\n".format(service_name)) + file.write("{\n") + file.write(" std::stringstream cola_ascii;\n") + file.write(" cola_ascii << \"sMN {}\"; // todo: convert the input parameter of service request Sick{}Srv to cola string\n".format(service_name, service_name)) + file.write(" return cola_ascii.str();\n") + file.write("}\n") + file.write("\n") + file.write("/*!\n") + file.write(" * Parses a cola response and converts the arguments to a service response for service Sick{}Srv\n".format(service_name)) + file.write(" * @param[in] cola_response cola ascii telegram (response from localization server)\n") + file.write(" * @param[out] service_response converted response for service Sick{}Srv\n".format(service_name)) + file.write(" * @return true on succes or false in case of parse errors\n") + file.write(" */\n") + file.write("bool sick_lidar_localization::ColaEncoder::parseServiceResponse(const sick_lidar_localization::SickLocColaTelegramMsg& cola_response, sick_lidar_localization::Sick{}Srv::Response& service_response)\n".format(service_name)) + file.write("{\n") + file.write(" service_response.success = false;\n") + file.write(" if(!cola_response.parameter.empty())\n") + file.write(" {\n") + file.write(" // todo: convert cola_response to output parameter of service Sick{}Srv\n".format(service_name)) + file.write(" service_response.success = sick_lidar_localization::ColaParser::convertColaResponseBool(cola_response.parameter[0], false);\n") + file.write(" }\n") + file.write(" return service_response.success;\n") + file.write("}\n") + file.write("\n") + +def createColaTestcaseROS1(file,service_name): + file.write(" call_service Sick{} \"{{}}\" \"success: True\"\n".format(service_name, service_name)) + +def createColaTestcaseROS2(file,service_name): + file.write(" call_service Sick{} sick_lidar_localization/srv/Sick{}Srv \"{{}}\" \"success=True\"\n".format(service_name, service_name)) + +def createTestServerResponse(file,service_name): + file.write(" if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == \"{}\" && cola_request.parameter.size() == 0)\n".format(service_name)) + file.write(" {\n") + file.write(" // todo: parse cola request for service {}\n".format(service_name)) + file.write(" return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {decstr(1)});\n") + file.write(" }\n") + file.write(" \n") + +if not os.path.exists("srv"): + os.makedirs("srv") + +services = [ + "DevSetLidarConfig", + "DevGetLidarConfig", + "LocSetMap", + "LocMap", + "LocMapState", + "LocInitializePose", + "LocInitialPose", + "LocSetPoseQualityCovWeight", + "LocPoseQualityCovWeight", + "LocSetPoseQualityMeanDistWeight", + "LocPoseQualityMeanDistWeight", + "LocSetReflectorsForSupportActive", + "LocReflectorsForSupportActive", + "LocSetOdometryActive", + "LocOdometryActive", + "LocSetOdometryPort", + "LocOdometryPort", + "LocSetOdometryRestrictYMotion", + "LocOdometryRestrictYMotion", + "LocSetAutoStartActive", + "LocAutoStartActive", + "LocSetAutoStartSavePoseInterval", + "LocAutoStartSavePoseInterval", + "LocSetRingBufferRecordingActive", + "LocRingBufferRecordingActive", + "DevGetLidarIdent", + "DevGetLidarState", + "GetSoftwareVersion", + "LocAutoStartSavePose", + "LocForceUpdate", + "LocSaveRingBufferRecording", + "LocStartDemoMapping", + "ReportUserMessage", + "SavePermanent", + "LocResultPort", + "LocResultMode", + "LocResultEndianness", + "LocResultState", + "LocResultPoseInterval" + ] + +for service in services: + createDefaultSrvFile(service) + +file = open("srv/cola_services.h" , "w") +for service in services: + createServerVariableDefinition(file, service) +file.close() + +file = open("srv/cola_services.cpp" , "w") +file.write("#if defined __ROS_VERSION && __ROS_VERSION == 1\n") +for service in services: + createServerVariableDeclarationROS1(file, service) +file.write("#elif defined __ROS_VERSION && __ROS_VERSION == 2\n") +for service in services: + createServerVariableDeclarationROS2(file, service) +file.write("#endif // __ROS_VERSION\n") +file.close() + +file = open("srv/cola_services.h" , "w") +for service in services: + createSrvCallbackDeclaration(file, service) +file.close() + +file = open("srv/cola_services.cpp" , "w") +for service in services: + createSrvCallbackImplementation(file, service) +file.close() + +file = open("srv/cola_encoder.h" , "w") +for service in services: + createColaEncoderDeclaration(file, service) +file.close() + +file = open("srv/cola_encoder.cpp" , "w") +for service in services: + createColaEncoderImplementation(file, service) +file.close() + +file = open("srv/send_cola_advanced_ros1.bash" , "w") +createColaTestcaseROS1(file, "LocIsSystemReady") +createColaTestcaseROS1(file, "LocStartLocalizing") +for service in services: + createColaTestcaseROS1(file, service) +file.close() + +file = open("srv/send_cola_advanced_ros2.bash" , "w") +createColaTestcaseROS2(file, "LocIsSystemReady") +createColaTestcaseROS2(file, "LocStartLocalizing") +for service in services: + createColaTestcaseROS2(file, service) +file.close() + +file = open("srv/testcase_generator.cpp" , "w") +for service in services: + createTestServerResponse(file, service) +file.close() diff --git a/test/scripts/cleanup.bash b/test/ros1_scripts/cleanup.bash similarity index 66% rename from test/scripts/cleanup.bash rename to test/ros1_scripts/cleanup.bash index 61502f6..0588837 100755 --- a/test/scripts/cleanup.bash +++ b/test/ros1_scripts/cleanup.bash @@ -2,8 +2,8 @@ pushd ../../../../ source /opt/ros/melodic/setup.bash -if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi -if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi killall rosmaster ; sleep 1 echo -e "\n# cleanup.bash: Deleting ros cache and logfiles and catkin folders ./build ./devel ./install" @@ -11,6 +11,8 @@ rosclean purge -y rm -rf ./build ./devel ./install rm -rf ~/.ros/* catkin clean --yes --all-profiles --verbose -catkin_make clean +catkin_make clean --cmake-args -DROS_VERSION=1 popd +rm -rf ./log/* +rm -rf ../ros2_scripts/log/* diff --git a/test/scripts/clion.bash b/test/ros1_scripts/clion.bash similarity index 100% rename from test/scripts/clion.bash rename to test/ros1_scripts/clion.bash diff --git a/test/scripts/killall.bash b/test/ros1_scripts/killall.bash similarity index 100% rename from test/scripts/killall.bash rename to test/ros1_scripts/killall.bash diff --git a/test/ros1_scripts/make.bash b/test/ros1_scripts/make.bash new file mode 100755 index 0000000..7fbd1fc --- /dev/null +++ b/test/ros1_scripts/make.bash @@ -0,0 +1,44 @@ +#!/bin/bash + +# +# Check/set/cleanup environment +# + +pushd ../../../.. +rm -f build/catkin_make_install.log +source /opt/ros/melodic/setup.bash +rm -f ./devel/lib/*sick* 2> /dev/null +rm -f ./devel/lib/sick_lidar_localization/*sick* 2> /dev/null +rm -f ./install/lib/*sick* 2> /dev/null +rm -f ./install/lib/sick_lidar_localization/*sick* 2> /dev/null + +# +# Build and install sick_lidar_localization binaries. +# + +if [ -d ./src/sick_lidar_localization ] && [ ! -f ./src/sick_lidar_localization/package.xml ] ; then pushd ./src/sick_lidar_localization ; ln -s ./package_ros1.xml ./package.xml ; popd ; fi +if [ -d ./src/sick_lidar_localization_pretest ] && [ ! -f ./src/sick_lidar_localization_pretest/package.xml ] ; then pushd ./src/sick_lidar_localization_pretest ; ln -s ./package_ros1.xml ./package.xml ; popd ; fi +catkin_make --cmake-args -DROS_VERSION=1 2>&1 | tee -a build/catkin_make_install.log +catkin_make install --cmake-args -DROS_VERSION=1 2>&1 | tee -a build/catkin_make_install.log +catkin_make RunDoxygen --cmake-args -DROS_VERSION=1 2>&1 | tee -a build/catkin_make_install.log +source ./install/setup.bash + +# lint +# catkin_lint -W1 ./src/sick_lidar_localization +# if [ -d ./src/sick_lidar_localization ] ; then catkin_lint -W1 ./src/sick_lidar_localization ; fi +# if [ -d ./src/sick_lidar_localization_pretest ] ; then catkin_lint -W1 ./src/sick_lidar_localization_pretest ; fi + +# print warnings and errors +echo -e "\nmake.bash finished.\n" +echo -e "catkin_make warnings:" +cat build/catkin_make_install.log | grep -i "warning:" +echo -e "\ncatkin_make errors:" +cat build/catkin_make_install.log | grep -i "error:" + +# print sick_lidar_localization install files, libraries, executables +echo -e "\ninstall/lib/sick_lidar_localization:" +ls -al install/lib/sick_lidar_localization/* +echo -e "\ninstall/share/sick_lidar_localization:" +ls install/share/sick_lidar_localization/*.* +popd + diff --git a/test/scripts/make_crc.bash b/test/ros1_scripts/make_crc.bash similarity index 100% rename from test/scripts/make_crc.bash rename to test/ros1_scripts/make_crc.bash diff --git a/test/scripts/makeall.bash b/test/ros1_scripts/makeall.bash similarity index 82% rename from test/scripts/makeall.bash rename to test/ros1_scripts/makeall.bash index 7edbc24..8a13717 100755 --- a/test/scripts/makeall.bash +++ b/test/ros1_scripts/makeall.bash @@ -1,7 +1,9 @@ #!/bin/bash +printf "\033c" echo -e "makeall.bash: build and install sick_lidar_localization binaries" source /opt/ros/melodic/setup.bash ./cleanup.bash +rm -f ../../package.xml ./make.bash echo -e "makeall.bash finished." diff --git a/test/scripts/run.bash b/test/ros1_scripts/run.bash similarity index 78% rename from test/scripts/run.bash rename to test/ros1_scripts/run.bash index e237744..1e31dc9 100755 --- a/test/scripts/run.bash +++ b/test/ros1_scripts/run.bash @@ -19,8 +19,8 @@ source ./devel/setup.bash # Cleanup # -if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi -if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi rm -rf ~/.ros/* rosclean purge -y if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi @@ -32,8 +32,8 @@ if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi # Run sim_loc_driver_check for a plausibility check of messages and telegrams published by the sick_lidar_localization ros driver # -roslaunch sick_lidar_localization sim_loc_driver_check.launch sim_loc_driver_check_cfg:=message_check_demo.yaml 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_driver_check_demo.log & -roslaunch sick_lidar_localization sim_loc_driver_check.launch sim_loc_driver_check_cfg:=message_check_default.yaml 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_driver_check_default.log & +roslaunch sick_lidar_localization sim_loc_driver_check.launch sim_loc_driver_check_cfg:=message_check_demo.yaml 2>&1 | tee -a ~/.ros/log/sim_loc_driver_check_demo.log & +roslaunch sick_lidar_localization sim_loc_driver_check.launch sim_loc_driver_check_cfg:=message_check_default.yaml 2>&1 | tee -a ~/.ros/log/sim_loc_driver_check_default.log & # # Log diagnostic messages @@ -45,7 +45,7 @@ rostopic echo -p "/sick_lidar_localization/driver/diagnostic" | tee -a ~/.ros/lo # Run ros driver, connect to localization controller and receive, convert and publish telegrams # -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=192.168.0.1 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_driver.log & +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=192.168.0.1 2>&1 | tee -a ~/.ros/log/sim_loc_driver.log & # # Run online test with localization controller and ros driver for some time, press 'q' to quit test diff --git a/test/scripts/run_cola_examples.bash b/test/ros1_scripts/run_cola_examples.bash similarity index 71% rename from test/scripts/run_cola_examples.bash rename to test/ros1_scripts/run_cola_examples.bash index cd1d121..6bf4344 100755 --- a/test/scripts/run_cola_examples.bash +++ b/test/ros1_scripts/run_cola_examples.bash @@ -19,12 +19,22 @@ source ./devel/setup.bash # Cleanup # -if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi -if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi rm -rf ~/.ros/* rosclean purge -y if [ ! -d ~/.ros/log/cola_examples ] ; then mkdir -p ~/.ros/log/cola_examples ; fi +# +# Run standalone unittests for sim_loc_parser +# + +roslaunch sick_lidar_localization unittest_sim_loc_parser.launch & +sleep 10 +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi +sleep 1 + # # Run test server, simulate localization controller for offline tests # @@ -36,7 +46,6 @@ sleep 3 # make sure ros core and sim_loc_test_server are up and running # Run ros driver, connect to localization controller and receive, convert and publish telegrams # -# roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 2>&1 | unbuffer -p tee -a ~/.ros/log/cola_examples/sim_loc_driver.log & roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 2>&1 >> ~/.ros/log/cola_examples/sim_loc_driver.log & sleep 1 # make sure sim_loc_test_server and sim_loc_driver are up and running @@ -45,7 +54,8 @@ sleep 1 # make sure sim_loc_test_server and sim_loc_driver are up and running # popd -./send_cola_examples.bash 2>&1 | unbuffer -p tee -a ~/.ros/log/cola_examples/send_cola_examples.log +./send_cola_advanced.bash 2>&1 | tee -a ~/.ros/log/cola_examples/send_cola_advanced.log +./send_cola_examples.bash 2>&1 | tee -a ~/.ros/log/cola_examples/send_cola_examples.log # # Cleanup and exit @@ -54,6 +64,7 @@ popd ./killall.bash sleep 1 ; killall roslaunch ; sleep 1 cat ~/.ros/log/cola_examples/send_cola_examples.log +cat ~/.ros/log/cola_examples/send_cola_advanced.log # sleep 20 mkdir -p ./log/cola_examples cp -rf ~/.ros/log/cola_examples/*.log ./log/cola_examples @@ -63,6 +74,4 @@ grep "ERR" ./log/cola_examples/*.log echo -e "\nsim_loc_driver check summary:" grep -i "check messages thread summary" ./log/cola_examples/*.log grep -i "verification summary" ./log/cola_examples/*.log -echo -e "\nsick_lidar_localization summary:" -grep -R "summary" ./log/* diff --git a/test/scripts/run_demo_simu.bash b/test/ros1_scripts/run_demo_simu.bash similarity index 78% rename from test/scripts/run_demo_simu.bash rename to test/ros1_scripts/run_demo_simu.bash index 21bb42d..0142d31 100755 --- a/test/scripts/run_demo_simu.bash +++ b/test/ros1_scripts/run_demo_simu.bash @@ -19,8 +19,8 @@ source ./devel/setup.bash # Cleanup # -if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi -if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi rm -rf ~/.ros/* rosclean purge -y if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi @@ -52,15 +52,16 @@ roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ # rosrun tf static_transform_publisher 0 0 0 0 0 0 map pointcloud_sick_lidar_localization 10 & -rosrun rviz rviz & +rosrun rviz rviz -d ./src/sick_lidar_localization_pretest/test/config/rviz_sick_lidar_localization_demo_pointcloud.rviz & +rosrun rviz rviz -d ./src/sick_lidar_localization_pretest/test/config/rviz_sick_lidar_localization_demo_tf.rviz & # # Run test server and driver for some time and exit # sleep 600 -if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi -if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi killall roslaunch killall rviz killall static_transform_publisher diff --git a/test/scripts/run_error_simu.bash b/test/ros1_scripts/run_error_simu.bash similarity index 93% rename from test/scripts/run_error_simu.bash rename to test/ros1_scripts/run_error_simu.bash index bf3654b..ce1b924 100755 --- a/test/scripts/run_error_simu.bash +++ b/test/ros1_scripts/run_error_simu.bash @@ -32,8 +32,8 @@ source ./devel/setup.bash # Cleanup # -if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi -if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi rm -rf ~/.ros/* rosclean purge -y if [ ! -d ~/.ros/log/error_simu ] ; then mkdir -p ~/.ros/log/error_simu ; fi @@ -43,7 +43,7 @@ if [ ! -d ~/.ros/log/error_simu ] ; then mkdir -p ~/.ros/log/error_simu ; fi # echo -e "sick_lidar_localization error simulation: starting sick_lidar_localization sim_loc_driver.launch ..." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 sim_loc_driver_check_cfg:=message_check_error_simu.yaml 2>&1 | unbuffer -p tee -a ~/.ros/log/error_simu/sim_loc_driver_error_simu.log & +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 sim_loc_driver_check_cfg:=message_check_error_simu.yaml 2>&1 | tee -a ~/.ros/log/error_simu/sim_loc_driver_error_simu.log & echo -e "sick_lidar_localization error simulation: sick_lidar_localization sim_loc_driver.launch started." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log sleep 10 @@ -98,8 +98,8 @@ fi # Cleanup and exit # -if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi -if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi killall roslaunch grep "WARN" ~/.ros/log/error_simu/*.log ; grep "ERR" ~/.ros/log/error_simu/*.log sleep 15 diff --git a/test/ros1_scripts/run_odom_simu.bash b/test/ros1_scripts/run_odom_simu.bash new file mode 100755 index 0000000..54fe59c --- /dev/null +++ b/test/ros1_scripts/run_odom_simu.bash @@ -0,0 +1,53 @@ +#!/bin/bash + +# +# Run odometry simulation (offline-test) of sick_lidar_localization driver +# Odometry simulation runs the sim_loc_driver, a odometry message sender +# (odem_msg_sender.py) and checks the odom udp packages sent by sim_loc_driver. +# + +# +# Environment +# + +pushd ../../../.. +printf "\033c" +source /opt/ros/melodic/setup.bash +source ./devel/setup.bash +export ROS_VERSION=1 + +# +# Cleanup +# + +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi +rm -rf ~/.ros/* +if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi + + +# +# Start test server and sim_loc_driver +# + +roslaunch sick_lidar_localization sim_loc_test_server.launch > ~/.ros/log/sim_loc_test_server.log & +sleep 3 # make sure ros core and sim_loc_test_server are up and running +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 > ~/.ros/log/sim_loc_driver.log & +sleep 3 # make sure sim_loc_driver is up and running before sending and verifying odometry telegrams + +# +# Visualize odom messages by rviz: +# + +rosrun rviz rviz -d ./src/sick_lidar_localization_pretest/test/config/rviz_odom_msg_sender.rviz & + +# +# Start odometry message sender, send 100 odometry messages and verify udp telegrams from sim_loc_driver +# + +roslaunch sick_lidar_localization odom_msg_sender.launch + +popd +./killall.bash +echo -e "\nrun_odom_simu for sick_lidar_localization finished." + diff --git a/test/scripts/run_simu.bash b/test/ros1_scripts/run_simu.bash similarity index 82% rename from test/scripts/run_simu.bash rename to test/ros1_scripts/run_simu.bash index a58651d..48cf23a 100755 --- a/test/scripts/run_simu.bash +++ b/test/ros1_scripts/run_simu.bash @@ -18,8 +18,8 @@ source ./devel/setup.bash # Cleanup # -if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi -if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi rm -rf ~/.ros/* rosclean purge -y if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi @@ -28,7 +28,7 @@ if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi # Run test server, simulate localization controller for offline tests # -roslaunch sick_lidar_localization sim_loc_test_server.launch 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_test_server.log & +roslaunch sick_lidar_localization sim_loc_test_server.launch 2>&1 | tee -a ~/.ros/log/sim_loc_test_server.log & sleep 3 # make sure ros core and sim_loc_test_server are up and running # @@ -41,7 +41,7 @@ rostopic echo -p "/sick_lidar_localization/driver/diagnostic" | tee -a ~/.ros/lo # Run ros driver, connect to localization controller and receive, convert and publish telegrams # -roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_driver.log & +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_address:=127.0.0.1 2>&1 | tee -a ~/.ros/log/sim_loc_driver.log & sleep 1 # make sure sim_loc_test_server and sim_loc_driver are up and running # @@ -50,15 +50,15 @@ sleep 1 # make sure sim_loc_test_server and sim_loc_driver are up and running # rostopic echo "/sick_lidar_localization/test_server/result_testcases" # -roslaunch sick_lidar_localization verify_sim_loc_driver.launch 2>&1 | unbuffer -p tee -a ~/.ros/log/verify_sim_loc_driver.log & +roslaunch sick_lidar_localization verify_sim_loc_driver.launch 2>&1 | tee -a ~/.ros/log/verify_sim_loc_driver.log & # # Run test server and driver for some time and verify results # sleep 180 -if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi -if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi sleep 1 ; killall roslaunch ; sleep 1 # @@ -67,8 +67,8 @@ sleep 1 ; killall roslaunch ; sleep 1 roslaunch sick_lidar_localization unittest_sim_loc_parser.launch 2>&1 | tee -a ~/.ros/log/unittest_sim_loc_parser.log & sleep 10 -if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi -if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi sleep 1 ; killall roslaunch cat ~/.ros/log/sim_loc_driver_diagnostic_messages.log grep "WARN" ~/.ros/log/*.log ; grep "ERR" ~/.ros/log/*.log diff --git a/test/ros1_scripts/send_cola_advanced.bash b/test/ros1_scripts/send_cola_advanced.bash new file mode 100755 index 0000000..2b458dd --- /dev/null +++ b/test/ros1_scripts/send_cola_advanced.bash @@ -0,0 +1,92 @@ +#!/bin/bash + +# +# Run examples for Cola-ASCII telegrams and sick_lidar_localization services provided by sim_loc_driver. +# Testset for the new sick_lidar_localization services implemented september 2020. +# +# Note: ROS sim_loc_driver and test server should be up and running, start by +# roslaunch sick_lidar_localization sim_loc_test_server.launch & +# roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=127.0.0.1 & +# + +# +# Function call_service "" "" "" just calls a ros service +# Example: call_service "SickLocIsSystemReady" "{}" "success: True" # Check if the system is ready +# +function call_service() { + sleep 1 + echo -e "rosservice call $1 \"$2\"" + answer=$(rosservice call $1 "$2") # call rosservice + answer=$(echo $answer|tr -d '\n') # remove line feeds + echo -e "expected answer: \"$3\"" + echo -e "received answer: \"$answer\"" + if [[ "$answer" == *"$3"* ]]; then + echo -e "OK: service responded with expected answer.\n" + else + testcase_error_counter=$((testcase_error_counter+1)) + echo -e "## ERROR: service did NOT respond with expected answer. Press any key to continue..." ; read -n1 -s key + fi + testcase_counter=$((testcase_counter+1)) +} + +# +# Environment +# + +pushd ../../../.. +source ./devel/setup.bash +testcase_counter=0 +testcase_error_counter=0 + +# +# Call services and check answer +# + +for ((n=0;n<1;n++)) ; do + # service servicename request expected response + call_service SickLocIsSystemReady "{}" "success: True" + call_service SickLocStartLocalizing "{}" "success: True" + call_service SickDevSetLidarConfig "{index: 0, minrange: 100, maxrange: 200000, minangle: -15000, maxangle: 15000, x: 1000, y: -1000, yaw: 2000, upsidedown: true, ip: 192.168.1.30, port: 2111, interfacetype: 0, maplayer: 0, active: true}" "set: True executed: True" + call_service SickDevGetLidarConfig "{scannerindex: 0}" "minrange: 100 maxrange: 200000 minangle: -15000 maxangle: 15000 x: 1000 y: -1000 yaw: 2000 upsidedown: True ip: \"192.168.1.30\" port: 2111 interfacetype: 0 maplayer: 0 active: True" + call_service SickLocSetMap "{mapfilename: test.map}" "set: True executed: True" + call_service SickLocMap "{}" "map: \"test.map\" success: True" + call_service SickLocMapState "{}" "mapstate: True success: True" + call_service SickLocInitializePose "{x: 100, y: -100, yaw: 2000, sigmatranslation: 1000}" "success: True" + call_service SickLocInitialPose "{}" "x: 100 y: -100 yaw: 2000 sigmatranslation: 1000 success: True" + call_service SickLocSetReflectorsForSupportActive "{active: 1}" "success: True" + call_service SickLocReflectorsForSupportActive "{}" "active: True success: True" + call_service SickLocSetOdometryActive "{active: 1}" "set: True executed: True" + call_service SickLocOdometryActive "{}" "active: True success: True" + call_service SickLocSetOdometryPort "{port: 3000}" "set: True executed: True" + call_service SickLocOdometryPort "{}" "port: 3000 success: True" + call_service SickLocSetOdometryRestrictYMotion "{active: 1}" "success: True" + call_service SickLocOdometryRestrictYMotion "{}" "active: True success: True" + call_service SickLocSetAutoStartActive "{active: 1}" "success: True" + call_service SickLocAutoStartActive "{}" "active: True success: True" + call_service SickLocSetAutoStartSavePoseInterval "{interval: 5}" "success: True" + call_service SickLocAutoStartSavePoseInterval "{}" "interval: 5 success: True" + call_service SickLocSetRingBufferRecordingActive "{active: 1}" "success: True" + call_service SickLocRingBufferRecordingActive "{}" "active: True success: True" + call_service SickDevGetLidarIdent "{index: 0}" "scannerident: \"TestcaseGenerator0\" success: True" + call_service SickDevGetLidarState "{index: 0}" "devicestatus: 2 deviceconnected: 2 receivingdata: 2 success: True" + call_service SickGetSoftwareVersion "{}" "version: \"1.0\" success: True" + call_service SickLocAutoStartSavePose "{}" "success: True" + call_service SickLocForceUpdate "{}" "success: True" + call_service SickLocSaveRingBufferRecording "{reason: test}" "success: True" + call_service SickLocStop "{}" "success: True" + call_service SickLocStartDemoMapping "{}" "success: True" + call_service SickLocStop "{}" "success: True" + call_service SickLocStartLocalizing "{}" "success: True" + call_service SickReportUserMessage "{usermessage: test_message}" "success: True" + call_service SickSavePermanent "{}" "success: True" + call_service SickLocResultPort "{}" "port: 2201 success: True" + call_service SickLocResultMode "{}" "mode: 0 success: True" + call_service SickLocResultEndianness "{}" "endianness: 0 success: True" + call_service SickLocResultState "{}" "state: 1 success: True" + call_service SickLocResultPoseInterval "{}" "interval: 1 success: True" + call_service SickDevSetIMUActive "{active: 1}" "success: True" + call_service SickDevIMUActive "{}" "active: True success: True" +done + +echo -e "send_cola_advanced.bash finished.\n" +echo -e "Advanced services and cola telegram verification summary: $testcase_counter testcases, $testcase_error_counter failures.\n" diff --git a/test/scripts/send_cola_examples.bash b/test/ros1_scripts/send_cola_examples.bash similarity index 93% rename from test/scripts/send_cola_examples.bash rename to test/ros1_scripts/send_cola_examples.bash index 2adc109..352fd34 100755 --- a/test/scripts/send_cola_examples.bash +++ b/test/ros1_scripts/send_cola_examples.bash @@ -87,7 +87,7 @@ for ((n=0;n<1;n++)) ; do send_cola_telegram "{cola_ascii_request: 'sMN IsSystemReady', wait_response_timeout: 1}" "sAN IsSystemReady 1" # 0:true, 1:false # Check if the system is ready send_cola_telegram "{cola_ascii_request: 'sRN LocState', wait_response_timeout: 1}" "sRA LocState 2" # 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING # Read localization state send_cola_telegram "{cola_ascii_request: 'sMN LocStop', wait_response_timeout: 1}" "sAN LocStop 1" # 0:failed, 1:success # Stop localization or demo mapping - send_cola_telegram "{cola_ascii_request: 'sMN LocStopAndSave', wait_response_timeout: 1}" "sAN LocStopAndSave 1" # 0:failed, 1:success # Stop localization, save settings + # send_cola_telegram "{cola_ascii_request: 'sMN LocStopAndSave', wait_response_timeout: 1}" "sAN LocStopAndSave 1" # Deprecated 0:failed, 1:success # Stop localization, save settings send_cola_telegram "{cola_ascii_request: 'sMN LocStartLocalizing', wait_response_timeout: 1}" "sAN LocStartLocalizing 1" # 0:failed, 1:success # Start localization # Cola-ASCII Result Output Configuration Telegrams send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPort +2201', wait_response_timeout: 1}" "sAN LocSetResultPort 1 1" # 0:failed, 1:success, : uint16 (default: 2201) # Set TCP-port for result output @@ -100,7 +100,6 @@ for ((n=0;n<1;n++)) ; do send_cola_telegram "{cola_ascii_request: 'sMN LocSetPose +10300 -5200 +30000 +1000', wait_response_timeout: 1}" "sAN LocSetPose 1" # 0:failed, 1:success, : int32 (x coordinate in mm), : int32 (y coordinate in mm), : int32 (yaw angle in millidegree, -180000 to +180000), : uint16 (translation uncertainty in mm) # Initialize vehicle pose # Cola-ASCII Result Output Settings (queries, ros service SickLocColaTelegram only) send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 1" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output - send_cola_telegram "{cola_ascii_request: 'sRN LocResultPort', wait_response_timeout: 1}" "sRA LocResultPort 899" # tcp port # Read TCP port used for result output (hex value) send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 0" # 0:stream, 1: poll # Read result mode send_cola_telegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" "sRA LocResultEndianness 0" # 0: big endian, 1: little endian # Read endianness of result output # Cola-ASCII Timestamp Telegrams @@ -113,9 +112,7 @@ for ((n=0;n<1;n++)) ; do send_cola_telegram "{cola_ascii_request: 'sRN LocState', wait_response_timeout: 1}" "sRA LocState 2" # 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING # Read localization state # Cola-ASCII Set tcp port for result telegram and query settings send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPort +2345', wait_response_timeout: 1}" "sAN LocSetResultPort 1 1" # 0:failed, 1:success, : uint16 (default: 2201) # Set TCP-port for result output - send_cola_telegram "{cola_ascii_request: 'sRN LocResultPort', wait_response_timeout: 1}" "sRA LocResultPort 929" # tcp port # Read TCP port used for result output (hex value) send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPort +2201', wait_response_timeout: 1}" "sAN LocSetResultPort 1 1" # 0:failed, 1:success, : uint16 (default: 2201) # Set TCP-port for result output - send_cola_telegram "{cola_ascii_request: 'sRN LocResultPort', wait_response_timeout: 1}" "sRA LocResultPort 899" # tcp port # Read TCP port used for result output (hex value) # Cola-ASCII Set result mode and query settings send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultMode 1', wait_response_timeout: 1}" "sAN LocSetResultMode 1" # 0:failed, 1:success, : uint8 (0:stream, 1:poll, default: stream) # Set mode of result output (stream or poll) send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 1" # 0:stream, 1: poll # Read result mode @@ -136,7 +133,7 @@ for ((n=0;n<1;n++)) ; do call_service SickLocIsSystemReady "{}" "success: True" # Check if the system is ready call_service SickLocState "{}" "state: 2 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING call_service SickLocStop "{}" "success: True" # Stop localization or demo mapping - call_service SickLocStopAndSave "{}" "success: True" # Stop localization, save settings + # call_service SickLocStopAndSave "{}" "success: True" # Deprecated, Stop localization, save settings call_service SickLocStartLocalizing "{}" "success: True" # Start localization # ROS services for Result Output Configuration Telegrams call_service SickLocSetResultPort "{port: 2201}" "success: True" # Set TCP-port for result output @@ -159,9 +156,7 @@ for ((n=0;n<1;n++)) ; do call_service SickLocState "{}" "state: 2 success: True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING # Change settings with ROS services for Result Output Configuration Telegrams call_service SickLocSetResultPort "{port: 2345}" "success: True" # Set TCP-port for result output - send_cola_telegram "{cola_ascii_request: 'sRN LocResultPort', wait_response_timeout: 1}" "sRA LocResultPort 929" # tcp port # Read TCP port used for result output (hex value) call_service SickLocSetResultPort "{port: 2201}" "success: True" # Set TCP-port for result output - send_cola_telegram "{cola_ascii_request: 'sRN LocResultPort', wait_response_timeout: 1}" "sRA LocResultPort 899" # tcp port # Read TCP port used for result output (hex value) call_service SickLocSetResultMode "{mode: 1}" "success: True" # Set mode of result output (stream or poll) send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 1" # 0:stream, 1:poll # Read result mode call_service SickLocSetResultMode "{mode: 0}" "success: True" # Set mode of result output (stream or poll) @@ -193,8 +188,8 @@ echo -e "Services and cola telegram verification summary: $testcase_counter test # sleep 1 ; rosservice call SickLocTimeSync "{timestamp_lidar_ms: 123456}" # expected reponse: "vehicle_time_valid: True vehicle_time_sec: 1573119167 vehicle_time_nsec: 380565047" # done # sleep 1 ; -# if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/scripts/killall.bash ; fi -# if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/scripts/killall.bash ; fi +# if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros1_scripts/killall.bash ; fi +# if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros1_scripts/killall.bash ; fi # sleep 1 ; killall roslaunch ; sleep 20 # ps -elf diff --git a/test/ros2_scripts/cleanup.bash b/test/ros2_scripts/cleanup.bash new file mode 100755 index 0000000..2c6947d --- /dev/null +++ b/test/ros2_scripts/cleanup.bash @@ -0,0 +1,15 @@ +#!/bin/bash + +pushd ../../../../ +source /opt/ros/eloquent/setup.bash +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros2_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros2_scripts/killall.bash ; fi + +echo -e "\n# cleanup.bash: Deleting ros cache and logfiles and colcon folders ./build ./devel ./install ./log" +rm -rf ./build ./devel ./install ./log +mkdir ./build ./devel ./install ./log ./install/sick_lidar_localization +rm -rf ~/.ros/* +popd +rm -rf ./log/* +rm -rf ../ros1_scripts/log/* + diff --git a/test/ros2_scripts/killall.bash b/test/ros2_scripts/killall.bash new file mode 100755 index 0000000..1675e8a --- /dev/null +++ b/test/ros2_scripts/killall.bash @@ -0,0 +1,9 @@ +#!/bin/bash + +killall -s SIGINT sim_loc_driver sim_loc_driver_check pointcloud_converter verify_sim_loc_driver ; sleep 3 +killall -s SIGINT sim_loc_test_server ; sleep 3 +killall sim_loc_driver sim_loc_driver_check pointcloud_converter verify_sim_loc_driver ; sleep 1 +killall sim_loc_test_server ; sleep 1 +killall -9 sim_loc_test_server ; sleep 1 +killall -9 sim_loc_driver sim_loc_driver_check pointcloud_converter verify_sim_loc_driver ; sleep 1 + diff --git a/test/ros2_scripts/make.bash b/test/ros2_scripts/make.bash new file mode 100755 index 0000000..b1b39b6 --- /dev/null +++ b/test/ros2_scripts/make.bash @@ -0,0 +1,40 @@ +#!/bin/bash + +# +# Check/set/cleanup environment +# + +pushd ../../../.. +source /opt/ros/eloquent/setup.bash +export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH +rm -f ./install/sick_lidar_localization/* 2> /dev/null + +# +# set build type (Debug or Release) and logfile +# +BUILDTYPE=Debug +# BUILDTYPE=Release + +# +# Build and install sick_lidar_localization binaries. +# + +#if [ ! -d ./install/sick_lidar_localization ] ; then mkdir -p ./install/sick_lidar_localization ; fi +if [ -d ./src/sick_lidar_localization ] && [ ! -f ./src/sick_lidar_localization/package.xml ] ; then pushd ./src/sick_lidar_localization ; ln -s ./package_ros2.xml ./package.xml ; popd ; fi +if [ -d ./src/sick_lidar_localization_pretest ] && [ ! -f ./src/sick_lidar_localization_pretest/package.xml ] ; then pushd ./src/sick_lidar_localization_pretest ; ln -s ./package_ros2.xml ./package.xml ; popd ; fi +colcon build --packages-select sick_lidar_localization --event-handlers console_direct+ --cmake-args " -DROS_VERSION=2" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" +colcon build --packages-select sick_lidar_localization --event-handlers console_direct+ --cmake-target RunDoxygen --cmake-args " -DROS_VERSION=2" " -DCMAKE_BUILD_TYPE=$BUILDTYPE" +source ./install/setup.bash + +# print warnings and errors +echo -e "\nmake.bash finished.\n" +echo -e "colcon build warnings:" +cat ./log/latest_build/*.* ./log/latest_build/sick_lidar_localization/*.* | grep -i "warning:" +echo -e "\ncolcon build errors:" +cat ./log/latest_build/*.* ./log/latest_build/sick_lidar_localization/*.* | grep -i "error:" + +# print sick_lidar_localization install files, libraries, executables +echo -e "\ninstall/sick_lidar_localization/lib/sick_lidar_localization:" +ls -al install/sick_lidar_localization/lib/sick_lidar_localization/* +popd + diff --git a/test/ros2_scripts/makeall.bash b/test/ros2_scripts/makeall.bash new file mode 100755 index 0000000..9309e88 --- /dev/null +++ b/test/ros2_scripts/makeall.bash @@ -0,0 +1,9 @@ +#!/bin/bash +printf "\033c" +echo -e "makeall.bash: build and install sick_lidar_localization binaries" +source /opt/ros/eloquent/setup.bash +./cleanup.bash +rm -f ../../package.xml +./make.bash +echo -e "makeall.bash finished." + diff --git a/test/ros2_scripts/run_cola_examples.bash b/test/ros2_scripts/run_cola_examples.bash new file mode 100755 index 0000000..b9f8b89 --- /dev/null +++ b/test/ros2_scripts/run_cola_examples.bash @@ -0,0 +1,86 @@ +#!/bin/bash + +# +# Run cola examples (offline-test for cola telegrams and ros services provided by the sick_lidar_localization driver) +# against local test server simulating a localization controller +# + +# +# Environment +# + +pushd ../../../.. +printf "\033c" +source /opt/ros/eloquent/setup.bash +source ./install/setup.bash + +# +# Cleanup +# + +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros2_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros2_scripts/killall.bash ; fi +rm -rf ~/.ros/* +if [ ! -d ~/.ros/log/cola_examples ] ; then mkdir -p ~/.ros/log/cola_examples ; fi + +# +# Global configuration for offline simulation +# + +export localization_controller_ip_address=127.0.0.1 +export sim_loc_test_server_demo_circles=0 +export sim_loc_test_server_error_simulation=0 + +# +# Run standalone unittests for sim_loc_parser +# + +ros2 launch sick_lidar_localization unittest_sim_loc_parser.launch.py & +sleep 10 +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros2_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros2_scripts/killall.bash ; fi +sleep 1 + +# +# Run test server, simulate localization controller for offline tests +# + +ros2 launch sick_lidar_localization sim_loc_test_server.launch.py 2>&1 >> ~/.ros/log/cola_examples/sim_loc_test_server.log & +sleep 3 # make sure ros core and sim_loc_test_server are up and running + +# +# Run ros driver, connect to localization controller and receive, convert and publish telegrams +# + +ros2 launch sick_lidar_localization sim_loc_driver.launch.py 2>&1 >> ~/.ros/log/cola_examples/sim_loc_driver.log & +sleep 1 # make sure sim_loc_test_server and sim_loc_driver are up and running + +# +# Send cola examples using ros services provided by the sick_lidar_localization driver +# Example: +# ros2 service list -t +# ros2 service call /SickLocIsSystemReady sick_lidar_localization/srv/SickLocIsSystemReadySrv "{}" +# ros2 service call /SickLocRequestTimestamp sick_lidar_localization/srv/SickLocRequestTimestampSrv "{}" +# ros2 service call /SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN IsSystemReady', wait_response_timeout: 1}" + +popd +./send_cola_advanced.bash 2>&1 | tee -a ~/.ros/log/cola_examples/send_cola_advanced.log +./send_cola_examples.bash 2>&1 | tee -a ~/.ros/log/cola_examples/send_cola_examples.log + +# +# Cleanup and exit +# + +./killall.bash +sleep 1 ; killall roslaunch ; sleep 1 +cat ~/.ros/log/cola_examples/send_cola_examples.log +cat ~/.ros/log/cola_examples/send_cola_advanced.log +# sleep 20 +mkdir -p ./log/cola_examples +cp -rf ~/.ros/log/cola_examples/*.log ./log/cola_examples +echo -e "\nsick_lidar_localization finished. Warnings and errors:" +grep "WARN" ./log/cola_examples/*.log +grep "ERR" ./log/cola_examples/*.log +echo -e "\nsim_loc_driver check summary:" +grep -i "check messages thread summary" ./log/cola_examples/*.log +grep -i "verification summary" ./log/cola_examples/*.log diff --git a/test/ros2_scripts/run_demo_simu.bash b/test/ros2_scripts/run_demo_simu.bash new file mode 100755 index 0000000..9621c7e --- /dev/null +++ b/test/ros2_scripts/run_demo_simu.bash @@ -0,0 +1,81 @@ +#!/bin/bash + +# +# Run demo simulation (offline-test) of sick_lidar_localization driver against local test server simulating a localization controller. +# Demo simulation shows a sensor moving in circles. +# + +# +# Environment +# + +pushd ../../../.. +printf "\033c" +source /opt/ros/eloquent/setup.bash +source ./install/setup.bash + +# +# Cleanup +# + +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros2_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros2_scripts/killall.bash ; fi +rm -rf ~/.ros/* +if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi + +# +# Global configuration for offline simulation +# + +export localization_controller_ip_address=127.0.0.1 +export sim_loc_test_server_demo_circles=1 +export sim_loc_test_server_error_simulation=0 + +# +# Run test server, simulate localization controller for offline tests. Run in demo mode, create telegrams of a sensor moving in circles. +# + +ros2 launch sick_lidar_localization sim_loc_test_server.launch.py & +# ros2 run sick_lidar_localization sim_loc_test_server & +sleep 3 # make sure ros core and sim_loc_test_server are up and running + +# +# Run ros driver, connect to localization controller and receive, convert and publish telegrams +# + +ros2 launch sick_lidar_localization sim_loc_driver.launch.py & +# ros2 param dump sim_loc_driver --print +# ros2 run sick_lidar_localization sim_loc_driver & +# ros2 run sick_lidar_localization cola_service_node & +# ros2 run sick_lidar_localization sim_loc_time_sync & +# ros2 run sick_lidar_localization pointcloud_converter & + +# +# Visualize PointCloud2 messages by rviz: +# +# rviz -> Add by topic /cloud/PointCloud2 +# -> Style: Points +# -> Size (Pixels): 5 +# +# Visualize TF messages by rviz: +# +# rviz -> Global options: Fixed Frame -> tf_demo_map +# -> Add by display type TF +# + +# ros2 topic echo /cloud +ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 map pointcloud_sick_lidar_localization & +ros2 run rviz2 rviz2 -d ./src/sick_lidar_localization_pretest/test/config/rviz2_sick_lidar_localization_demo_pointcloud.rviz & +ros2 run rviz2 rviz2 -d ./src/sick_lidar_localization_pretest/test/config/rviz2_sick_lidar_localization_demo_tf.rviz & + +# +# Run test server and driver for some time and exit +# + +sleep 600 +popd +./killall.bash +killall rviz2 +killall static_transform_publisher +echo -e "\nrun_demo_simu for sick_lidar_localization finished." + diff --git a/test/ros2_scripts/run_error_simu.bash b/test/ros2_scripts/run_error_simu.bash new file mode 100755 index 0000000..1d5cdc3 --- /dev/null +++ b/test/ros2_scripts/run_error_simu.bash @@ -0,0 +1,124 @@ +#!/bin/bash + +# +# Run error simulation of sick_lidar_localization driver (offline-test error handling) against local test server simulating error testcases. +# + +# +# Function wait_telegram_message() waits until at least one message on topic "/sick_lidar_localization/driver/result_telegrams" has been published +# Contrary to ROS1, "ros2 topic echo " has no option -n and runs forever until killed externally. +# +# function wait_telegram_message() { +# local telegram_messages_topic="/sick_lidar_localization/driver/result_telegrams" +# local telegram_message_length=0 +# while [ $telegram_message_length -le 1060 ] ; do +# echo -e "sick_lidar_localization error simulation: waiting for telegram messages from sim_loc_driver on topic $telegram_messages_topic ..." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +# telegram_message_length=`ros2 topic echo $telegram_messages_topic | wc -c` +# done +# echo -e "sick_lidar_localization error simulation: $telegram_message_length byte telegram messages received on topic $telegram_messages_topic" 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +# ros2 topic echo $telegram_messages_topic 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +# } + +# +# Environment +# + +pushd ../../../.. +printf "\033c" +source /opt/ros/eloquent/setup.bash +source ./install/setup.bash + +# +# Cleanup +# + +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros2_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros2_scripts/killall.bash ; fi +rm -rf ~/.ros/* +if [ ! -d ~/.ros/log/error_simu ] ; then mkdir -p ~/.ros/log/error_simu ; fi + +# +# Global configuration for offline simulation +# + +export localization_controller_ip_address=127.0.0.1 +export sim_loc_test_server_demo_circles=0 +export sim_loc_test_server_error_simulation=0 + +# +# Start sick_lidar_localization driver, log diagnostic messages +# + +echo -e "sick_lidar_localization error simulation: starting sick_lidar_localization sim_loc_driver.launch ..." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +ros2 launch sick_lidar_localization sim_loc_driver.launch.py 2>&1 | tee -a ~/.ros/log/error_simu/sim_loc_driver_error_simu.log & +echo -e "sick_lidar_localization error simulation: sick_lidar_localization sim_loc_driver.launch started." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +sleep 10 + +# +# Log diagnostic messages +# + +ros2 topic echo "/sick_lidar_localization/driver/diagnostic" | tee -a ~/.ros/log/error_simu/sim_loc_driver_diagnostic_messages.log & + +# +# Start test server, run some time, kill and restart. Check reconnect of ros driver after connection lost (ROS1 only) +# +# if true ; then +# echo -e "sick_lidar_localization error simulation: Starting test server ..." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +# ros2 launch sick_lidar_localization sim_loc_test_server.launch.py & # 2>&1 | tee -a ~/.ros/log/error_simu/sim_loc_test_server_error_simu.log & +# sleep 10 +# for testcnt in {1..3} ; do +# echo -e "sick_lidar_localization error simulation: running $testcnt. testcase ..." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +# echo -e "sick_lidar_localization error simulation: killing test server ..." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +# if [ $testcnt -eq 1 ] ; then +# killall -s SIGINT sim_loc_test_server ; sleep 10 ; killall -9 sim_loc_test_server +# elif [ $testcnt -eq 2 ] ; then +# killall -9 sim_loc_test_server +# else +# killall -s SIGINT sim_loc_test_server ; sleep 1 ; killall -9 sim_loc_test_server +# fi +# sleep 10 +# echo -e "sick_lidar_localization error simulation: restarting test server ..." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +# ros2 launch sick_lidar_localization sim_loc_test_server.launch.py & # 2>&1 | tee -a ~/.ros/log/error_simu/sim_loc_test_server_error_simu.log & +# sleep 10 +# sleep 30 # wait_telegram_message +# echo -e "sick_lidar_localization error simulation: finished $testcnt. testcase.\n" 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +# done +# echo -e "sick_lidar_localization error simulation: Tests 1 to $testcnt finished, stopping test server ..." 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +# killall -s SIGINT sim_loc_test_server ; sleep 1 ; killall -9 sim_loc_test_server ; sleep 15 +# echo -e "sick_lidar_localization error simulation summary: finished $testcnt testcases, reconnect after connection lost okay.\n" 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log +# fi + +# +# Run test server in error simulation mode (disconnect and reconnect, send no or invalid telegrams). +# sick_lidar_localization driver handles errors with diagnostic messages and reconnects if required. +# + +export sim_loc_test_server_error_simulation=1 +if true ; then + ros2 launch sick_lidar_localization sim_loc_test_server.launch.py 2>&1 | tee -a ~/.ros/log/error_simu/error_simu.log & + sleep 180 + killall -s SIGINT sim_loc_test_server ; sleep 1 ; killall -9 sim_loc_test_server +fi +export sim_loc_test_server_error_simulation=0 + +# +# Cleanup and exit +# + +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros2_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros2_scripts/killall.bash ; fi +grep "WARN" ~/.ros/log/error_simu/*.log ; grep "ERR" ~/.ros/log/error_simu/*.log +sleep 15 +echo -e "\nrun_error_simu.bash: sick_lidar_localization error simulation finished.\n" +cat ~/.ros/log/error_simu/sim_loc_driver_diagnostic_messages.log +cat ~/.ros/log/error_simu/error_simu.log +echo -e "\nrun_error_simu.bash: sick_lidar_localization error simulation summary:\n" +grep "summary" ~/.ros/log/error_simu/*.log +popd +rm -rf ./log/error_simu +mkdir ./log/error_simu +cp -rf ~/.ros/log/error_simu/*.log ./log/error_simu +echo -e "\nsick_lidar_localization summary:" +grep -R "summary" ./log/* + diff --git a/test/ros2_scripts/run_odom_simu.bash b/test/ros2_scripts/run_odom_simu.bash new file mode 100755 index 0000000..f34a3b5 --- /dev/null +++ b/test/ros2_scripts/run_odom_simu.bash @@ -0,0 +1,73 @@ +#!/bin/bash + +# +# Run odometry simulation (offline-test) of sick_lidar_localization driver +# Odometry simulation runs the sim_loc_driver, a odometry message sender +# (odem_msg_sender.py) and checks the odom udp packages sent by sim_loc_driver. +# + +# +# Environment +# + +pushd ../../../.. +printf "\033c" +source /opt/ros/eloquent/setup.bash +source ./install/setup.bash + +# +# Cleanup +# + +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros2_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros2_scripts/killall.bash ; fi +rm -rf ~/.ros/* +if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi + +# +# Global configuration for offline simulation +# + +export localization_controller_ip_address=127.0.0.1 +export sim_loc_test_server_demo_circles=0 +export sim_loc_test_server_error_simulation=0 +export ROS_VERSION=2 + +# +# Start test server and sim_loc_driver +# + +ros2 launch sick_lidar_localization sim_loc_test_server.launch.py > ~/.ros/log/sim_loc_test_server.log & +sleep 3 # make sure ros core and sim_loc_test_server are up and running +ros2 launch sick_lidar_localization sim_loc_driver.launch.py > ~/.ros/log/sim_loc_driver.log & +sleep 3 # make sure sim_loc_driver is up and running before sending and verifying odometry telegrams + +# +# Visualize odom messages by rviz: +# + +ros2 run rviz2 rviz2 -d ./src/sick_lidar_localization_pretest/test/config/rviz2_odom_msg_sender.rviz & + +# +# Disable firewall on udp port 3000 +# +# sudo iptables -A INPUT -p udp -m udp --dport 3000 -j ACCEPT +# sudo iptables -A OUTPUT -p udp -m udp --sport 3000 -j ACCEPT +# sudo iptables-save +# sudo ufw allow from any to any port 3000 proto udp +# sudo ufw status + +# +# Start odometry message sender, send 100 odometry messages and verify udp telegrams from sim_loc_driver +# + +# ros2 service call SickLocSetOdometryActive sick_lidar_localization/srv/SickLocSetOdometryActiveSrv "{active: 1}" +# ros2 service call SickLocSetOdometryPort sick_lidar_localization/srv/SickLocSetOdometryPortSrv "{port: 3000}" +# ros2 service call SickLocSetOdometryRestrictYMotion sick_lidar_localization/srv/SickLocSetOdometryRestrictYMotionSrv "{active: 0}" +python3 ./src/sick_lidar_localization_pretest/test/python/odom_msg_sender.py + +popd +./killall.bash +pkill -f 'python3 ./src/sick_lidar_localization_pretest/test/python/odom_msg_sender.py' +echo -e "\nrun_odom_simu for sick_lidar_localization finished." + diff --git a/test/ros2_scripts/run_simu.bash b/test/ros2_scripts/run_simu.bash new file mode 100755 index 0000000..be8c026 --- /dev/null +++ b/test/ros2_scripts/run_simu.bash @@ -0,0 +1,98 @@ +#!/bin/bash + +# +# Run simulation (offline-test) of sick_lidar_localization driver against local test server simulating a localization controller +# + +# +# Environment +# + +pushd ../../../.. +printf "\033c" +source /opt/ros/eloquent/setup.bash +source ./install/setup.bash + +# +# Cleanup +# + +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros2_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros2_scripts/killall.bash ; fi +rm -rf ~/.ros/* +if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi + +# +# Global configuration for offline simulation +# + +export localization_controller_ip_address=127.0.0.1 +export sim_loc_test_server_demo_circles=0 +export sim_loc_test_server_error_simulation=0 + +# +# Run test server, simulate localization controller for offline tests +# + +ros2 launch sick_lidar_localization sim_loc_test_server.launch.py 2>&1 | tee -a ~/.ros/log/sim_loc_test_server.log & +sleep 3 # make sure ros core and sim_loc_test_server are up and running + +# +# Log diagnostic messages +# + +ros2 topic echo "/sick_lidar_localization/driver/diagnostic" | tee -a ~/.ros/log/sim_loc_driver_diagnostic_messages.log & + +# +# Run ros driver, connect to localization controller and receive, convert and publish telegrams +# + +ros2 launch sick_lidar_localization sim_loc_driver.launch.py 2>&1 | tee -a ~/.ros/log/sim_loc_driver.log & +sleep 1 # make sure sim_loc_test_server and sim_loc_driver are up and running + +# +# Run verification and check driver messages against testcases from localization controller +# ros2 topic echo "/sick_lidar_localization/driver/result_telegrams" +# ros2 topic echo "/sick_lidar_localization/test_server/result_testcases" +# + +ros2 launch sick_lidar_localization verify_sim_loc_driver.launch.py 2>&1 | tee -a ~/.ros/log/verify_sim_loc_driver.log & + +# +# Run test server and driver for some time and verify results +# + +sleep 180 +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros2_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros2_scripts/killall.bash ; fi +sleep 3 + +# +# Run standalone unittests for sim_loc_parser +# + +ros2 launch sick_lidar_localization unittest_sim_loc_parser.launch.py 2>&1 | tee -a ~/.ros/log/unittest_sim_loc_parser.log & +sleep 10 +if [ -d ./src/sick_lidar_localization ] ; then ./src/sick_lidar_localization/test/ros2_scripts/killall.bash ; fi +if [ -d ./src/sick_lidar_localization_pretest ] ; then ./src/sick_lidar_localization_pretest/test/ros2_scripts/killall.bash ; fi +sleep 3 +cat ~/.ros/log/sim_loc_driver_diagnostic_messages.log +grep "WARN" ~/.ros/log/*.log ; grep "ERR" ~/.ros/log/*.log +sleep 20 + +# +# Cleanup and exit +# + +popd +rm -rf ./log +mkdir ./log +cp -rf ~/.ros/log/unittest_sim_loc_parser.log ~/.ros/log/sim_loc_driver.log ~/.ros/log/verify_sim_loc_driver.log ./log +echo -e "\nsick_lidar_localization finished. Warnings and errors:" +grep "WARN" ~/.ros/log/*.log +grep "ERR" ~/.ros/log/*.log +echo -e "\n\nsick_lidar_localization check and verification summary:" +grep -i "unittest_sim_loc_parser finished" ./log/unittest_sim_loc_parser.log +grep -i "check messages thread summary" ./log/sim_loc_driver.log +grep -i "verification thread summary" ./log/verify_sim_loc_driver.log + diff --git a/test/ros2_scripts/send_cola_advanced.bash b/test/ros2_scripts/send_cola_advanced.bash new file mode 100755 index 0000000..4c65e35 --- /dev/null +++ b/test/ros2_scripts/send_cola_advanced.bash @@ -0,0 +1,103 @@ +#!/bin/bash + +# +# Run examples for advanced Cola-ASCII telegrams and sick_lidar_localization services provided by sim_loc_driver. +# Testset for the new sick_lidar_localization services implemented september 2020. +# +# Note: ROS sim_loc_driver and test server should be up and running, start by +# ros2 launch sick_lidar_localization sim_loc_test_server.launch & +# ros2 launch sick_lidar_localization sim_loc_driver.launch & +# + +# +# Function call_service "" "" "" "" just calls a ros service +# Example: call_service SickLocIsSystemReady sick_lidar_localization/srv/SickLocIsSystemReadySrv "{}" "success: True" # Check if the system is ready +# +function call_service() { + sleep 1 + echo -e "ros2 service call $1 $2 \"$3\"" + answer=$(ros2 service call $1 $2 "$3") # call rosservice + answer=$(echo $answer|tr -d '\n') # remove line feeds + echo -e "expected answer: \"$4\"" + echo -e "received answer: \"$answer\"" + if [[ "$answer" == *"$4"* ]]; then + echo -e "OK: service responded with expected answer.\n" + else + testcase_error_counter=$((testcase_error_counter+1)) + echo -e "## ERROR: service did NOT respond with expected answer. Press any key to continue..." ; read -n1 -s key + fi + testcase_counter=$((testcase_counter+1)) +} + +# +# Environment +# + +pushd ../../../.. +source /opt/ros/eloquent/setup.bash +source ./install/setup.bash +testcase_counter=0 +testcase_error_counter=0 + +# +# Call services and check answer +# + +for ((n=0;n<1;n++)) ; do + # service servicename servicetype request expected response + call_service SickLocIsSystemReady sick_lidar_localization/srv/SickLocIsSystemReadySrv "{}" "success=True" + call_service SickLocStop sick_lidar_localization/srv/SickLocStopSrv "{}" "success=True" + call_service SickLocStartLocalizing sick_lidar_localization/srv/SickLocStartLocalizingSrv "{}" "success=True" + # ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN LocStartLocalizing', wait_response_timeout: 1}" + call_service SickDevSetLidarConfig sick_lidar_localization/srv/SickDevSetLidarConfigSrv "{index: 0, minrange: 100, maxrange: 200000, minangle: -15000, maxangle: 15000, x: 1000, y: -1000, yaw: 2000, upsidedown: true, ip: 192.168.1.30, port: 2111, interfacetype: 0, maplayer: 0, active: true}" "set=True, executed=True" + call_service SickDevGetLidarConfig sick_lidar_localization/srv/SickDevGetLidarConfigSrv "{scannerindex: 0}" "minrange=100, maxrange=200000, minangle=-15000, maxangle=15000, x=1000, y=-1000, yaw=2000, upsidedown=True, ip='192.168.1.30', port=2111, interfacetype=0, maplayer=0, active=True" + # call_service SickDevSetLidarConfig sick_lidar_localization/srv/SickDevSetLidarConfigSrv "{index: 0, minrange: 1, maxrange: 3, minangle: 0, maxangle: 2, x: 0, y: 0, yaw: 0, upsidedown: true, ip: 192.168.0.30, port: 2111, interfacetype: 0, maplayer: 0, active: true}" "set=True, executed=True" + # call_service SickDevGetLidarConfig sick_lidar_localization/srv/SickDevGetLidarConfigSrv "{scannerindex: 0}" "minrange=1, maxrange=3, minangle=0, maxangle=2, x=0, y=0, yaw=0, upsidedown=True, ip='192.168.0.30', port=2111, interfacetype=0, maplayer=0, active=True" + # ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN DevSetLidarConfig +0 +300 +100000 -180000 +180000 +1000 -300 -45000 0 +12 192.168.0.30 +2122 0 0 1', wait_response_timeout: 1}" + # ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN DevGetLidarConfig 0', wait_response_timeout: 1}" + call_service SickLocSetMap sick_lidar_localization/srv/SickLocSetMapSrv "{mapfilename: demo_map.smap}" "set=True, executed=True" + call_service SickLocMap sick_lidar_localization/srv/SickLocMapSrv "{}" "map='demo_map.smap', success=True" + call_service SickLocMapState sick_lidar_localization/srv/SickLocMapStateSrv "{}" "mapstate=True, success=True" + call_service SickLocInitializePose sick_lidar_localization/srv/SickLocInitializePoseSrv "{x: 100, y: -100, yaw: 2000, sigmatranslation: 1000}" "success=True" + # ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN LocInitializePose +10300 -5200 +30000 +1000', wait_response_timeout: 1}" + call_service SickLocInitialPose sick_lidar_localization/srv/SickLocInitialPoseSrv "{}" "x=100, y=-100, yaw=2000, sigmatranslation=1000, success=True" + call_service SickLocSetReflectorsForSupportActive sick_lidar_localization/srv/SickLocSetReflectorsForSupportActiveSrv "{active: 1}" "success=True" + call_service SickLocReflectorsForSupportActive sick_lidar_localization/srv/SickLocReflectorsForSupportActiveSrv "{}" "active=True, success=True" + call_service SickLocSetOdometryActive sick_lidar_localization/srv/SickLocSetOdometryActiveSrv "{active: 1}" "set=True, executed=True" + call_service SickLocOdometryActive sick_lidar_localization/srv/SickLocOdometryActiveSrv "{}" "active=True, success=True" + call_service SickLocSetOdometryPort sick_lidar_localization/srv/SickLocSetOdometryPortSrv "{port: 3000}" "set=True, executed=True" + call_service SickLocOdometryPort sick_lidar_localization/srv/SickLocOdometryPortSrv "{}" "port=3000, success=True" + call_service SickLocSetOdometryRestrictYMotion sick_lidar_localization/srv/SickLocSetOdometryRestrictYMotionSrv "{active: 1}" "success=True" + call_service SickLocOdometryRestrictYMotion sick_lidar_localization/srv/SickLocOdometryRestrictYMotionSrv "{}" "active=True, success=True" + call_service SickLocSetAutoStartActive sick_lidar_localization/srv/SickLocSetAutoStartActiveSrv "{active: 1}" "success=True" + call_service SickLocAutoStartActive sick_lidar_localization/srv/SickLocAutoStartActiveSrv "{}" "active=True, success=True" + call_service SickLocSetAutoStartSavePoseInterval sick_lidar_localization/srv/SickLocSetAutoStartSavePoseIntervalSrv "{interval: 5}" "success=True" + call_service SickLocAutoStartSavePoseInterval sick_lidar_localization/srv/SickLocAutoStartSavePoseIntervalSrv "{}" "interval=5, success=True" + call_service SickLocSetRingBufferRecordingActive sick_lidar_localization/srv/SickLocSetRingBufferRecordingActiveSrv "{active: 1}" "success=True" + call_service SickLocRingBufferRecordingActive sick_lidar_localization/srv/SickLocRingBufferRecordingActiveSrv "{}" "active=True, success=True" + call_service SickDevGetLidarIdent sick_lidar_localization/srv/SickDevGetLidarIdentSrv "{index: 0}" "scannerident='TestcaseGenerator0', success=True" + call_service SickDevGetLidarState sick_lidar_localization/srv/SickDevGetLidarStateSrv "{index: 0}" "devicestatus=2, deviceconnected=2, receivingdata=2, success=True" + call_service SickGetSoftwareVersion sick_lidar_localization/srv/SickGetSoftwareVersionSrv "{}" "version='1.0', success=True" + call_service SickLocAutoStartSavePose sick_lidar_localization/srv/SickLocAutoStartSavePoseSrv "{}" "success=True" + call_service SickLocForceUpdate sick_lidar_localization/srv/SickLocForceUpdateSrv "{}" "success=True" + call_service SickLocSaveRingBufferRecording sick_lidar_localization/srv/SickLocSaveRingBufferRecordingSrv "{reason: test}" + # ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN LocSaveRingBufferRecording +36 YYYY-MM-DD_HH-MM-SS pose quality low', wait_response_timeout: 1}" + call_service SickLocStop sick_lidar_localization/srv/SickLocStopSrv "{}" "success=True" + call_service SickLocStartDemoMapping sick_lidar_localization/srv/SickLocStartDemoMappingSrv "{}" + # ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "{cola_ascii_request: 'sMN LocStartDemoMapping', wait_response_timeout: 1}" + call_service SickLocStop sick_lidar_localization/srv/SickLocStopSrv "{}" "success=True" + call_service SickLocStartLocalizing sick_lidar_localization/srv/SickLocStartLocalizingSrv "{}" "success=True" + call_service SickReportUserMessage sick_lidar_localization/srv/SickReportUserMessageSrv "{usermessage: test_message}" "success=True" + call_service SickSavePermanent sick_lidar_localization/srv/SickSavePermanentSrv "{}" "success=True" + call_service SickLocResultPort sick_lidar_localization/srv/SickLocResultPortSrv "{}" "port=2201, success=True" + call_service SickLocResultMode sick_lidar_localization/srv/SickLocResultModeSrv "{}" "mode=0, success=True" + call_service SickLocResultEndianness sick_lidar_localization/srv/SickLocResultEndiannessSrv "{}" "endianness=0, success=True" + call_service SickLocResultState sick_lidar_localization/srv/SickLocResultStateSrv "{}" "state=1, success=True" + call_service SickLocResultPoseInterval sick_lidar_localization/srv/SickLocResultPoseIntervalSrv "{}" "interval=1, success=True" + call_service SickDevSetIMUActive sick_lidar_localization/srv/SickDevSetIMUActiveSrv "{active: 1}" "success=True" + call_service SickDevIMUActive sick_lidar_localization/srv/SickDevIMUActiveSrv "{}" "active=True, success=True" +done + +echo -e "send_cola_advanced.bash finished.\n" +echo -e "Advanced services and cola telegram verification summary: $testcase_counter testcases, $testcase_error_counter failures.\n" + diff --git a/test/ros2_scripts/send_cola_examples.bash b/test/ros2_scripts/send_cola_examples.bash new file mode 100755 index 0000000..74ee3ac --- /dev/null +++ b/test/ros2_scripts/send_cola_examples.bash @@ -0,0 +1,178 @@ +#!/bin/bash + +# +# Run examples for Cola-ASCII telegrams and sick_lidar_localization services provided by sim_loc_driver +# +# Note: ROS sim_loc_driver and test server should be up and running, start by +# ros2 launch sick_lidar_localization sim_loc_test_server.launch & +# ros2 launch sick_lidar_localization sim_loc_driver.launch & +# + +# +# Function send_cola_telegram "" "" sends and echos a cola telegram using ros service SickLocColaTelegram +# Example: send_cola_telegram "{cola_ascii_request: 'sMN IsSystemReady', wait_response_timeout: 1}" "sAN IsSystemReady 1" +# +function send_cola_telegram() { + sleep 1 + echo -e "ros2 service call SickLocColaTelegram \"$1\"" + echo -e "expected answer: \"$2\"" + answer=$(ros2 service call SickLocColaTelegram sick_lidar_localization/srv/SickLocColaTelegramSrv "$1") + echo -e "$answer" + if [[ "$answer" == *"$2"* ]]; then + echo -e "OK: service responded with expected answer.\n" + else + testcase_error_counter=$((testcase_error_counter+1)) + echo -e "## ERROR: service did NOT respond with expected answer. Press any key to continue..." ; read -n1 -s key + fi + testcase_counter=$((testcase_counter+1)) +} + +# +# Function call_service "" "" "" "" just calls a ros service +# Example: call_service SickLocIsSystemReady sick_lidar_localization/srv/SickLocIsSystemReadySrv "{}" "success: True" # Check if the system is ready +# +function call_service() { + sleep 1 + echo -e "ros2 service call $1 $2 \"$3\"" + answer=$(ros2 service call $1 $2 "$3") # call rosservice + answer=$(echo $answer|tr -d '\n') # remove line feeds + echo -e "expected answer: \"$4\"" + echo -e "received answer: \"$answer\"" + if [[ "$answer" == *"$4"* ]]; then + echo -e "OK: service responded with expected answer.\n" + else + testcase_error_counter=$((testcase_error_counter+1)) + echo -e "## ERROR: service did NOT respond with expected answer. Press any key to continue..." ; read -n1 -s key + fi + testcase_counter=$((testcase_counter+1)) +} + +# +# Environment +# + +pushd ../../../.. +source /opt/ros/eloquent/setup.bash +source ./install/setup.bash +testcase_counter=0 +testcase_error_counter=0 + +# +# Toggle localization on/off and result telegrams on/off +# + +for ((n=0;n<1;n++)) ; do + # service servicename servicetype request expected response comment + call_service SickLocIsSystemReady sick_lidar_localization/srv/SickLocIsSystemReadySrv "{}" "success=True" # Check if the system is ready + call_service SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" "state=2, success=True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocStop sick_lidar_localization/srv/SickLocStopSrv "{}" "success=True" # Stop localization or demo mapping + call_service SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" "state=1, success=True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" "state=1, success=True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocStartLocalizing sick_lidar_localization/srv/SickLocStartLocalizingSrv "{}" "success=True" # Start localization + call_service SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" "state=2, success=True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" "state=2, success=True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocSetResultPoseEnabled sick_lidar_localization/srv/SickLocSetResultPoseEnabledSrv "{enabled: 0}" "success=True" # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 0" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + call_service SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" "state=2, success=True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocSetResultPoseEnabled sick_lidar_localization/srv/SickLocSetResultPoseEnabledSrv "{enabled: 1}" "success=True" # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 1" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + call_service SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" "state=2, success=True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING +done + +# +# Configure SIM by both Cola telegrams and ros services +# + +for ((n=0;n<1;n++)) ; do + + # Cola-ASCII States Telegrams + send_cola_telegram "{cola_ascii_request: 'sMN IsSystemReady', wait_response_timeout: 1}" "sAN IsSystemReady 1" # 0:true, 1:false # Check if the system is ready + send_cola_telegram "{cola_ascii_request: 'sRN LocState', wait_response_timeout: 1}" "sRA LocState 2" # 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING # Read localization state + send_cola_telegram "{cola_ascii_request: 'sMN LocStop', wait_response_timeout: 1}" "sAN LocStop 1" # 0:failed, 1:success # Stop localization or demo mapping + # send_cola_telegram "{cola_ascii_request: 'sMN LocStopAndSave', wait_response_timeout: 1}" "sAN LocStopAndSave 1" # Deprecated, 0:failed, 1:success # Stop localization, save settings + send_cola_telegram "{cola_ascii_request: 'sMN LocStartLocalizing', wait_response_timeout: 1}" "sAN LocStartLocalizing 1" # 0:failed, 1:success # Start localization + # Cola-ASCII Result Output Configuration Telegrams + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPort +2201', wait_response_timeout: 1}" "sAN LocSetResultPort 1 1" # 0:failed, 1:success, : uint16 (default: 2201) # Set TCP-port for result output + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultMode 0', wait_response_timeout: 1}" "sAN LocSetResultMode 1" # 0:failed, 1:success, : uint8 (0:stream, 1:poll, default: stream) # Set mode of result output (stream or poll) + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPoseEnabled 1', wait_response_timeout: 1}" "sAN LocSetResultPoseEnabled 1" # 0:failed, 1:success, : uint8 (0: disabled, 1: enabled, default: enabled) # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultEndianness 0', wait_response_timeout: 1}" "sAN LocSetResultEndianness 1" # 0:failed, 1:success, : uint8 (0: big endian, 1: little endian, default: big endian) # Set endianness of result output + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPoseInterval 1', wait_response_timeout: 1}" "sAN LocSetResultPoseInterval 1" # 0:failed, 1:success, : uint8 (0-255, interval in number of scans, 1: result with each processed scan, default: 1) # Set interval of result output + send_cola_telegram "{cola_ascii_request: 'sMN LocRequestResultData', wait_response_timeout: 1}" "sAN LocRequestResultData 1" # 0:failed, 1:success # If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. + # Cola-ASCII SetPose Telegrams + send_cola_telegram "{cola_ascii_request: 'sMN LocSetPose +10300 -5200 +30000 +1000', wait_response_timeout: 1}" "sAN LocSetPose 1" # 0:failed, 1:success, : int32 (x coordinate in mm), : int32 (y coordinate in mm), : int32 (yaw angle in millidegree, -180000 to +180000), : uint16 (translation uncertainty in mm) # Initialize vehicle pose + # Cola-ASCII Result Output Settings (queries, ros service SickLocColaTelegram only) + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 1" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 0" # 0:stream, 1: poll # Read result mode + send_cola_telegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" "sRA LocResultEndianness 0" # 0: big endian, 1: little endian # Read endianness of result output + # Cola-ASCII Timestamp Telegrams + send_cola_telegram "{cola_ascii_request: 'sMN LocRequestTimestamp', wait_response_timeout: 1}" "sAN LocRequestTimestamp" # "sAN LocRequestTimestamp " with 4 byte timestamp (ticks in milliseconds) # Query timestamp, see "Time synchronization" + + # Cola-ASCII Start/stop localization and query settings + send_cola_telegram "{cola_ascii_request: 'sMN LocStop', wait_response_timeout: 1}" "sAN LocStop 1" # 0:failed, 1:success # Stop localization or demo mapping + send_cola_telegram "{cola_ascii_request: 'sRN LocState', wait_response_timeout: 1}" "sRA LocState 1" # 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING # Read localization state + send_cola_telegram "{cola_ascii_request: 'sMN LocStartLocalizing', wait_response_timeout: 1}" "sAN LocStartLocalizing 1" # 0:failed, 1:success # Start localization + send_cola_telegram "{cola_ascii_request: 'sRN LocState', wait_response_timeout: 1}" "sRA LocState 2" # 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING # Read localization state + # Cola-ASCII Set tcp port for result telegram and query settings + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPort +2345', wait_response_timeout: 1}" "sAN LocSetResultPort 1 1" # 0:failed, 1:success, : uint16 (default: 2201) # Set TCP-port for result output + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPort +2201', wait_response_timeout: 1}" "sAN LocSetResultPort 1 1" # 0:failed, 1:success, : uint16 (default: 2201) # Set TCP-port for result output + # Cola-ASCII Set result mode and query settings + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultMode 1', wait_response_timeout: 1}" "sAN LocSetResultMode 1" # 0:failed, 1:success, : uint8 (0:stream, 1:poll, default: stream) # Set mode of result output (stream or poll) + send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 1" # 0:stream, 1: poll # Read result mode + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultMode 0', wait_response_timeout: 1}" "sAN LocSetResultMode 1" # 0:failed, 1:success, : uint8 (0:stream, 1:poll, default: stream) # Set mode of result output (stream or poll) + send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 0" # 0:stream, 1: poll # Read result mode + # Cola-ASCII Set result pose enabled/disabled and query settings + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPoseEnabled 0', wait_response_timeout: 1}" "sAN LocSetResultPoseEnabled 1" # 0:failed, 1:success, : uint8 (0: disabled, 1: enabled, default: enabled) # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 0" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultPoseEnabled 1', wait_response_timeout: 1}" "sAN LocSetResultPoseEnabled 1" # 0:failed, 1:success, : uint8 (0: disabled, 1: enabled, default: enabled) # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 1" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + # Cola-ASCII Set result endianness and query settings + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultEndianness 1', wait_response_timeout: 1}" "sAN LocSetResultEndianness 1" # 0:failed, 1:success, : uint8 (0: big endian, 1: little endian, default: big endian) # Set endianness of result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" "sRA LocResultEndianness 1" # 0:big endian, 1:little endian # Read endianness of result output + send_cola_telegram "{cola_ascii_request: 'sMN LocSetResultEndianness 0', wait_response_timeout: 1}" "sAN LocSetResultEndianness 1" # 0:failed, 1:success, : uint8 (0: big endian, 1: little endian, default: big endian) # Set endianness of result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" "sRA LocResultEndianness 0" # 0:big endian, 1:little endian # Read endianness of result output + + # ROS services for States Telegrams + call_service SickLocIsSystemReady sick_lidar_localization/srv/SickLocIsSystemReadySrv "{}" "success=True" # Check if the system is ready + call_service SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" "state=2, success=True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocStop sick_lidar_localization/srv/SickLocStopSrv "{}" "success=True" # Stop localization or demo mapping + # call_service SickLocStopAndSave sick_lidar_localization/srv/SickLocStopAndSaveSrv "{}" "success=True" # Deprecated, Stop localization, save settings + call_service SickLocStartLocalizing sick_lidar_localization/srv/SickLocStartLocalizingSrv "{}" "success=True" # Start localization + # ROS services for Result Output Configuration Telegrams + call_service SickLocSetResultPort sick_lidar_localization/srv/SickLocSetResultPortSrv "{port: 2201}" "success=True" # Set TCP-port for result output + call_service SickLocSetResultMode sick_lidar_localization/srv/SickLocSetResultModeSrv "{mode: 0}" "success=True" # Set mode of result output (stream or poll) + call_service SickLocSetResultPoseEnabled sick_lidar_localization/srv/SickLocSetResultPoseEnabledSrv "{enabled: 1}" "success=True" # Disable/enable result output + call_service SickLocSetResultEndianness sick_lidar_localization/srv/SickLocSetResultEndiannessSrv "{endianness: 0}" "success=True" # Set endianness of result output + call_service SickLocSetResultPoseInterval sick_lidar_localization/srv/SickLocSetResultPoseIntervalSrv "{interval: 1}" "success=True" # Set interval of result output + call_service SickLocRequestResultData sick_lidar_localization/srv/SickLocRequestResultDataSrv "{}" "success=True" # If in poll mode, trigger sending the localization result of the next processed scan via TCP interface. + # ROS services for SetPose Telegrams + call_service SickLocSetPose sick_lidar_localization/srv/SickLocSetPoseSrv "{posex: 10300, posey: -5200, yaw: 30000, uncertainty: 1000}" "success=True" # Initialize vehicle pose + # ROS services for Timestamp Telegrams + call_service SickLocRequestTimestamp sick_lidar_localization/srv/SickLocRequestTimestampSrv "{}" "timestamp_lidar_ms" # "timestamp_lidar_ms: mean_time_vehicle_ms: delta_time_ms: ..." # Query timestamp, see "Time synchronization" + + # Change settings with ROS services for States Telegrams + call_service SickLocIsSystemReady sick_lidar_localization/srv/SickLocIsSystemReadySrv "{}" "success=True" # Check if the system is ready + call_service SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" "state=2, success=True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocStop sick_lidar_localization/srv/SickLocStopSrv "{}" "success=True" # Stop localization or demo mapping + call_service SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" "state=1, success=True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + call_service SickLocStartLocalizing sick_lidar_localization/srv/SickLocStartLocalizingSrv "{}" "success=True" # Start localization + call_service SickLocState sick_lidar_localization/srv/SickLocStateSrv "{}" "state=2, success=True" # Read localization state: 0:BOOTING, 1:IDLE, 2:LOCALIZING, 3:DEMO_MAPPING + # Change settings with ROS services for Result Output Configuration Telegrams + call_service SickLocSetResultPort sick_lidar_localization/srv/SickLocSetResultPortSrv "{port: 2345}" "success=True" # Set TCP-port for result output + call_service SickLocSetResultPort sick_lidar_localization/srv/SickLocSetResultPortSrv "{port: 2201}" "success=True" # Set TCP-port for result output + call_service SickLocSetResultMode sick_lidar_localization/srv/SickLocSetResultModeSrv "{mode: 1}" "success=True" # Set mode of result output (stream or poll) + send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 1" # 0:stream, 1:poll # Read result mode + call_service SickLocSetResultMode sick_lidar_localization/srv/SickLocSetResultModeSrv "{mode: 0}" "success=True" # Set mode of result output (stream or poll) + send_cola_telegram "{cola_ascii_request: 'sRN LocResultMode', wait_response_timeout: 1}" "sRA LocResultMode 0" # 0:stream, 1:poll # Read result mode + call_service SickLocSetResultPoseEnabled sick_lidar_localization/srv/SickLocSetResultPoseEnabledSrv "{enabled: 0}" "success=True" # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 0" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + call_service SickLocSetResultPoseEnabled sick_lidar_localization/srv/SickLocSetResultPoseEnabledSrv "{enabled: 1}" "success=True" # Disable/enable result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultState', wait_response_timeout: 1}" "sRA LocResultState 1" # 0:disabled, 1:enabled, MSB: error flag # Read state of the result output + call_service SickLocSetResultEndianness sick_lidar_localization/srv/SickLocSetResultEndiannessSrv "{endianness: 1}" "success=True" # Set endianness of result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" "sRA LocResultEndianness 1" # 0:big endian, 1:little endian # Read endianness of result output + call_service SickLocSetResultEndianness sick_lidar_localization/srv/SickLocSetResultEndiannessSrv "{endianness: 0}" "success=True" # Set endianness of result output + send_cola_telegram "{cola_ascii_request: 'sRN LocResultEndianness', wait_response_timeout: 1}" "sRA LocResultEndianness 0" # 0:big endian, 1:little endian # Read endianness of result output + +done +echo -e "send_cola_examples.bash finished.\n" +echo -e "Services and cola telegram verification summary: $testcase_counter testcases, $testcase_error_counter failures.\n" + diff --git a/test/ros2_scripts/vs_code.bash b/test/ros2_scripts/vs_code.bash new file mode 100755 index 0000000..b23e90f --- /dev/null +++ b/test/ros2_scripts/vs_code.bash @@ -0,0 +1,16 @@ +#!/bin/bash + +# +# Start Visual Studio Code +# + +echo -e "\nvs_code.bash: Starting visual studio code." +echo -e "set BUILDTYPE=Debug in make.bash for debugging and development." +echo -e "set BUILDTYPE=Release in make.bash for profiling and benchmarks.\n" +gedit ./make.bash ./run_demo_simu.bash ./run_simu.bash ./run_error_simu.bash ./run_cola_examples.bash ./run_odom_simu.bash & + +pushd ../../../.. +source /opt/ros/eloquent/setup.bash +source ./install/setup.bash +code ./lidar_localization_vscode.code-workspace +popd diff --git a/test/scripts/make.bash b/test/scripts/make.bash deleted file mode 100755 index 761b3f7..0000000 --- a/test/scripts/make.bash +++ /dev/null @@ -1,55 +0,0 @@ -#!/bin/bash - -# -# Check/set/cleanup environment -# - -pushd ../../../.. -rm -f build/catkin_make_install.log -source /opt/ros/melodic/setup.bash -rm -f ./devel/lib/*sick* 2> /dev/null -rm -f ./devel/lib/sick_lidar_localization/*sick* 2> /dev/null -rm -f ./install/lib/*sick* 2> /dev/null -rm -f ./install/lib/sick_lidar_localization/*sick* 2> /dev/null -# if [ ! -d ./src/sick_lidar_localization ] && [ -d ./src/sick_lidar_localization_pretest ] ; then -# pushd ./src # Note: development repo is https://github.com/michael1309/sick_lidar_localization_pretest.git, relase repo is https://github.com/SICKAG/sick_lidar_localization.git -# ln -s ./sick_lidar_localization_pretest ./sick_lidar_localization -# popd -# fi -# if [ ! -d ./src/sick_lidar_localization ] ; then -# echo -e "\n## ERROR make.bash: directory ./src/sick_lidar_localization not found.\n" -# exit -# fi - -# -# Build and install sick_lidar_localization binaries. -# - -catkin_make 2>&1 | tee -a build/catkin_make_install.log -catkin_make install 2>&1 | tee -a build/catkin_make_install.log -catkin_make RunDoxygen 2>&1 | tee -a build/catkin_make_install.log -source ./install/setup.bash - -# lint, install by running -catkin_lint -W1 ./src/sick_lidar_localization -if [ -d ./src/sick_lidar_localization ] ; then catkin_lint -W1 ./src/sick_lidar_localization ; fi -if [ -d ./src/sick_lidar_localization_pretest ] ; then catkin_lint -W1 ./src/sick_lidar_localization_pretest ; fi - -# print warnings and errors -echo -e "\nmake.bash finished.\n" -echo -e "catkin_make warnings:" -cat build/catkin_make_install.log | grep -i "warning:" -echo -e "\ncatkin_make errors:" -cat build/catkin_make_install.log | grep -i "error:" - -# print sick_lidar_localization install files, libraries, executables -echo -e "\ninstall/lib/sick_lidar_localization:" -ls -al install/lib/sick_lidar_localization/* -echo -e "\ninstall/share/sick_lidar_localization:" -ls install/share/sick_lidar_localization/*.* -echo -e "\ninstall/lib/sick_lidar_localization:" -ls -al install/lib/sick_lidar_localization/* -echo -e "\ninstall/share/sick_lidar_localization:" -ls install/share/sick_lidar_localization/*.* -popd - diff --git a/test/src/test_server.cpp b/test/src/test_server.cpp index 4a09442..ce7b387 100644 --- a/test/src/test_server.cpp +++ b/test/src/test_server.cpp @@ -57,7 +57,7 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include #include @@ -67,27 +67,33 @@ int main(int argc, char** argv) { // Ros configuration and initialization - ros::init(argc, argv, "sim_loc_test_server"); - ros::NodeHandle nh; + ROS::init(argc, argv, "sim_loc_test_server"); + ROS::NodePtr nh = ROS::createNode("sim_loc_test_server"); ROS_INFO_STREAM("sim_loc_test_server started."); // Create a server to simulate a localization controller, incl. a listener thread to accept tcp connections int tcp_port_results = 2201; // Default: The localization controller uses IP port number 2201 to send localization results int tcp_port_cola = 2111; // For requests and to transmit settings to the localization controller: IP port number 2111 and 2112 to send telegrams and to request data, SOPAS CoLa-A or CoLa-B protocols - ros::param::param("/sick_lidar_localization/test_server/result_telegrams_tcp_port", tcp_port_results, tcp_port_results); - ros::param::param("/sick_lidar_localization/test_server/cola_telegrams_tcp_port", tcp_port_cola, tcp_port_cola); - sick_lidar_localization::TestServerThread test_server_thread(&nh, tcp_port_results, tcp_port_cola); + ROS::param(nh, "/sick_lidar_localization/test_server/result_telegrams_tcp_port", tcp_port_results, tcp_port_results); + ROS::param(nh, "/sick_lidar_localization/test_server/cola_telegrams_tcp_port", tcp_port_cola, tcp_port_cola); + sick_lidar_localization::TestServerThread test_server_thread(nh, tcp_port_results, tcp_port_cola); // Subscribe to sim_loc_driver messages to monitor sim_loc_driver in error simulation mode std::string result_telegrams_topic = "/sick_lidar_localization/driver/result_telegrams"; // default topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) - ros::param::param("/sick_lidar_localization/driver/result_telegrams_topic", result_telegrams_topic, result_telegrams_topic); - ros::Subscriber result_telegram_subscriber = nh.subscribe(result_telegrams_topic, 1, &sick_lidar_localization::TestServerThread::messageCbResultPortTelegrams, &test_server_thread); + ROS::param(nh, "/sick_lidar_localization/driver/result_telegrams_topic", result_telegrams_topic, result_telegrams_topic); +#if defined __ROS_VERSION && __ROS_VERSION == 1 + sick_lidar_localization::SickLocResultPortTelegramMsgSubscriber result_telegram_subscriber + = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::SickLocResultPortTelegramMsg, result_telegrams_topic, &sick_lidar_localization::TestServerThread::messageCbResultPortTelegrams, &test_server_thread); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + sick_lidar_localization::SickLocResultPortTelegramMsgSubscriber result_telegram_subscriber + = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::SickLocResultPortTelegramMsg, result_telegrams_topic, &sick_lidar_localization::TestServerThread::messageCbResultPortTelegramsROS2, &test_server_thread); +#endif // Start simulation of a localization controller test_server_thread.start(); // Run ros event loop - ros::spin(); + ROS::spin(nh); // Cleanup and exit std::cout << "sim_loc_test_server finished." << std::endl; @@ -95,5 +101,6 @@ int main(int argc, char** argv) test_server_thread.stop(); std::cout << "sim_loc_test_server exits." << std::endl; ROS_INFO_STREAM("sim_loc_test_server exits."); + ROS::deleteNode(nh); return 0; } diff --git a/test/src/test_server_thread.cpp b/test/src/test_server_thread.cpp index 359818c..81e4458 100644 --- a/test/src/test_server_thread.cpp +++ b/test/src/test_server_thread.cpp @@ -55,7 +55,7 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/cola_parser.h" #include "sick_lidar_localization/cola_transmitter.h" @@ -70,7 +70,7 @@ * @param[in] ip_port_results ip port for result telegrams, default: 2201 * @param[in] ip_port_cola ip port for command requests and responses, default: 2111 */ -sick_lidar_localization::TestServerThread::TestServerThread(ros::NodeHandle* nh, int ip_port_results, int ip_port_cola) +sick_lidar_localization::TestServerThread::TestServerThread(ROS::NodePtr nh, int ip_port_results, int ip_port_cola) : m_ip_port_results(ip_port_results), m_ip_port_cola(ip_port_cola), m_ioservice(), m_tcp_connection_thread_results(0), m_tcp_connection_thread_cola(0), m_tcp_connection_thread_running(false), m_worker_thread_running(false), @@ -84,13 +84,20 @@ sick_lidar_localization::TestServerThread::TestServerThread(ros::NodeHandle* nh, if(nh) { std::string result_testcases_topic = "/sick_lidar_localization/test_server/result_testcases"; // default topic to publish testcases with result port telegrams (type SickLocResultPortTestcaseMsg) - ros::param::param("/sick_lidar_localization/test_server/result_telegrams_rate", m_result_telegram_rate, m_result_telegram_rate); - ros::param::param("/sick_lidar_localization/test_server/result_testcases_topic", result_testcases_topic, result_testcases_topic); - ros::param::param("/sick_lidar_localization/test_server/result_testcases_frame_id", m_result_testcases_frame_id, "result_testcases"); - ros::param::param("/sim_loc_test_server/demo_circles", m_demo_move_in_circles, m_demo_move_in_circles); - ros::param::param("/sim_loc_test_server/error_simulation", m_error_simulation_enabled, m_error_simulation_enabled); + ROS::param(nh, "/sick_lidar_localization/test_server/result_telegrams_rate", m_result_telegram_rate, m_result_telegram_rate); + ROS::param(nh, "/sick_lidar_localization/test_server/result_testcases_topic", result_testcases_topic, result_testcases_topic); + ROS::param(nh, "/sick_lidar_localization/test_server/result_testcases_frame_id", m_result_testcases_frame_id, "result_testcases"); + ROS::param(nh, "/sim_loc_test_server/demo_circles", m_demo_move_in_circles, m_demo_move_in_circles); + ROS::param(nh, "/sim_loc_test_server/error_simulation", m_error_simulation_enabled, m_error_simulation_enabled); + std::string sim_loc_test_server_demo_circles, sim_loc_test_server_error_simulation; + ROS::param(nh, "/system/test_server/demo_circles", sim_loc_test_server_demo_circles, sim_loc_test_server_demo_circles); + ROS::param(nh, "/system/test_server/error_simulation", sim_loc_test_server_error_simulation, sim_loc_test_server_error_simulation); + if(!sim_loc_test_server_demo_circles.empty()) + m_demo_move_in_circles = std::atoi(sim_loc_test_server_demo_circles.c_str()) > 0; + if(!sim_loc_test_server_error_simulation.empty()) + m_error_simulation_enabled = std::atoi(sim_loc_test_server_error_simulation.c_str()) > 0; // ros publisher for testcases with result port telegrams (type SickLocResultPortTestcaseMsg) - m_result_testcases_publisher = nh->advertise(result_testcases_topic, 1); + m_result_testcases_publisher = ROS_CREATE_PUBLISHER(nh, sick_lidar_localization::SickLocResultPortTestcaseMsg, result_testcases_topic); } } @@ -116,10 +123,13 @@ bool sick_lidar_localization::TestServerThread::start(void) m_error_simulation_thread_running = true; m_error_simulation_thread = new boost::thread(&sick_lidar_localization::TestServerThread::runErrorSimulationThreadCb, this); } + else if(m_demo_move_in_circles) + { + ROS_INFO_STREAM("TestServerThread: running in demo mode (moving in circles, no error simulation)."); + } else { - ROS_INFO_STREAM("TestServerThread: running in normal mode, no error simulation."); - + ROS_INFO_STREAM("TestServerThread: running in normal mode (no error simulation)."); } // Start and run 3 threads to create sockets for new tcp clients on ip ports 2201 (results), 2111 (requests) and 2112 (responses) m_tcp_connection_thread_running = true; @@ -282,11 +292,11 @@ void sick_lidar_localization::TestServerThread::runConnectionThreadColaCb(void) templatevoid sick_lidar_localization::TestServerThread::runConnectionThreadGenericCb(boost::asio::ip::tcp::acceptor & tcp_acceptor_results, int ip_port_results, Callable thread_function_cb) { ROS_INFO_STREAM("TestServerThread: connection thread started"); - while(ros::ok() && m_tcp_connection_thread_running && tcp_acceptor_results.is_open()) + while(ROS::ok() && m_tcp_connection_thread_running && tcp_acceptor_results.is_open()) { if(m_error_simulation_flag.get() == DONT_LISTEN) // error simulation: testserver does not open listening port { - ros::Duration(0.1).sleep(); + ROS::sleep(0.1); continue; } // normal mode: listen to tcp port, accept and connect to new tcp clients @@ -297,7 +307,7 @@ templatevoid sick_lidar_localization::TestServerThread::runCo tcp_acceptor_results.listen(); if(m_error_simulation_flag.get() == DONT_LISTEN || m_error_simulation_flag.get() == DONT_ACCECPT) // error simulation: testserver does not does not accecpt tcp clients { - ros::Duration(0.1).sleep(); + ROS::sleep(0.1); continue; } tcp_acceptor_results.accept(*tcp_client_socket, errorcode); // normal mode: accept new tcp client @@ -338,10 +348,9 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadResultCb(boost::a sick_lidar_localization::UniformRandomInteger random_length(1, 512); sick_lidar_localization::UniformRandomInteger random_integer(0, INT_MAX); double circle_yaw = 0; - while(ros::ok() && m_worker_thread_running && p_socket && p_socket->is_open()) + while(ROS::ok() && m_worker_thread_running && p_socket && p_socket->is_open()) { - ros::Duration send_telegrams_delay((double)sick_lidar_localization::TestcaseGenerator::ResultPoseInterval() / m_result_telegram_rate); - send_telegrams_delay.sleep(); + ROS::sleep((double)sick_lidar_localization::TestcaseGenerator::ResultPoseInterval() / m_result_telegram_rate); boost::system::error_code error_code; if (m_error_simulation_flag.get() == DONT_SEND) // error simulation: testserver does not send any telegrams { @@ -392,9 +401,9 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadResultCb(boost::a ROS_DEBUG_STREAM("TestServerThread for result telegrams: send binary result port telegram " << sick_lidar_localization::Utils::toHexString(testcase.binary_data)); } // publish testcases (SickLocResultPortTestcaseMsg, i.e. binary telegram and SickLocResultPortTelegramMsg messages) for test and verification of sim_loc_driver - testcase.header.stamp = ros::Time::now(); + testcase.header.stamp = ROS::now(); testcase.header.frame_id = m_result_testcases_frame_id; - m_result_testcases_publisher.publish(testcase); + ROS_PUBLISH(m_result_testcases_publisher, testcase); } } closeSocket(p_socket); @@ -413,11 +422,11 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadColaCb(boost::asi sick_lidar_localization::UniformRandomInteger random_generator(0,255); sick_lidar_localization::UniformRandomInteger random_length(1, 128); sick_lidar_localization::UniformRandomAsciiString random_ascii; - while(ros::ok() && m_worker_thread_running && p_socket && p_socket->is_open()) + while(ROS::ok() && m_worker_thread_running && p_socket && p_socket->is_open()) { // Read command request from tcp client ServerColaRequest request; - ros::Time receive_timestamp; + ROS::Time receive_timestamp; if(sick_lidar_localization::ColaTransmitter::receive(*p_socket, request.telegram_data, 1, receive_timestamp)) { if (m_error_simulation_flag.get() == DONT_SEND) // error simulation: testserver does not send any telegrams @@ -456,28 +465,28 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadColaCb(boost::asi if(cola_binary) binary_response = sick_lidar_localization::ColaAsciiBinaryConverter::ColaAsciiToColaBinary(binary_response); ROS_INFO_STREAM("TestServerThread: sending cola response " << ascii_response << (cola_binary ? " (Cola-Binary)" : " (Cola-ASCII)")); - ros::Time send_timestamp; + ROS::Time send_timestamp; if (!sick_lidar_localization::ColaTransmitter::send(*p_socket, binary_response, send_timestamp)) { ROS_WARN_STREAM("TestServerThread: failed to send cola response, ColaTransmitter::send() returned false, data hexdump: " << sick_lidar_localization::Utils::toHexString(binary_response)); } } - ros::Duration(0.0001).sleep(); + ROS::sleep(0.0001); } closeSocket(p_socket); ROS_INFO_STREAM("TestServerThread: worker thread for command requests finished"); } /*! - * Waits for a given time in seconds, as long as ros::ok() and m_error_simulation_thread_running == true. + * Waits for a given time in seconds, as long as ROS::ok() and m_error_simulation_thread_running == true. * @param[in] seconds delay in seconds */ void sick_lidar_localization::TestServerThread::errorSimulationWait(double seconds) { - ros::Time starttime = ros::Time::now(); - while(ros::ok() && m_error_simulation_thread_running && (ros::Time::now() - starttime).toSec() < seconds) + ROS::Time starttime = ROS::now(); + while(ROS::ok() && m_error_simulation_thread_running && ROS::seconds(ROS::now() - starttime) < seconds) { - ros::Duration(0.001).sleep(); + ROS::sleep(0.001); } } @@ -489,15 +498,15 @@ void sick_lidar_localization::TestServerThread::errorSimulationWait(double secon */ bool sick_lidar_localization::TestServerThread::errorSimulationWaitForTelegramReceived(double timeout_seconds, sick_lidar_localization::SickLocResultPortTelegramMsg & telegram_msg) { - ros::Time starttime = ros::Time::now(); - while(ros::ok() && m_error_simulation_thread_running) + ROS::Time starttime = ROS::now(); + while(ROS::ok() && m_error_simulation_thread_running) { telegram_msg = m_last_telegram_received.get(); - if(telegram_msg.header.stamp > starttime) + if(ROS::timeFromHeader(&telegram_msg.header) > starttime) return true; // new telegram received - if((ros::Time::now() - starttime).toSec() >= timeout_seconds) + if(ROS::seconds(ROS::now() - starttime) >= timeout_seconds) return false; // timeout - ros::Duration(0.001).sleep(); + ROS::sleep(0.001); } return false; } @@ -529,7 +538,7 @@ void sick_lidar_localization::TestServerThread::runErrorSimulationThreadCb(void) m_error_simulation_flag.set(DONT_LISTEN); ROS_INFO_STREAM("TestServerThread: 2. error simulation testcase: server not responding, not listening, no tcp connections accepted."); m_worker_thread_running = false; - ros::Duration(1.0 / m_result_telegram_rate).sleep(); + ROS::sleep(1.0 / m_result_telegram_rate); closeTcpConnections(true); errorSimulationWait(10); m_error_simulation_flag.set(NO_ERROR); @@ -548,7 +557,7 @@ void sick_lidar_localization::TestServerThread::runErrorSimulationThreadCb(void) m_error_simulation_flag.set(DONT_ACCECPT); ROS_INFO_STREAM("TestServerThread: 3. error simulation testcase: server not responding, listening on port " << m_ip_port_results << ", but accepting no tcp clients"); m_worker_thread_running = false; - ros::Duration(1.0 / m_result_telegram_rate).sleep(); + ROS::sleep(1.0 / m_result_telegram_rate); closeTcpConnections(true); errorSimulationWait(10); m_error_simulation_flag.set(NO_ERROR); @@ -611,5 +620,5 @@ void sick_lidar_localization::TestServerThread::runErrorSimulationThreadCb(void) ROS_INFO_STREAM("TestServerThread: error simulation thread finished"); ROS_INFO_STREAM("TestServerThread: error simulation summary: " << (number_testcases - number_testcases_failed) << " of " << number_testcases<< " testcases passed, " << number_testcases_failed << " failures."); ROS_INFO_STREAM("TestServerThread: exit with ros::shutdown()"); - ros::shutdown(); + ROS::shutdown(); } diff --git a/test/src/unittest_sim_loc_parser.cpp b/test/src/unittest_sim_loc_parser.cpp index 1f7f128..3bd4d4b 100644 --- a/test/src/unittest_sim_loc_parser.cpp +++ b/test/src/unittest_sim_loc_parser.cpp @@ -53,10 +53,11 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include #include +#include "sick_lidar_localization/cola_encoder.h" #include "sick_lidar_localization/cola_parser.h" #include "sick_lidar_localization/random_generator.h" #include "sick_lidar_localization/testcase_generator.h" @@ -81,17 +82,307 @@ std::vector cola_ascii_reference = // "sMN SetAccessMode 3 F472474 std::vector cola_binary_reference = // Example from Technical_information_Telegram_Listing_Ranging_sensors_....pdf { 0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x17, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x03, 0xF4, 0x72, 0x47, 0x44, 0xB3 }; +// Run sim_loc_parser unittest for encoding new services to Cola Ascii telegrams. Unittest for new services in release 4 and later +// Testcases taken from examples listed in manual 8024818_TI_LiDAR_Localization_Telegram_Listing_en_9317743_2020-09-11.pdf +static void RunColaEncoderUnitTests(int & testcase_cnt, int & failed_testcase_cnt) +{ + sick_lidar_localization::SickDevSetLidarConfigSrv::Request service_request_01; + sick_lidar_localization::SickLocSetMapSrv::Request service_request_03; + sick_lidar_localization::SickLocMapSrv::Request service_request_04; + sick_lidar_localization::SickLocMapStateSrv::Request service_request_05; + sick_lidar_localization::SickLocInitializePoseSrv::Request service_request_06; + sick_lidar_localization::SickLocSetReflectorsForSupportActiveSrv::Request service_request_12; + sick_lidar_localization::SickLocSetOdometryActiveSrv::Request service_request_14; + sick_lidar_localization::SickLocSetOdometryPortSrv::Request service_request_16; + sick_lidar_localization::SickLocSetOdometryRestrictYMotionSrv::Request service_request_18; + sick_lidar_localization::SickLocSetAutoStartActiveSrv::Request service_request_20; + sick_lidar_localization::SickLocSetAutoStartSavePoseIntervalSrv::Request service_request_22; + sick_lidar_localization::SickLocSetRingBufferRecordingActiveSrv::Request service_request_24; + sick_lidar_localization::SickDevGetLidarStateSrv::Request service_request_27; + sick_lidar_localization::SickGetSoftwareVersionSrv::Request service_request_28; + sick_lidar_localization::SickLocAutoStartSavePoseSrv::Request service_request_29; + sick_lidar_localization::SickLocForceUpdateSrv::Request service_request_30; + sick_lidar_localization::SickLocSaveRingBufferRecordingSrv::Request service_request_31; + sick_lidar_localization::SickSavePermanentSrv::Request service_request_34; + sick_lidar_localization::SickLocResultPortSrv::Request service_request_35; + sick_lidar_localization::SickLocResultModeSrv::Request service_request_36; + sick_lidar_localization::SickLocResultEndiannessSrv::Request service_request_37; + sick_lidar_localization::SickLocResultStateSrv::Request service_request_38; + sick_lidar_localization::SickDevSetIMUActiveSrv::Request service_request_40; + std::string cola_ascii; + + service_request_01.index = 0; // Index of the lidar that shall be configured. [0, 1] + service_request_01.minrange = 300; // Beams with a range lower than this distance will be discarded. [, 250000] in [mm] + service_request_01.maxrange = 100000; // Beams with a range greater than this distance will be discarded. [, 250000] in [mm] + service_request_01.minangle = -180000; // Beams with an angle lower than this threshold will be discarded. [-180000, 180000] in [mdeg] + service_request_01.maxangle = 180000; // Beams with an angle greater than this threshold will be discarded. [-180000, 180000] in [mdeg] + service_request_01.x = 1000; // X position relative to vehicle coordinate system. [-50000, 50000] in [mm] + service_request_01.y = -300; // Y position relative to vehicle coordinate system. [-50000, 50000] in [mm] + service_request_01.yaw = -45000; // Yaw angle relative to vehicle coordinate system. [-180000, 180000] in [mdeg] + service_request_01.upsidedown = 0; // Whether the sensor is mounted upside down. {0 (false), 1 (true)} + service_request_01.ip = "192.168.0.30"; // IP address of the sensor. Must be in the same subnet as the port which it is connected to. Max length = 15 + service_request_01.port = 2122; // COLA Port of the scanner used for communication. This is usually 2111 or 2122. [0, 65535] + service_request_01.interfacetype = 0; // The type of the interface to this lidar. {0 (TCP), 1 (SERIAL)} + service_request_01.maplayer = 0; // The index of the map layer on which this LiDAR operates. If no map layers are used, set it to 0. NOTE: This feature is not implemented yet but is reserved for later releases. [, ] + service_request_01.active = 1; // Whether this scanner shall be used for localization. For the LiDAR with the index 0 this should be always 1 (true). {0 (false), 1 (true)} + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_01)) != "sMN DevSetLidarConfig +0 +300 +100000 -180000 +180000 +1000 -300 -45000 0 +12 192.168.0.30 +2122 0 0 1") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN DevSetLidarConfig +0 +300 +100000 -180000 +180000 +1000 -300 -45000 0 +12 192.168.0.30 +2122 0 0 1, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_03.mapfilename = "our_hall.smap"; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_03)) != "sMN LocSetMap +13 our_hall.smap") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocSetMap +13 our_hall.smap, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_04)) != "sRN LocMap") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sRN LocMap, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_05)) != "sRN LocMapState") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sRN LocMapState, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_06.x = 10300; + service_request_06.y = -5200; + service_request_06.yaw = 30000; + service_request_06.sigmatranslation = 1000; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_06)) != "sMN LocInitializePose +10300 -5200 +30000 +1000") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocInitializePose +10300 -5200 +30000 +1000, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_12.active = true; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_12)) != "sMN LocSetReflectorsForSupportActive 1") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocSetReflectorsForSupportActive 1, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_14.active = true; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_14)) != "sMN LocSetOdometryActive 1") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocSetOdometryActive 1, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_16.port = 3000; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_16)) != "sMN LocSetOdometryPort +3000") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocSetOdometryPort +3000, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_18.active = true; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_18)) != "sMN LocSetOdometryRestrictYMotion 1") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocSetOdometryRestrictYMotion 1, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_20.active = true; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_20)) != "sMN LocSetAutoStartActive 1") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocSetAutoStartActive 1, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_22.interval = 3; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_22)) != "sMN LocSetAutoStartSavePoseInterval 3") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocSetAutoStartSavePoseInterval , returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_24.active = true; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_24)) != "sMN LocSetRingBufferRecordingActive 1") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocSetRingBufferRecordingActive 1, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_27.index = 0; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_27)) != "sMN DevGetLidarState 0") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN DevGetLidarState 0, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_28)) != "sMN GetSoftwareVersion") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN GetSoftwareVersion, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_29)) != "sMN LocAutoStartSavePose") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocAutoStartSavePose, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_30)) != "sMN LocForceUpdate") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocForceUpdate, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_31.reason = "YYYY-MM-DD_HH-MM-SS pose quality low"; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_31)) != "sMN LocSaveRingBufferRecording +36 YYYY-MM-DD_HH-MM-SS pose quality low") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN LocSaveRingBufferRecording +36 YYYY-MM-DD_HH-MM-SS pose quality low, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_34)) != "sMN SavePermanent") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN SavePermanent, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_35)) != "sRN LocResultPort") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sRN LocResultPort, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_36)) != "sRN LocResultMode") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sRN LocResultMode, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_37)) != "sRN LocResultEndianness") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sRN LocResultEndianness, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_38)) != "sRN LocResultState") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sRN LocResultState, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; + service_request_40.active = true; + if ((cola_ascii = sick_lidar_localization::ColaEncoder::encodeServiceRequest(service_request_40)) != "sMN DevSetIMUActive 1") + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest() failed, expected sMN DevSetIMUActive 1, returned \"" << cola_ascii << "\""); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: ColaEncoder::encodeServiceRequest testcase passed, returned \"" << cola_ascii << "\""); + } + testcase_cnt++; +} + int main(int argc, char** argv) { // Ros configuration and initialization - ros::init(argc, argv, "unittest_sim_loc_parser"); - ros::NodeHandle nh; + ROS::init(argc, argv, "unittest_sim_loc_parser"); + ROS::NodePtr nh = ROS::createNode("unittest_sim_loc_parser"); ROS_INFO_STREAM("unittest_sim_loc_parser started."); // Run sim_loc_parser unittest for result port telegrams int testcase_cnt = 0, failed_testcase_cnt = 0, number_result_port_testcases = 100; // default: run 100 random based result port testcases - ros::param::param("/unittest_sim_loc_parser/number_result_port_testcases", number_result_port_testcases, number_result_port_testcases); + ROS::param(nh, "/unittest_sim_loc_parser/number_result_port_testcases", number_result_port_testcases, number_result_port_testcases); sick_lidar_localization::ResultPortParser result_port_parser("sick_lidar_localization"); sick_lidar_localization::SickLocResultPortTestcaseMsg testcase = sick_lidar_localization::TestcaseGenerator::createDefaultResultPortTestcase(); // initial testcase is the default testcase for(; testcase_cnt < number_result_port_testcases; testcase_cnt++) @@ -265,6 +556,16 @@ int main(int argc, char** argv) testcase_cnt++; } + // Run sim_loc_parser unittest for encoding new services to Cola Ascii telegrams. Unittest for new services in release 4 and later + // Testcases taken from examples listed in manual 8024818_TI_LiDAR_Localization_Telegram_Listing_en_9317743_2020-09-11.pdf + RunColaEncoderUnitTests(testcase_cnt, failed_testcase_cnt); + ROS_INFO_STREAM("unittest_sim_loc_parser finished, " << (testcase_cnt - failed_testcase_cnt) << " of " << testcase_cnt << " testcases passed, " << failed_testcase_cnt << " testcases failed."); + if(failed_testcase_cnt > 0) + { + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser finished, " << (testcase_cnt - failed_testcase_cnt) << " of " << testcase_cnt << " testcases passed, " << failed_testcase_cnt << " testcases failed."); + ROS::spin(nh); + } + ROS::deleteNode(nh); return 0; } diff --git a/test/src/verifier_thread.cpp b/test/src/verifier_thread.cpp index 07a4acc..b8216f4 100644 --- a/test/src/verifier_thread.cpp +++ b/test/src/verifier_thread.cpp @@ -63,7 +63,7 @@ * Copyright 2019 Ing.-Buero Dr. Michael Lehning * */ -#include +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/utils.h" #include "sick_lidar_localization/verifier_thread.h" @@ -71,10 +71,10 @@ /* * Constructor */ -sick_lidar_localization::VerifierThread::VerifierThread() +sick_lidar_localization::VerifierThread::VerifierThread(ROS::NodePtr nh) : m_verification_thread_running(false), m_verification_thread(0), m_result_telegram_rate(10) { - ros::param::param("/sick_lidar_localization/test_server/result_telegrams_rate", m_result_telegram_rate, m_result_telegram_rate); + ROS::param(nh, "/sick_lidar_localization/test_server/result_telegrams_rate", m_result_telegram_rate, m_result_telegram_rate); } /* @@ -139,7 +139,7 @@ class TelegramCounterCondition : public sick_lidar_localization::FifoBuffer +#include "sick_lidar_localization/ros_wrapper.h" #include "sick_lidar_localization/verifier_thread.h" int main(int argc, char** argv) { // Ros configuration and initialization - ros::init(argc, argv, "verify_sim_loc_driver"); - ros::NodeHandle nh; + ROS::init(argc, argv, "verify_sim_loc_driver"); + ROS::NodePtr nh = ROS::createNode("verify_sim_loc_driver"); ROS_INFO_STREAM("verify_sim_loc_driver started."); std::string result_telegrams_topic = "/sick_lidar_localization/driver/result_telegrams"; // default topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) std::string result_testcases_topic = "/sick_lidar_localization/test_server/result_testcases"; // default topic to publish testcases with result port telegrams (type SickLocResultPortTestcaseMsg) - ros::param::param("/sick_lidar_localization/driver/result_telegrams_topic", result_telegrams_topic, result_telegrams_topic); - ros::param::param("/sick_lidar_localization/test_server/result_testcases_topic", result_testcases_topic, result_testcases_topic); + ROS::param(nh, "/sick_lidar_localization/driver/result_telegrams_topic", result_telegrams_topic, result_telegrams_topic); + ROS::param(nh, "/sick_lidar_localization/test_server/result_testcases_topic", result_testcases_topic, result_testcases_topic); // Init verifier to compare and check sim_loc_driver and sim_loc_test_server messages - sick_lidar_localization::VerifierThread verifier; - - // Subscribe to sim_loc_driver messages - ros::Subscriber result_telegram_subscriber = nh.subscribe(result_telegrams_topic, 1, &sick_lidar_localization::VerifierThread::messageCbResultPortTelegrams, &verifier); - - // Subscribe to sim_loc_test_server messages - ros::Subscriber testcase_subscriber = nh.subscribe(result_testcases_topic, 1, &sick_lidar_localization::VerifierThread::messageCbResultPortTestcases, &verifier); + sick_lidar_localization::VerifierThread verifier(nh); + // Subscribe to sim_loc_driver messages and sim_loc_test_server messages +#if defined __ROS_VERSION && __ROS_VERSION == 1 + sick_lidar_localization::SickLocResultPortTelegramMsgSubscriber result_telegram_subscriber + = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::SickLocResultPortTelegramMsg, result_telegrams_topic, &sick_lidar_localization::VerifierThread::messageCbResultPortTelegrams, &verifier); + sick_lidar_localization::SickLocResultPortTestcaseMsgSubscriber testcase_subscriber + = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::SickLocResultPortTestcaseMsg, result_testcases_topic, &sick_lidar_localization::VerifierThread::messageCbResultPortTestcases, &verifier); +#elif defined __ROS_VERSION && __ROS_VERSION == 2 + sick_lidar_localization::SickLocResultPortTelegramMsgSubscriber result_telegram_subscriber + = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::SickLocResultPortTelegramMsg, result_telegrams_topic, &sick_lidar_localization::VerifierThread::messageCbResultPortTelegramsROS2, &verifier); + sick_lidar_localization::SickLocResultPortTestcaseMsgSubscriber testcase_subscriber + = ROS_CREATE_SUBSCRIBER(nh, sick_lidar_localization::SickLocResultPortTestcaseMsg, result_testcases_topic, &sick_lidar_localization::VerifierThread::messageCbResultPortTestcasesROS2, &verifier); +#endif + + // Start verification thread verifier.start(); // Run ros event loop - ros::spin(); + ROS::spin(nh); std::cout << "verify_sim_loc_driver finished." << std::endl; ROS_INFO_STREAM("verify_sim_loc_driver finished."); verifier.stop(); std::cout << "verify_sim_loc_driver exits." << std::endl; ROS_INFO_STREAM("verify_sim_loc_driver exits."); + ROS::deleteNode(nh); return 0; } diff --git a/yaml/sim_loc_driver.yaml b/yaml/sim_loc_driver.yaml index 3c47dec..9f3dcd9 100644 --- a/yaml/sim_loc_driver.yaml +++ b/yaml/sim_loc_driver.yaml @@ -1,4 +1,4 @@ -# Configuration of ros driver for sick localization. +# Configuration of ros1 driver for sick localization. sick_lidar_localization: # Driver configuration. See Operation-Instruction-v1.1.0.241R.pdf, page 51, "IP port number and protocol" for default tcp ports. @@ -18,6 +18,11 @@ sick_lidar_localization: point_cloud_frame_id: "pointcloud_sick_lidar_localization" # ros frame id of PointCloud2 messages tf_parent_frame_id: "tf_demo_map" # parent frame of tf messages of of vehicles pose (typically frame of the loaded map) tf_child_frame_id: "tf_sick_lidar_localization" # child frame of tf messages of of vehicles pose + # Odometry configuration + odom_telegrams_udp_port: 3000 # Udp port to send odom packages to the localization controller + odom_topic: "/odom" # ROS topic for odometry messages + odom_telegrams_bigendian: 1 # Send udp odometry telegrams big endian (true) or little endian (false) + odom_telegrams_source_id: 100 # SourceID of udp odometry telegrams, e.g. vehicle controller 1 # Configuration for time sync service time_sync: