From 1dcaa9f2d35a11e63498e426cf9132fbe6c12a0b Mon Sep 17 00:00:00 2001 From: rostest Date: Thu, 25 Apr 2024 18:51:19 +0200 Subject: [PATCH] Fix blocking API-close in case of wrong ip-address, #316 --- .gitignore | 6 + CMakeLists.txt | 74 +++++- driver/src/sick_generic_laser.cpp | 29 ++- driver/src/sick_generic_monitoring.cpp | 3 +- driver/src/sick_scan_common.cpp | 2 +- .../sick_scansegment_xd/msgpack_converter.cpp | 3 +- .../sick_scansegment_xd/msgpack_exporter.cpp | 22 +- .../ros_msgpack_publisher.cpp | 56 ++--- .../scansegment_threads.cpp | 38 +-- .../src/sick_scansegment_xd/udp_receiver.cpp | 9 +- driver/src/softwarePLL.cpp | 49 +++- include/sick_scan/sick_scan_xd_version.h | 4 + include/sick_scan/softwarePLL.h | 23 +- include/sick_scan/tcp/SickThread.hpp | 2 +- .../sick_scansegment_xd/msgpack_exporter.h | 8 +- include/sick_scansegment_xd/udp_sockets.h | 44 +++- launch/sick_ldmrs.launch | 7 + launch/sick_lms_1xx.launch | 7 + launch/sick_lms_1xxx.launch | 7 + launch/sick_lms_1xxx_v2.launch | 7 + launch/sick_lms_4xxx.launch | 7 + launch/sick_lms_5xx.launch | 7 + launch/sick_lrs_36x0.launch | 7 + launch/sick_lrs_36x0_upside_down.launch | 7 + launch/sick_lrs_36x1.launch | 7 + launch/sick_lrs_36x1_upside_down.launch | 7 + launch/sick_lrs_4xxx.launch | 7 + launch/sick_mrs_1xxx.launch | 7 + launch/sick_mrs_1xxx_cartographer.launch | 7 + launch/sick_mrs_6xxx.launch | 7 + launch/sick_mrs_6xxx_first_echo.launch | 7 + launch/sick_mrs_6xxx_last_echo.launch | 7 + launch/sick_mrs_6xxx_narrow.launch | 7 + launch/sick_multiscan.launch | 7 + launch/sick_nav_2xx.launch | 7 + launch/sick_nav_31x.launch | 7 + launch/sick_nav_350.launch | 7 + launch/sick_new_ip.launch | 7 + launch/sick_oem_15xx.launch | 7 + launch/sick_picoscan.launch | 7 + launch/sick_picoscan_single_echo.launch | 7 + launch/sick_rms_xxxx.launch | 7 + launch/sick_tim_240.launch | 7 + launch/sick_tim_4xx.launch | 7 + launch/sick_tim_5xx.launch | 7 + launch/sick_tim_5xx_ASCII.launch | 7 + launch/sick_tim_7xx.launch | 7 + launch/sick_tim_7xxS.launch | 7 + package.xml | 2 +- python/tools/package_version_tool.py | 64 +++++ test/docker/data/mrs1xxx_test01_cfg.json | 7 +- test/docker/data/mrs1xxx_test01_tcp_data.json | 36 +++ .../multiscan_compact_errortest01_cfg.json | 5 +- .../data/multiscan_compact_test01_cfg.json | 5 +- .../picoscan_compact_errortest01_cfg.json | 5 +- .../data/picoscan_compact_test01_cfg.json | 7 +- test/docker/dockerfile_linux_x64_sick_scan_xd | 42 ++++ test/docker/python/sick_scan_xd_simu.py | 13 +- test/docker/python/sick_scan_xd_simu_cfg.py | 68 +++-- test/docker/python/sick_scan_xd_subscriber.py | 37 ++- ...run_dockerfile_linux_x64_sick_scan_xd.bash | 85 +++++++ .../sick_scan_xd_api_dockertest.cpp | 233 ++++++++++++++++++ .../sick_scan_xd_api_test.cpp | 44 +++- .../sick_scan_xd_api_wrapper.c | 2 +- tools/test_server/src/test_server_thread.cpp | 3 +- 65 files changed, 1079 insertions(+), 175 deletions(-) create mode 100644 include/sick_scan/sick_scan_xd_version.h create mode 100644 python/tools/package_version_tool.py create mode 100644 test/docker/dockerfile_linux_x64_sick_scan_xd create mode 100644 test/docker/run_dockerfile_linux_x64_sick_scan_xd.bash create mode 100644 test/src/sick_scan_xd_api/sick_scan_xd_api_dockertest.cpp diff --git a/.gitignore b/.gitignore index 9aea6ca4..f7b5feae 100644 --- a/.gitignore +++ b/.gitignore @@ -136,3 +136,9 @@ test/emulator/scandata/20230719_mrs1104_infringement/ doc/20230919_sick_scan_xd_timeout_settings.pptx test/docker/archive/ + +test/docker/python/__pycache__/ + +tools/pandoc/ + +test/docker/images/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 570fe60c..afc998cd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -61,7 +61,6 @@ if (POLICY CMP0048) cmake_policy(SET CMP0048 NEW) endif() - # Default to C++14 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -127,6 +126,48 @@ else() add_compile_options(-DRASPBERRY=0) endif() +# Get sick_scan_xd git version, see http://xit0.org/2013/04/cmake-use-git-branch-and-commit-details-in-project/ +option(QUERY_GIT_INFO "Get sick_scan_xd git version" ON) +if(${QUERY_GIT_INFO} OR QUERY_GIT_INFO EQUAL ON) + if (NOT DEFINED ENV{GITHASH}) + execute_process( + COMMAND git rev-parse HEAD + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + OUTPUT_VARIABLE GITHASH + OUTPUT_STRIP_TRAILING_WHITESPACE + ERROR_QUIET + ) + endif() + if (NOT DEFINED ENV{GITINFO}) + execute_process( + COMMAND git log -1 --pretty=format:%ad%x09%an%x09%s --date=iso + COMMAND iconv -f utf8 -t ascii//TRANSLIT + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + OUTPUT_VARIABLE GITINFO + OUTPUT_STRIP_TRAILING_WHITESPACE + ERROR_QUIET + ) + endif() + message(STATUS "GITHASH: ${GITHASH}") + message(STATUS "GITINFO: ${GITINFO}") + add_compile_options(-DGITHASH="\\"${GITHASH}\\"") + add_compile_options(-DGITINFO="\\"${GITINFO}\\"") +endif() + +# Extract sick_scan_xd version from package.xml and update sick_scan_xd_version.h if required +option(EXTRACT_PACKAGE_VERSION "Extract sick_scan_xd version from package.xml" ON) +if(${EXTRACT_PACKAGE_VERSION} OR EXTRACT_PACKAGE_VERSION EQUAL ON) + if(WIN32) + set(PYTHON_PKG_VERSION_TOOL_CMD python python/tools/package_version_tool.py) + else() + set(PYTHON_PKG_VERSION_TOOL_CMD python3 python/tools/package_version_tool.py) + endif() + execute_process( + COMMAND ${PYTHON_PKG_VERSION_TOOL_CMD} + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ERROR_QUIET + ) +endif() # Checks and prints a message, if a header file does not exist function(check_header_exists headerfilepath headerfilename packagename) @@ -153,10 +194,11 @@ endfunction() if(${ENABLE_EMULATOR} OR ENABLE_EMULATOR EQUAL ON) # emulator requires jsoncpp for pcapng and other files find_package(jsoncpp REQUIRED) # install libjsoncpp by running "sudo apt-get install libjsoncpp-dev" (Linux) resp. "vcpkg install jsoncpp:x64-windows" (Windows) - if(WIN32) - set(LIB_JSONCPP jsoncpp_lib) - else() - set(LIB_JSONCPP jsoncpp_lib) + set(LIB_JSONCPP jsoncpp_lib) +else() + find_package(jsoncpp QUIET) # jsoncpp optional (only required for dockertests of C++ API) + if (${jsoncpp_FOUND}) + set(LIB_JSONCPP jsoncpp_lib) endif() endif() @@ -767,6 +809,24 @@ if(BUILD_SICK_SCAN_XD_API_TEST EQUAL 1) target_link_libraries(sick_scan_xd_api_test "pthread") # pthread required for std::thread endif() endif() + add_executable(sick_scan_xd_api_dockertest + driver/src/sick_scan_xd_api/sick_scan_api_converter.cpp + test/src/sick_scan_xd_api/sick_scan_xd_api_dockertest.cpp + test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c + ) + target_link_libraries(sick_scan_xd_api_dockertest ${LIB_JSONCPP}) + if(NOT WIN32) + target_link_libraries(sick_scan_xd_api_dockertest "dl") # link with dl for dynamic library loading + if(ROS_VERSION EQUAL 0) + target_link_libraries(sick_scan_xd_api_dockertest "pthread") # pthread required for std::thread + target_link_options(sick_scan_xd_api_dockertest PUBLIC "LINKER:--no-as-needed") # fixes exception "Enable multithreading to use std::thread: Operation not permitted" + elseif(ROS_VERSION EQUAL 1) + target_link_libraries(sick_scan_xd_api_dockertest ${catkin_LIBRARIES}) + add_dependencies(sick_scan_xd_api_dockertest ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) + elseif(ROS_VERSION EQUAL 2) + target_link_libraries(sick_scan_xd_api_dockertest "pthread") # pthread required for std::thread + endif() + endif() endif() # install sick_scan_xd_shared_lib incl. API headerfiles @@ -796,7 +856,7 @@ if(ROS_VERSION EQUAL 1) RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) if(BUILD_SICK_SCAN_XD_API_TEST EQUAL 1) - install(TARGETS sick_scan_xd_api_test + install(TARGETS sick_scan_xd_api_test sick_scan_xd_api_dockertest RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) endif() @@ -822,7 +882,7 @@ endif() if(ROS_VERSION EQUAL 2) if(BUILD_SICK_SCAN_XD_API_TEST EQUAL 1) - install(TARGETS sick_scan_xd_api_test DESTINATION lib/${PROJECT_NAME}) + install(TARGETS sick_scan_xd_api_test sick_scan_xd_api_dockertest DESTINATION lib/${PROJECT_NAME}) endif() install(TARGETS sick_generic_caller DESTINATION lib/${PROJECT_NAME}) install(TARGETS ${PROJECT_NAME}_lib ${PROJECT_NAME}_shared_lib DESTINATION lib) diff --git a/driver/src/sick_generic_laser.cpp b/driver/src/sick_generic_laser.cpp index 052d1fa2..b4b904f8 100644 --- a/driver/src/sick_generic_laser.cpp +++ b/driver/src/sick_generic_laser.cpp @@ -66,6 +66,8 @@ #endif #include +#include +#include "softwarePLL.h" #if defined LDMRS_SUPPORT && LDMRS_SUPPORT > 0 #include #endif @@ -92,15 +94,23 @@ #include #include -#define SICK_GENERIC_MAJOR_VER "3" -#define SICK_GENERIC_MINOR_VER "4" -#define SICK_GENERIC_PATCH_LEVEL "0" +#ifdef GITHASH +#define GITHASH_STR (strlen(GITHASH)>2?(std::string(" githash:")+std::string(GITHASH)):(std::string(""))) +#else +#define GITHASH_STR std::string("") +#endif +#ifdef GITINFO +#define GITINFO_STR (strlen(GITINFO)>2?(std::string(" gitinfo:")+std::string(GITINFO)):(std::string(""))) +#else +#define GITINFO_STR std::string("") +#endif #define DELETE_PTR(p) if(p){delete(p);p=0;} static bool s_isInitialized = false; static sick_scan_xd::SickScanCommonTcp *s_scanner = NULL; -static std::string versionInfo = std::string(SICK_GENERIC_MAJOR_VER) + '.' + std::string(SICK_GENERIC_MINOR_VER) + '.' + std::string(SICK_GENERIC_PATCH_LEVEL); + +static std::string versionInfo = std::string(SICK_SCAN_XD_VERSION) + GITHASH_STR + GITINFO_STR; static bool s_shutdownSignalReceived = false; void setVersionInfo(std::string _versionInfo) @@ -110,7 +120,6 @@ void setVersionInfo(std::string _versionInfo) std::string getVersionInfo() { - return (versionInfo); } @@ -548,6 +557,14 @@ void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNo #endif } + // Optional timestamp mode: + // TICKS_TO_SYSTEM_TIMESTAMP = 0, // default: convert lidar ticks in microseconds to system timestamp by software-pll + // TICKS_TO_MICROSEC_OFFSET_TIMESTAMP = 1 // optional tick-mode: convert lidar ticks in microseconds to timestamp by 1.0e-6*(curtick-firstTick)+firstSystemTimestamp; + int tick_to_timestamp_mode = 0; + rosDeclareParam(nhPriv, "tick_to_timestamp_mode", tick_to_timestamp_mode); + rosGetParam(nhPriv, "tick_to_timestamp_mode", tick_to_timestamp_mode); + SoftwarePLL::instance().setTicksToTimestampMode(tick_to_timestamp_mode); + // Start TF publisher sick_scan_xd::SickTransformPublisher tf_publisher(nhPriv); tf_publisher.run(); @@ -838,7 +855,6 @@ void joinGenericLaser(void) { if (s_generic_laser_thread != 0) { - s_generic_laser_thread->join(); delete s_generic_laser_thread; s_generic_laser_thread = 0; @@ -861,7 +877,6 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP } // Send odometry data to NAV350 -#include "softwarePLL.h" #include "sick_scan_api.h" #include "sick_scan/sick_nav_scandata_parser.h" int32_t SickScanApiNavOdomVelocityImpl(SickScanApiHandle apiHandle, SickScanNavOdomVelocityMsg* src_msg) // odometry data in nav coordinates diff --git a/driver/src/sick_generic_monitoring.cpp b/driver/src/sick_generic_monitoring.cpp index eb99b6e6..1651ee9d 100644 --- a/driver/src/sick_generic_monitoring.cpp +++ b/driver/src/sick_generic_monitoring.cpp @@ -188,7 +188,8 @@ void sick_scan_xd::PointCloudMonitor::stopPointCloudMonitoring(void) m_monitoring_thread_running = false; if(m_monitoring_thread) { - m_monitoring_thread->join(); + if (m_monitoring_thread->joinable()) + m_monitoring_thread->join(); delete(m_monitoring_thread); m_monitoring_thread = 0; } diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp index 6ad4f9d0..92421d5f 100644 --- a/driver/src/sick_scan_common.cpp +++ b/driver/src/sick_scan_common.cpp @@ -3424,7 +3424,7 @@ namespace sick_scan_xd if(this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_MRS_1XXX_NAME) == 0 || this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_5XX_NAME) == 0) { - result &= sendSopasRunSetAccessMode(useBinaryCmd); + result = sendSopasRunSetAccessMode(useBinaryCmd); } lmp_scancfg.sector_cfg.push_back(lmp_scancfg_sector); std::string lmp_scancfg_sopas; diff --git a/driver/src/sick_scansegment_xd/msgpack_converter.cpp b/driver/src/sick_scansegment_xd/msgpack_converter.cpp index 71e2ca11..96888171 100644 --- a/driver/src/sick_scansegment_xd/msgpack_converter.cpp +++ b/driver/src/sick_scansegment_xd/msgpack_converter.cpp @@ -122,7 +122,8 @@ void sick_scansegment_xd::MsgPackConverter::Close(void) } if (m_converter_thread) { - m_converter_thread->join(); + if (m_converter_thread->joinable()) + m_converter_thread->join(); delete m_converter_thread; m_converter_thread = 0; } diff --git a/driver/src/sick_scansegment_xd/msgpack_exporter.cpp b/driver/src/sick_scansegment_xd/msgpack_exporter.cpp index da287037..7f2901c0 100644 --- a/driver/src/sick_scansegment_xd/msgpack_exporter.cpp +++ b/driver/src/sick_scansegment_xd/msgpack_exporter.cpp @@ -127,33 +127,33 @@ std::list sick_scansegment_xd::Ms bool sick_scansegment_xd::MsgPackExporter::Start(void) { m_run_exporter_thread = true; - m_exporter_thread = new std::thread(&sick_scansegment_xd::MsgPackExporter::Run, this); // Without visualization, msgpack export runs in background thread + m_exporter_thread = new std::thread(&sick_scansegment_xd::MsgPackExporter::RunCb, this); // Without visualization, msgpack export runs in background thread return true; } /* * @brief Stops the background thread */ +void sick_scansegment_xd::MsgPackExporter::Stop(void) +{ + m_run_exporter_thread = false; +} + +/* + * @brief Stops, joins and deletes the background thread + */ void sick_scansegment_xd::MsgPackExporter::Close(void) { m_run_exporter_thread = false; if (m_exporter_thread) { - m_exporter_thread->join(); + if (m_exporter_thread->joinable()) + m_exporter_thread->join(); delete m_exporter_thread; m_exporter_thread = 0; } } -/* - * @brief Runs the exporter in the current thread. Pops msgpack data packages from the input fifo and optionally export them to csv and/or plot the lidar points. - */ -bool sick_scansegment_xd::MsgPackExporter::Run(void) -{ - m_run_exporter_thread = true; - return RunCb(); -} - /* * @brief Thread callback, runs the exporter. Pops msgpack data packages from the input fifo and optionally export them to csv and/or plot the lidar points. */ diff --git a/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp b/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp index 6f83a193..ae0e6eaa 100644 --- a/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp +++ b/driver/src/sick_scansegment_xd/ros_msgpack_publisher.cpp @@ -875,37 +875,37 @@ void sick_scansegment_xd::RosMsgpackPublisher::HandleMsgPackData(const sick_scan // Publish optional IMU data if (msgpack_data.scandata.empty() && msgpack_data.imudata.valid) { - if (m_publisher_imu_initialized) - { - ROS_DEBUG_STREAM("Publishing IMU data: { " << msgpack_data.imudata.to_string() << " }"); - // Convert to ros_sensor_msgs::Imu - ros_sensor_msgs::Imu imu_msg; - imu_msg.header.stamp.sec = msgpack_data.timestamp_sec; + ROS_DEBUG_STREAM("Publishing IMU data: { " << msgpack_data.imudata.to_string() << " }"); + // Convert to ros_sensor_msgs::Imu + ros_sensor_msgs::Imu imu_msg; + imu_msg.header.stamp.sec = msgpack_data.timestamp_sec; #if defined __ROS_VERSION && __ROS_VERSION > 1 - imu_msg.header.stamp.nanosec = msgpack_data.timestamp_nsec; + imu_msg.header.stamp.nanosec = msgpack_data.timestamp_nsec; #else - imu_msg.header.stamp.nsec = msgpack_data.timestamp_nsec; + imu_msg.header.stamp.nsec = msgpack_data.timestamp_nsec; #endif - imu_msg.header.frame_id = m_frame_id; - imu_msg.orientation.w = msgpack_data.imudata.orientation_w; - imu_msg.orientation.x = msgpack_data.imudata.orientation_x; - imu_msg.orientation.y = msgpack_data.imudata.orientation_y; - imu_msg.orientation.z = msgpack_data.imudata.orientation_z; - imu_msg.angular_velocity.x = msgpack_data.imudata.angular_velocity_x; - imu_msg.angular_velocity.y = msgpack_data.imudata.angular_velocity_y; - imu_msg.angular_velocity.z = msgpack_data.imudata.angular_velocity_z; - imu_msg.linear_acceleration.x = msgpack_data.imudata.acceleration_x; - imu_msg.linear_acceleration.y = msgpack_data.imudata.acceleration_y; - imu_msg.linear_acceleration.z = msgpack_data.imudata.acceleration_z; - // ros imu message definition: A covariance matrix of all zeros will be interpreted as "covariance unknown" - for(int n = 0; n < 9; n++) - { - imu_msg.orientation_covariance[n] = 0; - imu_msg.angular_velocity_covariance[n] = 0; - imu_msg.linear_acceleration_covariance[n] = 0; - } - // Publish imu message - sick_scan_xd::notifyImuListener(m_node, &imu_msg); + imu_msg.header.frame_id = m_frame_id; + imu_msg.orientation.w = msgpack_data.imudata.orientation_w; + imu_msg.orientation.x = msgpack_data.imudata.orientation_x; + imu_msg.orientation.y = msgpack_data.imudata.orientation_y; + imu_msg.orientation.z = msgpack_data.imudata.orientation_z; + imu_msg.angular_velocity.x = msgpack_data.imudata.angular_velocity_x; + imu_msg.angular_velocity.y = msgpack_data.imudata.angular_velocity_y; + imu_msg.angular_velocity.z = msgpack_data.imudata.angular_velocity_z; + imu_msg.linear_acceleration.x = msgpack_data.imudata.acceleration_x; + imu_msg.linear_acceleration.y = msgpack_data.imudata.acceleration_y; + imu_msg.linear_acceleration.z = msgpack_data.imudata.acceleration_z; + // ros imu message definition: A covariance matrix of all zeros will be interpreted as "covariance unknown" + for(int n = 0; n < 9; n++) + { + imu_msg.orientation_covariance[n] = 0; + imu_msg.angular_velocity_covariance[n] = 0; + imu_msg.linear_acceleration_covariance[n] = 0; + } + // Publish imu message + sick_scan_xd::notifyImuListener(m_node, &imu_msg); + if (m_publisher_imu_initialized) + { #if defined __ROS_VERSION && __ROS_VERSION > 1 m_publisher_imu->publish(imu_msg); #else diff --git a/driver/src/sick_scansegment_xd/scansegment_threads.cpp b/driver/src/sick_scansegment_xd/scansegment_threads.cpp index 188fa630..3b3b9cb8 100644 --- a/driver/src/sick_scansegment_xd/scansegment_threads.cpp +++ b/driver/src/sick_scansegment_xd/scansegment_threads.cpp @@ -165,23 +165,6 @@ void sick_scansegment_xd::MsgPackThreads::join(void) } } -/* Send "start" trigger via UDP -static void sendStartTrigger(sick_scansegment_xd::Config& config) -{ - if (config.send_udp_start) - { - sick_scansegment_xd::UdpSenderSocketImpl udp_sender(config.hostname, config.port); - std::vector send_udp_start_bytes(config.send_udp_start_string.begin(), config.send_udp_start_string.end()); - if (!udp_sender.IsOpen()) - ROS_ERROR_STREAM ("## ERROR sick_scansegment_xd: could not open udp socket " << config.hostname << ":" << config.port); - else if (!udp_sender.Send(send_udp_start_bytes)) - ROS_ERROR_STREAM ("## ERROR sick_scansegment_xd: could not send start string \"" << config.send_udp_start_string << "\" using udp socket " << config.hostname << ":" << config.port); - else - ROS_INFO_STREAM ("sick_scansegment_xd: start string sent on udp socket " << config.hostname << ":" << config.port); - } -} -*/ - /* * @brief Thread callback, initializes and runs msgpack receiver, converter and publisher. */ @@ -198,12 +181,9 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) { ROS_INFO_STREAM("sick_scansegment_xd initializing..."); - // Send "start" trigger via UDP - // sendStartTrigger(m_config); - // Initialize udp receiver for scan data sick_scansegment_xd::UdpReceiver* udp_receiver = 0; - while(udp_receiver == 0) + while(m_run_scansegment_thread && rosOk() && udp_receiver == 0) { udp_receiver = new sick_scansegment_xd::UdpReceiver(); if(udp_receiver->Init(m_config.udp_sender, m_config.udp_port, m_config.udp_input_fifolength, m_config.verbose_level > 1, m_config.export_udp_msg, m_config.scandataformat, 0)) @@ -221,7 +201,7 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) // Initialize udp receiver for imu data sick_scansegment_xd::UdpReceiver* udp_receiver_imu = 0; - while(m_config.imu_enable && m_config.scandataformat == SCANDATA_COMPACT && udp_receiver_imu == 0) + while(m_run_scansegment_thread && rosOk() && m_config.imu_enable && m_config.scandataformat == SCANDATA_COMPACT && udp_receiver_imu == 0) { udp_receiver_imu = new sick_scansegment_xd::UdpReceiver(); if(udp_receiver_imu->Init(m_config.udp_sender, m_config.imu_udp_port, m_config.udp_input_fifolength, m_config.verbose_level > 1, m_config.export_udp_msg, m_config.scandataformat, udp_receiver->Fifo())) // udp receiver for scan and imu data share the same fifo @@ -268,9 +248,6 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) ROS_ERROR_STREAM("## ERROR sick_scansegment_xd: MsgPackConverter::Start(), UdpReceiver::Start() or MsgPackExporter::Start() failed, not receiving udp packages from " << m_config.udp_sender << ":" << m_config.udp_port); } - // Send "start" via UDP - // sendStartTrigger(m_config); - // Start SOPAS services (ROS-1 or ROS-2 only) sick_scan_xd::SickScanCommonTcp* sopas_tcp = 0; sick_scan_xd::SickScanServices* sopas_service = 0; @@ -378,8 +355,9 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) // Close msgpack receiver, converter and exporter setDiagnosticStatus(SICK_DIAGNOSTIC_STATUS::EXIT, "sick_scan_xd exit"); - ROS_INFO_STREAM("sick_scansegment_xd finishing."); + ROS_INFO_STREAM("sick_scansegment_xd finishing (" << __LINE__ << ")"); msgpack_exporter.RemoveExportListener(ros_msgpack_publisher->ExportListener()); + msgpack_exporter.Stop(); if (udp_receiver_imu) { udp_receiver_imu->Close(); @@ -387,7 +365,7 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) } udp_receiver->Stop(); udp_receiver->Fifo()->Shutdown(); - ROS_INFO_STREAM("sick_scansegment_xd finishing."); + ROS_INFO_STREAM("sick_scansegment_xd finishing (" << __LINE__ << ")"); // Send stop command (sopas and/or rest-api) if(sopas_tcp && sopas_service && m_config.send_sopas_start_stop_cmd && sopas_tcp->isConnected()) @@ -398,7 +376,7 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) std::cout << "sick_scansegment_xd exit: stop commands sent." << std::endl; } // Stop SOPAS services - std::cout << "sick_scansegment_xd exit: stopping services and communication..." << std::endl; + std::cout << "sick_scansegment_xd exit: stopping services and communication (" << __LINE__ << ")" << std::endl; try { // Shutdown, cleanup and exit @@ -409,7 +387,7 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) msgpack_converter.Close(); udp_receiver->Close(); DELETE_PTR(udp_receiver); - std::cout << "sick_scansegment_xd exit: services and communication stopped." << std::endl; + std::cout << "sick_scansegment_xd exit: services and communication stopped (" << __LINE__ << ")" << std::endl; } catch(const std::exception& e) { @@ -417,6 +395,6 @@ bool sick_scansegment_xd::MsgPackThreads::runThreadCb(void) } } - ROS_INFO_STREAM("sick_scansegment_xd::runThreadCb() finished"); + ROS_INFO_STREAM("sick_scansegment_xd::runThreadCb() finished (" << __LINE__ << ")"); return true; } diff --git a/driver/src/sick_scansegment_xd/udp_receiver.cpp b/driver/src/sick_scansegment_xd/udp_receiver.cpp index 04c63cf8..1c2eb60b 100644 --- a/driver/src/sick_scansegment_xd/udp_receiver.cpp +++ b/driver/src/sick_scansegment_xd/udp_receiver.cpp @@ -160,6 +160,8 @@ bool sick_scansegment_xd::UdpReceiver::Start(void) void sick_scansegment_xd::UdpReceiver::Stop(bool do_join) { m_run_receiver_thread = false; + if (m_socket_impl) + m_socket_impl->running() = false; if (do_join && m_receiver_thread && m_receiver_thread->joinable()) m_receiver_thread->join(); } @@ -170,6 +172,8 @@ void sick_scansegment_xd::UdpReceiver::Stop(bool do_join) void sick_scansegment_xd::UdpReceiver::Close(void) { m_run_receiver_thread = false; + if (m_socket_impl) + m_socket_impl->running() = false; if (m_fifo_impl && m_fifo_impl_created) { m_fifo_impl->Shutdown(); @@ -219,7 +223,7 @@ bool sick_scansegment_xd::UdpReceiver::Run(void) bool do_print_crc_error = (sick_scansegment_xd::Seconds(timestamp_last_print_crc_error, chrono_system_clock::now()) > 1.0); // avoid printing crc errors with more than 1 Hz // std::cout << "UdpReceiver::Run(): " << bytes_received << " bytes received" << std::endl; ROS_DEBUG_STREAM("UdpReceiver::Run(): " << bytes_received << " bytes received (port " << m_socket_impl->port() << ", udp_receiver.cpp:" << __LINE__ << ")"); - if(bytes_received > m_udp_msg_start_seq.size() + 8 && std::equal(udp_payload.begin(), udp_payload.begin() + m_udp_msg_start_seq.size(), m_udp_msg_start_seq.begin())) + if(m_run_receiver_thread && bytes_received > m_udp_msg_start_seq.size() + 8 && std::equal(udp_payload.begin(), udp_payload.begin() + m_udp_msg_start_seq.size(), m_udp_msg_start_seq.begin())) { // Received \x02\x02\x02\x02 | 4Bytes payload length | Payload | CRC32 bool crc_error = false, check_crc = true; @@ -276,6 +280,7 @@ bool sick_scansegment_xd::UdpReceiver::Run(void) ROS_ERROR_STREAM("## ERROR UdpReceiver::Run(): invalid scandataformat configuration, unsupported scandataformat=" << m_scandataformat << ", check configuration and use " << SCANDATA_MSGPACK << " for msgpack or " << SCANDATA_COMPACT << " for compact data"); m_run_receiver_thread = false; + m_socket_impl->running() = false; return false; } if (bytes_received != bytes_to_receive) @@ -358,6 +363,7 @@ bool sick_scansegment_xd::UdpReceiver::Run(void) udp_recv_timeout = m_udp_timeout_recv_nonblocking; // receive non-blocking with timeout } m_run_receiver_thread = false; + m_socket_impl->running() = false; return true; } catch (std::exception & e) @@ -365,6 +371,7 @@ bool sick_scansegment_xd::UdpReceiver::Run(void) ROS_ERROR_STREAM("## ERROR UdpReceiver::Run(): " << e.what()); } m_run_receiver_thread = false; + m_socket_impl->running() = false; return false; } diff --git a/driver/src/softwarePLL.cpp b/driver/src/softwarePLL.cpp index 47686252..c766a82a 100644 --- a/driver/src/softwarePLL.cpp +++ b/driver/src/softwarePLL.cpp @@ -125,6 +125,14 @@ int SoftwarePLL::findDiffInFifo(double diff, double tol) */ bool SoftwarePLL::updatePLL(uint32_t sec, uint32_t nanoSec, uint32_t curtick) { + if (offsetTimestampFirstLidarTick == 0) + { + // Store first timestamp and ticks for optional TICKS_TO_MICROSEC_OFFSET_TIMESTAMP + offsetTimestampFirstSystemSec = sec; + offsetTimestampFirstSystemMicroSec = nanoSec / 1000; + offsetTimestampFirstLidarTick = curtick; + } + if (curtick != this->lastcurtick) { this->lastcurtick = curtick; @@ -188,12 +196,20 @@ bool SoftwarePLL::getCorrectedTimeStamp(uint32_t &sec, uint32_t &nanoSec, uint32 { return (false); } - - double relTimeStamp = extraPolateRelativeTimeStamp(curtick); // evtl. hier wg. Ueberlauf noch einmal pruefen - double corrTime = relTimeStamp + this->FirstTimeStamp(); + double corrTime = 0; + if (ticksToTimestampMode == TICKS_TO_MICROSEC_OFFSET_TIMESTAMP) // optional tick-mode: convert lidar ticks in microseconds to timestamp by 1.0e-6*(curtick-firstTick)+firstSystemTimestamp + { + corrTime = 1.0e-6 * (curtick - offsetTimestampFirstLidarTick) + (offsetTimestampFirstSystemSec + 1.0e-6 * offsetTimestampFirstSystemMicroSec); + } + else // default: convert lidar ticks in microseconds to system timestamp by software-pll + { + double relTimeStamp = extraPolateRelativeTimeStamp(curtick); // evtl. hier wg. Ueberlauf noch einmal pruefen + corrTime = relTimeStamp + this->FirstTimeStamp(); + } sec = (uint32_t) corrTime; double frac = corrTime - sec; nanoSec = (uint32_t) (1E9 * frac); + // std::cout << "SoftwarePLL::getCorrectedTimeStamp(): timestamp_mode=" << (int)ticksToTimestampMode << ", curticks = " << curtick << " [microsec], system time = " << std::fixed << std::setprecision(9) << (sec + 1.0e-9 * nanoSec) << " [sec]" << std::endl; return (true); } @@ -204,15 +220,24 @@ bool SoftwarePLL::convSystemtimeToLidarTimestamp(uint32_t systemtime_sec, uint32 { return (false); } - double systemTimestamp = (double)systemtime_sec + 1.0e-9 * (double)systemtime_nanosec; // systemTimestamp := corrTime in getCorrectedTimeStamp - // getCorrectedTimeStamp(): corrTime = relTimeStamp + this->FirstTimeStamp() - // => inverse: relSystemTimestamp = systemTimestamp - this->FirstTimeStamp() - double relSystemTimestamp = systemTimestamp - this->FirstTimeStamp(); - // getCorrectedTimeStamp(): relSystemTimestamp = (tick - (uint32_t) (0xFFFFFFFF & FirstTick())) * this->InterpolationSlope() - //=> inverse: tick = (relSystemTimestamp / this->InterpolationSlope()) + (uint32_t) (0xFFFFFFFF & FirstTick()) - double relTicks = relSystemTimestamp / this->InterpolationSlope(); - uint32_t tick_offset = (uint32_t)(0xFFFFFFFF & FirstTick()); - tick = (uint32_t)std::round(relTicks + tick_offset); + if (ticksToTimestampMode == TICKS_TO_MICROSEC_OFFSET_TIMESTAMP) // optional tick-mode: convert lidar ticks in microseconds to timestamp by 1.0e-6*(curtick-firstTick)+firstSystemTimestamp + { + double relSystemTimestamp = (systemtime_sec + 1.0e-9 * systemtime_nanosec) - (offsetTimestampFirstSystemSec + 1.0e-6 * offsetTimestampFirstSystemMicroSec); + double relTicks = 1.0e6 * relSystemTimestamp; + tick = (uint32_t)std::round(relTicks + offsetTimestampFirstLidarTick); + } + else // default: convert lidar ticks in microseconds to system timestamp by software-pll + { + double systemTimestamp = (double)systemtime_sec + 1.0e-9 * (double)systemtime_nanosec; // systemTimestamp := corrTime in getCorrectedTimeStamp + // getCorrectedTimeStamp(): corrTime = relTimeStamp + this->FirstTimeStamp() + // => inverse: relSystemTimestamp = systemTimestamp - this->FirstTimeStamp() + double relSystemTimestamp = systemTimestamp - this->FirstTimeStamp(); + // getCorrectedTimeStamp(): relSystemTimestamp = (tick - (uint32_t) (0xFFFFFFFF & FirstTick())) * this->InterpolationSlope() + //=> inverse: tick = (relSystemTimestamp / this->InterpolationSlope()) + (uint32_t) (0xFFFFFFFF & FirstTick()) + double relTicks = relSystemTimestamp / this->InterpolationSlope(); + uint32_t tick_offset = (uint32_t)(0xFFFFFFFF & FirstTick()); + tick = (uint32_t)std::round(relTicks + tick_offset); + } return (true); } diff --git a/include/sick_scan/sick_scan_xd_version.h b/include/sick_scan/sick_scan_xd_version.h new file mode 100644 index 00000000..120d69cb --- /dev/null +++ b/include/sick_scan/sick_scan_xd_version.h @@ -0,0 +1,4 @@ +/* This file has been generated by package_version_tool.py. Do not edit this file. To provide a new version number, modify file package.xml and rebuild */ +#ifndef SICK_SCAN_XD_VERSION +#define SICK_SCAN_XD_VERSION "3.4.1" +#endif // SICK_SCAN_XD_VERSION diff --git a/include/sick_scan/softwarePLL.h b/include/sick_scan/softwarePLL.h index 49cd57a2..ddf9b545 100644 --- a/include/sick_scan/softwarePLL.h +++ b/include/sick_scan/softwarePLL.h @@ -42,7 +42,13 @@ class SoftwarePLL static void testbed(); bool IsInitialized() const - { return isInitialized; } + { + if (ticksToTimestampMode == TICKS_TO_MICROSEC_OFFSET_TIMESTAMP) + { + return (offsetTimestampFirstLidarTick > 0); + } + return isInitialized; + } void IsInitialized(bool val) { isInitialized = val; } @@ -86,6 +92,11 @@ class SoftwarePLL size_t packets_received = 0; // just for printing statusmessages when dropping packets double max_abs_delta_time = 0; // just for printing statusmessages when dropping packets + void setTicksToTimestampMode(int val) + { + ticksToTimestampMode = (TICKS_TO_TIMESTAMP_MODE)val; + } + private: int numberValInFifo; static const double MaxAllowedTimeDeviation; @@ -106,6 +117,16 @@ class SoftwarePLL double mostRecentTimeStamp; double interpolationSlope; + enum TICKS_TO_TIMESTAMP_MODE + { + TICKS_TO_SYSTEM_TIMESTAMP = 0, // default: convert lidar ticks in microseconds to system timestamp by software-pll + TICKS_TO_MICROSEC_OFFSET_TIMESTAMP = 1 // optional tick-mode: convert lidar ticks in microseconds to timestamp by 1.0e-6*(curtick-firstTick)+firstSystemTimestamp + }; + TICKS_TO_TIMESTAMP_MODE ticksToTimestampMode = TICKS_TO_SYSTEM_TIMESTAMP; + uint32_t offsetTimestampFirstSystemSec = 0; + uint32_t offsetTimestampFirstSystemMicroSec = 0; + uint32_t offsetTimestampFirstLidarTick = 0; + bool nearSameTimeStamp(double relTimeStamp1, double relTimeStamp2, double& delta_time_abs); bool updateInterpolationSlope(); diff --git a/include/sick_scan/tcp/SickThread.hpp b/include/sick_scan/tcp/SickThread.hpp index 724c1928..e9eff7e4 100644 --- a/include/sick_scan/tcp/SickThread.hpp +++ b/include/sick_scan/tcp/SickThread.hpp @@ -54,7 +54,7 @@ class ThreadWrapperBase void join() { // pthread_join(t_id, NULL); - if(t_id) + if(t_id && t_id->joinable()) t_id->join(); pthis = NULL; } diff --git a/include/sick_scansegment_xd/msgpack_exporter.h b/include/sick_scansegment_xd/msgpack_exporter.h index 2f22490b..7029d66a 100644 --- a/include/sick_scansegment_xd/msgpack_exporter.h +++ b/include/sick_scansegment_xd/msgpack_exporter.h @@ -115,12 +115,12 @@ namespace sick_scansegment_xd bool Start(void); /* - * @brief Runs the exporter in the current thread. Pops msgpack data packages from the input fifo and optionally export them to csv and/or plot the lidar points. + * @brief Stops the background thread */ - bool Run(void); + void Stop(void); - /* - * @brief Stops the background thread + /* + * @brief Stops, joins and deletes the background thread */ void Close(void); diff --git a/include/sick_scansegment_xd/udp_sockets.h b/include/sick_scansegment_xd/udp_sockets.h index 47de0f47..0bdc3328 100644 --- a/include/sick_scansegment_xd/udp_sockets.h +++ b/include/sick_scansegment_xd/udp_sockets.h @@ -108,7 +108,7 @@ namespace sick_scansegment_xd public: /** Default constructor */ - UdpReceiverSocketImpl() : m_udp_sender(""), m_udp_port(0), m_udp_socket(INVALID_SOCKET) + UdpReceiverSocketImpl() : m_udp_sender(""), m_udp_port(0), m_udp_socket(INVALID_SOCKET), m_running(false), m_recv_blocking(false), m_recv_flags(0) { } @@ -130,9 +130,15 @@ namespace sick_scansegment_xd } } - /** Opens a udp socket */ - bool Init(const std::string& udp_sender, int udp_port) + /* + ** @brief Opens a udp socket + ** @param[in] udp_sender ip v4 address of sender (e.g. "192.168.0.100"), or "" for any + ** @param[in] udp_port port number (e.g. 2115 for multiScan) + ** @param[in] blocking option to recv blocking (true) or non-blocking (false, default) + */ + bool Init(const std::string& udp_sender, int udp_port, bool blocking = false) { + m_running = false; try { wsa_init(); @@ -166,6 +172,16 @@ namespace sick_scansegment_xd m_udp_socket = INVALID_SOCKET; return false; } + // Set socket to blocking or non-blocking recv mode + m_recv_blocking = blocking; +# ifdef _MSC_VER + u_long recv_mode = (m_recv_blocking ? 0 : 1); // FIONBIO enables or disables the blocking mode for the socket. If iMode = 0, blocking is enabled, if iMode != 0, non-blocking mode is enabled. + ioctlsocket(m_udp_socket, FIONBIO, &recv_mode); +# else + if (!m_recv_blocking) + m_recv_flags |= MSG_DONTWAIT; +# endif + m_running = true; return true; } catch (std::exception & e) @@ -180,9 +196,13 @@ namespace sick_scansegment_xd size_t Receive(std::vector& msg_payload) { int64_t bytes_received = 0; - while(bytes_received == 0) - bytes_received = recv(m_udp_socket, (char*)msg_payload.data(), (int)msg_payload.size(), 0); - if (bytes_received < 0) + while (m_running && bytes_received <= 0) + { + bytes_received = recv(m_udp_socket, (char*)msg_payload.data(), (int)msg_payload.size(), m_recv_flags); + if (m_recv_blocking && bytes_received < 0) + return 0; // socket error + } + if (!m_running || bytes_received < 0) return 0; // socket error return (size_t)bytes_received; } @@ -195,9 +215,9 @@ namespace sick_scansegment_xd size_t bytes_received = 0; size_t bytes_to_receive = msg_payload.size(); // Receive \x02\x02\x02\x02 | 4Bytes payloadlength incl. CRC | Payload | CRC32 - while (bytes_received < bytes_to_receive && (timeout < 0 || sick_scansegment_xd::Seconds(start_timestamp, chrono_system_clock::now()) < timeout)) + while (m_running && bytes_received < bytes_to_receive && (timeout < 0 || sick_scansegment_xd::Seconds(start_timestamp, chrono_system_clock::now()) < timeout)) { - int64_t chunk_bytes_received = recv(m_udp_socket, (char*)msg_payload.data() + bytes_received, (int)msg_payload.size() - bytes_received, 0); + int64_t chunk_bytes_received = recv(m_udp_socket, (char*)msg_payload.data() + bytes_received, (int)msg_payload.size() - bytes_received, m_recv_flags); if (chunk_bytes_received <= 0) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); @@ -226,17 +246,23 @@ namespace sick_scansegment_xd bytes_received += chunk_bytes_received; } } - return bytes_received; + return (m_running ? bytes_received : 0); } /** Return the udp port */ int port(void) const { return m_udp_port; } + /** Return true, if socket is ready to receive, or false otherwise. Set false to signal stop receiving */ + bool& running(void) { return m_running; } + protected: std::string m_udp_sender; // IP of udp sender int m_udp_port; // udp port SOCKET m_udp_socket; // udp raw socket + bool m_running; // true: ready to receive, false: not initialized / stop receiving + bool m_recv_blocking; // option to recv blocking (true) or non-blocking (false) + int m_recv_flags; // flag for socket recv, 0 (default) or MSG_DONTWAIT for non-blocking operation }; /*! diff --git a/launch/sick_ldmrs.launch b/launch/sick_ldmrs.launch index b8ffe72a..44b152d1 100644 --- a/launch/sick_ldmrs.launch +++ b/launch/sick_ldmrs.launch @@ -99,5 +99,12 @@ + + + diff --git a/launch/sick_lms_1xx.launch b/launch/sick_lms_1xx.launch index 3beaa7de..e17f2400 100644 --- a/launch/sick_lms_1xx.launch +++ b/launch/sick_lms_1xx.launch @@ -97,5 +97,12 @@ + + + diff --git a/launch/sick_lms_1xxx.launch b/launch/sick_lms_1xxx.launch index c5739c18..2195a0df 100644 --- a/launch/sick_lms_1xxx.launch +++ b/launch/sick_lms_1xxx.launch @@ -106,5 +106,12 @@ + + + diff --git a/launch/sick_lms_1xxx_v2.launch b/launch/sick_lms_1xxx_v2.launch index 81bff187..2e12785d 100644 --- a/launch/sick_lms_1xxx_v2.launch +++ b/launch/sick_lms_1xxx_v2.launch @@ -111,5 +111,12 @@ + + + diff --git a/launch/sick_lms_4xxx.launch b/launch/sick_lms_4xxx.launch index 903e3a49..5e129497 100644 --- a/launch/sick_lms_4xxx.launch +++ b/launch/sick_lms_4xxx.launch @@ -130,6 +130,13 @@ + + + diff --git a/launch/sick_lms_5xx.launch b/launch/sick_lms_5xx.launch index 9368d83f..9b4f2021 100644 --- a/launch/sick_lms_5xx.launch +++ b/launch/sick_lms_5xx.launch @@ -122,6 +122,13 @@ Check IP-address, if you scanner is not found after roslaunch. + + + + + + diff --git a/launch/sick_lrs_36x0_upside_down.launch b/launch/sick_lrs_36x0_upside_down.launch index 5419caa0..229837c6 100644 --- a/launch/sick_lrs_36x0_upside_down.launch +++ b/launch/sick_lrs_36x0_upside_down.launch @@ -105,5 +105,12 @@ + + + diff --git a/launch/sick_lrs_36x1.launch b/launch/sick_lrs_36x1.launch index e71b5ac9..63153214 100644 --- a/launch/sick_lrs_36x1.launch +++ b/launch/sick_lrs_36x1.launch @@ -104,5 +104,12 @@ + + + diff --git a/launch/sick_lrs_36x1_upside_down.launch b/launch/sick_lrs_36x1_upside_down.launch index 3cb93702..2dc08f1c 100644 --- a/launch/sick_lrs_36x1_upside_down.launch +++ b/launch/sick_lrs_36x1_upside_down.launch @@ -104,5 +104,12 @@ + + + diff --git a/launch/sick_lrs_4xxx.launch b/launch/sick_lrs_4xxx.launch index 20ef6841..682b5a97 100644 --- a/launch/sick_lrs_4xxx.launch +++ b/launch/sick_lrs_4xxx.launch @@ -109,5 +109,12 @@ + + + diff --git a/launch/sick_mrs_1xxx.launch b/launch/sick_mrs_1xxx.launch index 9bbdff36..58236527 100644 --- a/launch/sick_mrs_1xxx.launch +++ b/launch/sick_mrs_1xxx.launch @@ -128,6 +128,13 @@ default max_angle for this scanner type is +137.5 degree. + + + + + + + + + diff --git a/launch/sick_mrs_6xxx_first_echo.launch b/launch/sick_mrs_6xxx_first_echo.launch index cb02c062..e8cdd4a1 100644 --- a/launch/sick_mrs_6xxx_first_echo.launch +++ b/launch/sick_mrs_6xxx_first_echo.launch @@ -93,5 +93,12 @@ + + + diff --git a/launch/sick_mrs_6xxx_last_echo.launch b/launch/sick_mrs_6xxx_last_echo.launch index aafd11b4..e4e7530b 100644 --- a/launch/sick_mrs_6xxx_last_echo.launch +++ b/launch/sick_mrs_6xxx_last_echo.launch @@ -92,5 +92,12 @@ + + + diff --git a/launch/sick_mrs_6xxx_narrow.launch b/launch/sick_mrs_6xxx_narrow.launch index fd9d6615..d7231a9e 100644 --- a/launch/sick_mrs_6xxx_narrow.launch +++ b/launch/sick_mrs_6xxx_narrow.launch @@ -94,5 +94,12 @@ + + + diff --git a/launch/sick_multiscan.launch b/launch/sick_multiscan.launch index 1bbc4b35..f033becb 100644 --- a/launch/sick_multiscan.launch +++ b/launch/sick_multiscan.launch @@ -250,6 +250,13 @@ + + + diff --git a/launch/sick_nav_2xx.launch b/launch/sick_nav_2xx.launch index 12c5f363..f46a16a3 100644 --- a/launch/sick_nav_2xx.launch +++ b/launch/sick_nav_2xx.launch @@ -80,5 +80,12 @@ + + + diff --git a/launch/sick_nav_31x.launch b/launch/sick_nav_31x.launch index c33e0c1d..dcd4de9c 100644 --- a/launch/sick_nav_31x.launch +++ b/launch/sick_nav_31x.launch @@ -97,6 +97,13 @@ + + + diff --git a/launch/sick_nav_350.launch b/launch/sick_nav_350.launch index eddb46af..1b65318c 100644 --- a/launch/sick_nav_350.launch +++ b/launch/sick_nav_350.launch @@ -71,6 +71,13 @@ + + + diff --git a/launch/sick_new_ip.launch b/launch/sick_new_ip.launch index c207ac05..577c5132 100644 --- a/launch/sick_new_ip.launch +++ b/launch/sick_new_ip.launch @@ -39,6 +39,13 @@ + + + diff --git a/launch/sick_oem_15xx.launch b/launch/sick_oem_15xx.launch index 5811d939..b2232737 100644 --- a/launch/sick_oem_15xx.launch +++ b/launch/sick_oem_15xx.launch @@ -104,6 +104,13 @@ + + + diff --git a/launch/sick_picoscan.launch b/launch/sick_picoscan.launch index e061fdd1..5a97e549 100644 --- a/launch/sick_picoscan.launch +++ b/launch/sick_picoscan.launch @@ -238,6 +238,13 @@ + + + diff --git a/launch/sick_picoscan_single_echo.launch b/launch/sick_picoscan_single_echo.launch index cba02b33..9263bb8d 100644 --- a/launch/sick_picoscan_single_echo.launch +++ b/launch/sick_picoscan_single_echo.launch @@ -236,6 +236,13 @@ + + + diff --git a/launch/sick_rms_xxxx.launch b/launch/sick_rms_xxxx.launch index 975f938a..c8ad7702 100644 --- a/launch/sick_rms_xxxx.launch +++ b/launch/sick_rms_xxxx.launch @@ -79,5 +79,12 @@ + + + diff --git a/launch/sick_tim_240.launch b/launch/sick_tim_240.launch index bbdc5bfe..1273e01c 100644 --- a/launch/sick_tim_240.launch +++ b/launch/sick_tim_240.launch @@ -101,5 +101,12 @@ + + + diff --git a/launch/sick_tim_4xx.launch b/launch/sick_tim_4xx.launch index ed2a9fb8..183c5fd0 100644 --- a/launch/sick_tim_4xx.launch +++ b/launch/sick_tim_4xx.launch @@ -125,6 +125,13 @@ + + + diff --git a/launch/sick_tim_5xx.launch b/launch/sick_tim_5xx.launch index de8abdbb..ae0af9eb 100644 --- a/launch/sick_tim_5xx.launch +++ b/launch/sick_tim_5xx.launch @@ -102,6 +102,13 @@ + + + + + + + + + diff --git a/launch/sick_tim_7xxS.launch b/launch/sick_tim_7xxS.launch index ff76d4f8..f07927cc 100644 --- a/launch/sick_tim_7xxS.launch +++ b/launch/sick_tim_7xxS.launch @@ -115,6 +115,13 @@ AND IS IN THE SAME STATE AS AFTER INITIALIZATION BY THIS LAUNCHFILE! --> + + + diff --git a/package.xml b/package.xml index 24837cc8..dd45fcfa 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ sick_scan_xd - 3.4.0 + 3.4.1 ROS 1 and 2 driver for SICK scanner rostest Michael Lehning diff --git a/python/tools/package_version_tool.py b/python/tools/package_version_tool.py new file mode 100644 index 00000000..a984db17 --- /dev/null +++ b/python/tools/package_version_tool.py @@ -0,0 +1,64 @@ +# +# package_version_tool.py extracts the version number from package.xml +# and generates a headerfile sick_scan_xd_version.h defining that version. +# +# Usage example: +# python python/tools/package_version_tool.py --package_xml_file package.xml --cpp_header_path include/sick_scan/sick_scan_xd_version.h +# + +import xml.etree.ElementTree as ET +import argparse + +def extract_version_from_header(file_path): + # Define the exact line keyword with version number + search_keyword = '#define SICK_SCAN_XD_VERSION' + + # Open the header file and read line by line + with open(file_path, 'r') as file: + for line in file: + # Ensure the line starts with the keyword including the version number + if line.strip().startswith(search_keyword): + # Extract the version number enclosed in quotes + try: + version = line.split()[2] # Split the line at the quotes and take the second element + return version + except IndexError: + # Handle unexpected format + print("Version format in the file does not match expected format.") + return "" + +def parse_xml_and_generate_header(package_xml_file, cpp_header_path): + # Parse the XML file + tree = ET.parse(package_xml_file) + root = tree.getroot() + + # Extract the version number from the tag in package.xml + package_version = root.find('version').text + package_version = f'\"{package_version}\"' + + # Extract the version number from "define SICK_SCAN_XD_VERSION" in cpp_header_path + header_version = extract_version_from_header(cpp_header_path) + print(f"Found version {package_version} in file \"{package_xml_file}\" and version {header_version} in file \"{cpp_header_path}\"") + + if package_version != header_version: + # Create a C++ header file content with the version defined and write to C++ header file + cpp_header_content = f"#ifndef SICK_SCAN_XD_VERSION\n#define SICK_SCAN_XD_VERSION {package_version}\n#endif // SICK_SCAN_XD_VERSION\n" + with open(cpp_header_path, 'w') as cpp_file: + cpp_file.write(f'/* This file has been generated by package_version_tool.py. Do not edit this file. To provide a new version number, modify file package.xml and rebuild */\n') + cpp_file.write(cpp_header_content) + print(f"Generated file \"{cpp_header_path}\" with version {package_version}") + else: + print(f"Found identical versions, leaving file \"{cpp_header_path}\" unmodified") + + +def main(): + parser = argparse.ArgumentParser(description="Parse XML to extract the version and generate a C++ header file.") + parser.add_argument('--package_xml_file', type=str, default='package.xml', help='Path to the package.xml file.') + parser.add_argument('--cpp_header_path', type=str, default='include/sick_scan/sick_scan_xd_version.h', help='Path to save the C++ header file.') + + args = parser.parse_args() + + parse_xml_and_generate_header(args.package_xml_file, args.cpp_header_path) + +if __name__ == '__main__': + main() diff --git a/test/docker/data/mrs1xxx_test01_cfg.json b/test/docker/data/mrs1xxx_test01_cfg.json index 304dad07..8ff940fb 100644 --- a/test/docker/data/mrs1xxx_test01_cfg.json +++ b/test/docker/data/mrs1xxx_test01_cfg.json @@ -27,7 +27,9 @@ "sick_scan_xd_imu_topics": [ "/sick_mrs_1xxx/imu" ], "info.args_sick_scan_xd_launch": "# Launch parameter for sick_scan_xd", - "args_sick_scan_xd_launch": [ "sick_scan_xd sick_mrs_1xxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False cloud_topic:=/cloud laserscan_topic:=/scan imu_topic:=/sick_mrs_1xxx/imu" ], + "args_sick_scan_xd_launch": [ + "sick_mrs_1xxx.launch hostname:=127.0.0.1 sw_pll_only_publish:=False cloud_topic:=/cloud laserscan_topic:=/scan imu_topic:=/sick_mrs_1xxx/imu" + ], "info.args_udp_scandata_sender": "# Launch parameter for udp scandata sender (multiScan or picoScan)", "args_udp_scandata_sender": [ ], @@ -38,6 +40,9 @@ "info.run_simu_seconds_before_shutdown": "# Delay before simulation shutdown: 0 for multiScan/picoScan (i.e. wait for udp scandata sender), simulation time in seconds otherwise (typically some seconds, e.g. 10)", "run_simu_seconds_before_shutdown": 10, + "info.args_rviz_linux_none": "# rviz visualization not supported without ROS", + "args_rviz_linux_none": [ ], + "info.args_rviz_linux_noetic": "# Optional config files for rviz visualization on ros1 noetic", "args_rviz_linux_noetic": [ "./src/sick_scan_xd/test/emulator/config/rviz_emulator_cfg_mrs1104_imu.rviz" ], diff --git a/test/docker/data/mrs1xxx_test01_tcp_data.json b/test/docker/data/mrs1xxx_test01_tcp_data.json index ed272728..19ff9581 100644 --- a/test/docker/data/mrs1xxx_test01_tcp_data.json +++ b/test/docker/data/mrs1xxx_test01_tcp_data.json @@ -575,6 +575,42 @@ } } }, + { + "_source": { + "layers": { + "tcp": { + "tcp.analysis": { + "tcp.analysis.push_bytes_sent": "41" + }, + "Timestamps": { + "tcp.time_relative": "2.437190294265748" + }, + "tcp.comment": "All entries created by pcap_json_converter, not by Wireshark.", + "tcp.producer": "pcap_json_converter.py --pcap_filename=20211201_MRS_1xxx_IMU_with_movement.pcapng", + "tcp.description": "....... sWN LMDscandatacfg .............E", + "tcp.payload": "02:02:02:02:00:00:00:20:73:57:4e:20:4c:4d:44:73:63:61:6e:64:61:74:61:63:66:67:20:07:00:00:00:00:00:00:00:00:00:01:00:01:45" + } + } + } + }, + { + "_source": { + "layers": { + "tcp": { + "tcp.analysis": { + "tcp.analysis.push_bytes_sent": "28" + }, + "Timestamps": { + "tcp.time_relative": "2.437190294265749" + }, + "tcp.comment": "All entries created by pcap_json_converter, not by Wireshark.", + "tcp.producer": "pcap_json_converter.py --pcap_filename=20211201_MRS_1xxx_IMU_with_movement.pcapng", + "tcp.description": "........sWA LMDscandatacfg M", + "tcp.payload": "02:02:02:02:00:00:00:13:73:57:41:20:4c:4d:44:73:63:61:6e:64:61:74:61:63:66:67:20:4d" + } + } + } + }, { "_source": { "layers": { diff --git a/test/docker/data/multiscan_compact_errortest01_cfg.json b/test/docker/data/multiscan_compact_errortest01_cfg.json index 75d2f6e3..e65ec970 100644 --- a/test/docker/data/multiscan_compact_errortest01_cfg.json +++ b/test/docker/data/multiscan_compact_errortest01_cfg.json @@ -40,7 +40,7 @@ "info.args_sick_scan_xd_launch": "# Launch parameter for sick_scan_xd", "args_sick_scan_xd_launch": [ - "sick_scan_xd sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1" + "sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1" ], "info.args_udp_scandata_sender": "# Launch parameter for udp scandata sender (multiScan or picoScan), here we simulate a lidar failure (error testcase), i.e. the lidar does not send any scan data", @@ -54,6 +54,9 @@ "info.run_simu_seconds_before_shutdown": "# Delay before simulation shutdown: 0 for multiScan/picoScan (i.e. wait for udp scandata sender), simulation time in seconds otherwise (typically some seconds, e.g. 10)", "run_simu_seconds_before_shutdown": 10, + "info.args_rviz_linux_none": "# rviz visualization not supported without ROS", + "args_rviz_linux_none": [ ], + "info.args_rviz_linux_noetic": "# Optional config files for rviz visualization on ros1 noetic", "args_rviz_linux_noetic": [ ], diff --git a/test/docker/data/multiscan_compact_test01_cfg.json b/test/docker/data/multiscan_compact_test01_cfg.json index a07f4a04..705e5eba 100644 --- a/test/docker/data/multiscan_compact_test01_cfg.json +++ b/test/docker/data/multiscan_compact_test01_cfg.json @@ -39,7 +39,7 @@ "info.args_sick_scan_xd_launch": "# Launch parameter for sick_scan_xd", "args_sick_scan_xd_launch": [ - "sick_scan_xd sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_laserscan_segment_topic:=/multiScan/scan_segment publish_laserscan_fullframe_topic:=/multiScan/scan_fullframe imu_topic:=/multiScan/imu" + "sick_multiscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 publish_laserscan_segment_topic:=/multiScan/scan_segment publish_laserscan_fullframe_topic:=/multiScan/scan_fullframe imu_topic:=/multiScan/imu" ], "info.args_udp_scandata_sender": "# Launch parameter for udp scandata sender (multiScan or picoScan)", @@ -55,6 +55,9 @@ "info.run_simu_seconds_before_shutdown": "# Delay before simulation shutdown: 0 for multiScan/picoScan (i.e. wait for udp scandata sender), simulation time in seconds otherwise (typically some seconds, e.g. 10)", "run_simu_seconds_before_shutdown": 0, + "info.args_rviz_linux_none": "# rviz visualization not supported without ROS", + "args_rviz_linux_none": [ ], + "info.args_rviz_linux_noetic": "# Optional config files for rviz visualization on ros1 noetic", "args_rviz_linux_noetic": [ "./src/sick_scan_xd/test/emulator/config/rviz_cfg_multiscan_emu.rviz", diff --git a/test/docker/data/picoscan_compact_errortest01_cfg.json b/test/docker/data/picoscan_compact_errortest01_cfg.json index a4ca43c3..a60bd163 100644 --- a/test/docker/data/picoscan_compact_errortest01_cfg.json +++ b/test/docker/data/picoscan_compact_errortest01_cfg.json @@ -38,7 +38,7 @@ "info.args_sick_scan_xd_launch": "# Launch parameter for sick_scan_xd", "args_sick_scan_xd_launch": [ - "sick_scan_xd sick_picoscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1" + "sick_picoscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1" ], "info.args_udp_scandata_sender": "# Launch parameter for udp scandata sender (multiScan or picoScan), here we simulate a network failure (error testcase), i.e. the lidar does not send scan data", @@ -50,6 +50,9 @@ "info.run_simu_seconds_before_shutdown": "# Delay before simulation shutdown: 0 for multiScan/picoScan (i.e. wait for udp scandata sender), simulation time in seconds otherwise (typically some seconds, e.g. 10)", "run_simu_seconds_before_shutdown": 10, + "info.args_rviz_linux_none": "# rviz visualization not supported without ROS", + "args_rviz_linux_none": [ ], + "info.args_rviz_linux_noetic": "# Optional config files for rviz visualization on ros1 noetic", "args_rviz_linux_noetic": [ ], diff --git a/test/docker/data/picoscan_compact_test01_cfg.json b/test/docker/data/picoscan_compact_test01_cfg.json index 0bc3db58..80d4a0fd 100644 --- a/test/docker/data/picoscan_compact_test01_cfg.json +++ b/test/docker/data/picoscan_compact_test01_cfg.json @@ -37,7 +37,7 @@ "info.args_sick_scan_xd_launch": "# Launch parameter for sick_scan_xd, optional host_FREchoFilter:=1 for all echos (reference scan contains 3 echos)", "args_sick_scan_xd_launch": [ - "sick_scan_xd sick_picoscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 host_FREchoFilter:=1 publish_laserscan_segment_topic:=/sick_picoscan/scan_segment publish_laserscan_fullframe_topic:=/sick_picoscan/scan_fullframe imu_topic:=/sick_picoscan/imu" + "sick_picoscan.launch hostname:=127.0.0.1 udp_receiver_ip:=127.0.0.1 host_FREchoFilter:=1 publish_laserscan_segment_topic:=/sick_picoscan/scan_segment publish_laserscan_fullframe_topic:=/sick_picoscan/scan_fullframe imu_topic:=/sick_picoscan/imu" ], "info.args_udp_scandata_sender": "# Launch parameter for udp scandata sender (multiScan or picoScan)", @@ -53,6 +53,9 @@ "info.run_simu_seconds_before_shutdown": "# Delay before simulation shutdown: 0 for multiScan/picoScan (i.e. wait for udp scandata sender), simulation time in seconds otherwise (typically some seconds, e.g. 10)", "run_simu_seconds_before_shutdown": 0, + "info.args_rviz_linux_none": "# rviz visualization not supported without ROS", + "args_rviz_linux_none": [ ], + "info.args_rviz_linux_noetic": "# Optional config files for rviz visualization on ros1 noetic", "args_rviz_linux_noetic": [ "./src/sick_scan_xd/test/emulator/config/rviz_cfg_picoscan_emu.rviz", @@ -75,4 +78,4 @@ "args_shutdown_server": [ "multiscan_sopas_test_server.py" ] -} +} \ No newline at end of file diff --git a/test/docker/dockerfile_linux_x64_sick_scan_xd b/test/docker/dockerfile_linux_x64_sick_scan_xd new file mode 100644 index 00000000..bf5045f7 --- /dev/null +++ b/test/docker/dockerfile_linux_x64_sick_scan_xd @@ -0,0 +1,42 @@ +# +# install linux without ROS +# +FROM ubuntu:22.04 +RUN apt-get update && apt-get -y install cmake git libjsoncpp-dev psmisc python3-pip +RUN python3 -m pip install argparse datetime numpy pathlib pypcapfile python-pcapng scapy + +# +# Default: copy local sick_scan_xd sources into the docker image +# +ENV NONE="None" +ARG SICK_SCAN_XD_BUILD_FROM_SOURCES=1 +RUN /bin/bash -c "if [ -d /workspace ] ; then rm -rf /workspace ; fi" +RUN /bin/bash -c "mkdir -p /workspace/src/sick_scan_xd" +COPY ./src/sick_scan_xd /workspace/src/sick_scan_xd + +# +# Optionally clone sick_scan_xd from a git url given by build argument SICK_SCAN_XD_GIT_URL +# Example: docker build --build-arg SICK_SCAN_XD_GIT_URL=https://github.com/SICKAG/sick_scan_xd ... +# +ARG SICK_SCAN_XD_GIT_URL="None" +RUN /bin/bash -c "if [ $SICK_SCAN_XD_GIT_URL != $NONE ] ; then ( pushd /workspace/src ; rm -rf ./sick_scan_xd ; git clone -b master $SICK_SCAN_XD_GIT_URL ; popd ) ; fi" + +# +# Optionally install prebuild sick_scan_xd binaries given by build argument SICK_SCAN_XD_APT_PKG +# Example: docker build --build-arg SICK_SCAN_XD_APT_PKG=ros-noetic-sick-scan-xd ... +# +ARG SICK_SCAN_XD_APT_PKG="None" +RUN /bin/bash -c "if [ $SICK_SCAN_XD_APT_PKG != $NONE ] ; then ( rm -rf /workspace/src ; apt-get install -y $SICK_SCAN_XD_APT_PKG ) ; fi" + +# +# build sick_scan_xd in docker container +# +WORKDIR /workspace +RUN /bin/bash -c "if [ $SICK_SCAN_XD_BUILD_FROM_SOURCES -ne 0 ] ; then (ls -al /workspace /workspace/src) ; fi" +RUN /bin/bash -c "if [ $SICK_SCAN_XD_BUILD_FROM_SOURCES -ne 0 ] ; then (export ROS_VERSION=0 ; mkdir -p /workspace/src/sick_scan_xd/build ; pushd /workspace/src/sick_scan_xd/build ; cmake -DROS_VERSION=0 -G \"Unix Makefiles\" .. ; make -j4 ; make -j4 install ; popd) ; fi" +RUN /bin/bash -c "if [ $SICK_SCAN_XD_BUILD_FROM_SOURCES -ne 0 ] ; then (echo -e \"\nsick_scan_xd cmake build finished:\" ; ls -al /workspace/src/sick_scan_xd/build) ; fi" + +# +# launch sick_scan_xd with multiScan emulator +# +CMD /bin/bash -c "cd /workspace ; python3 ./src/sick_scan_xd/test/docker/python/sick_scan_xd_simu.py --ros=none" diff --git a/test/docker/python/sick_scan_xd_simu.py b/test/docker/python/sick_scan_xd_simu.py index 5a2f3f02..2cf1036e 100644 --- a/test/docker/python/sick_scan_xd_simu.py +++ b/test/docker/python/sick_scan_xd_simu.py @@ -77,11 +77,17 @@ def simu_main(): print(f"Finished process {proc_scandata_sender}, exit_status = {status_scandata_sender}") if config.run_simu_seconds_before_shutdown > 0: time.sleep(config.run_simu_seconds_before_shutdown) + if config.ros_version == "none": # Shutdown and cleanup + proc_shutdown = start_process(config.os_name, config.cmd_init + config.cmd_shutdown, 0, 5) + proc_shutdown.wait() # Verification of received pointcloud and laserscan messages verify_success = False if len(config.save_messages_jsonfile) > 0: - sick_scan_xd_monitor.export_received_messages_to_jsonfile(f"{config.log_folder}/{config.save_messages_jsonfile}") + if config.ros_version == "none": + sick_scan_xd_monitor.import_received_messages_from_jsonfile(f"{config.log_folder}/{config.save_messages_jsonfile}") + else: + sick_scan_xd_monitor.export_received_messages_to_jsonfile(f"{config.log_folder}/{config.save_messages_jsonfile}") report.append_file_links(SickScanXdMsgStatus.INFO, "sick_scan_xd_simu: received messages exported to file ", [config.save_messages_jsonfile]) if len(config.reference_messages_jsonfile) > 0: report.append_file_links(SickScanXdMsgStatus.INFO, "sick_scan_xd_simu: references messages from file ", [config.reference_messages_jsonfile]) @@ -111,8 +117,9 @@ def simu_main(): print("\n## sick_scan_xd_simu finished with ERROR, TEST FAILED\n") # Shutdown and cleanup - proc_shutdown = start_process(config.os_name, config.cmd_init + config.cmd_shutdown, 0, 5) - proc_shutdown.wait() + if config.ros_version != "none": # Shutdown and cleanup + proc_shutdown = start_process(config.os_name, config.cmd_init + config.cmd_shutdown, 0, 5) + proc_shutdown.wait() # Print final report print(f"\nsick_scan_xd_simu finished with exit status {int(report.get_exit_status())}\n") diff --git a/test/docker/python/sick_scan_xd_simu_cfg.py b/test/docker/python/sick_scan_xd_simu_cfg.py index cb7a8079..9d4bb341 100644 --- a/test/docker/python/sick_scan_xd_simu_cfg.py +++ b/test/docker/python/sick_scan_xd_simu_cfg.py @@ -86,31 +86,33 @@ def __init__(self, simu_start_time): self.cmd_sick_scan_xd = [] self.cmd_udp_scandata_sender = [] self.cmd_shutdown = [] + if self.ros_version == "none": - pass # TODO ... + if self.os_name == "linux": + for arg_sick_scan_xd_launch in args_sick_scan_xd_launch: + self.cmd_sick_scan_xd.append(f"./src/sick_scan_xd/build/sick_scan_xd_api_dockertest ./src/sick_scan_xd/launch/{arg_sick_scan_xd_launch} _jsonfile:={self.log_folder}/{self.save_messages_jsonfile}") + if len(self.cmd_sick_scan_xd) < 1: + self.error_messages.append(f"## ERROR SickScanXdSimuConfig: sick_scan_xd launch parameter not configured, check configuration file \"{self.config_file}\"") + + self.cmd_shutdown.append("pkill -f sick_scan_xd_api_dockertest") + for arg_shutdown_server in args_shutdown_server: + self.cmd_shutdown.append(f"pkill -f {arg_shutdown_server}") + elif self.ros_version == "noetic": if self.os_name == "linux": self.cmd_init.append("source /opt/ros/noetic/setup.bash") self.cmd_init.append("source ./devel_isolated/setup.bash") - for arg_sopas_server in args_sopas_server: - self.cmd_sopas_server.append(f"python3 {arg_sopas_server}") - if len(self.cmd_sopas_server) < 1: - print(f"SickScanXdSimuConfig: sopas server parameter not configured, sopas server not started. This is a valid option for error testcases, but check configuration file \"{self.config_file}\" if this is not intended.") - for arg_rviz in args_rviz: self.cmd_rviz.append(f"(rosrun rviz rviz -d {arg_rviz} &)") self.cmd_rviz.append("sleep 1") for arg_sick_scan_xd_launch in args_sick_scan_xd_launch: - self.cmd_sick_scan_xd.append(f"roslaunch {arg_sick_scan_xd_launch}") + self.cmd_sick_scan_xd.append(f"roslaunch sick_scan_xd {arg_sick_scan_xd_launch}") if len(self.cmd_sick_scan_xd) < 1: self.error_messages.append(f"## ERROR SickScanXdSimuConfig: sick_scan_xd launch parameter not configured, check configuration file \"{self.config_file}\"") - for arg_udp_scandata_sender in args_udp_scandata_sender: - self.cmd_udp_scandata_sender.append(f"python3 {arg_udp_scandata_sender}") - for arg_shutdown_nodes in args_shutdown_nodes: self.cmd_shutdown.append(f"rosnode kill {arg_shutdown_nodes}") self.cmd_shutdown.append("sleep 3") @@ -125,38 +127,22 @@ def __init__(self, simu_start_time): for sick_scan_xd_imu_topic in sick_scan_xd_imu_topics: self.sick_scan_xd_imu_topics.append(sick_scan_xd_imu_topic) - # Copy config and reference files to logfolder - if len(reference_messages_jsonfile) > 0 and os.path.exists(reference_messages_jsonfile): - shutil.copy(reference_messages_jsonfile, self.log_folder) - self.reference_messages_jsonfile = os.path.basename(reference_messages_jsonfile) - if len(self.config_file) > 0 and os.path.exists(self.config_file): - shutil.copy(self.config_file, self.log_folder) - self.config_file = os.path.basename(self.config_file) - elif self.ros_version == "foxy" or self.ros_version == "humble": if self.os_name == "linux": self.cmd_init.append("source /opt/ros/{}/setup.bash".format(self.ros_version)) self.cmd_init.append("source ./install/setup.bash") - for arg_sopas_server in args_sopas_server: - self.cmd_sopas_server.append(f"python3 {arg_sopas_server}") - if len(self.cmd_sopas_server) < 1: - print(f"SickScanXdSimuConfig: sopas server parameter not configured, sopas server not started. This is a valid option for error testcases, but check configuration file \"{self.config_file}\" if this is not intended.") - for arg_rviz in args_rviz: self.cmd_rviz.append(f"(ros2 run rviz2 rviz2 -d {arg_rviz} &)") self.cmd_rviz.append("sleep 1") for arg_sick_scan_xd_launch in args_sick_scan_xd_launch: arg_sick_scan_xd_launch = arg_sick_scan_xd_launch.replace(".launch", ".launch.py") - self.cmd_sick_scan_xd.append(f"ros2 launch {arg_sick_scan_xd_launch}") + self.cmd_sick_scan_xd.append(f"ros2 launch sick_scan_xd {arg_sick_scan_xd_launch}") if len(self.cmd_sick_scan_xd) < 1: self.error_messages.append(f"## ERROR SickScanXdSimuConfig: sick_scan_xd launch parameter not configured, check configuration file \"{self.config_file}\"") - for arg_udp_scandata_sender in args_udp_scandata_sender: - self.cmd_udp_scandata_sender.append(f"python3 {arg_udp_scandata_sender}") - self.cmd_shutdown.append(f"pkill -f sick_generic_caller") self.cmd_shutdown.append("sleep 3") for arg_shutdown_server in args_shutdown_server: @@ -169,14 +155,24 @@ def __init__(self, simu_start_time): for sick_scan_xd_imu_topic in sick_scan_xd_imu_topics: self.sick_scan_xd_imu_topics.append(sick_scan_xd_imu_topic) - # Copy config and reference files to logfolder - if len(reference_messages_jsonfile) > 0 and os.path.exists(reference_messages_jsonfile): - shutil.copy(reference_messages_jsonfile, self.log_folder) - self.reference_messages_jsonfile = os.path.basename(reference_messages_jsonfile) - if len(self.config_file) > 0 and os.path.exists(self.config_file): - shutil.copy(self.config_file, self.log_folder) - self.config_file = os.path.basename(self.config_file) - elif self.os_name == "windows": self.cmd_sick_scan_xd.append("echo %COMPUTERNAME% && dir /b/on") # TODO ... - \ No newline at end of file + + # Sopas server configuration for all dockertest (Linux, Windows, ROS1, ROS1, API) + for arg_sopas_server in args_sopas_server: + self.cmd_sopas_server.append(f"python3 {arg_sopas_server}") + if len(self.cmd_sopas_server) < 1: + print(f"SickScanXdSimuConfig: sopas server parameter not configured, sopas server not started. This is a valid option for error testcases, but check configuration file \"{self.config_file}\" if this is not intended.") + + # Udp sender configuration for all dockertest (Linux, Windows, ROS1, ROS1, API) + for arg_udp_scandata_sender in args_udp_scandata_sender: + self.cmd_udp_scandata_sender.append(f"python3 {arg_udp_scandata_sender}") + + # Copy config and reference files to logfolder for all dockertest (Linux, Windows, ROS1, ROS1, API) + if len(reference_messages_jsonfile) > 0 and os.path.exists(reference_messages_jsonfile): + shutil.copy(reference_messages_jsonfile, self.log_folder) + self.reference_messages_jsonfile = os.path.basename(reference_messages_jsonfile) + if len(self.config_file) > 0 and os.path.exists(self.config_file): + shutil.copy(self.config_file, self.log_folder) + self.config_file = os.path.basename(self.config_file) + diff --git a/test/docker/python/sick_scan_xd_subscriber.py b/test/docker/python/sick_scan_xd_subscriber.py index 36978a42..1ee2c931 100644 --- a/test/docker/python/sick_scan_xd_subscriber.py +++ b/test/docker/python/sick_scan_xd_subscriber.py @@ -38,6 +38,8 @@ def ros_init(os_name = "linux", ros_version = "noetic", node_name = "sick_scan_x rospy.init_node(node_name) elif ros2_found and (ros_version == "humble" or ros_version == "foxy"): rclpy.init() + elif ros_version == "none": + pass else: print(f"## ERROR sick_scan_xd_subscriber.ros_init(): ros version {ros_version} not found or not supported") @@ -313,12 +315,14 @@ def export_dictionary(self, dict): class SickScanXdMonitor(): def __init__(self, config, run_ros_init): + self.ros_version = config.ros_version self.ros_node = None self.ros_spin_executor = None self.ros_spin_thread = None self.laserscan_subscriber = [] self.pointcloud_subscriber = [] self.imu_subscriber = [] + self.messages_received = {} self.reference_messages_jsonfile = f"{config.log_folder}/{config.reference_messages_jsonfile}" if run_ros_init: ros_init(os_name = config.os_name, ros_version = config.ros_version, node_name = "sick_scan_xd_simu") @@ -341,21 +345,20 @@ def __init__(self, config, run_ros_init): self.ros_spin_thread.start() def export_received_messages(self): - messages = {} - messages["RefLaserscanMsg"] = {} - messages["RefPointcloudMsg"] = {} - messages["RefImuMsg"] = {} + self.messages_received["RefLaserscanMsg"] = {} + self.messages_received["RefPointcloudMsg"] = {} + self.messages_received["RefImuMsg"] = {} num_messages = 0 for subscriber in self.laserscan_subscriber: - subscriber.export_dictionary(messages["RefLaserscanMsg"]) + subscriber.export_dictionary(self.messages_received["RefLaserscanMsg"]) num_messages += len(subscriber.messages_received) for subscriber in self.pointcloud_subscriber: - subscriber.export_dictionary(messages["RefPointcloudMsg"]) + subscriber.export_dictionary(self.messages_received["RefPointcloudMsg"]) num_messages += len(subscriber.messages_received) for subscriber in self.imu_subscriber: - subscriber.export_dictionary(messages["RefImuMsg"]) + subscriber.export_dictionary(self.messages_received["RefImuMsg"]) num_messages += len(subscriber.messages_received) - return num_messages, messages + return num_messages, self.messages_received def export_received_messages_to_jsonfile(self, jsonfile): num_messages, messages = self.export_received_messages() @@ -369,16 +372,32 @@ def export_received_messages_to_jsonfile(self, jsonfile): else: print(f"## ERROR SickScanXdMonitor.export_received_messages(): no messages received, file \"{jsonfile}\" not written") + def import_received_messages_from_jsonfile(self, jsonfile): + # with open(jsonfile, "r") as file_stream: + # print(file_stream.read()) + try: + with open(jsonfile, "r") as file_stream: + self.messages_received = json.load(file_stream) + print(f"SickScanXdMonitor: messages imported from file \"{jsonfile}\"") + except Exception as exc: + print(f"## ERROR in SickScanXdMonitor.import_received_messages_from_jsonfile(\"{jsonfile}\"): exception {exc}") + def verify_messages(self, report): try: - _, received_messages = self.export_received_messages() + received_messages = self.messages_received reference_messages = {} with open(self.reference_messages_jsonfile, "r") as file_stream: reference_messages = json.load(file_stream) converter = { "RefLaserscanMsg": RefLaserscanMsg(), "RefPointcloudMsg": RefPointcloudMsg(), "RefImuMsg": RefImuMsg() } num_messages_verified = 0 for msg_type in reference_messages.keys(): + if self.ros_version == "none" and msg_type == "RefLaserscanMsg": + continue # laserscan messages are not exported via API, i.e. verify laserscan messages only under ROS for topic in reference_messages[msg_type].keys(): + if self.ros_version == "none" and topic == "/cloud_all_fields_fullframe": + continue # only polar and cartesian pointcloud messages are exported via API # TODO: exclude topics from config + #if self.ros_version == "none" and msg_type == "RefImuMsg": + # continue # TODO !!! for ref_msg in reference_messages[msg_type][topic]: if topic not in received_messages[msg_type] or not self.find_message(ref_msg, received_messages[msg_type][topic], converter[msg_type]): ref_msg_frame_id = ref_msg["frame_id"] diff --git a/test/docker/run_dockerfile_linux_x64_sick_scan_xd.bash b/test/docker/run_dockerfile_linux_x64_sick_scan_xd.bash new file mode 100644 index 00000000..ec27d72a --- /dev/null +++ b/test/docker/run_dockerfile_linux_x64_sick_scan_xd.bash @@ -0,0 +1,85 @@ +#!/bin/bash + + +function prompt() +{ + read -n 1 -p "Press any key to continue..." +} + +# +# Build sick_scan_xd for Linux x64 in Docker +# +printf "\033c" +pushd ../../../.. + +# Default: Create a docker-image named "sick_scan_xd/linux_x64" from dockerfile "./sick_scan_xd/test/docker/dockerfile_linux_x64_sick_scan_xd" with local sources in folder "./src/sick_scan_xd" +docker build --progress=plain -t sick_scan_xd/linux_x64 -f ./src/sick_scan_xd/test/docker/dockerfile_linux_x64_sick_scan_xd . +# Alternative: Create a docker-image named "sick_scan_xd/linux_x64" from dockerfile "./sick_scan_xd/test/docker/dockerfile_linux_x64_sick_scan_xd" with github sources from https://github.com/SICKAG/sick_scan_xd +# docker build --build-arg SICK_SCAN_XD_GIT_URL=https://github.com/SICKAG/sick_scan_xd --progress=plain -t sick_scan_xd/linux_x64 -f ./src/sick_scan_xd/test/docker/dockerfile_linux_x64_sick_scan_xd . +# Alternative: Create a docker-image named "sick_scan_xd/linux_x64" from dockerfile "./sick_scan_xd/test/docker/dockerfile_linux_x64_sick_scan_xd" with "apt-get install ros-humble-sick-scan-xd" +# docker build --build-arg SICK_SCAN_XD_APT_PKG=ros-humble-sick-scan-xd --build-arg SICK_SCAN_XD_BUILD_FROM_SOURCES=0 --progress=plain -t sick_scan_xd/linux_x64 -f ./src/sick_scan_xd/test/docker/dockerfile_linux_x64_sick_scan_xd . +docker image ls sick_scan_xd/linux_x64 +# docker rm $(docker ps -a -q) # optional cleanup: remove all docker container + +# Run the docker-image named "sick_scan_xd/linux_x64" and execute a multiscan test within the docker container +# docker run option --rm automatically removes the container when it exits +docker_status_final=0 # success +# for cfg in multiscan_compact_test01_cfg picoscan_compact_test01_cfg mrs1xxx_test01_cfg ; do +# for cfg in multiscan_compact_test01_cfg ; do +for cfg in picoscan_compact_test01_cfg ; do +# for cfg in mrs1xxx_test01_cfg ; do + echo -e "Running sick_scan_xd docker test with configuration file $cfg" + container_name=sick_scan_xd_$cfg + container_id=$(docker ps -aqf "name=$container_name") + docker rm $container_id # remove previous docker container + docker run -it -v /tmp/.X11-unix:/tmp/.X11-unix --env=DISPLAY -w /workspace --name $container_name sick_scan_xd/linux_x64 python3 ./src/sick_scan_xd/test/docker/python/sick_scan_xd_simu.py --ros=none --cfg=./src/sick_scan_xd/test/docker/data/$cfg.json + docker_exit_status=$? + echo -e "docker_exit_status for $cfg: $docker_exit_status" + if [ $docker_exit_status -eq 0 ] ; then + echo -e "\nSUCCESS: sick_scan_xd docker test passed for configuration $cfg\n" + else + echo -e "\n## ERROR: sick_scan_xd docker test FAILED for configuration $cfg\n" + docker_status_final=$docker_exit_status + # prompt + fi + # docker ps -a # list all container +done + +prompt +exit + +# Export sick_scan_xd output folder from docker container to local folder ./log/sick_scan_xd_simu +docker ps -a # list all container +if [ -d ./log/sick_scan_xd_simu ] ; then rm -rf ./log/sick_scan_xd_simu ; fi +mkdir -p ./log +for cfg in multiscan_compact_test01_cfg picoscan_compact_test01_cfg mrs1xxx_test01_cfg ; do + container_name=sick_scan_xd_$cfg + container_id=$(docker ps -aqf "name=$container_name") + docker cp $container_id:/workspace/log/sick_scan_xd_simu ./log +done +pushd ./log/sick_scan_xd_simu +echo -e "# sick_scan_xd test report summary\n" > sick_scan_xd_testreport.md +if [ $docker_status_final -eq 0 ] ; then + echo -e "**SUCCESS: all sick_scan_xd docker tests passed**\n" >> sick_scan_xd_testreport.md +else + echo -e "**ERROR: sick_scan_xd docker tests with status FAILED**\n" >> sick_scan_xd_testreport.md +fi +for summary in ./*/sick_scan_xd_summary.md ; do (cat $summary >> sick_scan_xd_testreport.md) ; done +for mdfile in ./*.md ; do pandoc -f markdown -t html -s $mdfile -o $mdfile.html ; done +for mdfile in ./*/*.md ; do pandoc -f markdown -t html -s $mdfile -o $mdfile.html ; done +popd +echo -e "\nlogfiles exported from docker container to local folder ./log/sick_scan_xd_simu:" +ls -al ./log/sick_scan_xd_simu/* +echo -e "\n" +cat ./log/sick_scan_xd_simu/sick_scan_xd_testreport.md +firefox ./log/sick_scan_xd_simu/sick_scan_xd_testreport.md.html & + +popd +if [ $docker_status_final -eq 0 ] ; then + echo -e "\nSUCCESS: all sick_scan_xd docker tests passed\n" +else + echo -e "\n## ERROR: sick_scan_xd docker tests with status FAILED\n" + prompt +fi +exit $docker_status_final + diff --git a/test/src/sick_scan_xd_api/sick_scan_xd_api_dockertest.cpp b/test/src/sick_scan_xd_api/sick_scan_xd_api_dockertest.cpp new file mode 100644 index 00000000..5bebc42f --- /dev/null +++ b/test/src/sick_scan_xd_api/sick_scan_xd_api_dockertest.cpp @@ -0,0 +1,233 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef _MSC_VER +#include +#include +#else +// #include +#include +#endif + +#include "sick_scan_api.h" +#include "sick_scan_api_dump.h" +#include "sick_scan_api_converter.h" + +#include +#include +#include +#include + +static Json::Value s_json_pointcloud_messages; +static Json::Value s_json_imu_messages; +static std::string s_json_export_file; +static bool s_shutdown_signal_received = false; + +void exportJsonMessages() +{ + if (!s_json_export_file.empty()) + { + Json::Value json_root; + if (s_json_pointcloud_messages.size() > 0) + json_root["RefPointcloudMsg"] = s_json_pointcloud_messages; + if (s_json_imu_messages.size() > 0) + json_root["RefImuMsg"] = s_json_imu_messages; + std::ofstream fs_json(s_json_export_file); + fs_json << json_root; + std::cout << "sick_scan_xd_api_dockertest: exported messages to json file \"" << s_json_export_file << "\"" << std::endl; + } +} + +// signal handler to close docker test after SIGINT and SIGKILL +void signalHandler(int signalRecv) +{ + printf("sick_scan_xd_api_dockertest: caught signal %d, aborting ...\n", signalRecv); + s_shutdown_signal_received = true; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + exportJsonMessages(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + printf("sick_scan_xd_api_dockertest: exit (line %d)\n", __LINE__); +} + +// Exit with error message +static void exitOnError(const char* msg, int32_t error_code) +{ + printf("## ERROR sick_scan_xd_api_dockertest: %s, error code %d\n", msg, error_code); + exit(EXIT_FAILURE); +} + +// Convert a pointcloud message to json +static void apiMessageToJson(const SickScanPointCloudMsg* msg, Json::Value& json_msg) +{ + json_msg["frame_id"] = msg->header.frame_id; + json_msg["width"] = msg->width; + json_msg["height"] = msg->height; + json_msg["point_step"] = msg->point_step; + json_msg["row_step"] = msg->row_step; + Json::Value json_fields; + for(int field_cnt = 0; field_cnt < msg->fields.size; field_cnt++) + { + Json::Value json_field; + json_field["name"] = msg->fields.buffer[field_cnt].name; + json_field["offset"] = msg->fields.buffer[field_cnt].offset; + json_field["datatype"] = msg->fields.buffer[field_cnt].datatype; + json_field["count"] = msg->fields.buffer[field_cnt].count; + json_fields.append(json_field); + } + json_msg["fields"] = json_fields; + std::stringstream hex_data_str; + for(int data_cnt = 0; data_cnt < msg->data.size; data_cnt++) + { + hex_data_str << std::setfill('0') << std::setw(2) << std::hex << (int)(msg->data.buffer[data_cnt]); + } + json_msg["data"] = hex_data_str.str(); +} + +// Convert an imu message to json +static void apiMessageToJson(const SickScanImuMsg* msg, Json::Value& json_msg) +{ + json_msg["frame_id"] = "imu_link"; // msg->header.frame_id; // "world" // frame_id: TODO !!! + json_msg["orientation"].resize(4); + json_msg["angular_velocity"].resize(3); + json_msg["linear_acceleration"].resize(3); + json_msg["orientation_covariance"].resize(9); + json_msg["angular_velocity_covariance"].resize(9); + json_msg["linear_acceleration_covariance"].resize(9); + json_msg["orientation"][0] = msg->orientation.x; + json_msg["orientation"][1] = msg->orientation.y; + json_msg["orientation"][2] = msg->orientation.z; + json_msg["orientation"][3] = msg->orientation.w; + json_msg["angular_velocity"][0] = msg->angular_velocity.x; + json_msg["angular_velocity"][1] = msg->angular_velocity.y; + json_msg["angular_velocity"][2] = msg->angular_velocity.z; + json_msg["linear_acceleration"][0] = msg->linear_acceleration.x; + json_msg["linear_acceleration"][1] = msg->linear_acceleration.y; + json_msg["linear_acceleration"][2] = msg->linear_acceleration.z; + for(int n = 0; n < 9; n++) + { + json_msg["orientation_covariance"][n] = msg->orientation_covariance[n]; + json_msg["angular_velocity_covariance"][n] = msg->angular_velocity_covariance[n]; + json_msg["linear_acceleration_covariance"][n] = msg->linear_acceleration_covariance[n]; + } +} + +// Example callback for cartesian pointcloud messages, converts and publishes a SickScanPointCloudMsg to sensor_msgs::PointCloud2 on ROS-1 +static void apiTestCartesianPointCloudMsgCallback(SickScanApiHandle apiHandle, const SickScanPointCloudMsg* msg) +{ + printf("[Info]: apiTestCartesianPointCloudMsgCallback(apiHandle:%p): %dx%d pointcloud callback...\n", apiHandle, msg->width, msg->height); + Json::Value json_msg; + apiMessageToJson(msg, json_msg); + s_json_pointcloud_messages[msg->topic].append(json_msg); +} + +// Example callback for polar pointcloud messages, converts and publishes a SickScanPointCloudMsg to sensor_msgs::PointCloud2 on ROS-1 +static void apiTestPolarPointCloudMsgCallback(SickScanApiHandle apiHandle, const SickScanPointCloudMsg* msg) +{ + printf("[Info]: apiTestPolarPointCloudMsgCallback(apiHandle:%p): %dx%d pointcloud callback...\n", apiHandle, msg->width, msg->height); + Json::Value json_msg; + apiMessageToJson(msg, json_msg); + s_json_pointcloud_messages[msg->topic].append(json_msg); +} + +// Example callback for imu messages +static void apiTestImuMsgCallback(SickScanApiHandle apiHandle, const SickScanImuMsg* msg) +{ + printf("[Info]: apiTestImuMsgCallback(apiHandle:%p): Imu message, orientation=(%.6f,%.6f,%.6f,%.6f), angular_velocity=(%.6f,%.6f,%.6f), linear_acceleration=(%.6f,%.6f,%.6f)\n", + apiHandle, msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w, + msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.y, + msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z); + Json::Value json_msg; + apiMessageToJson(msg, json_msg); + //s_json_imu_messages["/multiScan/imu"].append(json_msg); // topic: TODO !!! + s_json_imu_messages["/sick_mrs_1xxx/imu"].append(json_msg); // topic: TODO !!! +} + +// sick_scan_api_test main: Initialize, receive and process lidar messages via sick_scan_xd API. +void sick_scan_api_test_main(int argc, char** argv) +{ + int32_t ret = SICK_SCAN_API_SUCCESS; + SickScanApiHandle apiHandle = 0; + + if ((apiHandle = SickScanApiCreate(argc, argv)) == 0) + exitOnError("SickScanApiCreate failed", -1); + + // Initialize a lidar and starts message receiving and processing + if ((ret = SickScanApiInitByCli(apiHandle, argc, argv)) != SICK_SCAN_API_SUCCESS) + exitOnError("SickScanApiInitByCli failed", ret); + + // Register a callback for PointCloud messages + if ((ret = SickScanApiRegisterCartesianPointCloudMsg(apiHandle, apiTestCartesianPointCloudMsgCallback)) != SICK_SCAN_API_SUCCESS) + exitOnError("SickScanApiRegisterCartesianPointCloudMsg failed", ret); + if ((ret = SickScanApiRegisterPolarPointCloudMsg(apiHandle, apiTestPolarPointCloudMsgCallback)) != SICK_SCAN_API_SUCCESS) + exitOnError("SickScanApiRegisterCartesianPointCloudMsg failed", ret); + + // Register a callback for Imu messages + if ((ret = SickScanApiRegisterImuMsg(apiHandle, apiTestImuMsgCallback)) != SICK_SCAN_API_SUCCESS) + exitOnError("SickScanApiRegisterImuMsg failed", ret); + + // Run main loop + while (!s_shutdown_signal_received) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + // Cleanup and exit + printf("sick_scan_xd_api_dockertest finishing...\n"); + SickScanApiDeregisterCartesianPointCloudMsg(apiHandle, apiTestCartesianPointCloudMsgCallback); + SickScanApiDeregisterPolarPointCloudMsg(apiHandle, apiTestPolarPointCloudMsgCallback); + SickScanApiDeregisterImuMsg(apiHandle, apiTestImuMsgCallback); + if ((ret = SickScanApiClose(apiHandle)) != SICK_SCAN_API_SUCCESS) + exitOnError("SickScanApiClose failed", ret); + if ((ret = SickScanApiRelease(apiHandle)) != SICK_SCAN_API_SUCCESS) + exitOnError("SickScanApiRelease failed", ret); +} + +// sick_scan_api_test main: Initialize, receive and process lidar messages via sick_scan_xd API. +int main(int argc, char** argv) +{ + int32_t ret = SICK_SCAN_API_SUCCESS; + for (int n = 0; n < argc; n++) + { + printf("%s%s", (n > 0 ? " " : ""), argv[n]); + if (strncmp(argv[n], "_jsonfile:=", 11) == 0) + s_json_export_file = argv[n] + 11; + } + signal(SIGINT, signalHandler); // SIGINT = 2, Ctrl-C or kill -2 + signal(SIGTERM, signalHandler); // SIGTERM = 15, default kill level + printf("\nsick_scan_xd_api_dockertest started\n"); + +#ifdef _MSC_VER + std::string sick_scan_api_lib = "sick_scan_xd_shared_lib.dll"; + std::vector search_library_path = { "", "build/Debug/", "build_win64/Debug/", "src/build/Debug/", "src/build_win64/Debug/", "src/sick_scan_xd/build/Debug/", "src/sick_scan_xd/build_win64/Debug/", "./", "../" }; +#else + std::string sick_scan_api_lib = "libsick_scan_xd_shared_lib.so"; + std::vector search_library_path = { "", "build/", "build_linux/", "src/build/", "src/build_linux/", "src/sick_scan_xd/build/", "src/sick_scan_xd/build_linux/", "./", "../" }; +#endif + ret = SICK_SCAN_API_NOT_LOADED; + for(int search_library_cnt = 0; search_library_cnt < search_library_path.size(); search_library_cnt++) + { + std::string libfilepath = search_library_path[search_library_cnt] + sick_scan_api_lib; + if ((ret = SickScanApiLoadLibrary(libfilepath.c_str())) == SICK_SCAN_API_SUCCESS) + { + printf("sick_scan_xd library \"%s\" loaded successfully\n", libfilepath.c_str()); + break; + } + } + if (ret != SICK_SCAN_API_SUCCESS) + exitOnError("SickScanApiLoadLibrary failed", ret); + + // Initialize and run sick_scan_xd_api_dockertest + sick_scan_api_test_main(argc, argv); + + // Unload and exit + printf("sick_scan_xd_api_dockertest finishing...\n"); + if ((ret = SickScanApiUnloadLibrary()) != SICK_SCAN_API_SUCCESS) + exitOnError("SickScanApiUnloadLibrary failed", ret); + printf("sick_scan_xd_api_dockertest finished successfully\n"); + exit(EXIT_SUCCESS); +} diff --git a/test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp b/test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp index f7a2d3f9..59bf8e6c 100644 --- a/test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp +++ b/test/src/sick_scan_xd_api/sick_scan_xd_api_test.cpp @@ -1,7 +1,9 @@ #include #include #include +#include #include +#include #ifdef _MSC_VER #include #else @@ -462,7 +464,8 @@ int sick_scan_api_test_main(int argc, char** argv, const std::string& sick_scan_ if (polling) { run_polling = false; - run_polling_thread->join(); + if (run_polling_thread->joinable()) + run_polling_thread->join(); delete run_polling_thread; run_polling_thread = 0; } @@ -517,11 +520,24 @@ int main(int argc, char** argv) printf("\nsick_scan_xd_api_test started\n"); #ifdef _MSC_VER - const char* sick_scan_api_lib = "sick_scan_xd_shared_lib.dll"; + std::string sick_scan_api_lib = "sick_scan_xd_shared_lib.dll"; + std::vector search_library_path = { "", "build/Debug/", "build_win64/Debug/", "src/build/Debug/", "src/build_win64/Debug/", "src/sick_scan_xd/build/Debug/", "src/sick_scan_xd/build_win64/Debug/", "./", "../" }; #else - const char* sick_scan_api_lib = "libsick_scan_xd_shared_lib.so"; + std::string sick_scan_api_lib = "libsick_scan_xd_shared_lib.so"; + std::vector search_library_path = { "", "build/", "build_linux/", "src/build/", "src/build_linux/", "src/sick_scan_xd/build/", "src/sick_scan_xd/build_linux/", "./", "../" }; #endif - if ((ret = SickScanApiLoadLibrary(sick_scan_api_lib)) != SICK_SCAN_API_SUCCESS) + ret = SICK_SCAN_API_NOT_LOADED; + for(int search_library_cnt = 0; search_library_cnt < search_library_path.size(); search_library_cnt++) + { + std::string libfilepath = search_library_path[search_library_cnt] + sick_scan_api_lib; + if ((ret = SickScanApiLoadLibrary(libfilepath.c_str())) == SICK_SCAN_API_SUCCESS) + { + printf("sick_scan_xd library \"%s\" loaded successfully\n", libfilepath.c_str()); + sick_scan_api_lib = libfilepath; + break; + } + } + if (ret != SICK_SCAN_API_SUCCESS) exitOnError("SickScanApiLoadLibrary failed", ret); // (Re-)Initialize and run sick_scan_xd_api_test @@ -538,5 +554,25 @@ int main(int argc, char** argv) if ((ret = SickScanApiUnloadLibrary()) != SICK_SCAN_API_SUCCESS) exitOnError("SickScanApiUnloadLibrary failed", ret); printf("sick_scan_xd_api_test finished successfully\n"); + + if (false) // Optional test: reload, initialize and run sick_scan_xd_api_test + { + printf("\nsick_scan_xd_api_test: reload %s\n", sick_scan_api_lib.c_str()); + if ((ret = SickScanApiLoadLibrary(sick_scan_api_lib.c_str())) != SICK_SCAN_API_SUCCESS) + exitOnError("SickScanApiLoadLibrary failed", ret); + printf("\nsick_scan_xd_api_test: restart\n"); + int user_key = 0; + do + { + user_key = sick_scan_api_test_main(argc, argv, sick_scan_args, polling); + std::this_thread::sleep_for(std::chrono::seconds(1)); + printf("sick_scan_xd_api_test finished with user key '%c' (%d), re-initialize and repeat sick_scan_xd_api_test ...\n", (char)user_key, user_key); + } while (user_key == 'R' || user_key == 'r'); + printf("sick_scan_xd_api_test finishing...\n"); + if ((ret = SickScanApiUnloadLibrary()) != SICK_SCAN_API_SUCCESS) + exitOnError("SickScanApiUnloadLibrary failed", ret); + printf("sick_scan_xd_api_test finished successfully\n"); + } + exit(EXIT_SUCCESS); } diff --git a/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c b/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c index 3004dfa7..e9795f82 100644 --- a/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c +++ b/test/src/sick_scan_xd_api/sick_scan_xd_api_wrapper.c @@ -224,7 +224,7 @@ int32_t SickScanApiLoadLibrary(const char* library_filepath) } if (hinstLib == NULL) { - printf("## ERROR SickScanApiLoadLibrary: LoadLibrary(%s) failed\n", library_filepath); + printf("## WARNING SickScanApiLoadLibrary: LoadLibrary(\"%s\") failed\n", library_filepath); ret = SICK_SCAN_API_NOT_LOADED; } return ret; diff --git a/tools/test_server/src/test_server_thread.cpp b/tools/test_server/src/test_server_thread.cpp index 62bb5a6e..6e05ca73 100644 --- a/tools/test_server/src/test_server_thread.cpp +++ b/tools/test_server/src/test_server_thread.cpp @@ -111,7 +111,8 @@ bool sick_scan_xd::test::TestServerThread::stop(void) m_run_server_thread = false; if(m_server_thread != 0) { - m_server_thread->join(); + if (m_server_thread->joinable()) + m_server_thread->join(); delete m_server_thread; m_server_thread = 0; }