diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b38867c --- /dev/null +++ b/.gitignore @@ -0,0 +1,34 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +test/scripts/log/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 820f4d8..d3544ad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs std_msgs + tf ) ## System dependencies are found with CMake's conventions @@ -59,6 +60,7 @@ find_package(Doxygen) ## Generate messages in the 'msg' folder add_message_files( FILES + SickLocColaTelegramMsg.msg SickLocDiagnosticMsg.msg SickLocResultPortHeaderMsg.msg SickLocResultPortPayloadMsg.msg @@ -68,11 +70,11 @@ add_message_files( ) ## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) +add_service_files( + FILES + SickLocColaTelegramSrv.srv + SickLocRequestTimestampSrv.srv +) ## Generate actions in the 'action' folder # add_action_files( @@ -132,6 +134,7 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( include + test/include ${catkin_INCLUDE_DIRS} ${BOOST_INCLUDE_DIRS} # ${TinyXML_INCLUDE_DIRS} @@ -140,16 +143,17 @@ include_directories( ## Declare a C++ library add_library(sick_localization_lib - src/sim_loc_driver_check_thread.cpp - src/sim_loc_driver_monitor.cpp - src/sim_loc_driver_thread.cpp - src/sim_loc_pointcloud_converter.cpp - src/sim_loc_random.cpp - src/sim_loc_result_port_parser.cpp - src/sim_loc_testcase_generator.cpp - src/sim_loc_test_server_thread.cpp - src/sim_loc_utils.cpp - src/sim_loc_verifier_thread.cpp + src/cola_converter.cpp + src/cola_parser.cpp + src/cola_transmitter.cpp + src/driver_check_thread.cpp + src/driver_monitor.cpp + src/driver_thread.cpp + src/pointcloud_converter_thread.cpp + src/random_generator.cpp + src/result_port_parser.cpp + src/testcase_generator.cpp + src/utils.cpp src/crc/crc16ccitt_false.cpp ) @@ -161,12 +165,16 @@ add_library(sick_localization_lib ## 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 -add_executable(sim_loc_driver src/sim_loc_driver.cpp) -add_executable(sim_loc_driver_check src/sim_loc_driver_check.cpp) -add_executable(sim_loc_test_server src/sim_loc_test_server.cpp) +## +## 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(pointcloud_converter src/pointcloud_converter.cpp) -add_executable(unittest_sim_loc_parser src/unittest_sim_loc_parser.cpp) -add_executable(verify_sim_loc_driver src/verify_sim_loc_driver.cpp) + +## Executables for test purposes +add_executable(sim_loc_test_server test/src/test_server.cpp test/src/test_server_thread.cpp) +add_executable(unittest_sim_loc_parser test/src/unittest_sim_loc_parser.cpp) +add_executable(verify_sim_loc_driver test/src/verify_sim_loc_driver.cpp test/src/verifier_thread.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the diff --git a/README.md b/README.md index ea11cc5..a757264 100644 --- a/README.md +++ b/README.md @@ -9,9 +9,10 @@ To install and build ROS support for SICK LiDAR Localization, run the following ```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.git # get ros driver sources for sick localization +# 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/melodic/setup.bash catkin_make @@ -25,7 +26,7 @@ To run SICK LiDAR Localization under ROS, install the SICK localization controll 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://www.sick.com/de/en/search?text=NAV-LOC. +https://supportportal.sick.com/Product_notes/lidar-loc-operation-instruction/. 2. Start the sick_lidar_localization driver: @@ -50,7 +51,7 @@ rostopic echo "/sick_lidar_localization/driver/result_telegrams" ## Result port telegrams -Result port telegrams are continously received and published to inform about a sensors pose (position and orientation). +Result port telegrams are continously received and published to inform about a vehicles pose (position and orientation). ROS result telegram messages have the datatype [msg/SickLocResultPortTelegramMsg.msg](msg/SickLocResultPortTelegramMsg.msg), containing 4 sub-elements: @@ -85,7 +86,7 @@ int32 | telegram_payload.PoseY | Position Y of the vehicle on the map in cartesi 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 [1 … 100], 1 = bad pose quality, 100 = good pose quality +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] @@ -161,7 +162,7 @@ message: "sim_loc_driver: status okay, receiving and publishing result telegrams ``` Note: In case of errors (f.e. connection lost, parse errors or invalid telegrams), diagnostic messages with an error code -are published. Error codes defined in [include/sick_lidar_localization/sim_loc_driver_thread.h](include/sick_lidar_localization/sim_loc_driver_thread.h) are: +are published. Error codes defined in [include/sick_lidar_localization/driver_thread.h](include/sick_lidar_localization/driver_thread.h) are: Error code | Value | Description --- | --- | --- @@ -304,7 +305,7 @@ After switching to normal mode, sim_loc_test_server checks for telegram messages error, if the driver isn't reconnecting or isn't publishing telegrams. Currently, the following errors are simulated and tested by sim_loc_test_server (enumerated in -[include/sick_lidar_localization/sim_loc_test_server_thread.h](include/sick_lidar_localization/sim_loc_test_server_thread.h): +[include/sick_lidar_localization/test_server_thread.h](include/sick_lidar_localization/test_server_thread.h): Error testcase | Description --- | --- @@ -334,50 +335,66 @@ TestServerThread: error simulation summary: 6 of 6 testcases passed, 0 failures. MessageCheckThread: check messages thread summary: 1153 messages checked, 0 failures. ``` -## Usage example: pointcloud_convert +## Visualization and usage example: pointcloud_convert pointcloud_convert in file [src/pointcloud_converter.cpp](src/pointcloud_converter.cpp) implements a subscriber to -sim_loc_driver messages and converts them to PointCloud2 messages, which are published on topic "/cloud". -They can be viewed by rviz: +sim_loc_driver messages. The driver messages are converted to both PointCloud2 on topic "/cloud" and and TF messages, +which can be viewed by rviz. + +To run and visualize an example with a simulated vehicle moving in circles, run the following commands: ```console -rosrun tf static_transform_publisher 0 0 0 0 0 0 map sick_lidar_localization 10 & +cd ~/catkin_ws +source ./devel/setup.bash +# Run test server, simulate localization controller with a vehicle moving in circles. +roslaunch sick_lidar_localization sim_loc_test_server.launch demo_circles:=true & +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 +roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=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 & ``` -Example output: +To view vehicles poses by TF messages, add a display with type TF and select frame tf_demo_map in global options: + +![doc/screenshot-rviz-simu2.png](doc/screenshot-rviz-simu2.png) + +To view pointcloud messages, add a display with topic /cloud/PointCloud2 and select frame map in global options: ![doc/screenshot-rviz-simu1.png](doc/screenshot-rviz-simu1.png) +Both visualizations view the poses of the same simulated vehicle. + pointcloud_convert is an usage example for sick_lidar_localization, too, and shows how to subscribe and use the sick_lidar_localization messages published by the driver. Feel free to use this example as a starting point for customization. pointcloud_convert in file [src/pointcloud_converter.cpp](src/pointcloud_converter.cpp) is just the main entry point. Conversion and message handling is implemented in class -sick_lidar_localization::PointCloudConverter in file [src/sim_loc_pointcloud_converter.cpp](src/sim_loc_pointcloud_converter.cpp). +sick_lidar_localization::PointCloudConverter in file [src/pointcloud_converter.cpp](src/pointcloud_converter_thread.cpp). ## Source code, doxygen -The main entry point of the ros driver is implemented in file [src/sim_loc_driver.cpp](src/sim_loc_driver.cpp). -It creates an instance of class sick_lidar_localization::DriverMonitor implemented in [src/sim_loc_driver_monitor.cpp](src/sim_loc_driver_monitor.cpp). +The main entry point of the ros driver is implemented in file [src/driver.cpp](src/driver.cpp). +It creates an instance of class sick_lidar_localization::DriverMonitor implemented in [src/driver_monitor.cpp](src/driver_monitor.cpp). sick_lidar_localization::DriverMonitor creates and monitors an instance of class sick_lidar_localization::DriverThread implemented in -[src/sim_loc_driver_thread.cpp](src/sim_loc_driver_thread.cpp), which runs all driver functions, including the +[src/driver_thread.cpp](src/driver_thread.cpp), which runs all driver functions, including the telegram parser implemented by class sick_lidar_localization::ResultPortParser. After successful initialization, the driver runs 3 threads: - The receiver thread implemented by sick_lidar_localization::DriverThread::runReceiverThreadCb in file -[src/sim_loc_driver_thread.cpp](src/sim_loc_driver_thread.cpp). The receiver thread connects to the localization +[src/driver_thread.cpp](src/driver_thread.cpp). The receiver thread connects to the localization controller, receives binary result telegram and buffers them in a fifo (first-in, first-out) - The converter thread implemented by sick_lidar_localization::DriverThread::runConverterThreadCb in file -[src/sim_loc_driver_thread.cpp](src/sim_loc_driver_thread.cpp). The converter thread pops binary telegrams from the +[src/driver_thread.cpp](src/driver_thread.cpp). The converter thread pops binary telegrams from the fifo, decodes and parses result port telegrams and publishes telegram messages on ros topic "/sick_lidar_localization/driver/result_telegrams". Telegram decoding is implemented by sick_lidar_localization::ResultPortParser::decode -in file [src/sim_loc_result_port_parser.cpp](src/sim_loc_result_port_parser.cpp). +in file [src/result_port_parser.cpp](src/result_port_parser.cpp). - The monitoring thread implemented by sick_lidar_localization::DriverMonitor::runMonitorThreadCb in file -[src/sim_loc_driver_monitor.cpp](src/sim_loc_driver_monitor.cpp). It subscribes and monitors the telegram messages from +[src/driver_monitor.cpp](src/driver_monitor.cpp). It subscribes and monitors the telegram messages from sick_lidar_localization::DriverThread. In case of errors or missing telegram messages, the tcp connection to the localization controller is closed and re-established. @@ -410,4 +427,4 @@ Quickstart, tutorials and manuals: * Quickstart Setup LiDAR-LOC: [doc/Quickstart-Setup-SOPASair.md](doc/Quickstart-Setup-SOPASair.md) -* Operation manuals: https://www.sick.com/de/en/search?text=NAV-LOC +* Operation manuals: https://supportportal.sick.com/Product_notes/lidar-loc-operation-instruction/ diff --git a/doc/Quickstart-Setup-SOPASair.md b/doc/Quickstart-Setup-SOPASair.md index beea048..b42a460 100644 --- a/doc/Quickstart-Setup-SOPASair.md +++ b/doc/Quickstart-Setup-SOPASair.md @@ -1,7 +1,7 @@ # Quickstart: LiDAR-LOC with SOPASair SOPASair is used to setup and configure SICK LiDAR Localization. The following quickstart gives a brief overview -about setup and configuration. See the operation manuals published on https://www.sick.com/de/en/search?text=NAV-LOC +about setup and configuration. See the operation manuals published on https://supportportal.sick.com/Product_notes/lidar-loc-operation-instruction/ for details and further information. ## LiDAR-LOC brief setup and configuration with SOPASair diff --git a/doc/faq.md b/doc/faq.md index a9b7693..81d6568 100644 --- a/doc/faq.md +++ b/doc/faq.md @@ -6,7 +6,7 @@ :white_check_mark: To setup and configure LiDAR-LOC with SOPASair, use of Chrome-browser under Windows is highly recommended. See [Quickstart-Setup-SOPASair.md](Quickstart-Setup-SOPASair.md) for a quickstart. -Find detailed information in the operation manuals published on https://www.sick.com/de/en/search?text=NAV-LOC. +Find detailed information in the operation manuals published on https://supportportal.sick.com/Product_notes/lidar-loc-operation-instruction/. ## Reconnect after network error diff --git a/doc/screenshot-rviz-simu2.png b/doc/screenshot-rviz-simu2.png new file mode 100644 index 0000000..d8d490c Binary files /dev/null and b/doc/screenshot-rviz-simu2.png differ diff --git a/include/sick_lidar_localization/cola_converter.h b/include/sick_lidar_localization/cola_converter.h new file mode 100644 index 0000000..e958360 --- /dev/null +++ b/include/sick_lidar_localization/cola_converter.h @@ -0,0 +1,142 @@ +/* + * @brief sim_loc_cola_converter converts between Cola-ASCII and Cola-Binary telegrams. + * See Operation-Instruction-v1.1.0.241R.pdf, chapter 5.8 "About CoLa-A telegrams", page 46-48, + * Telegram-Listing-v1.1.0.241R.pdf, chapter 2.3.9 "Command: LocRequestTimestamp", page 21, and + * Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.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 + * + */ +#ifndef __SIM_LOC_COLA_CONVERTER_H_INCLUDED +#define __SIM_LOC_COLA_CONVERTER_H_INCLUDED + +#include +#include +#include +#include + +namespace sick_lidar_localization +{ + /*! + * @brief class ColaAsciiBinaryConverter converts between Cola-ASCII and Cola-Binary telegrams. + * See Operation-Instruction-v1.1.0.241R.pdf, chapter 5.8 "About CoLa-A telegrams", page 46-48, + * Telegram-Listing-v1.1.0.241R.pdf, chapter 2.3.9 "Command: LocRequestTimestamp", page 21, and + * Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.PDF for further details about + * Cola telegrams. + */ + class ColaAsciiBinaryConverter + { + public: + + /*! + * @brief Converts and returns a Cola-ASCII telegram to string. + * @param[in] cola_telegram Cola-ASCII telegram, starting with 0x02 and ending with 0x03 + * @return Cola-ASCII string, f.e. "sMN SetAccessMode 3 F4724744" + */ + static std::string ConvertColaAscii(const std::vector & cola_telegram); + + /*! + * @brief Converts and returns a Cola telegram from Cola-ASCII to Cola-Binary. + * @param[in] cola_telegram Cola-ASCII string, f.e. "sMN SetAccessMode 3 F4724744" + * @return Cola-ASCII telegram, f.e. { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, + * 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 } + */ + static std::vector ConvertColaAscii(const std::string & cola_telegram); + + /*! + * @brief Converts and returns a Cola telegram from Cola-ASCII to Cola-Binary. + * @param[in] cola_telegram Cola-ASCII telegram, f.e. { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, + * 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 } + * ("sMN SetAccessMode 3 F4724744") + * @param[in] parameter_is_ascii Command parameter given in ascii (f.e. if true, a parameter 0x31 == ASCII-"1" will be converted to 0x01 (default), otherwise to 0x01) + * @return Cola-Binary telegram + */ + static std::vector ColaAsciiToColaBinary(const std::vector & cola_telegram, bool parameter_is_ascii = true); + + /*! + * @brief Converts and returns a Cola telegram from Cola-ASCII to Cola-Binary. + * @param[in] cola_telegram Cola-Binary telegram + * @param[in] parameter_to_ascii Conversion of command parameter to ascii (f.e. if true, a parameter 0x01 will be converted to 0x31 == ASCII-"1" (default), otherwise to 0x01) + * @return Cola-ASCII telegram, f.e. { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, + * 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 } + * ("sMN SetAccessMode 3 F4724744") + */ + static std::vector ColaBinaryToColaAscii(const std::vector & cola_telegram, bool parameter_to_ascii = true); + + /*! + * @brief Returns true for Cola-Binary, if a given telegram is Cola-Binary encoded and starts with 4 Bytes + * { 0x02, 0x02, 0x02, 0x02 }, otherwise false (Cola-ASCII telegram) + * @param[in] cola_telegram Cola telegram (Cola-ASCII to Cola-Binary) + * @return true for Cola-Binary, false for Cola-ASCII + */ + static bool IsColaBinary(const std::vector & cola_telegram); + + /*! + * @brief Decodes the header and returns the length of a Cola-Binary telegram + * @param[in] cola_telegram Cola-Binary telegram + * @return expected telegram length incl. header, payload and crc, or 0 in case of errors. + */ + static uint32_t ColaBinaryTelegramLength(const std::vector & cola_telegram); + + protected: + + /* + * member data + */ + + static const std::string s_ascii_table[256]; ///< static ascii table to convert binary to ascii, f.e. s_ascii_table[0x02]:="", s_ascii_table[0x03]:="", s_ascii_table[0x41]:="A" and so on + static const std::map s_ascii_map; ///< static ascii map to convert ascii to binary, f.e. s_ascii_map[""]:=0x02, s_ascii_map[""]:=0x03, s_ascii_map["A"]:=0x41 and so on + + }; // class ColaAsciiBinaryConverter + +} // namespace sick_lidar_localization +#endif // __SIM_LOC_COLA_CONVERTER_H_INCLUDED diff --git a/include/sick_lidar_localization/cola_parser.h b/include/sick_lidar_localization/cola_parser.h new file mode 100644 index 0000000..2470086 --- /dev/null +++ b/include/sick_lidar_localization/cola_parser.h @@ -0,0 +1,203 @@ +/* + * @brief sim_loc_cola_parser parses and converts binary Cola telegrams to ros messages SickLocColaTelegramMsg + * and vice versa. + * + * See Operation-Instruction-v1.1.0.241R.pdf, chapter 5.8 "About CoLa-A telegrams", page 46-48, + * Telegram-Listing-v1.1.0.241R.pdf, chapter 2.3.9 "Command: LocRequestTimestamp", page 21, and + * Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.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 + * + */ +#ifndef __SIM_LOC_COLA_PARSER_H_INCLUDED +#define __SIM_LOC_COLA_PARSER_H_INCLUDED + +#include "sick_lidar_localization/SickLocColaTelegramMsg.h" +#include "sick_lidar_localization/cola_converter.h" + +namespace sick_lidar_localization +{ + /*! + * @brief class ColaParser parses and converts binary Cola telegrams to ros messages SickLocColaTelegramMsg + * and vice versa. + * + * See Operation-Instruction-v1.1.0.241R.pdf, chapter 5.8 "About CoLa-A telegrams", page 46-48, + * Telegram-Listing-v1.1.0.241R.pdf, chapter 2.3.9 "Command: LocRequestTimestamp", page 21, and + * Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.PDF for further details about + * Cola telegrams. + */ + class ColaParser + { + public: + + /*! + * @brief Enumeration of SOPAS commands in cola telegrams: + * The command_type in SickLocColaTelegramMsg is one of the following SOPAS Commands: + * sINVALID (0, uninitialized, command_type should never have this value) + * 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) + */ + typedef enum COLA_SOPAS_COMMAND_ENUM + { + sINVALID = 0, ///< uninitialized, command_type should never have this value) + sRN = 1, ///< Read by name (request) + sRA = 2, ///< Read by name (response) + sMN = 3, ///< Method by name (request) + sAN = 4, ///< Response to sMN + sMA = 5, ///< Method by name (response) + sWN = 6, ///< Write by name (request) + sWA = 7, ///< Write answer (response) + sEN = 8, ///< Event by name (request) + sEA = 9, ///< Event answer (response) + sSN = 10, ///< Answer (response) + MAX_COLA_COMMAND_NUMBER ///< Number of possible COLA_SOPAS_COMMANDs incl. invalid command + } COLA_SOPAS_COMMAND; + + /*! + * @brief Creates and returns a Cola telegram (type SickLocColaTelegramMsg). + * @param[in] command_type One of the SOPAS Commands enumerated in COLA_SOPAS_COMMAND (sRN, sRA, sMN, sMA, or sWN) + * @param[in] command_name Name of command like "SetAccessMode", "LocSetResultPoseEnabled", "LocRequestTimestamp", etc. + * @param[in] parameter Optional list of command parameter + * @return Cola telegram of type SickLocColaTelegramMsg + */ + static sick_lidar_localization::SickLocColaTelegramMsg createColaTelegram(const COLA_SOPAS_COMMAND & command_type, + const std::string & command_name, const std::vector & parameter = std::vector()); + + /*! + * @brief Decodes and returns a Cola message of type SickLocColaTelegramMsg from a Cola-Binary telegram. + * Note: If decoding fails (invalid binary telegram or parse error), command_type of the returned Cola telegram message + * will be sINVALID. + * @param[in] cola_binary Cola-Binary telegram, f.e. { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, + * 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 } + * @return Cola telegram message (type SickLocColaTelegramMsg) + */ + static sick_lidar_localization::SickLocColaTelegramMsg decodeColaTelegram(const std::vector & cola_binary); + + /*! + * @brief Converts and returns a Cola message of type SickLocColaTelegramMsg from a Cola-ASCII telegram. + * @param[in] cola_ascii Cola-ASCII telegram, f.e. "sMN LocRequestTimestamp" + * @return Cola telegram message (type SickLocColaTelegramMsg) + */ + static sick_lidar_localization::SickLocColaTelegramMsg decodeColaTelegram(const std::string & cola_ascii); + + /*! + * @brief Encodes and returns a Cola Binary telegram from ros message SickLocColaTelegramMsg. + * @param[in] cola_telegram Cola telegram, f.e. createColaTelegram(sMN, "SetAccessMode", {"3", "F4724744"}) + * @return Cola-Binary telegram, f.e. { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, + * 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 } + */ + static std::vector encodeColaTelegram(const sick_lidar_localization::SickLocColaTelegramMsg & cola_telegram); + + /*! + * @brief Encodes and returns a Cola telegram. + * @param[in] command_type One of the SOPAS Commands enumerated in COLA_SOPAS_COMMAND (sRN, sRA, sMN, sMA, or sWN) + * @param[in] command_name Name of command like "SetAccessMode", "LocSetResultPoseEnabled", "LocRequestTimestamp", etc. + * @param[in] parameter Optional list of command parameter + * @return Cola-Binary telegram + */ + static std::vector encodeColaTelegram(const COLA_SOPAS_COMMAND & command_type, const std::string & command_name, + const std::vector & parameter = std::vector()); + + /*! + * @brief Returns the "start of text" tag in a Cola ASCII command, i.e. "". + * @return "" + */ + static const std::string & asciiSTX(void) { return s_cola_ascii_start_tag; } + + /*! + * @brief Returns the "end of text" tag in a Cola ASCII command, i.e. "". + * @return "" + */ + static const std::string & asciiETX(void) { return s_cola_ascii_end_tag; } + + /*! + * @brief Returns the binary "start of text" tag in a Cola-Binary command, i.e. {0x02}. + * @return {0x02} + */ + static std::vector binarySTX(void){ return sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(asciiSTX()); } + + /*! + * @brief Returns the binary "end of text" tag in a Cola-Binary command, i.e. {0x03}. + * @return {0x03} + */ + static std::vector binaryETX(void){ return sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(asciiETX()); } + + /*! + * @brief Converts and returns a COLA_SOPAS_COMMAND to string. + * Example: convertSopasCommand(sMN) returns "sMN". + * @param[in] command_type One of the SOPAS Commands enumerated in COLA_SOPAS_COMMAND (sRN, sRA, sMN, sMA, or sWN) + * @return COLA_SOPAS_COMMAND as string. + */ + static std::string convertSopasCommand(COLA_SOPAS_COMMAND command_type); + + /*! + * @brief Converts and returns a COLA_SOPAS_COMMAND from string. + * Example: convertSopasCommand("sMN") returns sMN. + * @param[in] sopas_command One of the SOPAS commands ("sRN", "sRA", "sMN", "sMA", or "sWN") + * @return COLA_SOPAS_COMMAND from string. + */ + static COLA_SOPAS_COMMAND convertSopasCommand(const std::string & sopas_command); + + 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 + static const std::map s_command_type_map; ///< static map to convert COLA_SOPAS_COMMANDs from string to enum, f.e. s_command_type_map["sRN"]:=sRN, s_command_type_map["sRA"]:=sRA and so on + static const std::string s_cola_ascii_start_tag; ///< All Cola-ACSII telegrams start with s_cola_ascii_start_tag := "" ("Start of TeXt") + static const std::string s_cola_ascii_end_tag; ///< All Cola-ACSII telegrams start with s_cola_ascii_start_tag := "" ("End of TeXt") + + }; // class ColaParser + +} // namespace sick_lidar_localization +#endif // __SIM_LOC_COLA_PARSER_H_INCLUDED diff --git a/include/sick_lidar_localization/cola_transmitter.h b/include/sick_lidar_localization/cola_transmitter.h new file mode 100644 index 0000000..8068b92 --- /dev/null +++ b/include/sick_lidar_localization/cola_transmitter.h @@ -0,0 +1,207 @@ +/* + * @brief sim_loc_cola_transmitter connects to the localization controller, sends cola command requests, + * and waits for the response with timeout. + * + * 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_TRANSMITTER_H_INCLUDED +#define __SIM_LOC_COLA_TRANSMITTER_H_INCLUDED + +#include +#include +#include +#include +#include + +#include "sick_lidar_localization/fifo_buffer.h" + +namespace sick_lidar_localization +{ + /*! + * Class sick_lidar_localization::ColaTransmitter connects to the localization controller and + * sends or receives cola telegrams. Base class for ColaSender and ColaReceiver. + */ + class ColaTransmitter + { + public: + + /*! + * Constructor. + * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 + * @param[in] tcp_port tcp port for command requests, default: 2111 for command requests and 2112 for command responses + * @param[in] default_receive_timeout default timeout in seconds for receive functions + */ + ColaTransmitter(const std::string & server_adress = "192.168.0.1", int tcp_port = 2111, double default_receive_timeout = 1); + + /*! + * Destructor, closes all tcp connections. + */ + virtual ~ColaTransmitter(); + + /*! + * Connects to the localization server. + * @return true on success, false on failure (localization server unknown or unreachable) + */ + virtual bool connect(void); + + /*! + * Closes the tcp connection to the localization server. + * @return always true + */ + virtual bool close(void); + + /*! + * Send data to the localization server. + * @param[in] data data to be send + * @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); + + /*! + * Send data to the localization server. + * @param[in] socket socket to write to + * @param[in] data data to be send + * @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); + + /*! + * Receive a cola telegram from the localization server. + * @param[out] telegram telegram received (Cola-Binary or Cola-Ascii) + * @param[in] timeout timeout in seconds + * @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); + + /*! + * Receive a cola telegram from a socket. + * @param[in] socket socket to read from + * @param[out] telegram telegram received (Cola-Binary or Cola-Ascii) + * @param[in] timeout timeout in seconds + * @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); + + /*! + * Starts a thread to receive response telegrams from the localization server. + * The receiver thread pushes responses to a fifo buffer, which can be popped by waitPopResponse(). + * @return always true + */ + virtual bool startReceiverThread(void); + + /*! + * Stops the thread to receive response telegrams from the localization server (if a thread has been started by startReceiverThread=. + * @return always true + */ + virtual bool stopReceiverThread(void); + + /*! + * Returns a response telegram from the localization server. + * This function waits with timout, until the receiver thread received a response telegram from the localization server. + * Note: The receiver thread must have been started by startReceiverThread(), otherwise waitPopResponse() will fail + * after timout. + * @param[out] telegram telegram received (Cola-Binary or Cola-Ascii) + * @param[in] timeout timeout in seconds + * @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); + + protected: + + /*! + * class ServerColaRequest: utility container for a response received from localization server + * (telegram data plus timestamp) + */ + class ColaResponseContainer + { + public: + ColaResponseContainer(const std::vector & data = std::vector(),const ros::Time & timestamp = ros::Time::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) + }; + + /*! + * Returns true, if data end with etx, i.e. the trailing (etx.size()) byte in data are identical to etx. + * @param[in] data data received + * @param[in] etx ETX tag (binary or ascii) + * @return true if data end with etx, false otherwise + */ + static bool dataEndWithETX(const std::vector & data, const std::vector & etx); + + /*! + * Thread callback, receives response telegrams from localization server and pushes them to m_response_fifo. + */ + void runReceiverThreadCb(void); + + /* + * member data + */ + + std::string m_server_adress; ///< ip adress of the localization controller, default: 192.168.0.1 + int m_tcp_port; ///< tcp port of the localization controller, default: 2111 for command requests and 2112 for command responses + boost::asio::io_service m_ioservice; ///< boost io service for tcp connections + boost::asio::ip::tcp::socket m_tcp_socket; ///< tcp socket connected to the localization controller + double m_receive_timeout; ///< default timeout in seconds for receive functions + bool m_receiver_thread_running; ///< true: m_receiver_thread is running, otherwise false + boost::thread* m_receiver_thread; ///< thread to receive responses from localization server + sick_lidar_localization::FifoBuffer m_response_fifo; ///< fifo buffer for receiver thread for responses from localization server + + }; // class ColaTransmitter + +} // namespace sick_lidar_localization +#endif // __SIM_LOC_COLA_TRANSMITTER_H_INCLUDED diff --git a/include/sick_lidar_localization/sim_loc_driver_check_thread.h b/include/sick_lidar_localization/driver_check_thread.h similarity index 99% rename from include/sick_lidar_localization/sim_loc_driver_check_thread.h rename to include/sick_lidar_localization/driver_check_thread.h index 3793d3a..0dea684 100644 --- a/include/sick_lidar_localization/sim_loc_driver_check_thread.h +++ b/include/sick_lidar_localization/driver_check_thread.h @@ -69,7 +69,7 @@ #include "sick_lidar_localization/SickLocResultPortPayloadMsg.h" #include "sick_lidar_localization/SickLocResultPortCrcMsg.h" #include "sick_lidar_localization/SickLocResultPortTelegramMsg.h" -#include "sick_lidar_localization/sim_loc_fifo.h" +#include "sick_lidar_localization/fifo_buffer.h" namespace sick_lidar_localization { diff --git a/include/sick_lidar_localization/sim_loc_driver_monitor.h b/include/sick_lidar_localization/driver_monitor.h similarity index 65% rename from include/sick_lidar_localization/sim_loc_driver_monitor.h rename to include/sick_lidar_localization/driver_monitor.h index 5ef049c..0a21fa0 100644 --- a/include/sick_lidar_localization/sim_loc_driver_monitor.h +++ b/include/sick_lidar_localization/driver_monitor.h @@ -56,8 +56,9 @@ #ifndef __SIM_LOC_DRIVER_MONITOR_H_INCLUDED #define __SIM_LOC_DRIVER_MONITOR_H_INCLUDED -#include "sick_lidar_localization/sim_loc_driver_thread.h" -#include "sick_lidar_localization/sim_loc_utils.h" +#include "sick_lidar_localization/cola_transmitter.h" +#include "sick_lidar_localization/driver_thread.h" +#include "sick_lidar_localization/utils.h" namespace sick_lidar_localization { @@ -71,11 +72,21 @@ namespace sick_lidar_localization /*! * Constructor. The driver monitor does not start automatically, call start() and stop() to start and stop. + * + * 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. + * - SOPAS CoLa-A or CoLa-B protocols. + * 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. + * - Binary result port protocol TCP/IP. + * * @param[in] nh ros node handle * @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 + * @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 tcp_port = 2201); + DriverMonitor(ros::NodeHandle * 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. @@ -99,8 +110,30 @@ namespace sick_lidar_localization * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) */ virtual void messageCbResultPortTelegrams(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg); + + /*! + * Callback for service messages (SickLocColaTelegramSrv). Handles Cola requests and responses, sends and receives Cola-ASCII telegrams to resp. from + * the localization controller + * @param[in] cola_request Cola command request, will be encoded and send to the localization controller + * @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); protected: + + /*! + * Initializes cola sender and receiver for cola requests and responses + * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 + * @param[in] ip_port_cola ip port for command requests and responses, default: 2111 + * @param[in] receive_timeout timeout in seconds for receive functions + */ + bool initColaTransmitter(const std::string & server_adress, int ip_port_cola, double receive_timeout); + + /*! + * Stops cola sender and receiver for cola requests and responses + * (if started by a cola service request "SickLocColaTelegram") + */ + void stopColaTransmitter(void); /*! * Thread callback, implements the monitoring and restarts a DriverThread in case of tcp-errors. @@ -114,13 +147,17 @@ namespace sick_lidar_localization bool m_initialized; ///< true after successfull configuration, otherwise false ros::NodeHandle* m_nh; ///< ros node handle std::string m_server_adress; ///< ip adress of the localization controller, default: 192.168.0.1 - int m_tcp_port; ///< tcp port of the localization controller, default: The localization controller uses IP port number 2201 to send localization results + 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) 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 - + sick_lidar_localization::ColaTransmitter* m_cola_transmitter; ///< transmitter for cola commands (send requests, receive responses) + boost::mutex m_service_cb_mutex; ///< mutex to protect serviceCbColaTelegram (one service request at a time) + }; // class DriverMonitor } // namespace sick_lidar_localization diff --git a/include/sick_lidar_localization/sim_loc_driver_thread.h b/include/sick_lidar_localization/driver_thread.h similarity index 99% rename from include/sick_lidar_localization/sim_loc_driver_thread.h rename to include/sick_lidar_localization/driver_thread.h index a9504ca..667fd77 100644 --- a/include/sick_lidar_localization/sim_loc_driver_thread.h +++ b/include/sick_lidar_localization/driver_thread.h @@ -72,7 +72,7 @@ #include #include -#include "sick_lidar_localization/sim_loc_fifo.h" +#include "sick_lidar_localization/fifo_buffer.h" namespace sick_lidar_localization { diff --git a/include/sick_lidar_localization/sim_loc_fifo.h b/include/sick_lidar_localization/fifo_buffer.h similarity index 93% rename from include/sick_lidar_localization/sim_loc_fifo.h rename to include/sick_lidar_localization/fifo_buffer.h index fbaf4bf..29504b8 100644 --- a/include/sick_lidar_localization/sim_loc_fifo.h +++ b/include/sick_lidar_localization/fifo_buffer.h @@ -75,7 +75,10 @@ namespace sick_lidar_localization /*! * Destructor */ - ~FifoBuffer() {} + ~FifoBuffer() + { + notify(); // interrupt a possible wait in waitForNotify() + } /*! * Returns true, if the fifo buffer is empty. @@ -130,6 +133,25 @@ namespace sick_lidar_localization waitForNotify(); } } + + /*! + * Waits until there's at least one element in the fifo buffer, or a notification has been signalled. + */ + void waitOnceForElement() + { + if (ros::ok() && empty()) + { + waitForNotify(); + } + } + + /*! + * Signal a notification to interrupt a waiting waitForElement() call + */ + void notify(void) + { + notifyAll(); + } /*! * Interface class to search for an element with a unary condition. diff --git a/include/sick_lidar_localization/sim_loc_pointcloud_converter.h b/include/sick_lidar_localization/pointcloud_converter.h similarity index 82% rename from include/sick_lidar_localization/sim_loc_pointcloud_converter.h rename to include/sick_lidar_localization/pointcloud_converter.h index fb877df..0e66bf1 100644 --- a/include/sick_lidar_localization/sim_loc_pointcloud_converter.h +++ b/include/sick_lidar_localization/pointcloud_converter.h @@ -2,6 +2,9 @@ * @brief sim_loc_pointcloud_converts sim_loc_driver messages (type sick_lidar_localization::SickLocResultPortTelegramMsg), * to PointCloud2 messages and publishes PointCloud2 messages on topic "/cloud". * + * The vehicle poses (PoseX, PoseY, PoseYaw of result port telegrams) are transformed and published + * by tf-messages with configurable parent and child frame id. + * * It also serves as an usage example for sick_lidar_localization and shows how to use sick_lidar_localization * in a custumized application. * @@ -65,9 +68,12 @@ #include #include #include +#include +#include +#include -#include "sick_lidar_localization/sim_loc_fifo.h" -#include "sick_lidar_localization/sim_loc_result_port_parser.h" +#include "sick_lidar_localization/fifo_buffer.h" +#include "sick_lidar_localization/result_port_parser.h" namespace sick_lidar_localization { @@ -126,16 +132,24 @@ namespace sick_lidar_localization std::vector poseToDemoPoints(double posx, double posy, double yaw, double triangle_height); /*! - * Converts a telegram from type SickLocResultPortTelegramMsg to PointCloud2 + * Converts the vehicle position from a result port telegram to PointCloud2 message with 4 points + * (centerpoint plus 3 demo corner points showing a triangle). * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) * @return PointCloud2 message */ - sensor_msgs::PointCloud2 convert(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg); + sensor_msgs::PointCloud2 convertToPointCloud(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg); + + /*! + * Converts the vehicle pose from a result port telegram to a tf transform. + * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) + * @return tf transform + */ + geometry_msgs::TransformStamped convertToTransform(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg); /*! * Thread callback, pops received telegrams from the fifo buffer m_result_port_telegram_fifo, * converts the telegrams from type SickLocResultPortTelegramMsg to PointCloud2 and publishes - * PointCloud2 messages. + * PointCloud2 messages. The vehicle pose is converted to a tf transform and broadcasted. */ virtual void runPointCloudConverterThreadCb(void); @@ -145,6 +159,8 @@ namespace sick_lidar_localization 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 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/sim_loc_random.h b/include/sick_lidar_localization/random_generator.h similarity index 100% rename from include/sick_lidar_localization/sim_loc_random.h rename to include/sick_lidar_localization/random_generator.h diff --git a/include/sick_lidar_localization/sim_loc_result_port_parser.h b/include/sick_lidar_localization/result_port_parser.h similarity index 100% rename from include/sick_lidar_localization/sim_loc_result_port_parser.h rename to include/sick_lidar_localization/result_port_parser.h diff --git a/include/sick_lidar_localization/sim_loc_testcase_generator.h b/include/sick_lidar_localization/testcase_generator.h similarity index 82% rename from include/sick_lidar_localization/sim_loc_testcase_generator.h rename to include/sick_lidar_localization/testcase_generator.h index a87b99a..695b4b3 100644 --- a/include/sick_lidar_localization/sim_loc_testcase_generator.h +++ b/include/sick_lidar_localization/testcase_generator.h @@ -56,7 +56,8 @@ #ifndef __SIM_LOC_TESTCASE_GENERATOR_H_INCLUDED #define __SIM_LOC_TESTCASE_GENERATOR_H_INCLUDED -#include "sick_lidar_localization/sim_loc_result_port_parser.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 @@ -90,7 +91,25 @@ namespace sick_lidar_localization * @return SickLocResultPortTestcaseMsg with the binary telegram and SickLocResultPortTelegramMsg */ static sick_lidar_localization::SickLocResultPortTestcaseMsg createResultPortCircles(double circle_radius, double circle_yaw); + + /*! + * Creates and returns a synthetical cola response to a cola command request. + * Note: Just a few cola responses are implemented for test purposes, f.e. responses to "LocRequestTimestamp". + * By default, a response: "sAN " without any parameter is returned (sAN: Response to sMN) + * @param[in] cola_request Cola request from client + * @return Synthetical cola response from server + */ + static sick_lidar_localization::SickLocColaTelegramMsg createColaResponse(const sick_lidar_localization::SickLocColaTelegramMsg & cola_request); + protected: + + /*! + * Creates and returns a timestamp in milliseconds ticks. + * To simulate time jitter, network latency and time drift, + * a random value of +/- 2 milliseconds is added. + * @return timestamp in milliseconds ticks + */ + static uint32_t createTimestampTicksMilliSec(void); }; // class TestcaseGenerator diff --git a/include/sick_lidar_localization/sim_loc_utils.h b/include/sick_lidar_localization/utils.h similarity index 98% rename from include/sick_lidar_localization/sim_loc_utils.h rename to include/sick_lidar_localization/utils.h index 3702fe2..85fc0ae 100644 --- a/include/sick_lidar_localization/sim_loc_utils.h +++ b/include/sick_lidar_localization/utils.h @@ -61,11 +61,13 @@ #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" namespace sick_lidar_localization { diff --git a/msg/SickLocColaTelegramMsg.msg b/msg/SickLocColaTelegramMsg.msg new file mode 100644 index 0000000..ef01586 --- /dev/null +++ b/msg/SickLocColaTelegramMsg.msg @@ -0,0 +1,12 @@ +# Definition of ros message SickLocColaTelegramMsg. +# SickLocColaTelegramMsg publishes Cola-ASCII telegrams for sick localization. +# 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 + +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) + +string command_name # Name of command like "SetAccessMode", "LocSetResultPoseEnabled", "LocRequestTimestamp", etc. + +string[] parameter # Optional parameter, type and number parameter depend on the command, f.e. "sMN SetAccessMode 3 F4724744" with 2 parameter diff --git a/msg/SickLocResultPortPayloadMsg.msg b/msg/SickLocResultPortPayloadMsg.msg index 211afaa..d7371e0 100644 --- a/msg/SickLocResultPortPayloadMsg.msg +++ b/msg/SickLocResultPortPayloadMsg.msg @@ -11,7 +11,7 @@ int32 PoseY # Position Y of the vehicle on the map in cartesian global 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 [1 … 100], 1 = bad pose quality, 100 = good pose quality. Size: UInt8 = 1 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 diff --git a/package.xml b/package.xml index e03cc1e..ee3cf8a 100644 --- a/package.xml +++ b/package.xml @@ -48,6 +48,7 @@ geometry_msgs std_msgs sensor_msgs + tf message_generation message_generation message_runtime @@ -56,6 +57,7 @@ geometry_msgs std_msgs sensor_msgs + tf message_runtime roscpp rospy diff --git a/src/cola_converter.cpp b/src/cola_converter.cpp new file mode 100644 index 0000000..9f797cc --- /dev/null +++ b/src/cola_converter.cpp @@ -0,0 +1,343 @@ +/* + * @brief sim_loc_cola_converter converts between Cola-ASCII and Cola-Binary telegrams. + * See Operation-Instruction-v1.1.0.241R.pdf, chapter 5.8 "About CoLa-A telegrams", page 46-48, + * Telegram-Listing-v1.1.0.241R.pdf, chapter 2.3.9 "Command: LocRequestTimestamp", page 21, and + * Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.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 +#include + +#include "sick_lidar_localization/cola_converter.h" +#include "sick_lidar_localization/cola_parser.h" + +/*! + * @brief static ascii table to convert binary to ascii, f.e. s_ascii_table[0x02]:="", s_ascii_table[0x03]:="", s_ascii_table[0x41]:="A" and so on + */ +const std::string sick_lidar_localization::ColaAsciiBinaryConverter::s_ascii_table[256] = + { + "", "", "", "", "", "", "", "", "\b", "", "\n", "", "", "\n", + "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", + " ", "!", "\"", "#", "$", "%", "&", "'", "(", ")", "*", "+", ",", "-", ".", "/", + "0", "1", "2", "3", "4", "5", "6", "7", "8", "9", ":", ";", "<", "=", ">", "?", "@", + "A", "B", "C", "D", "E", "F", "G", "H", "I", "J", "K", "L", "M", "N", "O", "P", "Q", "R", "S", "T", "U", "V", "W", "X", "Y", "Z", "[", "\\", "]", "^", "_", "`", + "a", "b", "c", "d", "e", "f", "g", "h", "i", "j", "k", "l", "m", "n", "o", "p", "q", "r", "s", "t", "u", "v", "w", "x", "y", "z", "{", "|", "}", "~", "", + "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", + "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", + "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", "", + "", "", "", "", "", "", "", "" + }; + +/*! + * @brief static ascii map to convert ascii to binary, f.e. s_ascii_map[""]:=0x02, s_ascii_map[""]:=0x03, s_ascii_map["A"]:=0x41 and so on + */ +const std::map sick_lidar_localization::ColaAsciiBinaryConverter::s_ascii_map = + { + {"", 0x01}, {"", 0x02}, {"", 0x03}, {"", 0x04}, {"", 0x05}, {"", 0x06}, {"", 0x07}, {"\b", 0x08}, {"", 0x09}, + {"\n", 0x0A}, {"", 0x0B}, {"", 0x0C}, {"\n", 0x0D}, {"", 0x0E}, {"", 0x0F}, {"", 0x10}, {"", 0x11}, {"", 0x12}, + {"", 0x13}, {"", 0x14}, {"", 0x15}, {"", 0x16}, {"", 0x17}, {"", 0x18}, {"", 0x19}, {"", 0x1A}, {"", 0x1B}, + {"", 0x1C}, {"", 0x1D}, {"", 0x1E}, {"", 0x1F}, {" ", 0x20}, {"!", 0x21}, {"\"", 0x22}, {"#", 0x23}, {"$", 0x24}, {"%", 0x25}, {"&", 0x26}, + {"'", 0x27}, {"(", 0x28}, {")", 0x29}, {"*", 0x2A}, {"+", 0x2B}, {",", 0x2C}, {"-", 0x2D}, {".", 0x2E}, {"/", 0x2F}, + {"0", 0x30}, {"1", 0x31}, {"2", 0x32}, {"3", 0x33}, {"4", 0x34}, {"5", 0x35}, {"6", 0x36}, {"7", 0x37}, {"8", 0x38}, {"9", 0x39}, {":", 0x3A}, {";", 0x3B}, {"=", 0x3D}, {"?", 0x3F}, {"@", 0x40}, + {"A", 0x41}, {"B", 0x42}, {"C", 0x43}, {"D", 0x44}, {"E", 0x45}, {"F", 0x46}, {"G", 0x47}, {"H", 0x48}, {"I", 0x49}, {"J", 0x4A}, {"K", 0x4B}, {"L", 0x4C}, {"M", 0x4D}, {"N", 0x4E}, {"O", 0x4F}, + {"P", 0x50}, {"Q", 0x51}, {"R", 0x52}, {"S", 0x53}, {"T", 0x54}, {"U", 0x55}, {"V", 0x56}, {"W", 0x57}, {"X", 0x58}, {"Y", 0x59}, {"Z", 0x5A}, {"[", 0x5B}, {"\\", 0x5C}, {"]", 0x5D}, {"^", 0x5E}, {"_", 0x5F}, {"`", 0x60}, + {"a", 0x61}, {"b", 0x62}, {"c", 0x63}, {"d", 0x64}, {"e", 0x65}, {"f", 0x66}, {"g", 0x67}, {"h", 0x68}, {"i", 0x69}, {"j", 0x6A}, {"k", 0x6B}, {"l", 0x6C}, {"m", 0x6D}, {"n", 0x6E}, {"o", 0x6F}, + {"p", 0x70}, {"q", 0x71}, {"r", 0x72}, {"s", 0x73}, {"t", 0x74}, {"u", 0x75}, {"v", 0x76}, {"w", 0x77}, {"x", 0x78}, {"y", 0x79}, {"z", 0x7A}, {"{", 0x7B}, {"|", 0x7C}, {"}", 0x7D}, {"~", 0x7E}, {"", 0x7F} + }; + +/*! + * @brief Converts and returns a Cola-ASCII telegram to string. + * @param[in] cola_telegram Cola-ASCII telegram, starting with 0x02 and ending with 0x03 + * @return Cola-ASCII string, f.e. "sMN SetAccessMode 3 F4724744" + */ +std::string sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(const std::vector & cola_telegram) +{ + std::stringstream cola_ascii; + for(std::vector::const_iterator iter = cola_telegram.cbegin(); iter != cola_telegram.cend(); iter++) + { + // Convert binary to ascii by lookup table + cola_ascii << s_ascii_table[((*iter) & 0xFF)]; + } + return cola_ascii.str(); +} + +/*! + * @brief Converts and returns a Cola telegram from Cola-ASCII to Cola-Binary. + * @param[in] cola_telegram Cola-ASCII string, f.e. "sMN SetAccessMode 3 F4724744" + * @return Cola-ASCII telegram, f.e. { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, + * 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 } + */ +std::vector sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(const std::string & cola_telegram) +{ + std::vector cola_ascii; + cola_ascii.reserve(cola_telegram.size()); + for(size_t char_cnt = 0; char_cnt < cola_telegram.size(); char_cnt++) + { + cola_ascii.push_back(0); // default, overwrite by value converted from ascii + // Convert ascii string to binary by lookup table + for(std::map::const_iterator map_iter = s_ascii_map.cbegin(); map_iter != s_ascii_map.cend(); map_iter++) + { + if(strncmp(map_iter->first.c_str(), cola_telegram.c_str() + char_cnt, map_iter->first.size()) == 0) + { + cola_ascii.back() = map_iter->second; + char_cnt += (map_iter->first.size() - 1); + break; + } + } + } + return cola_ascii; +} + +static uint8_t CRC8XOR(uint8_t* msgBlock, size_t len) +{ + uint8_t xorVal = 0x00; + for (size_t i = 0; i < len; i++) + xorVal ^= (msgBlock[i]); + return xorVal; +} + +/*! + * @brief Converts and returns a Cola telegram from Cola-ASCII to Cola-Binary. + * @param[in] cola_telegram Cola-ASCII telegram, f.e. { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, + * 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 } + * ("sMN SetAccessMode 3 F4724744") + * @param[in] parameter_is_ascii Command parameter given in ascii (f.e. if true, a parameter 0x31 == ASCII-"1" will be converted to 0x01 (default), otherwise to 0x01) + * @return Cola-Binary telegram + */ +std::vector sick_lidar_localization::ColaAsciiBinaryConverter::ColaAsciiToColaBinary(const std::vector & cola_telegram, bool parameter_is_ascii) +{ + std::vector cola_binary; + cola_binary.reserve(cola_telegram.size()); + const uint8_t binary_stx = 0x02; + const uint8_t binary_separator = 0x20; + + // Split telegram in command_type (sRN,sRA,sMN,sMA,sWN), command_name ("SetAccessMode", "LocSetResultPoseEnabled", + // "LocRequestTimestamp", etc.) and command parameter + sick_lidar_localization::SickLocColaTelegramMsg cola_msg = sick_lidar_localization::ColaParser::decodeColaTelegram(cola_telegram); + if(cola_msg.command_type < 0 || cola_msg.command_type >= sick_lidar_localization::ColaParser::MAX_COLA_COMMAND_NUMBER) + { + ROS_ERROR_STREAM("## ERROR in ColaAsciiToColaBinary(): invalid SOPAS command type " << cola_msg.command_type); + return cola_binary; + } + std::string command_type = sick_lidar_localization::ColaParser::convertSopasCommand((sick_lidar_localization::ColaParser::COLA_SOPAS_COMMAND)cola_msg.command_type); + + // Encode the payload + std::vector binary_payload; + binary_payload.reserve(cola_telegram.size()); + for(size_t n = 0; n < command_type.size(); n++) + binary_payload.push_back(command_type[n]); + binary_payload.push_back(binary_separator); + for(size_t n = 0; n < cola_msg.command_name.size(); n++) + binary_payload.push_back(cola_msg.command_name[n]); + if(cola_msg.parameter.size() > 0) + { + binary_payload.push_back(binary_separator); + for(size_t n = 0; n < cola_msg.parameter.size(); n++) + { + std::string parameter = cola_msg.parameter[n]; + if(parameter_is_ascii) // parameter given in ascii, default: true + { + if ((parameter.size() % 2) != 0) + parameter = "0" + cola_msg.parameter[n]; + for (size_t m = 1; m < parameter.size(); m += 2) + { + try + { + 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) + { + ROS_ERROR_STREAM("## ERROR in ColaAsciiToColaBinary(): can't convert parameter value " << parameter.substr(m - 1, 2) << " to hex, exception " << exc.what()); + return cola_binary; + } + } + } + else // copy parameter (parameter is byte array, default: false) + { + if(n > 0) + binary_payload.push_back(binary_separator); // parameter is byte array -> payload including 0x20 + for (size_t m = 0; m < parameter.size(); m++) + { + uint8_t val = static_cast(parameter[m] & 0xFF); + binary_payload.push_back(val); + } + } + } + } + + // Concatenate Cola binary telegram: { 4 byte STX } + { 4 byte length } + { payload } + { checksum } + for(size_t n = 0; n < 4; n++) + cola_binary.push_back(binary_stx); // 4 byte STX + uint32_t payload_length = binary_payload.size(); + for(int n = 24; n >= 0; n-=8) + cola_binary.push_back((payload_length >> n) & 0xFF); // 4 byte payload length, big endian + for(size_t n = 0; n < binary_payload.size(); n++) + cola_binary.push_back(binary_payload[n]); // payload + uint8_t checksum = CRC8XOR(binary_payload.data(), binary_payload.size()); + cola_binary.push_back(checksum); // CRC8XOR checksum + + return cola_binary; +} + +/*! + * @brief Converts and returns a Cola telegram from Cola-ASCII to Cola-Binary. + * @param[in] cola_telegram Cola-Binary telegram + * @param[in] parameter_to_ascii Conversion of command parameter to ascii (f.e. if true, a parameter 0x01 == ASCII-"1" will be converted to 0x31 (default), otherwise to 0x01) + * @return Cola-ASCII telegram, f.e. { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, + * 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 } + * ("sMN SetAccessMode 3 F4724744") + */ +std::vector sick_lidar_localization::ColaAsciiBinaryConverter::ColaBinaryToColaAscii(const std::vector & cola_telegram, bool parameter_to_ascii) +{ + const uint8_t binary_separator = 0x20; + std::vector cola_ascii; + // Decode header + if(!IsColaBinary(cola_telegram)) + { + ROS_ERROR_STREAM("## ERROR in ColaBinaryToColaAscii(): cola telegram is not Cola-Binary"); + return cola_ascii; + } + uint32_t telegram_length = ColaBinaryTelegramLength(cola_telegram); + if(telegram_length != cola_telegram.size()) + { + ROS_ERROR_STREAM("## ERROR in ColaBinaryToColaAscii(): invalid cola telegram length (" << cola_telegram.size() << " byte, expected " << telegram_length << " byte)"); + return cola_ascii; + } + // Extract payload + std::vector binary_payload; + binary_payload.reserve(cola_telegram.size()); + for(size_t n = 8; n < telegram_length - 1; n++) + binary_payload.push_back(cola_telegram[n]); + // Check crc checksum + uint8_t checksum = CRC8XOR(binary_payload.data(), binary_payload.size()); + if(checksum != cola_telegram.back()) + { + ROS_ERROR_STREAM("## ERROR in ColaBinaryToColaAscii(): invalid checksum (expected: " << checksum << ", received: " << cola_telegram.back() << ")"); + return cola_ascii; + } + // Find start of command parameter + size_t parameter_start_idx = 0; + while(parameter_start_idx < binary_payload.size() && binary_payload[parameter_start_idx] != binary_separator) + parameter_start_idx++; + parameter_start_idx++; + while(parameter_start_idx < binary_payload.size() && binary_payload[parameter_start_idx] != binary_separator) + parameter_start_idx++; + parameter_start_idx++; + // Concatenate Cola-ASCII telegram + std::vector stx = sick_lidar_localization::ColaParser::binarySTX(); + std::vector etx = sick_lidar_localization::ColaParser::binaryETX(); + cola_ascii.reserve(2 * cola_telegram.size()); + // Copy STX + for(size_t n = 0; n < stx.size(); n++) + cola_ascii.push_back(stx[n]); + if(parameter_to_ascii) // Convert command parameter to ASCII, default: true + { + // Copy command type and name + for (size_t n = 0; n < binary_payload.size() && n < parameter_start_idx; n++) + cola_ascii.push_back(binary_payload[n]); + // Convert command parameter to ASCII + for (size_t n = parameter_start_idx; n < binary_payload.size(); n++) + { + std::stringstream hexstring; + hexstring << std::hex << (uint32_t) binary_payload[n]; + for (size_t m = 0; m < hexstring.str().size(); m++) + cola_ascii.push_back(hexstring.str()[m]); + } + } + else + { + // Copy command type, name and parameter + for (size_t n = 0; n < binary_payload.size(); n++) + cola_ascii.push_back(binary_payload[n]); + } + // Copy ETX + for(size_t n = 0; n < etx.size(); n++) + cola_ascii.push_back(etx[n]); + return cola_ascii; +} + +/*! + * @brief Returns true for Cola-Binary, if a given telegram is Cola-Binary encoded and starts with 4 Bytes + * { 0x02, 0x02, 0x02, 0x02 }, otherwise false (Cola-ASCII telegram) + * @param[in] cola_telegram Cola telegram (Cola-ASCII to Cola-Binary) + * @return true for Cola-Binary, false for Cola-ASCII + */ +bool sick_lidar_localization::ColaAsciiBinaryConverter::IsColaBinary(const std::vector & cola_telegram) +{ + const uint8_t binary_stx = 0x02; + return cola_telegram.size() >= 4 && cola_telegram[0] == binary_stx && cola_telegram[1] == binary_stx + && cola_telegram[2] == binary_stx && cola_telegram[3] == binary_stx; +} + +/*! + * @brief Decodes the header and returns the length of a Cola-Binary telegram + * @param[in] cola_telegram Cola-Binary telegram + * @return expected telegram length incl. header, payload and crc, or 0 in case of errors. + */ +uint32_t sick_lidar_localization::ColaAsciiBinaryConverter::ColaBinaryTelegramLength(const std::vector & cola_telegram) +{ + if(cola_telegram.size() >= 8 && IsColaBinary(cola_telegram)) + { + uint32_t payload_length = 0; + for(size_t n = 4; n < 8; n++) + { + payload_length = payload_length << 8; + payload_length |= ((cola_telegram[n]) & 0xFF); + } + return payload_length + 9; // 8 byte header + payload_length + 1 byte checksum + } + return 0; +} \ No newline at end of file diff --git a/src/cola_parser.cpp b/src/cola_parser.cpp new file mode 100644 index 0000000..910ceaa --- /dev/null +++ b/src/cola_parser.cpp @@ -0,0 +1,231 @@ +/* + * @brief sim_loc_cola_parser parses and converts binary Cola telegrams to ros messages SickLocColaTelegramMsg + * and vice versa. + * + * See Operation-Instruction-v1.1.0.241R.pdf, chapter 5.8 "About CoLa-A telegrams", page 46-48, + * Telegram-Listing-v1.1.0.241R.pdf, chapter 2.3.9 "Command: LocRequestTimestamp", page 21, and + * Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.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 +#include + +#include "sick_lidar_localization/cola_parser.h" + +/*! + * @brief 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 + */ +const std::string sick_lidar_localization::ColaParser::s_command_type_string[MAX_COLA_COMMAND_NUMBER] = + { + "sINVALID", "sRN", "sRA", "sMN", "sAN", "sMA", "sWN", "sWA", "sEN", "sEA", "sSN" + }; + +/*! + * @brief static map to convert COLA_SOPAS_COMMANDs from string to enum, f.e. s_command_type_map["sRN"]:=sRN, s_command_type_map["sRA"]:=sRA and so on + */ +const std::map sick_lidar_localization::ColaParser::s_command_type_map = + { + {"", sINVALID}, {"sINVALID", sINVALID}, {"sRN", sRN}, {"sRA", sRA}, {"sMN", sMN}, {"sAN", sAN}, {"sMA", sMA}, {"sWN", sWN}, {"sWA", sWA}, {"sEN", sEN}, {"sEA", sEA}, {"sSN", sSN} + }; + +/*! + * @brief All Cola-ACSII telegrams start with s_cola_ascii_start_tag := "" ("Start of TeXt") + */ +const std::string sick_lidar_localization::ColaParser::s_cola_ascii_start_tag = ""; + +/*! + * @brief All Cola-ACSII telegrams start with s_cola_ascii_start_tag := "" ("End of TeXt") + */ +const std::string sick_lidar_localization::ColaParser::s_cola_ascii_end_tag = ""; + +/*! + * @brief Create and returns a Cola telegram (type SickLocColaTelegramMsg). + * @param[in] command_type One of the SOPAS Commands enumerated in COLA_SOPAS_COMMAND (sRN, sRA, sMN, sMA, or sWN) + * @param[in] command_name Name of command like "SetAccessMode", "LocSetResultPoseEnabled", "LocRequestTimestamp", etc. + * @param[in] parameter Optional list of command parameter + * @return Cola telegram of type SickLocColaTelegramMsg + */ +sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::ColaParser::createColaTelegram(const COLA_SOPAS_COMMAND & command_type, + const std::string & command_name, const std::vector & parameter) +{ + sick_lidar_localization::SickLocColaTelegramMsg cola_telegram; + cola_telegram.header.stamp = ros::Time::now(); + cola_telegram.command_type = command_type; + cola_telegram.command_name = command_name; + cola_telegram.parameter = parameter; + return cola_telegram; +} + +/*! + * @brief Decodes and returns a Cola message of type SickLocColaTelegramMsg from a Cola-Binary telegram. + * Note: If decoding fails (invalid binary telegram or parse error), command_type of the returned Cola telegram message + * will be sINVALID. + * @param[in] cola_binary Cola-Binary telegram, f.e. { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, + * 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 } + * @return Cola telegram message (type SickLocColaTelegramMsg) + */ +sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::ColaParser::decodeColaTelegram(const std::vector & cola_binary) +{ + std::string cola_ascii = sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(cola_binary); + return decodeColaTelegram(cola_ascii); +} + +/*! + * @brief Converts and returns a Cola message of type SickLocColaTelegramMsg from a Cola-ASCII telegram. + * @param[in] cola_ascii Cola-ASCII telegram, f.e. "sMN LocRequestTimestamp" + * @return Cola telegram message (type SickLocColaTelegramMsg) + */ +sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::ColaParser::decodeColaTelegram(const std::string & cola_ascii) +{ + // Check and remove start and end tags ("" and "") + std::string cola_ascii_cmd; + if (cola_ascii.size() > s_cola_ascii_start_tag.size() + s_cola_ascii_end_tag.size() + && cola_ascii.substr(0, s_cola_ascii_start_tag.size()) == s_cola_ascii_start_tag + && cola_ascii.substr(cola_ascii.size() - s_cola_ascii_end_tag.size()) == s_cola_ascii_end_tag) + { + cola_ascii_cmd = cola_ascii.substr(s_cola_ascii_start_tag.size(), cola_ascii.size() - s_cola_ascii_start_tag.size() - s_cola_ascii_end_tag.size()); + } + else + { + cola_ascii_cmd = cola_ascii; + } + // Split in command_type, command_name and optional parameter by spaces + std::vector cola_parts; + boost::split(cola_parts, cola_ascii_cmd, boost::algorithm::is_space()); + if(cola_parts.size() < 2) // at least command_type and command_name required + { + ROS_WARN_STREAM("## ERROR Parse error in ColaParser::decodeColaTelegram(\"" << cola_ascii_cmd << "\"): to few arguments, at least command_type and command_name required"); + return createColaTelegram(sINVALID, ""); + } + // Convert command_type from string to COLA_SOPAS_COMMAND + sick_lidar_localization::ColaParser::COLA_SOPAS_COMMAND command_type = convertSopasCommand(cola_parts[0]); + if(command_type == sINVALID) + { + ROS_WARN_STREAM("## ERROR Parse error in ColaParser::decodeColaTelegram(\"" << cola_ascii_cmd << "\"): invalid command_type \"" << cola_parts[0] << "\""); + return createColaTelegram(sINVALID, ""); + } + // Check command_name + if(cola_parts[1].empty()) + { + ROS_WARN_STREAM("## ERROR Parse error in ColaParser::decodeColaTelegram(\"" << cola_ascii_cmd << "\"): invalid command_name \"" << cola_parts[1] << "\""); + return createColaTelegram(sINVALID, ""); + } + // Append command parameter + sick_lidar_localization::SickLocColaTelegramMsg cola_telegram = createColaTelegram(command_type, cola_parts[1]); + if(cola_parts.size() > 2) + { + cola_telegram.parameter.reserve(cola_parts.size() - 2); + for(size_t n = 2; n < cola_parts.size(); n++) + cola_telegram.parameter.push_back(cola_parts[n]); + } + return cola_telegram; +} + +/*! + * @brief Encodes and returns a Cola Binary telegram from ros message SickLocColaTelegramMsg. + * @param[in] cola_telegram Cola telegram, f.e. createColaTelegram(sMN, "SetAccessMode", {"3", "F4724744"}) + * @return Cola-Binary telegram, f.e. { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, + * 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 } + */ +std::vector sick_lidar_localization::ColaParser::encodeColaTelegram(const sick_lidar_localization::SickLocColaTelegramMsg & cola_telegram) +{ + assert(cola_telegram.command_type > sINVALID && cola_telegram.command_type < MAX_COLA_COMMAND_NUMBER); + std::stringstream cola_ascii; + cola_ascii << s_cola_ascii_start_tag; + cola_ascii << convertSopasCommand((COLA_SOPAS_COMMAND)cola_telegram.command_type); + cola_ascii << " " << cola_telegram.command_name; + for(size_t n = 0; n < cola_telegram.parameter.size(); n++) + cola_ascii << " " << cola_telegram.parameter[n]; + cola_ascii << s_cola_ascii_end_tag; + return sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(cola_ascii.str()); +} + +/*! + * @brief Encodes and returns a Cola telegram. + * @param[in] command_type One of the SOPAS Commands enumerated in COLA_SOPAS_COMMAND (sRN, sRA, sMN, sMA, or sWN) + * @param[in] command_name Name of command like "SetAccessMode", "LocSetResultPoseEnabled", "LocRequestTimestamp", etc. + * @param[in] parameter Optional list of command parameter + * @return Cola-Binary telegram + */ +std::vector sick_lidar_localization::ColaParser::encodeColaTelegram(const COLA_SOPAS_COMMAND & command_type, const std::string & command_name, + const std::vector & parameter) +{ + return encodeColaTelegram(createColaTelegram(command_type, command_name, parameter)); +} + +/*! + * @brief Converts and returns a COLA_SOPAS_COMMAND to string. + * Example: convertSopasCommand(sMN) returns "sMN". + * @param[in] command_type One of the SOPAS Commands enumerated in COLA_SOPAS_COMMAND (sRN, sRA, sMN, sMA, or sWN) + * @return COLA_SOPAS_COMMAND as string. + */ +std::string sick_lidar_localization::ColaParser::convertSopasCommand(sick_lidar_localization::ColaParser::COLA_SOPAS_COMMAND command_type) +{ + return s_command_type_string[command_type]; +} + +/*! + * @brief Converts and returns a COLA_SOPAS_COMMAND from string. + * Example: convertSopasCommand("sMN") returns sMN. + * @param[in] sopas_command One of the SOPAS commands ("sRN", "sRA", "sMN", "sMA", or "sWN") + * @return COLA_SOPAS_COMMAND from string. + */ +sick_lidar_localization::ColaParser::COLA_SOPAS_COMMAND sick_lidar_localization::ColaParser::convertSopasCommand(const std::string & sopas_command) +{ + std::map::const_iterator iter_command_type = s_command_type_map.find(sopas_command); + sick_lidar_localization::ColaParser::COLA_SOPAS_COMMAND command_type = (iter_command_type != s_command_type_map.cend()) ? (iter_command_type->second) : sINVALID; + return command_type; +} + diff --git a/src/cola_transmitter.cpp b/src/cola_transmitter.cpp new file mode 100644 index 0000000..f682e2e --- /dev/null +++ b/src/cola_transmitter.cpp @@ -0,0 +1,331 @@ +/* + * @brief sim_loc_cola_transmitter connects to the localization controller, sends cola command requests, + * and waits for the response with timeout. + * + * 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_parser.h" +#include "sick_lidar_localization/cola_transmitter.h" +#include "sick_lidar_localization/utils.h" + +/*! + * Constructor. + * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 + * @param[in] tcp_port tcp port for command requests, default: 2111 for command requests and 2112 for command responses + * @param[in] default_receive_timeout default timeout in seconds for receive functions + */ +sick_lidar_localization::ColaTransmitter::ColaTransmitter(const std::string & server_adress, int tcp_port, double default_receive_timeout) +: m_server_adress(server_adress), m_tcp_port(tcp_port), m_ioservice(), m_tcp_socket(m_ioservice), + m_receive_timeout(default_receive_timeout), m_receiver_thread_running(false), m_receiver_thread(0) +{ + +} + +/*! + * Destructor, closes all tcp connections. + */ +sick_lidar_localization::ColaTransmitter::~ColaTransmitter() +{ + m_receiver_thread_running = false; + close(); + stopReceiverThread(); +} + +/*! + * Connects to the localization server. + * @return true on success, false on failure (localization server unknown or unreachable) + */ +bool sick_lidar_localization::ColaTransmitter::connect(void) +{ + try + { + // Connect to localization controller + boost::asio::ip::tcp::resolver tcpresolver(m_ioservice); + boost::asio::ip::tcp::resolver::query tcpquery(m_server_adress, std::to_string(m_tcp_port)); + boost::asio::ip::tcp::resolver::iterator it = tcpresolver.resolve(tcpquery); + boost::system::error_code errorcode; + m_tcp_socket.connect(*it, errorcode); + return !errorcode && m_tcp_socket.is_open(); + } + catch(std::exception & exc) + { + ROS_WARN_STREAM("## ERROR ColaTransmitter::connect(): connect to " << m_server_adress << ":" << m_tcp_port << " failed, exception " << exc.what()); + } + return false; +} + +/*! + * Closes the tcp connection to the localization server. + * @return always true + */ +bool sick_lidar_localization::ColaTransmitter::close(void) +{ + try + { + if (m_tcp_socket.is_open()) + { + m_tcp_socket.shutdown(boost::asio::ip::tcp::socket::shutdown_both); + m_tcp_socket.close(); + } + m_ioservice.stop(); + return true; + } + catch(std::exception & exc) + { + ROS_WARN_STREAM("ColaTransmitter::closeTcpConnections(): exception " << exc.what() << " on closing connection."); + } + return false; +} + +/*! + * Send data to the localization server. + * @param[in] data data to be send + * @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) +{ + return send(m_tcp_socket, data, send_timestamp); +} + +/*! + * Send data to the localization server. + * @param[in] socket socket to write to + * @param[in] data data to be send + * @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) +{ + try + { + if (ros::ok() && socket.is_open()) + { + boost::system::error_code errorcode; + send_timestamp = ros::Time::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; + ROS_WARN_STREAM("## ERROR ColaTransmitter::send: tcp socket write error, " << bytes_written << " of " << data.size() << " bytes written, errorcode " << errorcode.value() << " \"" << errorcode.message() << "\""); + } + } + catch(std::exception & exc) + { + ROS_WARN_STREAM("## ERROR ColaTransmitter::send(): exception " << exc.what()); + } + return false; +} + +/*! + * Receive a cola telegram from the localization server. + * @param[out] telegram telegram received (Cola-Binary or Cola-Ascii) + * @param[in] timeout timeout in seconds + * @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) +{ + return receive(m_tcp_socket, telegram, timeout, receive_timestamp); +} + +/*! + * Receive a cola telegram from a socket. + * @param[in] socket socket to read from + * @param[out] telegram telegram received (Cola-Binary or Cola-Ascii) + * @param[in] timeout timeout in seconds + * @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) +{ + telegram.clear(); + try + { + std::vector binETX = sick_lidar_localization::ColaParser::binaryETX(); + ros::Time start_time = ros::Time::now(); + while (ros::ok() && socket.is_open()) + { + // Read 1 byte + uint8_t byte_received = 0; + boost::system::error_code errorcode; + 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 + telegram.push_back(byte_received); + } + else + ros::Duration(0.0001).sleep(); + // 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) + { + // Cola-Binary: Check telegram length and return if all bytes received + uint32_t telegram_length = sick_lidar_localization::ColaAsciiBinaryConverter::ColaBinaryTelegramLength(telegram); + if(telegram_length > 0 && telegram.size() >= telegram_length) + return true; // all bytes received, telegram completed + } + else + { + // Cola-ASCII: Check for "" (message completed) and return if received data ends with "" + if (dataEndWithETX(telegram, binETX)) + return true; // received, telegram completed + } + // Check for timeout + if ((ros::Time::now() - start_time).toSec() >= timeout) + { + // ROS_DEBUG_STREAM("ColaTransmitter::receive(): timeout, " << telegram.size() << " byte received: " << sick_lidar_localization::Utils::toHexString(telegram)); + break; + } + } + } + catch(std::exception & exc) + { + ROS_WARN_STREAM("## ERROR ColaTransmitter::receive(): exception " << exc.what()); + } + return false; // no tcp connection or timeout +} + +/*! + * Returns true, if data end with etx, i.e. the trailing (etx.size()) byte in data are identical to etx. + * @param[in] data data received + * @param[in] etx ETX tag (binary or ascii) + * @return true if data end with etx, false otherwise + */ +bool sick_lidar_localization::ColaTransmitter::dataEndWithETX(const std::vector & data, const std::vector & etx) +{ + if(data.size() < etx.size()) + return false; + for(size_t n = data.size() - etx.size(), m = 0; m < etx.size(); n++, m++) + { + if(data[n] != etx[m]) + return false; + } + return true; +} + +/*! + * Starts a thread to receive response telegrams from the localization server. + * The receiver thread pushes responses to a fifo buffer, which can be popped by waitPopResponse(). + * @return always true + */ +bool sick_lidar_localization::ColaTransmitter::startReceiverThread(void) +{ + stopReceiverThread(); + m_receiver_thread_running = true; + m_receiver_thread = new boost::thread(&sick_lidar_localization::ColaTransmitter::runReceiverThreadCb, this); + return true; +} + +/*! + * Stops the thread to receive response telegrams from the localization server (if a thread has been started by startReceiverThread=. + * @return always true + */ +bool sick_lidar_localization::ColaTransmitter::stopReceiverThread(void) +{ + m_receiver_thread_running = false; + if(m_receiver_thread) + { + m_receiver_thread->join(); + delete(m_receiver_thread); + m_receiver_thread = 0; + } + return true; +} + +/*! + * Returns a response telegram from the localization server. + * This function waits with timout, until the receiver thread received a response telegram from the localization server. + * Note: The receiver thread must have been started by startReceiverThread(), otherwise waitPopResponse() will fail + * after timout. + * @param[out] telegram telegram received (Cola-Binary or Cola-Ascii) + * @param[in] timeout timeout in seconds + * @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) +{ + ros::Time start_time = ros::Time::now(); + while(ros::ok() && m_receiver_thread_running && m_response_fifo.empty()) + { + ros::Duration(0.0001).sleep(); + m_response_fifo.waitOnceForElement(); + if((ros::Time::now() - start_time).toSec() >= timeout) + break; + } + if(!m_response_fifo.empty()) + { + ColaResponseContainer response = m_response_fifo.pop(); + telegram = response.telegram_data; + receive_timestamp = response.receive_timestamp; + return true; + } + return false; // timeout +} + +/*! + * Thread callback, receives response telegrams from localization server and pushes them to m_response_fifo. + */ +void sick_lidar_localization::ColaTransmitter::runReceiverThreadCb(void) +{ + while(ros::ok() && m_receiver_thread_running) + { + ColaResponseContainer response; + if(receive(response.telegram_data, m_receive_timeout, response.receive_timestamp)) + m_response_fifo.push(response); + else + m_response_fifo.notify(); + } +} diff --git a/src/sim_loc_driver.cpp b/src/driver.cpp similarity index 75% rename from src/sim_loc_driver.cpp rename to src/driver.cpp index 085cf55..a74ae45 100644 --- a/src/sim_loc_driver.cpp +++ b/src/driver.cpp @@ -59,7 +59,7 @@ #include #include -#include "sick_lidar_localization/sim_loc_driver_monitor.h" +#include "sick_lidar_localization/driver_monitor.h" int main(int argc, char** argv) { @@ -68,26 +68,40 @@ int main(int argc, char** argv) ros::NodeHandle nh; ROS_INFO_STREAM("sim_loc_driver started."); - // Create a worker thread, which - // - connects to the localization controller (f.e. SIM1000FXA), - // - receives binary result port telegrams, - // - converts them to SickLocResultPortTelegramMsg - // - publishes the sim location data + // 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" - int tcp_port = 2201; // Default: The localization controller uses IP port number 2201 to send localization results + // 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. + // * SOPAS CoLa-A or CoLa-B protocols + // 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. + // * 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_adress" , server_adress, server_adress); ros::param::param("/sick_lidar_localization/driver/localization_controller_default_ip_adress", server_default_adress, server_default_adress); - ros::param::param("/sick_lidar_localization/driver/result_telegrams_tcp_port", tcp_port, tcp_port); + 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); server_adress = (server_adress.empty()) ? server_default_adress : server_adress; // Initialize driver threads to connect to localization controller and to monitor driver messages - sick_lidar_localization::DriverMonitor driver_monitor(&nh, server_adress, tcp_port); + // DriverMonitor creates a worker thread, which + // - connects to the localization controller (f.e. SIM1000FXA), + // - 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); // 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); + // 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); + 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 if(!driver_monitor.start()) { diff --git a/src/sim_loc_driver_check.cpp b/src/driver_check.cpp similarity index 98% rename from src/sim_loc_driver_check.cpp rename to src/driver_check.cpp index a581fa9..cd9da32 100644 --- a/src/sim_loc_driver_check.cpp +++ b/src/driver_check.cpp @@ -60,7 +60,7 @@ */ #include -#include "sick_lidar_localization/sim_loc_driver_check_thread.h" +#include "sick_lidar_localization/driver_check_thread.h" int main(int argc, char** argv) { diff --git a/src/sim_loc_driver_check_thread.cpp b/src/driver_check_thread.cpp similarity index 99% rename from src/sim_loc_driver_check_thread.cpp rename to src/driver_check_thread.cpp index 04fb5d4..7b9bcc8 100644 --- a/src/sim_loc_driver_check_thread.cpp +++ b/src/driver_check_thread.cpp @@ -62,8 +62,8 @@ #include #include -#include "sick_lidar_localization/sim_loc_driver_check_thread.h" -#include "sick_lidar_localization/sim_loc_utils.h" +#include "sick_lidar_localization/driver_check_thread.h" +#include "sick_lidar_localization/utils.h" /* * Constructor, reads the configuration parameter incl. the @@ -226,7 +226,7 @@ sick_lidar_localization::SickLocResultPortTelegramMsg sick_lidar_localization::M 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 [1 … 100], 1 = bad pose quality, 100 = good pose quality. uint8, size:= UInt8 = 1 byte + 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 diff --git a/src/driver_monitor.cpp b/src/driver_monitor.cpp new file mode 100644 index 0000000..12018dd --- /dev/null +++ b/src/driver_monitor.cpp @@ -0,0 +1,298 @@ +/* + * @brief sim_loc_driver_monitor monitors the driver thread implemented by sick_lidar_localization::DriverThread + * and starts a new driver thread after tcp errors (connection lost, socket shutdown, message timeouts etc.). + * + * 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_parser.h" +#include "sick_lidar_localization/driver_monitor.h" + +/*! + * Constructor. The driver monitor does not start automatically, call start() and stop() to start and stop. + * + * 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. + * - SOPAS CoLa-A or CoLa-B protocols. + * 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. + * - Binary result port protocol TCP/IP. + * + * @param[in] nh ros node handle + * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 + * @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) +: 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) +{ + 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); + 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 + m_initialized = true; + } +} + +/*! + * Destructor. Stops the driver thread and monitor and closes all tcp connections. + */ +sick_lidar_localization::DriverMonitor::~DriverMonitor() +{ + stop(); +} + +/*! + * Starts the driver monitor and driver thread. + * @return true on success, false on failure. + */ +bool sick_lidar_localization::DriverMonitor::start(void) +{ + if(!m_initialized) // DriverMonitor not properly initialized + { + ROS_ERROR_STREAM("## ERROR sick_lidar_localization::DriverMonitor::start(): DriverMonitor not initialized"); + return false; + } + // Create receiver thread to receive telegrams from the localization controller + m_monitoring_thread_running = true; + m_monitoring_thread = new boost::thread(&sick_lidar_localization::DriverMonitor::runMonitorThreadCb, this); + return true; +} + +/*! + * Stops the driver monitor and thread and closes all tcp connections. + * @return true on success, false on failure. + */ +bool sick_lidar_localization::DriverMonitor::stop(void) +{ + m_monitoring_thread_running = false; + if(m_monitoring_thread) + { + m_monitoring_thread->join(); + delete(m_monitoring_thread); + m_monitoring_thread = 0; + } + stopColaTransmitter(); + return true; +} + +/*! + * Initializes cola sender and receiver for cola requests and responses + * @param[in] server_adress ip adress of the localization controller, default: 192.168.0.1 + * @param[in] ip_port_cola ip port for command requests and responses, default: 2111 + * @param[in] receive_timeout timeout in seconds for receive functions + */ +bool sick_lidar_localization::DriverMonitor::initColaTransmitter(const std::string & server_adress, int ip_port_cola, double receive_timeout) +{ + if(!m_cola_transmitter) + { + m_cola_transmitter = new sick_lidar_localization::ColaTransmitter(server_adress, ip_port_cola); + if (!m_cola_transmitter->connect()) + { + ROS_WARN_STREAM("## ERROR DriverMonitor::serviceCbColaTelegram: can't connect to localization server " << server_adress << ":" << ip_port_cola); + return false; + } + if (!m_cola_transmitter->startReceiverThread()) + { + ROS_WARN_STREAM("## ERROR DriverMonitor::serviceCbColaTelegram: can't start receiver thread"); + return false; + } + } + return true; +} + +/*! + * Stops cola sender and receiver for cola requests and responses + * (if started by a cola service request "SickLocColaTelegram") + */ +void sick_lidar_localization::DriverMonitor::stopColaTransmitter(void) +{ + if(m_cola_transmitter) + { + delete(m_cola_transmitter); + m_cola_transmitter = 0; + } +} + +/*! + * Callback for result telegram messages (SickLocResultPortTelegramMsg) from sim_loc_driver. + * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) + */ +void sick_lidar_localization::DriverMonitor::messageCbResultPortTelegrams(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg) +{ + m_driver_message_recv_timestamp.set(ros::Time::now()); +} + +/*! + * Callback for service messages (SickLocColaTelegramSrv). Handles Cola requests and responses, sends and receives Cola-ASCII telegrams to resp. from + * the localization controller + * @param[in] cola_request Cola command request, will be encoded and send to the localization controller + * @param[out] cola_response Cola command response from localization controller + */ +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 + // initialize cola_response with default values + cola_response.cola_ascii_response = ""; + const std::string & asciiSTX = sick_lidar_localization::ColaParser::asciiSTX(); + const std::string & asciiETX = sick_lidar_localization::ColaParser::asciiETX(); + // Convert to binary (optional, default: false) + std::string ascii_request = asciiSTX + cola_request.cola_ascii_request + asciiETX; + std::vector binary_request = sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(ascii_request); + if(m_cola_binary) // cola_request.send_binary + binary_request = sick_lidar_localization::ColaAsciiBinaryConverter::ColaAsciiToColaBinary(binary_request); + // Initialize cola sender and receiver, connect to localization controlle + if(!initColaTransmitter(m_server_adress, m_ip_port_cola, cola_request.wait_response_timeout)) + { + ROS_WARN_STREAM("## ERROR DriverMonitor::serviceCbColaTelegram: can't initialize cola transmission to localization server " << m_server_adress); + stopColaTransmitter(); + return false; + } + // Send request and wait for response with timeout + std::vector binary_response; + 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; + } + 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); + stopColaTransmitter(); + return false; + } + // Convert reponse from controller to Cola-ASCII telegram + bool is_binary_cola = sick_lidar_localization::ColaAsciiBinaryConverter::IsColaBinary(binary_response); + ROS_INFO_STREAM("DriverMonitor::serviceCbColaTelegram: " << (is_binary_cola?"cola-binary":"cola-ascii") << " response received (hex): " << sick_lidar_localization::Utils::toHexString(binary_response)); + if(m_cola_binary && is_binary_cola) // if(cola_request.send_binary) + binary_response = sick_lidar_localization::ColaAsciiBinaryConverter::ColaBinaryToColaAscii(binary_response); + cola_response.cola_ascii_response = sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(binary_response); + if (cola_response.cola_ascii_response.size() > asciiSTX.size() + asciiETX.size() + && cola_response.cola_ascii_response.substr(0, asciiSTX.size()) == asciiSTX + && cola_response.cola_ascii_response.substr(cola_response.cola_ascii_response.size() - asciiETX.size()) == asciiETX) + { + // 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) << " }"); + return true; + } + else + { + ROS_WARN_STREAM("## ERROR DriverMonitor::serviceCbColaTelegram: parse error, response from localization server \"" << cola_response.cola_ascii_response << "\" not enclosed with \"" << asciiSTX << "\" and \"" << asciiETX << "\""); + } + stopColaTransmitter(); + return false; +} + + +/*! + * Thread callback, implements the monitoring and restarts a DriverThread in case of tcp-errors. + */ +void sick_lidar_localization::DriverMonitor::runMonitorThreadCb(void) +{ + ROS_INFO_STREAM("DriverMonitor: monitoring thread started"); + 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); + if(!driver_thread->start()) + { + 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()) + { + ROS_INFO_STREAM("DriverMonitor: waiting for connection to localization controller"); + ros::Duration(1).sleep(); // 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::Duration(1).sleep(); // wait for monitoring start + } + // Monitor driver messages + ros::Duration monitoring_delay(1.0/m_monitoring_rate); + while(ros::ok() + && m_monitoring_thread_running + && driver_thread->isRunning() + && driver_thread->isConnected() + && (ros::Time::now() - m_driver_message_recv_timestamp.get()).toSec() <= m_receive_telegrams_timeout) + { + monitoring_delay.sleep(); + } + 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(); + } + delete(driver_thread); + driver_thread = 0; + } + m_monitoring_thread_running = false; + ROS_INFO_STREAM("DriverMonitor: monitoring thread finished"); +} + diff --git a/src/sim_loc_driver_thread.cpp b/src/driver_thread.cpp similarity index 76% rename from src/sim_loc_driver_thread.cpp rename to src/driver_thread.cpp index 904fccd..c50ba5e 100644 --- a/src/sim_loc_driver_thread.cpp +++ b/src/driver_thread.cpp @@ -64,9 +64,9 @@ #include #include "sick_lidar_localization/SickLocDiagnosticMsg.h" -#include "sick_lidar_localization/sim_loc_driver_thread.h" -#include "sick_lidar_localization/sim_loc_testcase_generator.h" -#include "sick_lidar_localization/sim_loc_utils.h" +#include "sick_lidar_localization/driver_thread.h" +#include "sick_lidar_localization/testcase_generator.h" +#include "sick_lidar_localization/utils.h" /* @@ -210,13 +210,20 @@ bool sick_lidar_localization::DriverThread::isRunning(void) void sick_lidar_localization::DriverThread::closeTcpConnections(bool force_shutdown) { m_tcp_connected = false; - if(force_shutdown || m_tcp_socket.is_open()) + try { - publishDiagnosticMessage(NO_ERROR, "sim_loc_driver: closing socket"); - ROS_INFO_STREAM("DriverThread::closeTcpConnections(force_shutdown=" << force_shutdown << "): shutdown socket"); - m_tcp_socket.shutdown(boost::asio::ip::tcp::socket::shutdown_both); - ROS_INFO_STREAM("DriverThread::closeTcpConnections(force_shutdown=" << force_shutdown << "): close socket"); - m_tcp_socket.close(); + if (force_shutdown || m_tcp_socket.is_open()) + { + publishDiagnosticMessage(NO_ERROR, "sim_loc_driver: closing socket"); + ROS_INFO_STREAM("DriverThread::closeTcpConnections(force_shutdown=" << force_shutdown << "): shutdown socket"); + m_tcp_socket.shutdown(boost::asio::ip::tcp::socket::shutdown_both); + ROS_INFO_STREAM("DriverThread::closeTcpConnections(force_shutdown=" << force_shutdown << "): close socket"); + m_tcp_socket.close(); + } + } + catch(std::exception & exc) + { + ROS_WARN_STREAM("DriverThread::closeTcpConnections(): exception " << exc.what() << " on closing connection."); } } @@ -227,67 +234,74 @@ void sick_lidar_localization::DriverThread::runReceiverThreadCb(void) { publishDiagnosticMessage(NO_ERROR, "sim_loc_driver: receiver thread started"); ROS_INFO_STREAM("DriverThread: receiver thread started"); - ros::Time diagnostic_msg_published; - // Connect to localization controller - while(ros::ok() && m_tcp_receiver_thread_running && !m_tcp_connected) + try { - boost::asio::ip::tcp::resolver tcpresolver(m_ioservice); - boost::asio::ip::tcp::resolver::query tcpquery(m_server_adress, std::to_string(m_tcp_port)); - boost::asio::ip::tcp::resolver::iterator it = tcpresolver.resolve(tcpquery); - boost::system::error_code errorcode; - m_tcp_socket.connect(*it, errorcode); - if(!errorcode && m_tcp_socket.is_open()) + ros::Time diagnostic_msg_published; + // Connect to localization controller + while(ros::ok() && m_tcp_receiver_thread_running && !m_tcp_connected) { - m_tcp_connected = true; - publishDiagnosticMessage(NO_ERROR, std::string("sim_loc_driver: tcp connection established to localization controller ") + m_server_adress + ":" + std::to_string(m_tcp_port)); - ROS_INFO_STREAM("DriverThread: tcp connection established to localization controller " << m_server_adress << ":" << m_tcp_port); - } - else - { - 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 - << ", error " << errorcode.value() << " \"" << errorcode.message() << "\", retry in " << m_tcp_connection_retry_delay << " seconds"); - ros::Duration(m_tcp_connection_retry_delay).sleep(); + boost::asio::ip::tcp::resolver tcpresolver(m_ioservice); + boost::asio::ip::tcp::resolver::query tcpquery(m_server_adress, std::to_string(m_tcp_port)); + boost::asio::ip::tcp::resolver::iterator it = tcpresolver.resolve(tcpquery); + boost::system::error_code errorcode; + m_tcp_socket.connect(*it, errorcode); + if(!errorcode && m_tcp_socket.is_open()) + { + m_tcp_connected = true; + publishDiagnosticMessage(NO_ERROR, std::string("sim_loc_driver: tcp connection established to localization controller ") + m_server_adress + ":" + std::to_string(m_tcp_port)); + ROS_INFO_STREAM("DriverThread: tcp connection established to localization controller " << m_server_adress << ":" << m_tcp_port); + } + else + { + 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 + << ", error " << errorcode.value() << " \"" << errorcode.message() << "\", retry in " << m_tcp_connection_retry_delay << " seconds"); + ros::Duration(m_tcp_connection_retry_delay).sleep(); + } } - } - // 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.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.is_open() && bytes_received < bytes_required) + // 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.is_open()) { - size_t bytes_to_read = bytes_required - bytes_received; - bytes_received += boost::asio::read(m_tcp_socket, boost::asio::buffer(&receive_buffer[bytes_received],bytes_to_read), boost::asio::transfer_exactly(bytes_to_read), errorcode); - if(errorcode) + 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.is_open() && bytes_received < bytes_required) { - std::stringstream error_info_stream; - error_info_stream << "DriverThread: tcp socket read errorcode " << errorcode.value() << " \"" << errorcode.message() << "\""; - if(error_info != error_info_stream.str()) + size_t bytes_to_read = bytes_required - bytes_received; + bytes_received += boost::asio::read(m_tcp_socket, boost::asio::buffer(&receive_buffer[bytes_received],bytes_to_read), boost::asio::transfer_exactly(bytes_to_read), errorcode); + if(errorcode) { - publishDiagnosticMessage(NO_TCP_CONNECTION, std::string("sim_loc_driver: tcp socket read errorcode") + std::to_string(errorcode.value()) + ", " + errorcode.message()); - ROS_WARN_STREAM(error_info_stream.str()); + std::stringstream error_info_stream; + error_info_stream << "DriverThread: tcp socket read errorcode " << errorcode.value() << " \"" << errorcode.message() << "\""; + if(error_info != error_info_stream.str()) + { + publishDiagnosticMessage(NO_TCP_CONNECTION, std::string("sim_loc_driver: tcp socket read errorcode") + std::to_string(errorcode.value()) + ", " + errorcode.message()); + ROS_WARN_STREAM(error_info_stream.str()); + } + error_info = error_info_stream.str(); } - error_info = error_info_stream.str(); + else if( (ros::Time::now() - diagnostic_msg_published).toSec() >= 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(); + } + if(bytes_received < bytes_required) + ros::Duration(0.0001).sleep(); } - else if( (ros::Time::now() - diagnostic_msg_published).toSec() >= 60) + // Copy received telegram to fifo + if(bytes_received >= bytes_required) { - 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(); - } - 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(); + } } - // 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(); - } + } + catch(std::exception & exc) + { + ROS_WARN_STREAM("## ERROR DriverThread::runReceiverThreadCb(): exception " << exc.what()); } m_tcp_receiver_thread_running = false; ROS_INFO_STREAM("DriverThread: receiver thread finished"); diff --git a/src/pointcloud_converter.cpp b/src/pointcloud_converter.cpp index 6d106b1..e9172c9 100644 --- a/src/pointcloud_converter.cpp +++ b/src/pointcloud_converter.cpp @@ -58,7 +58,7 @@ */ #include -#include "sick_lidar_localization/sim_loc_pointcloud_converter.h" +#include "sick_lidar_localization/pointcloud_converter.h" int main(int argc, char** argv) { diff --git a/src/sim_loc_pointcloud_converter.cpp b/src/pointcloud_converter_thread.cpp similarity index 76% rename from src/sim_loc_pointcloud_converter.cpp rename to src/pointcloud_converter_thread.cpp index dd420dc..f01c478 100644 --- a/src/sim_loc_pointcloud_converter.cpp +++ b/src/pointcloud_converter_thread.cpp @@ -2,6 +2,9 @@ * @brief sim_loc_pointcloud_converts sim_loc_driver messages (type sick_lidar_localization::SickLocResultPortTelegramMsg), * to PointCloud2 messages and publishes PointCloud2 messages on topic "/cloud". * + * The vehicle poses (PoseX, PoseY, PoseYaw of result port telegrams) are transformed and published + * by tf-messages with configurable parent and child frame id. + * * It also serves as an usage example for sick_lidar_localization and shows how to use sick_lidar_localization * in a custumized application. * @@ -58,8 +61,8 @@ */ #include -#include "sick_lidar_localization/sim_loc_utils.h" -#include "sick_lidar_localization/sim_loc_pointcloud_converter.h" +#include "sick_lidar_localization/utils.h" +#include "sick_lidar_localization/pointcloud_converter.h" /*! * Constructor @@ -73,6 +76,8 @@ sick_lidar_localization::PointCloudConverter::PointCloudConverter(ros::NodeHandl 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); } } @@ -105,6 +110,8 @@ bool sick_lidar_localization::PointCloudConverter::stop(void) m_converter_thread_running = false; if(m_converter_thread) { + sick_lidar_localization::SickLocResultPortTelegramMsg empty_msg; + m_result_port_telegram_fifo.push(empty_msg); // push empty telegram to interrupt converter thread waiting for notification m_converter_thread->join(); delete(m_converter_thread); m_converter_thread = 0; @@ -157,11 +164,12 @@ std::vector sick_lidar_localization::PointCloudConverter:: } /*! - * Converts a telegram from type SickLocResultPortTelegramMsg to PointCloud2 + * Converts the vehicle position from a result port telegram to PointCloud2 message with 4 points + * (centerpoint plus 3 demo corner points showing a triangle). * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) * @return PointCloud2 message */ -sensor_msgs::PointCloud2 sick_lidar_localization::PointCloudConverter::convert(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg) +sensor_msgs::PointCloud2 sick_lidar_localization::PointCloudConverter::convertToPointCloud(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg) { sensor_msgs::PointCloud2 pointcloud_msg; // Create center point (PoseX,PoseY) and 3 corner points of a triangle showing the orientation (PoseYaw) @@ -208,14 +216,41 @@ sensor_msgs::PointCloud2 sick_lidar_localization::PointCloudConverter::convert(c return pointcloud_msg; } +/*! + * Converts the vehicle pose from a result port telegram to a tf transform. + * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) + * @return tf transform + */ +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 + geometry_msgs::TransformStamped vehicle_transform; + vehicle_transform.header.stamp = ros::Time::now(); + vehicle_transform.header.frame_id = m_tf_parent_frame_id; + vehicle_transform.child_frame_id = m_tf_child_frame_id; + vehicle_transform.transform.translation.x = posx; + vehicle_transform.transform.translation.y = posy; + vehicle_transform.transform.translation.z = 0.0; + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + vehicle_transform.transform.rotation.x = q.x(); + vehicle_transform.transform.rotation.y = q.y(); + vehicle_transform.transform.rotation.z = q.z(); + vehicle_transform.transform.rotation.w = q.w(); + return vehicle_transform; +} + /*! * Thread callback, pops received telegrams from the fifo buffer m_result_port_telegram_fifo, * converts the telegrams from type SickLocResultPortTelegramMsg to PointCloud2 and publishes - * PointCloud2 messages. + * PointCloud2 messages. The vehicle pose is converted to a tf transform and broadcasted. */ void sick_lidar_localization::PointCloudConverter::runPointCloudConverterThreadCb(void) { ROS_INFO_STREAM("PointCloudConverter: converter thread for sim_loc_driver messages started"); + tf2_ros::TransformBroadcaster tf_broadcaster; while(ros::ok() && m_converter_thread_running) { // Wait for next telegram @@ -224,11 +259,16 @@ void sick_lidar_localization::PointCloudConverter::runPointCloudConverterThreadC ros::Duration(0.0001).sleep(); m_result_port_telegram_fifo.waitForElement(); } - sick_lidar_localization::SickLocResultPortTelegramMsg telegram = m_result_port_telegram_fifo.pop(); - // Convert telegram to PointCloud2 - sensor_msgs::PointCloud2 pointcloud_msg = convert(telegram); - // Publish PointCloud2 data - m_point_cloud_publisher.publish(pointcloud_msg); + 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); + // Convert vehicle pose from result telegram to tf transform + geometry_msgs::TransformStamped tf2_vehicle_transform = convertToTransform(telegram); + tf_broadcaster.sendTransform(tf2_vehicle_transform); + } } ROS_INFO_STREAM("PointCloudConverter: converter thread for sim_loc_driver messages finished"); } diff --git a/src/sim_loc_random.cpp b/src/random_generator.cpp similarity index 98% rename from src/sim_loc_random.cpp rename to src/random_generator.cpp index 3a7082b..ce18b9d 100644 --- a/src/sim_loc_random.cpp +++ b/src/random_generator.cpp @@ -54,7 +54,7 @@ */ #include -#include "sick_lidar_localization/sim_loc_random.h" +#include "sick_lidar_localization/random_generator.h" /* * Constructor diff --git a/src/sim_loc_result_port_parser.cpp b/src/result_port_parser.cpp similarity index 99% rename from src/sim_loc_result_port_parser.cpp rename to src/result_port_parser.cpp index 69fbd5f..d33173f 100644 --- a/src/sim_loc_result_port_parser.cpp +++ b/src/result_port_parser.cpp @@ -57,7 +57,7 @@ #include #include "crc16ccitt_false.h" -#include "sick_lidar_localization/sim_loc_result_port_parser.h" +#include "sick_lidar_localization/result_port_parser.h" /** assert macro to verify result telegrams, throws an std::invalid_argument exception in case of assertion failures */ #define PARSE_ASSERT(assertion,info) sick_lidar_localization::ResultPortParser::parseAssert((assertion),(#assertion),info,__FILE__,__LINE__) @@ -263,9 +263,9 @@ size_t sick_lidar_localization::ResultPortParser::decodeResultPortPayload(const // 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); - // Decode Quality: Quality of pose [1 … 100], 1 = bad pose quality, 100 = good pose quality. Size: UInt8 = 1 byte + // 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 >= 1 && telegram_payload.Quality <= 100, std::string("ResultPortParser::decodeResultPortPayload(): invalid Payload.Quality ") + std::to_string(telegram_payload.Quality)); + 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); diff --git a/src/sim_loc_driver_monitor.cpp b/src/sim_loc_driver_monitor.cpp deleted file mode 100644 index 730ae78..0000000 --- a/src/sim_loc_driver_monitor.cpp +++ /dev/null @@ -1,176 +0,0 @@ -/* - * @brief sim_loc_driver_monitor monitors the driver thread implemented by sick_lidar_localization::DriverThread - * and starts a new driver thread after tcp errors (connection lost, socket shutdown, message timeouts etc.). - * - * 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/sim_loc_driver_monitor.h" - -/* - * Constructor. The driver monitor does not start automatically, call start() and stop() to start and stop. - * @param[in] nh ros node handle - * @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::DriverMonitor::DriverMonitor(ros::NodeHandle * nh, const std::string & server_adress, int tcp_port) -: m_initialized(false), m_nh(nh), m_server_adress(server_adress), m_tcp_port(tcp_port), m_monitoring_thread_running(false), - m_monitoring_thread(0), m_monitoring_rate(1.0), m_receive_telegrams_timeout(1.0) -{ - if(m_nh) - { - // Query configuration - 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 - m_initialized = true; - } -} - -/* - * Destructor. Stops the driver thread and monitor and closes all tcp connections. - */ -sick_lidar_localization::DriverMonitor::~DriverMonitor() -{ - stop(); -} - -/* - * Starts the driver monitor and driver thread. - * @return true on success, false on failure. - */ -bool sick_lidar_localization::DriverMonitor::start(void) -{ - if(!m_initialized) // DriverMonitor not properly initialized - { - ROS_ERROR_STREAM("## ERROR sick_lidar_localization::DriverMonitor::start(): DriverMonitor not initialized"); - return false; - } - // Create receiver thread to receive telegrams from the localization controller - m_monitoring_thread_running = true; - m_monitoring_thread = new boost::thread(&sick_lidar_localization::DriverMonitor::runMonitorThreadCb, this); - return true; -} - -/* - * Stops the driver monitor and thread and closes all tcp connections. - * @return true on success, false on failure. - */ -bool sick_lidar_localization::DriverMonitor::stop(void) -{ - m_monitoring_thread_running = false; - if(m_monitoring_thread) - { - m_monitoring_thread->join(); - delete(m_monitoring_thread); - m_monitoring_thread = 0; - } - return true; -} - -/* - * Callback for result telegram messages (SickLocResultPortTelegramMsg) from sim_loc_driver. - * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) - */ -void sick_lidar_localization::DriverMonitor::messageCbResultPortTelegrams(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg) -{ - m_driver_message_recv_timestamp.set(ros::Time::now()); -} - -/* - * Thread callback, implements the monitoring and restarts a DriverThread in case of tcp-errors. - */ -void sick_lidar_localization::DriverMonitor::runMonitorThreadCb(void) -{ - ROS_INFO_STREAM("DriverMonitor: monitoring thread started"); - 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_tcp_port); - if(!driver_thread->start()) - { - 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()) - { - ROS_INFO_STREAM("DriverMonitor: waiting for connection to localization controller"); - ros::Duration(1).sleep(); // 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::Duration(1).sleep(); // wait for monitoring start - } - // Monitor driver messages - ros::Duration monitoring_delay(1.0/m_monitoring_rate); - while(ros::ok() - && m_monitoring_thread_running - && driver_thread->isRunning() - && driver_thread->isConnected() - && (ros::Time::now() - m_driver_message_recv_timestamp.get()).toSec() <= m_receive_telegrams_timeout) - { - monitoring_delay.sleep(); - } - 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(); - } - delete(driver_thread); - driver_thread = 0; - } - m_monitoring_thread_running = false; - ROS_INFO_STREAM("DriverMonitor: monitoring thread finished"); -} - diff --git a/src/sim_loc_testcase_generator.cpp b/src/testcase_generator.cpp similarity index 79% rename from src/sim_loc_testcase_generator.cpp rename to src/testcase_generator.cpp index 44dbd70..0b16a20 100644 --- a/src/sim_loc_testcase_generator.cpp +++ b/src/testcase_generator.cpp @@ -55,11 +55,12 @@ */ #include -#include "sick_lidar_localization/sim_loc_random.h" -#include "sick_lidar_localization/sim_loc_testcase_generator.h" -#include "sick_lidar_localization/sim_loc_utils.h" +#include "sick_lidar_localization/cola_parser.h" +#include "sick_lidar_localization/random_generator.h" +#include "sick_lidar_localization/testcase_generator.h" +#include "sick_lidar_localization/utils.h" -/* +/*! * Creates and returns a deterministic default testcase for result port telegrams (binary telegrams and SickLocResultPortTelegramMsg) * @return SickLocResultPortTestcaseMsg with the binary telegram and SickLocResultPortTelegramMsg */ @@ -96,7 +97,7 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T return testcase; } -/* +/*! * Creates and returns a random testcase for result port telegrams (binary telegrams and SickLocResultPortTelegramMsg) * @return SickLocResultPortTestcaseMsg with the binary telegram and SickLocResultPortTelegramMsg */ @@ -107,7 +108,7 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T static sick_lidar_localization::UniformRandomInteger random8_generator(0, 255); static sick_lidar_localization::UniformRandomInteger random32_generator(-INT32_MAX, INT32_MAX); static sick_lidar_localization::UniformRandomInteger random_yaw_generator(-180000, 180000); - static sick_lidar_localization::UniformRandomInteger random_quality_generator(1, 100); + static sick_lidar_localization::UniformRandomInteger random_quality_generator(0, 100); static sick_lidar_localization::UniformRandomInteger random_covariance_generator(0, INT32_MAX); // Create default SickLocResultPortTelegramMsg @@ -127,7 +128,7 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T 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 [1 … 100], 1 = bad pose quality, 100 = good pose quality. Size: UInt8 = 1 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 @@ -178,8 +179,8 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T // Update telegram timestamps double delta_time_seconds = (ros::Time::now() - start_time).toSec(); - telegram_msg.telegram_payload.Timestamp += (uint32_t)(1000.0 * delta_time_seconds); // 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 + 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); @@ -195,3 +196,55 @@ sick_lidar_localization::SickLocResultPortTestcaseMsg sick_lidar_localization::T testcase.header.stamp = ros::Time::now(); return testcase; } + +/*! + * Creates and returns a synthetical cola response to a cola command request. + * Note: Just a few cola responses are implemented for test purposes, f.e. responses to "LocRequestTimestamp". + * By default, a response: "sAN " without any parameter is returned (sAN: Response to sMN) + * @param[in] cola_request Cola request from client + * @return Synthetical cola response from server + */ +sick_lidar_localization::SickLocColaTelegramMsg sick_lidar_localization::TestcaseGenerator::createColaResponse(const sick_lidar_localization::SickLocColaTelegramMsg & cola_request) +{ + // Generate a synthetical response to LocMapState requests: "sRA LocMapState 1" + if(cola_request.command_type == sick_lidar_localization::ColaParser::sRN && cola_request.command_name == "LocMapState") + { + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sRA, cola_request.command_name, {"1"}); + } + + // Generate a synthetical response to LocRequestTimestamp requests: "sAN LocRequestTimestamp " with uint32_t timestamp in hex and ms + if(cola_request.command_type == sick_lidar_localization::ColaParser::sMN && cola_request.command_name == "LocRequestTimestamp") + { + 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(); + // Create current timestamp in ticks + uint32_t ticks_ms = createTimestampTicksMilliSec(); + // Simulate some network latency + ros::Duration(0.001 * time_jitter_network_ms.generate()).sleep(); + std::stringstream timestamp_hex; + timestamp_hex << std::hex << std::uppercase << ticks_ms; + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name, {timestamp_hex.str()}); + } + + // Default response: "sAN " without parameter (sAN: Response to sMN) + return sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, cola_request.command_name); +} + +/*! + * Creates and returns a timestamp in milliseconds ticks. + * To simulate time jitter, network latency and time drift, + * a random value of +/- 2 milliseconds is added. + * @return timestamp in milliseconds ticks + */ +uint32_t sick_lidar_localization::TestcaseGenerator::createTimestampTicksMilliSec(void) +{ + static ros::Time start = ros::Time::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); + // Create some jitter, simulation network latency and time drift + ticks_ms += time_jitter_ticks_ms.generate(); + return ticks_ms; +} \ No newline at end of file diff --git a/src/unittest_sim_loc_parser.cpp b/src/unittest_sim_loc_parser.cpp deleted file mode 100644 index 7491e76..0000000 --- a/src/unittest_sim_loc_parser.cpp +++ /dev/null @@ -1,105 +0,0 @@ -/* - * @brief unittest_sim_loc_parser implements a unittest for parsing CoLa - * and result port telegrams for SIM Localization. - * - * 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 -#include - -#include "sick_lidar_localization/sim_loc_testcase_generator.h" -#include "sick_lidar_localization/sim_loc_utils.h" - -int main(int argc, char** argv) -{ - // Ros configuration and initialization - ros::init(argc, argv, "unittest_sim_loc_parser"); - ros::NodeHandle nh; - - // Run testcases for sim_loc_parser - ROS_INFO_STREAM("unittest_sim_loc_parser started."); - int 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); - 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(int testcase_cnt = 0; testcase_cnt < number_result_port_testcases; testcase_cnt++) - { - // Decode binary result port telegram, re-encode and check identity - std::vector recoded_binary; - if (!result_port_parser.decode(testcase.binary_data) - || (recoded_binary = result_port_parser.encode()) != testcase.binary_data - || !sick_lidar_localization::Utils::identicalByStream(result_port_parser.getTelegramMsg(), testcase.telegram_msg)) - { - ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: sick_lidar_localization::ResultPortParser::decode() failed. " << testcase.binary_data.size() << " byte input (hex):"); - ROS_ERROR_STREAM(sick_lidar_localization::Utils::toHexString(testcase.binary_data)); - ROS_ERROR_STREAM("## output (decoded): " << sick_lidar_localization::Utils::flattenToString(result_port_parser.getTelegramMsg())); - ROS_ERROR_STREAM("## recoded:"); - ROS_ERROR_STREAM(sick_lidar_localization::Utils::toHexString(recoded_binary)); - ROS_ERROR_STREAM("## output (expected): " << sick_lidar_localization::Utils::flattenToString(testcase.telegram_msg)); - ROS_ERROR_STREAM("## unittest_sim_loc_parser: " << (testcase_cnt + 1) << ". testcase FAILED."); - } - else - { - ROS_INFO_STREAM("unittest_sim_loc_parser: " << (testcase_cnt + 1) << ". testcase okay (result port telegram " << sick_lidar_localization::Utils::toHexString(testcase.binary_data) << ")"); - ROS_DEBUG_STREAM("unittest_sim_loc_parser decoded telegram = " << sick_lidar_localization::Utils::flattenToString(result_port_parser.getTelegramMsg()) - << " identical to expected telegram = " << sick_lidar_localization::Utils::flattenToString(testcase.telegram_msg)); - ROS_DEBUG_STREAM("Input (hex): " << sick_lidar_localization::Utils::toHexString(testcase.binary_data)); - ROS_DEBUG_STREAM("Recoded telegram (hex): " << sick_lidar_localization::Utils::toHexString(recoded_binary)); - } - // next testcase is a result port telegram with random data - testcase = sick_lidar_localization::TestcaseGenerator::createRandomResultPortTestcase(); - } - - ROS_INFO_STREAM("unittest_sim_loc_parser finished."); - return 0; -} diff --git a/src/sim_loc_utils.cpp b/src/utils.cpp similarity index 98% rename from src/sim_loc_utils.cpp rename to src/utils.cpp index 6e04cfc..af59596 100644 --- a/src/sim_loc_utils.cpp +++ b/src/utils.cpp @@ -59,7 +59,7 @@ #include #include -#include "sick_lidar_localization/sim_loc_utils.h" +#include "sick_lidar_localization/utils.h" /* * Converts and returns binary data to hex string diff --git a/srv/SickLocColaTelegramSrv.srv b/srv/SickLocColaTelegramSrv.srv new file mode 100644 index 0000000..49d4cc8 --- /dev/null +++ b/srv/SickLocColaTelegramSrv.srv @@ -0,0 +1,26 @@ +# Definition of ROS service SickLocColaTelegram for sick localization. +# ROS service SickLocColaTelegram encodes and sends a Cola telegram to the localization controller, +# receives and encodes the answer from the controller. +# +# See Telegram-Listing-v1.1.0.241R.pdf and Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.pdf +# for further details about Cola telgrams. + +# +# Request (input) +# + +string cola_ascii_request # Cola-ASCII telegram to send to the localization controller, f.e. "sRN SetAccessMode 3 F4724744". +float32 wait_response_timeout # Timeout for the controller response in secondes. Use 0.0 for Cola requests without controller response. +# bool send_binary # Not used (true: send binary telegram, false/default: send ASCII telegram) + +--- + +# +# Response (output) +# + +string cola_ascii_response # Response from the localization controller (empty in case of connection failures, communication errors or timeouts) +uint32 send_timestamp_sec # Send timestamp (seconds part of ros timestamp immediately before tcp send) +uint32 send_timestamp_nsec # Send timestamp (nano seconds part of ros timestamp immediately before tcp send) +uint32 receive_timestamp_sec # Receive timestamp (seconds part of ros timestamp immediately after first response byte received) +uint32 receive_timestamp_nsec # Receive timestamp (nano seconds part of ros timestamp immediately after first response byte received) diff --git a/srv/SickLocRequestTimestampSrv.srv b/srv/SickLocRequestTimestampSrv.srv new file mode 100644 index 0000000..5ceb95b --- /dev/null +++ b/srv/SickLocRequestTimestampSrv.srv @@ -0,0 +1,37 @@ +# Definition of ROS service SickLocRequestTimestamp for sick localization. +# +# 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 Telegram-Listing-v1.1.0.241R.pdf and Technical_information_Telegram_Listing_NAV_LOC_en_IM0076556.pdf +# for further details about Cola telegram LocRequestTimestamp. + +# +# Request (input) +# + +--- + +# +# Response (output) +# + +uint32 timestamp_lidar_ms # Lidar timestamp in milliseconds from LocRequestTimestamp response +uint64 mean_time_vehicle_ms # Vehicle mean timestamp in milliseconds: (send_time_vehicle + receive_time_vehicle) / 2 +uint64 delta_time_ms # Time offset: mean_time_vehicle_ms - timestamp_lidar_ms + +uint32 send_time_vehicle_sec # Vehicle timestamp when sending LocRequestTimestamp (seconds part of ros timestamp immediately before tcp send) +uint32 send_time_vehicle_nsec # Vehicle timestamp when sending LocRequestTimestamp (nano seconds part of ros timestamp immediately before tcp send) +uint32 receive_time_vehicle_sec # Vehicle timestamp when receiving the LocRequestTimestamp response (seconds part of ros timestamp immediately after first response byte received) +uint32 receive_time_vehicle_nsec # Vehicle timestamp when receiving the LocRequestTimestamp response (nano seconds part of ros timestamp immediately after first response byte received) diff --git a/include/sick_lidar_localization/sim_loc_test_server_thread.h b/test/include/sick_lidar_localization/test_server_thread.h old mode 100644 new mode 100755 similarity index 70% rename from include/sick_lidar_localization/sim_loc_test_server_thread.h rename to test/include/sick_lidar_localization/test_server_thread.h index bff0d0c..3bd55db --- a/include/sick_lidar_localization/sim_loc_test_server_thread.h +++ b/test/include/sick_lidar_localization/test_server_thread.h @@ -68,7 +68,8 @@ #include #include -#include "sick_lidar_localization/sim_loc_utils.h" +#include "sick_lidar_localization/fifo_buffer.h" +#include "sick_lidar_localization/utils.h" namespace sick_lidar_localization { @@ -83,9 +84,10 @@ namespace sick_lidar_localization /*! * Constructor. The server thread does not start automatically, call start() and stop() to start and stop the server. * @param[in] nh ros node handle - * @param[in] tcp_port tcp server port, default: The localization controller uses IP port number 2201 to send localization results + * @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 tcp_port = 2201); + TestServerThread(ros::NodeHandle* nh = 0, int ip_port_results = 2201, int ip_port_cola = 2111); /*! * Destructor. Stops the server thread and closes all tcp connections. @@ -112,29 +114,69 @@ namespace sick_lidar_localization void messageCbResultPortTelegrams(const sick_lidar_localization::SickLocResultPortTelegramMsg & msg); protected: + + /*! + * class ServerColaRequest: utility container for a received telegram (telegram data plus flag for Cola-Ascii or Cola-Binary) + */ + class ServerColaRequest + { + public: + ServerColaRequest(const std::vector & data = std::vector(), bool ascii = false) : telegram_data(data), telegram_is_ascii(ascii) {} ///< Constructor + bool telegram_is_ascii; ///< true: received telegram_data is Cola-Ascii encoded, false: received telegram_data is Cola-Binary encoded + std::vector telegram_data; ///< received telegram_data (Cola-Ascii or Cola-Binary) + }; + + typedef boost::thread* thread_ptr; ///< shortcut for pointer to boost::thread + typedef boost::asio::ip::tcp::socket* socket_ptr; ///< shortcut for pointer to boost::asio::ip::tcp::socket /*! * Closes all tcp connections */ virtual void closeTcpConnections(void); + /*! + * Closes a socket. + * @param[in,out] p_socket socket to be closed + */ + void closeSocket(socket_ptr & p_socket); + /*! * Stops all worker threads */ void closeWorkerThreads(void); /*! - * Thread callback, listens and accept tcp connections from clients. + * Thread callback, listens and accept tcp connections from clients for result telegrams. * Starts a new worker thread to generate result port telegrams for each tcp client. */ - virtual void runConnectionThreadCb(void); + virtual void runConnectionThreadResultCb(void); + + /*! + * Thread callback, listens and accept tcp connections from clients for cola telegrams. + * Starts a new worker thread to receive command requests for each tcp client. + */ + virtual void runConnectionThreadColaCb(void); + + /*! + * Generic thread callback, listens and accept tcp connections from clients. + * Starts a worker thread running thread_function_cb for each tcp client. + */ + template void runConnectionThreadGenericCb(boost::asio::ip::tcp::acceptor & tcp_acceptor_results, int ip_port_results, Callable thread_function_cb); + + /*! + * Worker thread callback, generates and sends result telegrams to a tcp client. + * There's one result worker thread for each tcp client. + * @param[in] p_socket socket to send result telegrams to the tcp client + */ + virtual void runWorkerThreadResultCb(boost::asio::ip::tcp::socket* p_socket); /*! - * Worker thread callback, generates and sends telegrams to a tcp client. - * There's one worker thread for each tcp client. - * @param[in] p_socket socket to send telegrams to the tcp client + * Worker thread callback, receives command requests from a tcp client + * and sends a synthetical command response. + * There's one request worker thread for each tcp client. + * @param[in] p_socket socket to receive command requests from the tcp client */ - virtual void runWorkerThreadCb(boost::asio::ip::tcp::socket* p_socket); + virtual void runWorkerThreadColaCb(boost::asio::ip::tcp::socket* p_socket); /*! * Thread callback, runs an error simulation and switches m_error_simulation_flag through the error test cases. @@ -159,14 +201,17 @@ namespace sick_lidar_localization * member data */ - int m_tcp_port; ///< tcp server port, default: The localization controller uses IP port number 2201 to send localization results + 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 double m_result_telegram_rate; ///< frequency to generate and send result port telegrams, default: 10 Hz - boost::thread* m_tcp_connection_thread; ///< thread to accept tcp clients + thread_ptr m_tcp_connection_thread_results; ///< thread to accept tcp clients for result port telegrams + thread_ptr m_tcp_connection_thread_cola; ///< thread to accept tcp clients for command requests and responses bool m_tcp_connection_thread_running; ///< true: m_tcp_connection_thread is running, otherwise false boost::asio::io_service m_ioservice; ///< boost io service for tcp connections - boost::asio::ip::tcp::acceptor m_tcp_acceptor; ///< boost acceptor for tcp clients + boost::asio::ip::tcp::acceptor m_tcp_acceptor_results; ///< boost acceptor for tcp clients for result telegrams + boost::asio::ip::tcp::acceptor m_tcp_acceptor_cola; ///< boost acceptor for tcp clients for command requests and responses std::list m_tcp_sockets; ///< list of tcp sockets (one socket for each tcp client) - std::list m_tcp_worker_threads; ///< list of tcp worker thread (one thread for each tcp client, generating telegrams) + 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) @@ -189,7 +234,7 @@ namespace sick_lidar_localization sick_lidar_localization::SetGet m_error_simulation_flag; ///< current error simulation flag, default: NO_ERROR bool m_error_simulation_enabled; ///< default: false (no error simulation), if true, test server simulates errors and communication failures of type ERROR_SIMULATION_FLAG - boost::thread* m_error_simulation_thread; ///< thread to run error simulation, switches m_error_simulation_flag through the error test cases + thread_ptr m_error_simulation_thread; ///< thread to run error simulation, switches m_error_simulation_flag through the error test cases bool m_error_simulation_thread_running; ///< true: m_error_simulation_thread is running, otherwise false sick_lidar_localization::SetGet m_last_telegram_received; ///< last telegram message received from sick_lidar_localization driver diff --git a/include/sick_lidar_localization/sim_loc_verifier_thread.h b/test/include/sick_lidar_localization/verifier_thread.h old mode 100644 new mode 100755 similarity index 98% rename from include/sick_lidar_localization/sim_loc_verifier_thread.h rename to test/include/sick_lidar_localization/verifier_thread.h index 49a5d40..3ebd5f1 --- a/include/sick_lidar_localization/sim_loc_verifier_thread.h +++ b/test/include/sick_lidar_localization/verifier_thread.h @@ -69,8 +69,8 @@ #include #include -#include "sick_lidar_localization/sim_loc_fifo.h" -#include "sick_lidar_localization/sim_loc_testcase_generator.h" +#include "sick_lidar_localization/fifo_buffer.h" +#include "sick_lidar_localization/testcase_generator.h" namespace sick_lidar_localization { diff --git a/test/scripts/clion.bash b/test/scripts/clion.bash index 2d145b1..8e5c685 100755 --- a/test/scripts/clion.bash +++ b/test/scripts/clion.bash @@ -6,7 +6,7 @@ if [ -f ../../../../devel/setup.bash ] ; then source ../../../../devel/setup.b if [ -f ../../../../install/setup.bash ] ; then source ../../../../install/setup.bash ; fi # start edit resource-files -gedit ./run_simu.bash run_demo_simu.bash ./run_error_simu.bash ./run.bash ../../yaml/sim_loc_driver.yaml ../../yaml/sim_loc_test_server.yaml ../../yaml/message_check_default.yaml ../../yaml/message_check_demo.yaml & +gedit ./run_simu.bash run_demo_simu.bash ./run_error_simu.bash ./run.bash & # start clion echo -e "Starting clion...\nNote in case of clion/cmake errors:" diff --git a/test/scripts/killall.bash b/test/scripts/killall.bash index 19bd5ae..e37500e 100755 --- a/test/scripts/killall.bash +++ b/test/scripts/killall.bash @@ -1,5 +1,5 @@ #!/bin/bash echo -e "#\n# killall.bash: Stopping all rosnodes...\n# rosnode kill -a ; sleep 1\n#" -rosnode kill -a ; sleep 1 +rosnode kill -a ; sleep 1 ; killall -9 sim_loc_test_server ; sleep 1 diff --git a/test/scripts/make.bash b/test/scripts/make.bash index 38b8e4a..895919a 100755 --- a/test/scripts/make.bash +++ b/test/scripts/make.bash @@ -1,25 +1,37 @@ #!/bin/bash # -# Build and install sick_lidar_localization binaries. +# 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 ./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 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 +catkin_lint -W1 ./src/sick_lidar_localization # print warnings and errors echo -e "\nmake.bash finished.\n" diff --git a/test/scripts/run.bash b/test/scripts/run.bash index bedbf8b..f47a9bd 100755 --- a/test/scripts/run.bash +++ b/test/scripts/run.bash @@ -24,6 +24,9 @@ rm -rf ~/.ros/* rosclean purge -y if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi +# upload demo_map: +# => file "/Z5....smap + # # Run sim_loc_driver_check for a plausibility check of messages and telegrams published by the sick_lidar_localization ros driver # diff --git a/test/scripts/run_demo_simu.bash b/test/scripts/run_demo_simu.bash index b456ed7..c6aa6b6 100755 --- a/test/scripts/run_demo_simu.bash +++ b/test/scripts/run_demo_simu.bash @@ -44,10 +44,14 @@ roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ # -> Style: Points # -> Size (Pixels): 5 # +# Visualize TF messages by rviz: +# +# rviz -> Global options: Fixed Frame -> tf_demo_map +# -> Add by display type TF +# -rosrun tf static_transform_publisher 0 0 0 0 0 0 map sick_lidar_localization 10 & +rosrun tf static_transform_publisher 0 0 0 0 0 0 map pointcloud_sick_lidar_localization 10 & rosrun rviz rviz & -# rostopic echo "/cloud" & # # Run test server and driver for some time and exit diff --git a/test/scripts/run_error_simu.bash b/test/scripts/run_error_simu.bash index 1954ec3..521a2b5 100755 --- a/test/scripts/run_error_simu.bash +++ b/test/scripts/run_error_simu.bash @@ -64,7 +64,7 @@ if true ; then 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 - rosnode kill /sim_loc_test_server + rosnode kill /sim_loc_test_server ; sleep 10 ; killall -9 sim_loc_test_server elif [ $testcnt -eq 2 ] ; then killall -9 sim_loc_test_server else diff --git a/test/scripts/run_simu.bash b/test/scripts/run_simu.bash index 2c33590..c095633 100755 --- a/test/scripts/run_simu.bash +++ b/test/scripts/run_simu.bash @@ -55,19 +55,42 @@ roslaunch sick_lidar_localization verify_sim_loc_driver.launch 2>&1 | unbuffer - # Run test server and driver for some time and verify results # -sleep 60 +sleep 120 ./src/sick_lidar_localization/test/scripts/killall.bash -killall roslaunch +sleep 1 ; killall roslaunch ; sleep 1 # # Run standalone unittests for sim_loc_parser # -roslaunch sick_lidar_localization unittest_sim_loc_parser.launch 2>&1 | tee -a ~/.ros/log/unittest_sim_loc_parser.log -./src/sick_lidar_localization/test/scripts/killall.bash +roslaunch sick_lidar_localization unittest_sim_loc_parser.launch 2>&1 | tee -a ~/.ros/log/unittest_sim_loc_parser.log & +sleep 10 ; ./src/sick_lidar_localization/test/scripts/killall.bash ; sleep 1 ; killall roslaunch cat ~/.ros/log/sim_loc_driver_diagnostic_messages.log grep "WARN" ~/.ros/log/*.log ; grep "ERR" ~/.ros/log/*.log -sleep 15 +sleep 20 + +# +# Start sim_loc_driver, start test server and send and receive cola commands +# Note: debug only - time synchronization is started by sim_loc_driver.launch and by default running in background. +# +# source ./devel/setup.bash +# ps -elf +# roslaunch sick_lidar_localization sim_loc_test_server.launch 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_test_server.log & +# sleep 1 +# roslaunch sick_lidar_localization sim_loc_driver.launch localization_controller_ip_adress:=127.0.0.1 2>&1 | unbuffer -p tee -a ~/.ros/log/sim_loc_driver.log & +# sleep 1 +# rossrv show SickLocColaTelegramSrv +# rosservice info SickLocColaTelegram +# rossrv show SickLocRequestTimestampSrv +# rosservice info SickLocRequestTimestamp +# for ((n=0;n<=10;n++)) ; do +# sleep 1 ; rosservice call SickLocColaTelegram "{cola_ascii_request: 'sRN LocMapState', wait_response_timeout: 1}" # expected reponse: cola_ascii_response="sRA LocMapState 1" +# sleep 1 ; rosservice call SickLocColaTelegram "{cola_ascii_request: 'sMN LocRequestTimestamp', wait_response_timeout: 1}" # expected reponse: cola_ascii_response="sAN LocRequestTimestamp " +# sleep 1 ; rosservice call SickLocRequestTimestamp "{}" # expected reponse: "timestamp_lidar_ms: , mean_time_vehicle_ms: , delta_time_ms: , ..." +# done +# sleep 1 ; ./src/sick_lidar_localization/test/scripts/killall.bash +# sleep 1 ; killall roslaunch ; sleep 20 +# ps -elf # # Cleanup and exit @@ -78,10 +101,12 @@ rm -rf ./log mkdir ./log cp -rf ~/.ros/log/*.log ./log rm -f ./log/rostopic_*.log +rm -f ./log/rosservice_*.log echo -e "\nsick_lidar_localization finished. Warnings and errors:" grep "WARN" ./log/*.log grep "ERR" ./log/*.log echo -e "\nsim_loc_driver check and verification summary:" -grep -i "check messages thread summary" ./log/*.log -grep -i "verification thread summary" ./log/*.log +grep -i "unittest_sim_loc_parser finished" ./log/*.log +grep -i "check messages thread summary" ./log/*.log +grep -i "verification thread summary" ./log/*.log diff --git a/src/sim_loc_test_server.cpp b/test/src/test_server.cpp old mode 100644 new mode 100755 similarity index 87% rename from src/sim_loc_test_server.cpp rename to test/src/test_server.cpp index 9ab4a59..4a09442 --- a/src/sim_loc_test_server.cpp +++ b/test/src/test_server.cpp @@ -61,8 +61,8 @@ #include #include -#include "sick_lidar_localization/sim_loc_test_server_thread.h" -#include "sick_lidar_localization/sim_loc_utils.h" +#include "sick_lidar_localization/test_server_thread.h" +#include "sick_lidar_localization/utils.h" int main(int argc, char** argv) { @@ -72,9 +72,11 @@ int main(int argc, char** argv) 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 = 2201; // Default: The localization controller uses IP port number 2201 to send localization results - ros::param::param("/sick_lidar_localization/test_server/result_telegrams_tcp_port", tcp_port, tcp_port); - sick_lidar_localization::TestServerThread test_server_thread(&nh, tcp_port); + 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); // 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) diff --git a/src/sim_loc_test_server_thread.cpp b/test/src/test_server_thread.cpp old mode 100644 new mode 100755 similarity index 66% rename from src/sim_loc_test_server_thread.cpp rename to test/src/test_server_thread.cpp index d4ddaf6..ec05b69 --- a/src/sim_loc_test_server_thread.cpp +++ b/test/src/test_server_thread.cpp @@ -57,23 +57,30 @@ */ #include -#include "sick_lidar_localization/sim_loc_test_server_thread.h" -#include "sick_lidar_localization/sim_loc_testcase_generator.h" -#include "sick_lidar_localization/sim_loc_random.h" -#include "sick_lidar_localization/sim_loc_utils.h" +#include "sick_lidar_localization/cola_parser.h" +#include "sick_lidar_localization/cola_transmitter.h" +#include "sick_lidar_localization/random_generator.h" +#include "sick_lidar_localization/test_server_thread.h" +#include "sick_lidar_localization/testcase_generator.h" +#include "sick_lidar_localization/utils.h" -/* +/*! * Constructor. The server thread does not start automatically, call start() and stop() to start and stop the server. * @param[in] nh ros node handle - * @param[in] tcp_port tcp server port, default: The localization controller uses IP port number 2201 to send localization results + * @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 tcp_port) : - m_tcp_port(tcp_port), m_tcp_connection_thread(0), m_tcp_connection_thread_running(false), m_worker_thread_running(false), - m_ioservice(), m_tcp_acceptor(m_ioservice, boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), m_tcp_port)), +sick_lidar_localization::TestServerThread::TestServerThread(ros::NodeHandle* 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), + m_tcp_acceptor_results(m_ioservice, boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), m_ip_port_results)), + m_tcp_acceptor_cola(m_ioservice, boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), m_ip_port_cola)), m_result_telegram_rate(10), m_demo_move_in_circles(false), m_error_simulation_enabled(false), m_error_simulation_flag(NO_ERROR), m_error_simulation_thread(0), m_error_simulation_thread_running(false) { - m_tcp_acceptor.set_option(boost::asio::ip::tcp::acceptor::reuse_address(true)); + m_tcp_acceptor_results.set_option(boost::asio::ip::tcp::acceptor::reuse_address(true)); + m_tcp_acceptor_cola.set_option(boost::asio::ip::tcp::acceptor::reuse_address(true)); 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) @@ -87,16 +94,15 @@ sick_lidar_localization::TestServerThread::TestServerThread(ros::NodeHandle* nh, } } -/* +/*! * Destructor. Stops the server thread and closes all tcp connections. - * @param[in] tcp_port tcp server port, default: The localization controller uses IP port number 2201 to send localization results */ sick_lidar_localization::TestServerThread::~TestServerThread() { stop(); } -/* +/*! * Starts the server thread, starts to listen and accept tcp connections from clients. * @return true on success, false on failure. */ @@ -115,12 +121,14 @@ bool sick_lidar_localization::TestServerThread::start(void) 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; - m_tcp_connection_thread = new boost::thread(&sick_lidar_localization::TestServerThread::runConnectionThreadCb, this); + m_tcp_connection_thread_results = new boost::thread(&sick_lidar_localization::TestServerThread::runConnectionThreadResultCb, this); + m_tcp_connection_thread_cola = new boost::thread(&sick_lidar_localization::TestServerThread::runConnectionThreadColaCb, this); return true; } -/* +/*! * Stops the server thread and closes all tcp connections. * @return true on success, false on failure. */ @@ -136,23 +144,36 @@ bool sick_lidar_localization::TestServerThread::stop(void) m_error_simulation_thread = 0; } m_ioservice.stop(); - if(m_tcp_acceptor.is_open()) + // close both tcp acceptors for ip ports 2201 (results), 2111 (cola command requests) + std::vector tcp_acceptors = { &m_tcp_acceptor_results, & m_tcp_acceptor_cola }; + for(std::vector::iterator iter_acceptor = tcp_acceptors.begin(); iter_acceptor != tcp_acceptors.end(); iter_acceptor++) { - m_tcp_acceptor.cancel(); // cancel the blocking call m_tcp_acceptor.listen() in the connection thread - m_tcp_acceptor.close(); + if((*iter_acceptor)->is_open()) + { + (*iter_acceptor)->cancel(); // cancel possibly blocking calls of m_tcp_acceptor_results.listen() in the connection thread + (*iter_acceptor)->close(); // close tcp acceptor + } } - if(m_tcp_connection_thread) + // close 2 threads creating sockets for new tcp clients on ip ports 2201 (results), 2111 (cola command requests) + std::vector tcp_connection_threads = { &m_tcp_connection_thread_results, &m_tcp_connection_thread_cola }; + for(std::vector::iterator iter_connection_thread = tcp_connection_threads.begin(); iter_connection_thread != tcp_connection_threads.end(); iter_connection_thread++) { - m_tcp_connection_thread->join(); - delete(m_tcp_connection_thread); - m_tcp_connection_thread = 0; + thread_ptr & tcp_connection_thread = **iter_connection_thread; + if(tcp_connection_thread) + { + tcp_connection_thread->join(); + delete(tcp_connection_thread); + tcp_connection_thread = 0; + } } + // close sockets closeTcpConnections(); + // close worker/client threads closeWorkerThreads(); return true; } -/* +/*! * Closes all tcp connections */ void sick_lidar_localization::TestServerThread::closeTcpConnections(void) @@ -160,24 +181,63 @@ void sick_lidar_localization::TestServerThread::closeTcpConnections(void) for(std::list::iterator socket_iter = m_tcp_sockets.begin(); socket_iter != m_tcp_sockets.end(); socket_iter++) { boost::asio::ip::tcp::socket* p_socket = *socket_iter; + try + { + if(p_socket && p_socket->is_open()) + { + p_socket->shutdown(boost::asio::ip::tcp::socket::shutdown_both); + p_socket->close(); + } + } + catch(std::exception & exc) + { + ROS_WARN_STREAM("TestServerThread::closeTcpConnections(): exception " << exc.what() << " on closing socket."); + } + *socket_iter = 0; + } + m_tcp_sockets.clear(); +} + +/*! + * Closes a socket. + * @param[in,out] p_socket socket to be closed + */ +void sick_lidar_localization::TestServerThread::closeSocket(socket_ptr & p_socket) +{ + boost::lock_guard worker_thread_lockguard(m_tcp_worker_threads_mutex); + try + { if(p_socket && p_socket->is_open()) { p_socket->shutdown(boost::asio::ip::tcp::socket::shutdown_both); p_socket->close(); } - *socket_iter = 0; + if(p_socket) + { + for(std::list::iterator socket_iter = m_tcp_sockets.begin(); socket_iter != m_tcp_sockets.end(); ) + { + if(p_socket == *socket_iter) + socket_iter = m_tcp_sockets.erase(socket_iter); + else + socket_iter++; + } + p_socket = 0; + } + } + catch(std::exception & exc) + { + ROS_WARN_STREAM("TestServerThread::closeSocket(): exception " << exc.what() << " on closing socket."); } - m_tcp_sockets.clear(); } -/* +/*! * Stops all worker threads */ void sick_lidar_localization::TestServerThread::closeWorkerThreads(void) { - boost::lock_guard worker_thread_lockguard(m_tcp_worker_threads_mutex); m_worker_thread_running = false; - for(std::list::iterator thread_iter = m_tcp_worker_threads.begin(); thread_iter != m_tcp_worker_threads.end(); thread_iter++) + boost::lock_guard worker_thread_lockguard(m_tcp_worker_threads_mutex); + for(std::list::iterator thread_iter = m_tcp_worker_threads.begin(); thread_iter != m_tcp_worker_threads.end(); thread_iter++) { boost::thread *p_thread = *thread_iter; p_thread->join(); @@ -186,7 +246,7 @@ void sick_lidar_localization::TestServerThread::closeWorkerThreads(void) m_tcp_worker_threads.clear(); } -/* +/*! * Callback for result telegram messages (SickLocResultPortTelegramMsg) from sim_loc_driver. * Buffers the last telegram to monitor sim_loc_driver messages in error simulation mode. * @param[in] msg result telegram message (SickLocResultPortTelegramMsg) @@ -196,14 +256,32 @@ void sick_lidar_localization::TestServerThread::messageCbResultPortTelegrams(con m_last_telegram_received.set(msg); } -/* - * Thread callback, listens and accept tcp connections from clients. +/*! + * Thread callback, listens and accept tcp connections from clients for result telegrams. * Starts a new worker thread to generate result port telegrams for each tcp client. */ -void sick_lidar_localization::TestServerThread::runConnectionThreadCb(void) +void sick_lidar_localization::TestServerThread::runConnectionThreadResultCb(void) +{ + runConnectionThreadGenericCb(m_tcp_acceptor_results, m_ip_port_results, &sick_lidar_localization::TestServerThread::runWorkerThreadResultCb); +} + +/*! + * Thread callback, listens and accept tcp connections from clients for cola telegrams. + * Starts a new worker thread to receive command requests for each tcp client. + */ +void sick_lidar_localization::TestServerThread::runConnectionThreadColaCb(void) +{ + runConnectionThreadGenericCb(m_tcp_acceptor_cola, m_ip_port_cola, &sick_lidar_localization::TestServerThread::runWorkerThreadColaCb); +} + +/*! + * Thread callback, listens and accept tcp connections from clients. + * Starts a worker thread for each tcp client. + */ +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 && m_tcp_acceptor.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 { @@ -214,23 +292,22 @@ void sick_lidar_localization::TestServerThread::runConnectionThreadCb(void) boost::asio::ip::tcp::socket* tcp_client_socket = new boost::asio::ip::tcp::socket(m_ioservice); boost::system::error_code errorcode; if(m_error_simulation_flag.get() != DONT_ACCECPT) - ROS_INFO_STREAM("TestServerThread: listening to tcp connections on port " << m_tcp_port); - m_tcp_acceptor.listen(); + ROS_INFO_STREAM("TestServerThread: listening to tcp connections on port " << ip_port_results); + tcp_acceptor_results.listen(); if(m_error_simulation_flag.get() == DONT_ACCECPT) // error simulation: testserver does not does not accecpt tcp clients { ros::Duration(0.1).sleep(); continue; } - m_tcp_acceptor.accept(*tcp_client_socket, errorcode); // normal mode: accept new tcp client + tcp_acceptor_results.accept(*tcp_client_socket, errorcode); // normal mode: accept new tcp client if (!errorcode && tcp_client_socket->is_open()) { - m_tcp_sockets.push_back(tcp_client_socket); // tcp client connected, start worker thread ROS_INFO_STREAM("TestServerThread: established new tcp client connection"); boost::lock_guard worker_thread_lockguard(m_tcp_worker_threads_mutex); + m_tcp_sockets.push_back(tcp_client_socket); m_worker_thread_running = true; - boost::thread* tcp_worker_thread = new boost::thread(&sick_lidar_localization::TestServerThread::runWorkerThreadCb, this, tcp_client_socket); - m_tcp_worker_threads.push_back(tcp_worker_thread); + m_tcp_worker_threads.push_back(new boost::thread(thread_function_cb, this, tcp_client_socket)); } } closeTcpConnections(); @@ -238,14 +315,14 @@ void sick_lidar_localization::TestServerThread::runConnectionThreadCb(void) ROS_INFO_STREAM("TestServerThread: connection thread finished"); } -/* - * Worker thread callback, generates and sends telegrams to a tcp client. - * There's one worker thread for each tcp client. - * @param[in] p_socket socket to send telegrams to the tcp client +/*! + * Worker thread callback, generates and sends result telegrams to a tcp client. + * There's one result worker thread for each tcp client. + * @param[in] p_socket socket to send result telegrams to the tcp client */ -void sick_lidar_localization::TestServerThread::runWorkerThreadCb(boost::asio::ip::tcp::socket* p_socket) +void sick_lidar_localization::TestServerThread::runWorkerThreadResultCb(boost::asio::ip::tcp::socket* p_socket) { - ROS_INFO_STREAM("TestServerThread: worker thread started"); + ROS_INFO_STREAM("TestServerThread: worker thread for result telegrams started"); ros::Duration send_telegrams_delay(1.0 / m_result_telegram_rate); sick_lidar_localization::SickLocResultPortTestcaseMsg testcase = sick_lidar_localization::TestcaseGenerator::createDefaultResultPortTestcase(); // initial testcase is the default testcase sick_lidar_localization::UniformRandomInteger random_generator(0,255); @@ -262,7 +339,7 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadCb(boost::asio::i { std::vector random_data = random_generator.generate(random_length.generate()); // binary random data of random size boost::asio::write(*p_socket, boost::asio::buffer(random_data.data(), random_data.size()), boost::asio::transfer_exactly(random_data.size()), error_code); - ROS_DEBUG_STREAM("TestServerThread: send random data " << sick_lidar_localization::Utils::toHexString(random_data)); + ROS_DEBUG_STREAM("TestServerThread for result telegrams: send random data " << sick_lidar_localization::Utils::toHexString(random_data)); } else { @@ -279,19 +356,24 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadCb(boost::asio::i int byte_cnt = ((random_integer.generate()) % (testcase.binary_data.size())); testcase.binary_data[byte_cnt] = (uint8_t)(random_generator.generate() & 0xFF); } - ROS_DEBUG_STREAM("TestServerThread: send random binary telegram " << sick_lidar_localization::Utils::toHexString(testcase.binary_data)); + ROS_DEBUG_STREAM("TestServerThread for result telegrams: send random binary telegram " << sick_lidar_localization::Utils::toHexString(testcase.binary_data)); } // send binary result port telegram to tcp client size_t bytes_written = boost::asio::write(*p_socket, boost::asio::buffer(testcase.binary_data.data(), testcase.binary_data.size()), boost::asio::transfer_exactly(testcase.binary_data.size()), error_code); if (error_code || bytes_written != testcase.binary_data.size()) { std::stringstream error_info; - error_info << "## ERROR TestServerThread: failed to send binary result port telegram, " << bytes_written << " of " << testcase.binary_data.size() << " bytes send, error code: " << error_code.message(); - ROS_LOG_STREAM((m_error_simulation_flag.get() == NO_ERROR) ? (::ros::console::levels::Warn) : (::ros::console::levels::Debug), ROSCONSOLE_DEFAULT_NAME, error_info.str()); + error_info << "## ERROR TestServerThread for result telegrams: failed to send binary result port telegram, " << bytes_written << " of " << testcase.binary_data.size() << " bytes send, error code: " << error_code.message(); + if(m_error_simulation_flag.get() == NO_ERROR) + { + ROS_WARN_STREAM(error_info.str() << ", close socket and leave worker thread for result telegrams"); + break; + } + ROS_DEBUG_STREAM(error_info.str()); } else { - ROS_DEBUG_STREAM("TestServerThread: send binary result port telegram " << sick_lidar_localization::Utils::toHexString(testcase.binary_data)); + 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(); @@ -301,10 +383,54 @@ void sick_lidar_localization::TestServerThread::runWorkerThreadCb(boost::asio::i // next testcase is a result port telegram with random data testcase = sick_lidar_localization::TestcaseGenerator::createRandomResultPortTestcase(); } - ROS_INFO_STREAM("TestServerThread: worker thread finished"); + closeSocket(p_socket); + ROS_INFO_STREAM("TestServerThread: worker thread for result telegrams finished"); } -/* +/*! + * Worker thread callback, receives command requests from a tcp client + * and sends a synthetical command response. + * There's one request worker thread for each tcp client. + * @param[in] p_socket socket to receive command requests from the tcp client + */ +void sick_lidar_localization::TestServerThread::runWorkerThreadColaCb(boost::asio::ip::tcp::socket* p_socket) +{ + ROS_INFO_STREAM("TestServerThread: worker thread for command requests started"); + 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; + if(sick_lidar_localization::ColaTransmitter::receive(*p_socket, request.telegram_data, 1, receive_timestamp)) + { + // command requests received, generate and send a synthetical response + bool cola_binary = sick_lidar_localization::ColaAsciiBinaryConverter::IsColaBinary(request.telegram_data); + if(cola_binary) + request.telegram_data = sick_lidar_localization::ColaAsciiBinaryConverter::ColaBinaryToColaAscii(request.telegram_data); + std::string ascii_telegram = sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(request.telegram_data); + ROS_INFO_STREAM("TestServerThread: received cola request " << ascii_telegram); + sick_lidar_localization::SickLocColaTelegramMsg telegram_msg = sick_lidar_localization::ColaParser::decodeColaTelegram(ascii_telegram); + // Generate a synthetical response depending on the request + sick_lidar_localization::SickLocColaTelegramMsg telegram_answer = sick_lidar_localization::TestcaseGenerator::createColaResponse(telegram_msg); + // Send command response to tcp client + std::vector binary_response = sick_lidar_localization::ColaParser::encodeColaTelegram(telegram_answer); + std::string ascii_response = sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(binary_response); + 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; + 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(); + } + 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. * @param[in] seconds delay in seconds */ @@ -317,7 +443,7 @@ void sick_lidar_localization::TestServerThread::errorSimulationWait(double secon } } -/* +/*! * Waits for and returns the next telegram message from sick_lidar_localization driver. * @param[in] timeout_seconds wait timeout in seconds * @param[out] telegram_msg last telegram message received @@ -338,7 +464,7 @@ bool sick_lidar_localization::TestServerThread::errorSimulationWaitForTelegramRe return false; } -/* +/*! * Thread callback, runs an error simulation and switches m_error_simulation_flag through the error test cases. */ void sick_lidar_localization::TestServerThread::runErrorSimulationThreadCb(void) @@ -381,7 +507,7 @@ void sick_lidar_localization::TestServerThread::runErrorSimulationThreadCb(void) // Error simulation testcase: testserver does not accecpt tcp clients for 10 seconds, normal execution afterwards. number_testcases++; m_error_simulation_flag.set(DONT_ACCECPT); - ROS_INFO_STREAM("TestServerThread: 3. error simulation testcase: server not responding, listening on port " << m_tcp_port << ", but accepting no tcp clients"); + ROS_INFO_STREAM("TestServerThread: 3. error simulation testcase: server not responding, listening on port " << m_ip_port_results << ", but accepting no tcp clients"); closeWorkerThreads(); closeTcpConnections(); errorSimulationWait(10); diff --git a/test/src/unittest_sim_loc_parser.cpp b/test/src/unittest_sim_loc_parser.cpp new file mode 100755 index 0000000..1f7f128 --- /dev/null +++ b/test/src/unittest_sim_loc_parser.cpp @@ -0,0 +1,270 @@ +/* + * @brief unittest_sim_loc_parser implements a unittest for parsing CoLa + * and result port telegrams for SIM Localization. + * + * 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 +#include + +#include "sick_lidar_localization/cola_parser.h" +#include "sick_lidar_localization/random_generator.h" +#include "sick_lidar_localization/testcase_generator.h" +#include "sick_lidar_localization/utils.h" + +/*! + * Container for testcase data to convert between Cola-ASCII and Cola-Binary, + * containing the reference data for both Cola-ASCII and Cola-Binary telegrams. + */ +class ColaBinaryTestcase +{ +public: + ColaBinaryTestcase(const std::string & _description = std::string(), const std::vector & _cola_ascii = std::vector(), const std::vector & _cola_binary = std::vector(), bool _parameter_to_ascii = true) + : description(_description), cola_ascii(_cola_ascii), cola_binary(_cola_binary), parameter_to_ascii(_parameter_to_ascii) {} ///< Constructor with default values + std::string description; ///< Descripition, f.e. "sMN SetAccessMode 3 F4724744" + std::vector cola_ascii; ///< Cola-ASCII reference, f.e. { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 } + std::vector cola_binary; ///< Cola-Binaryreference, f.e. { 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 } + bool parameter_to_ascii; ///< Conversion of command parameter to ascii (f.e. if true, a parameter 0x01 will be converted to 0x31 (default), otherwise to 0x01) +}; +std::vector cola_ascii_reference = // "sMN SetAccessMode 3 F4724744" + { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 }; +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 }; + + +int main(int argc, char** argv) +{ + // Ros configuration and initialization + ros::init(argc, argv, "unittest_sim_loc_parser"); + ros::NodeHandle nh; + 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); + 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++) + { + // Decode binary result port telegram, re-encode and check identity + std::vector recoded_binary; + if (!result_port_parser.decode(testcase.binary_data) + || (recoded_binary = result_port_parser.encode()) != testcase.binary_data + || !sick_lidar_localization::Utils::identicalByStream(result_port_parser.getTelegramMsg(), testcase.telegram_msg)) + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: sick_lidar_localization::ResultPortParser::decode() failed. " << testcase.binary_data.size() << " byte input (hex):"); + ROS_ERROR_STREAM(sick_lidar_localization::Utils::toHexString(testcase.binary_data)); + ROS_ERROR_STREAM("## output (decoded): " << sick_lidar_localization::Utils::flattenToString(result_port_parser.getTelegramMsg())); + ROS_ERROR_STREAM("## recoded:"); + ROS_ERROR_STREAM(sick_lidar_localization::Utils::toHexString(recoded_binary)); + ROS_ERROR_STREAM("## output (expected): " << sick_lidar_localization::Utils::flattenToString(testcase.telegram_msg)); + ROS_ERROR_STREAM("## unittest_sim_loc_parser: " << (testcase_cnt + 1) << ". testcase FAILED."); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: " << (testcase_cnt + 1) << ". testcase passed (result port telegram " << sick_lidar_localization::Utils::toHexString(testcase.binary_data) << ")"); + ROS_DEBUG_STREAM("unittest_sim_loc_parser decoded telegram = " << sick_lidar_localization::Utils::flattenToString(result_port_parser.getTelegramMsg()) + << " identical to expected telegram = " << sick_lidar_localization::Utils::flattenToString(testcase.telegram_msg)); + ROS_DEBUG_STREAM("Input (hex): " << sick_lidar_localization::Utils::toHexString(testcase.binary_data)); + ROS_DEBUG_STREAM("Recoded telegram (hex): " << sick_lidar_localization::Utils::toHexString(recoded_binary)); + } + // next testcase is a result port telegram with random data + testcase = sick_lidar_localization::TestcaseGenerator::createRandomResultPortTestcase(); + } + + // Run sim_loc_parser unittest for Cola Ascii telegrams + std::string cola_ascii = "sMN SetAccessMode 3 F4724744"; + std::vector cola_binary = { 0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03 }; + if (sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(cola_binary) != cola_ascii + || sick_lidar_localization::Utils::toHexString(sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(cola_ascii)) != sick_lidar_localization::Utils::toHexString(cola_binary)) + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: sick_lidar_localization::ColaAsciiBinaryConverter::convert() failed."); + ROS_ERROR_STREAM("## Cola ASCII input: " << cola_ascii); + ROS_ERROR_STREAM("## Converted Cola Binary ouput: " << sick_lidar_localization::Utils::toHexString(sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(cola_ascii))); + ROS_ERROR_STREAM("## Expected Cola Binary ouput: " << sick_lidar_localization::Utils::toHexString(cola_binary)); + ROS_ERROR_STREAM("## Cola Binary input: " << sick_lidar_localization::Utils::toHexString(cola_binary)); + ROS_ERROR_STREAM("## Converted Cola ASCII ouput: " << sick_lidar_localization::ColaAsciiBinaryConverter::ConvertColaAscii(cola_binary)); + ROS_ERROR_STREAM("## Expected Cola ASCII ouput: " << cola_ascii); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: " << (testcase_cnt + 1) << ". testcase passed (ColaAsciiBinaryConverter: \"" << cola_ascii << "\", " << sick_lidar_localization::Utils::toHexString(cola_binary) << ")"); + } + testcase_cnt++; + + // Run sim_loc_parser unittest for parsing Cola telegram messages + sick_lidar_localization::UniformRandomInteger random32_generator(0, INT32_MAX); + std::vector cola_telegram_tests_input; + cola_telegram_tests_input.push_back(sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sRN, "SetAccessMode", {"3", "F4724744"})); + cola_telegram_tests_input.push_back(sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sMN, "LocRequestTimestamp")); + for(size_t n = 0; n < 10; n++) // Create some "sAN, "LocRequestTimestamp" with timestamp (response to "sMN LocRequestTimestamp") + cola_telegram_tests_input.push_back(sick_lidar_localization::ColaParser::createColaTelegram(sick_lidar_localization::ColaParser::sAN, "LocRequestTimestamp", { std::to_string(random32_generator.generate())})); + for(std::vector::iterator iter_testcase = cola_telegram_tests_input.begin(); iter_testcase != cola_telegram_tests_input.end(); iter_testcase++, testcase_cnt++) + { + sick_lidar_localization::SickLocColaTelegramMsg & cola_telegram_input = *iter_testcase; + std::vector cola_binary = sick_lidar_localization::ColaParser::encodeColaTelegram(cola_telegram_input); + sick_lidar_localization::SickLocColaTelegramMsg cola_telegram_output = sick_lidar_localization::ColaParser::decodeColaTelegram(cola_binary); + cola_telegram_output.header = cola_telegram_input.header; // ignore different message timestamps + if (cola_telegram_output.command_type == sick_lidar_localization::ColaParser::sINVALID + || !sick_lidar_localization::Utils::identicalByStream(cola_telegram_input, cola_telegram_output)) + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: sick_lidar_localization::ColaParser::encodeColaTelegram/decodeColaTelegram failed:"); + ROS_ERROR_STREAM("## input: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_input)); + ROS_ERROR_STREAM("## output: " << sick_lidar_localization::Utils::flattenToString(cola_telegram_output)); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: " << (testcase_cnt + 1) << ". testcase passed (cola_telegram telegram " << sick_lidar_localization::Utils::flattenToString(cola_telegram_output) << ")"); + } + } + + // Run sim_loc_parser unittest for converting Cola Ascii to Cola Binary telegrams. + // Testcases taken from examples listed in manual Technical_information_Telegram_Listing_Ranging_sensors_LMS1xx_LMS5xx_TiM5xx_MRS1000_MRS6000_NAV310_LD_OEM15xx_LD_LRS36xx_LMS4000_en_IM0045927.pdf + std::vector cola_ascii_to_cola_binary_testcases = + { + ColaBinaryTestcase("sAN SetAccessMode 1", + {0x02, 0x73, 0x41, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x31, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x13, 0x73, 0x41, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x01, 0x38}), + ColaBinaryTestcase("sMN SetAccessMode 3 F4724744", + {0x02, 0x73, 0x4D, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x33, 0x20, 0x46, 0x34, 0x37, 0x32, 0x34, 0x37, 0x34, 0x34, 0x03}, + {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}), + ColaBinaryTestcase("sWA MMAlignmentMode", + {0x02, 0x73, 0x57, 0x41, 0x20, 0x4D, 0x4D, 0x41, 0x6C, 0x69, 0x67, 0x6E, 0x6D, 0x65, 0x6E, 0x74, 0x4D, 0x6F, 0x64, 0x65, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x13, 0x73, 0x57, 0x41, 0x20, 0x4D, 0x4D, 0x41, 0x6C, 0x69, 0x67, 0x6E, 0x6D, 0x65, 0x6E, 0x74, 0x4D, 0x6F, 0x64, 0x65, 0x39}), + ColaBinaryTestcase("sAN SetPassword 1", + {0x02, 0x73, 0x41, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x50, 0x61, 0x73, 0x73, 0x77, 0x6F, 0x72, 0x64, 0x20, 0x31, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x11, 0x73, 0x41, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x50, 0x61, 0x73, 0x73, 0x77, 0x6F, 0x72, 0x64, 0x20, 0x31, 0x30}, false), + ColaBinaryTestcase("sMN Run", + {0x02, 0x73, 0x4D, 0x4E, 0x20, 0x52, 0x75, 0x6E, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x07, 0x73, 0x4D, 0x4E, 0x20, 0x52, 0x75, 0x6E, 0x19}), + ColaBinaryTestcase("sAN Run 1", + {0x02, 0x73, 0x41, 0x4E, 0x20, 0x52, 0x75, 0x6E, 0x20, 0x31, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x09, 0x73, 0x41, 0x4E, 0x20, 0x52, 0x75, 0x6E, 0x20, 0x01, 0x34}), + ColaBinaryTestcase("sWN EIHstCola 0", + {0x02, 0x73, 0x57, 0x4E, 0x20, 0x45, 0x49, 0x48, 0x73, 0x74, 0x43, 0x6F, 0x6C, 0x61, 0x20, 0x30, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x0F, 0x73, 0x57, 0x4E, 0x20, 0x45, 0x49, 0x48, 0x73, 0x74, 0x43, 0x6F, 0x6C, 0x61, 0x20, 0x00, 0x08}), + ColaBinaryTestcase("sWA EIHstCola", + {0x02, 0x73, 0x57, 0x41, 0x20, 0x45, 0x49, 0x48, 0x73, 0x74, 0x43, 0x6F, 0x6C, 0x61, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x0D, 0x73, 0x57, 0x41, 0x20, 0x45, 0x49, 0x48, 0x73, 0x74, 0x43, 0x6F, 0x6C, 0x61, 0x27}) + + }; + for(std::vector::iterator iter_testcase = cola_ascii_to_cola_binary_testcases.begin(); iter_testcase != cola_ascii_to_cola_binary_testcases.end(); iter_testcase++) + { + std::vector cola_binary_test = sick_lidar_localization::ColaAsciiBinaryConverter::ColaAsciiToColaBinary(iter_testcase->cola_ascii, iter_testcase->parameter_to_ascii); + if (sick_lidar_localization::Utils::toHexString(cola_binary_test) != sick_lidar_localization::Utils::toHexString(iter_testcase->cola_binary)) + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: sick_lidar_localization::ColaAsciiBinaryConverter::ColaAsciiToColaBinary(" << iter_testcase->description << ") failed."); + ROS_ERROR_STREAM("## encoded: " << sick_lidar_localization::Utils::toHexString(cola_binary_test)); + ROS_ERROR_STREAM("## expected: " << sick_lidar_localization::Utils::toHexString(iter_testcase->cola_binary)); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: " << (testcase_cnt + 1) << ". testcase passed (ColaAsciiToColaBinary, " << iter_testcase->description << ")"); + } + testcase_cnt++; + } + + // Run sim_loc_parser unittest for converting Cola Binary to Cola Ascii telegrams. + // Testcases taken from examples listed in manual Technical_information_Telegram_Listing_Ranging_sensors_LMS1xx_LMS5xx_TiM5xx_MRS1000_MRS6000_NAV310_LD_OEM15xx_LD_LRS36xx_LMS4000_en_IM0045927.pdf + std::vector cola_binary_to_cola_ascii_testcases = + { + ColaBinaryTestcase("sAN SetAccessMode 1", + {0x02, 0x73, 0x41, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x31, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x13, 0x73, 0x41, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x41, 0x63, 0x63, 0x65, 0x73, 0x73, 0x4D, 0x6F, 0x64, 0x65, 0x20, 0x01, 0x38}), + ColaBinaryTestcase("sRA TSCTCmaxoffset 468CA000", + {0x02, 0x73, 0x52, 0x41, 0x20, 0x54, 0x53, 0x43, 0x54, 0x43, 0x6D, 0x61, 0x78, 0x6F, 0x66, 0x66, 0x73, 0x65, 0x74, 0x20, 0x46, 0x8C, 0xA0, 0x00, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x17, 0x73, 0x52, 0x41, 0x20, 0x54, 0x53, 0x43, 0x54, 0x43, 0x6D, 0x61, 0x78, 0x6F, 0x66, 0x66, 0x73, 0x65, 0x74, 0x20, 0x46, 0x8C, 0xA0, 0x00, 0x20}, false), + ColaBinaryTestcase("sWA MMAlignmentMode", + {0x02, 0x73, 0x57, 0x41, 0x20, 0x4D, 0x4D, 0x41, 0x6C, 0x69, 0x67, 0x6E, 0x6D, 0x65, 0x6E, 0x74, 0x4D, 0x6F, 0x64, 0x65, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x13, 0x73, 0x57, 0x41, 0x20, 0x4D, 0x4D, 0x41, 0x6C, 0x69, 0x67, 0x6E, 0x6D, 0x65, 0x6E, 0x74, 0x4D, 0x6F, 0x64, 0x65, 0x39}), + ColaBinaryTestcase("sAN SetPassword 1", + {0x02, 0x73, 0x41, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x50, 0x61, 0x73, 0x73, 0x77, 0x6F, 0x72, 0x64, 0x20, 0x31, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x11, 0x73, 0x41, 0x4E, 0x20, 0x53, 0x65, 0x74, 0x50, 0x61, 0x73, 0x73, 0x77, 0x6F, 0x72, 0x64, 0x20, 0x31, 0x30}, false), + ColaBinaryTestcase("sMN Run", + {0x02, 0x73, 0x4D, 0x4E, 0x20, 0x52, 0x75, 0x6E, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x07, 0x73, 0x4D, 0x4E, 0x20, 0x52, 0x75, 0x6E, 0x19}), + ColaBinaryTestcase("sAN Run 1", + {0x02, 0x73, 0x41, 0x4E, 0x20, 0x52, 0x75, 0x6E, 0x20, 0x31, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x09, 0x73, 0x41, 0x4E, 0x20, 0x52, 0x75, 0x6E, 0x20, 0x01, 0x34}), + ColaBinaryTestcase("sWN EIHstCola 0", + {0x02, 0x73, 0x57, 0x4E, 0x20, 0x45, 0x49, 0x48, 0x73, 0x74, 0x43, 0x6F, 0x6C, 0x61, 0x20, 0x30, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x0F, 0x73, 0x57, 0x4E, 0x20, 0x45, 0x49, 0x48, 0x73, 0x74, 0x43, 0x6F, 0x6C, 0x61, 0x20, 0x00, 0x08}), + ColaBinaryTestcase("sWA EIHstCola", + {0x02, 0x73, 0x57, 0x41, 0x20, 0x45, 0x49, 0x48, 0x73, 0x74, 0x43, 0x6F, 0x6C, 0x61, 0x03}, + {0x02, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, 0x0D, 0x73, 0x57, 0x41, 0x20, 0x45, 0x49, 0x48, 0x73, 0x74, 0x43, 0x6F, 0x6C, 0x61, 0x27}) + + }; + for(std::vector::iterator iter_testcase = cola_binary_to_cola_ascii_testcases.begin(); iter_testcase != cola_binary_to_cola_ascii_testcases.end(); iter_testcase++) + { + std::vector cola_ascii_test = sick_lidar_localization::ColaAsciiBinaryConverter::ColaBinaryToColaAscii(iter_testcase->cola_binary, iter_testcase->parameter_to_ascii); + if (sick_lidar_localization::Utils::toHexString(cola_ascii_test) != sick_lidar_localization::Utils::toHexString(iter_testcase->cola_ascii)) + { + failed_testcase_cnt++; + ROS_ERROR_STREAM("## ERROR unittest_sim_loc_parser: sick_lidar_localization::ColaAsciiBinaryConverter::ColaBinaryToColaAscii(" << iter_testcase->description << ") failed."); + ROS_ERROR_STREAM("## encoded: " << sick_lidar_localization::Utils::toHexString(cola_ascii_test)); + ROS_ERROR_STREAM("## expected: " << sick_lidar_localization::Utils::toHexString(iter_testcase->cola_ascii)); + } + else + { + ROS_INFO_STREAM("unittest_sim_loc_parser: " << (testcase_cnt + 1) << ". testcase passed (ColaBinaryToColaAscii, " << iter_testcase->description << ")"); + } + 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."); + return 0; +} diff --git a/src/sim_loc_verifier_thread.cpp b/test/src/verifier_thread.cpp old mode 100644 new mode 100755 similarity index 98% rename from src/sim_loc_verifier_thread.cpp rename to test/src/verifier_thread.cpp index a8c1e95..917ab76 --- a/src/sim_loc_verifier_thread.cpp +++ b/test/src/verifier_thread.cpp @@ -65,8 +65,8 @@ */ #include -#include "sick_lidar_localization/sim_loc_utils.h" -#include "sick_lidar_localization/sim_loc_verifier_thread.h" +#include "sick_lidar_localization/utils.h" +#include "sick_lidar_localization/verifier_thread.h" /* * Constructor diff --git a/src/verify_sim_loc_driver.cpp b/test/src/verify_sim_loc_driver.cpp old mode 100644 new mode 100755 similarity index 98% rename from src/verify_sim_loc_driver.cpp rename to test/src/verify_sim_loc_driver.cpp index 3fe8bbe..a3e91f0 --- a/src/verify_sim_loc_driver.cpp +++ b/test/src/verify_sim_loc_driver.cpp @@ -65,7 +65,7 @@ */ #include -#include "sick_lidar_localization/sim_loc_verifier_thread.h" +#include "sick_lidar_localization/verifier_thread.h" int main(int argc, char** argv) { diff --git a/yaml/message_check_default.yaml b/yaml/message_check_default.yaml index 7444161..cdfc795 100644 --- a/yaml/message_check_default.yaml +++ b/yaml/message_check_default.yaml @@ -37,7 +37,7 @@ sick_lidar_localization: PoseYaw: -360000 # Orientation (yaw) of the vehicle on the map [mdeg]. int32, size:= Int32 = 4 byte Reserved1: 0.0 # Reserved. uint32, size:= UInt32 = 4 byte Reserved2: -2147483648 # Reserved. int32, size:= Int32 = 4 byte - Quality: 1 # Quality of pose [1 … 100], 1 = bad pose quality, 100 = good pose quality. uint8, size:= UInt8 = 1 byte + Quality: 0 # Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality. uint8, size:= UInt8 = 1 byte OutliersRatio: 0 # Ratio of beams that cannot be assigned to the current reference map [%]. uint8, size:= UInt8 = 1 byte CovarianceX: 0 # Covariance c1 of the pose X [mm^2]. int32, size:= Int32 = 4 byte CovarianceY: 0 # Covariance c5 of the pose Y [mm^2]. int32, size:= Int32 = 4 byte diff --git a/yaml/message_check_demo.yaml b/yaml/message_check_demo.yaml index f0d8ac9..adcddc9 100644 --- a/yaml/message_check_demo.yaml +++ b/yaml/message_check_demo.yaml @@ -37,7 +37,7 @@ sick_lidar_localization: PoseYaw: -360000 # Orientation (yaw) of the vehicle on the map [mdeg]. int32, size:= Int32 = 4 byte Reserved1: 0.0 # Reserved. uint32, size:= UInt32 = 4 byte Reserved2: -2147483648 # Reserved. int32, size:= Int32 = 4 byte - Quality: 1 # Quality of pose [1 … 100], 1 = bad pose quality, 100 = good pose quality. uint8, size:= UInt8 = 1 byte + Quality: 0 # Quality of pose [0 … 100], 1 = bad pose quality, 100 = good pose quality. uint8, size:= UInt8 = 1 byte OutliersRatio: 0 # Ratio of beams that cannot be assigned to the current reference map [%]. uint8, size:= UInt8 = 1 byte CovarianceX: 0 # Covariance c1 of the pose X [mm^2]. int32, size:= Int32 = 4 byte CovarianceY: 0 # Covariance c5 of the pose Y [mm^2]. int32, size:= Int32 = 4 byte diff --git a/yaml/sim_loc_driver.yaml b/yaml/sim_loc_driver.yaml index b9decb4..bee99fc 100644 --- a/yaml/sim_loc_driver.yaml +++ b/yaml/sim_loc_driver.yaml @@ -1,10 +1,12 @@ # Configuration of ros driver for sick localization. sick_lidar_localization: - # Driver configuration + # Driver configuration. See Operation-Instruction-v1.1.0.241R.pdf, page 51, "IP port number and protocol" for default tcp ports. driver: localization_controller_default_ip_adress: "192.168.0.1" # Default IP adress "192.168.0.1" of the localization controller (if not otherwise set by parameter "localization_controller_ip_adress") - result_telegrams_tcp_port: 2201 # TCP port number of the localization controller sending localization results + 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. + 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 + cola_binary: 0 # 0: send Cola-ASCII (default), 1: send Cola-Binary, 2: toggle between Cola-ASCII and Cola-Binary (test and development only!) tcp_connection_retry_delay: 1.0 # Delay in seconds to retry to connect to the localization controller, default 1 second result_telegrams_topic: "/sick_lidar_localization/driver/result_telegrams" # ros topic to publish result port telegram messages (type SickLocResultPortTelegramMsg) result_telegrams_frame_id: "sick_lidar_localization" # ros frame id of result port telegram messages (type SickLocResultPortTelegramMsg) @@ -13,5 +15,7 @@ sick_lidar_localization: monitoring_rate: 1.0 # frequency to monitor driver messages, once per second by default monitoring_message_timeout: 1.0 # timeout for driver messages, shutdown tcp-sockets and reconnect after message timeout, 1 second by default point_cloud_topic: "/cloud" # ros topic to publish PointCloud2 data - point_cloud_frame_id: "sick_lidar_localization" # ros frame id of PointCloud2 messages + 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 diff --git a/yaml/sim_loc_test_server.yaml b/yaml/sim_loc_test_server.yaml index de4e05a..4f1916c 100644 --- a/yaml/sim_loc_test_server.yaml +++ b/yaml/sim_loc_test_server.yaml @@ -3,10 +3,11 @@ # It implements a simple tcp server, accepting tcp connections from clients and generating telegrams to test the ros driver for sim localization. sick_lidar_localization: - # Server configuration + # Server configuration. See Operation-Instruction-v1.1.0.241R.pdf, page 51, "IP port number and protocol" for default tcp ports. 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) - result_telegrams_rate: 10 # Rate to generate and send result port telegrams - result_testcases_topic: "/sick_lidar_localization/test_server/result_testcases" # ROS topic to publish testcases with result port telegrams (type SickLocResultPortTestcaseMsg) - result_testcases_frame_id: "result_testcases" # ROS frame id of testcase messages (type SickLocResultPortTestcaseMsg) + 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) + 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 + result_telegrams_rate: 10 # Rate to generate and send result port telegrams + result_testcases_topic: "/sick_lidar_localization/test_server/result_testcases" # ROS topic to publish testcases with result port telegrams (type SickLocResultPortTestcaseMsg) + result_testcases_frame_id: "result_testcases" # ROS frame id of testcase messages (type SickLocResultPortTestcaseMsg)