Fix blocking API-close in case of wrong ip-address, #316

.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/

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)

driver/src/sick_generic_laser.cpp

@@ -66,6 +66,8 @@
 #endif
 
 #include <sick_scan/sick_ros_wrapper.h>
+#include <sick_scan/sick_scan_xd_version.h>
+#include "softwarePLL.h"
 #if defined LDMRS_SUPPORT && LDMRS_SUPPORT > 0
 #include <sick_scan/ldmrs/sick_ldmrs_node.h>
 #endif
@@ -92,15 +94,23 @@
 #include <stdlib.h>
 #include <signal.h>
 
-#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

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;
   }

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;

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;
     }

driver/src/sick_scansegment_xd/msgpack_exporter.cpp

@@ -127,33 +127,33 @@ std::list<sick_scansegment_xd::MsgPackExportListenerIF*> 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.
  */

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