Merge pull request #29 from SICKAG/feature/lidar_concurrent_event_loops

Error after SOPAS command SetAccessMode #27

CHANGELOG.md

@@ -8,6 +8,9 @@ features that will be removed in future versions **Removed** for deprecated feat
 
 ## Released ##
 
+### v2.5.1 - 
+  - **Fixed**  Error after SOPAS command SetAccessMode #27
+
 ### v2.5.0 - 
   - **Fixed** Issue #24 (stop scanner at exit)
   - **Added** new ros service SickScanExit to stop scanner and exit

README.md

@@ -576,6 +576,18 @@ Note:
 * The COLA commands are sensor specific. See the user manual and telegram listing for further details.
 * ROS services require installation of ROS-1 or ROS-2, i.e. services for Cola commands are currently not supported on native Linux or native Windows.
 * ROS services are currently not available for the LDMRS.
+* Some SOPAS commands like `sMN SetAccessMode 3 F4724744` stops the current measurement. In this case, the driver restarts after a timeout (5 seconds by default). To process those SOPAS commands without restart, you can
+   * send `sMN LMCstartmeas` and `sMN Run` to switch again into measurement mode within the timeout, or
+   * increase the driver timeout `read_timeout_millisec_default` in the launch-file.
+
+Example sequence with stop and start measurement to set a particle filter (TiM-7xxx on ROS-1):
+```
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN SetAccessMode 3 F4724744'}"
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sRN LFPparticle'}" # response: "sRA LFPparticle \\x00\\x01\\xf4"
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sWN LFPparticle 0101F4'}" # response: "sWA LFPparticle"
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN LMCstartmeas'}"
+rosservice call /sick_tim_7xx/ColaMsg "{request: 'sMN Run'}"
+```
 
 ### Driver states, timeouts
 

driver/src/sick_generic_caller.cpp

@@ -81,7 +81,7 @@
 
 #define SICK_GENERIC_MAJOR_VER "2"
 #define SICK_GENERIC_MINOR_VER "5"
-#define SICK_GENERIC_PATCH_LEVEL "0"
+#define SICK_GENERIC_PATCH_LEVEL "1"
 
 #include <algorithm> // for std::min
 
@@ -175,15 +175,24 @@ int main(int argc, char **argv)
   int result = 0;
   try
   {
-    result = mainGenericLaser(argc_tmp, argv_tmp, scannerName, node);
+    // result = mainGenericLaser(argc_tmp, argv_tmp, scannerName, node);
+    if (!startGenericLaser(argc_tmp, argv_tmp, scannerName, node, &result))
+    {
+      ROS_ERROR_STREAM("## ERROR in sick_generic_caller::main(): startGenericLaser() failed, could not start generic laser event loop");
+    }
+    else
+    {
+      rosSpin(node);
+    }
+    stopScannerAndExit();
   }
   catch(const std::exception& e)
   {
-    ROS_ERROR_STREAM("## ERROR in mainGenericLaser: exception " << e.what());
+    ROS_ERROR_STREAM("## ERROR in ick_generic_caller::main(): exception " << e.what());
   }
   catch(...)
   {
-    ROS_ERROR_STREAM("## ERROR in mainGenericLaser: unknown exception ");
+    ROS_ERROR_STREAM("## ERROR in ick_generic_caller::main(): unknown exception ");
   }
 
   return result;

driver/src/sick_generic_laser.cpp

@@ -105,6 +105,34 @@ std::string getVersionInfo()
   return (versionInfo);
 }
 
+void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv, bool do_ros_spin, int & exit_code);
+
+class GenericLaserCallable
+{
+public:
+  GenericLaserCallable(int _argc, char** _argv, std::string _nodeName, rosNodePtr _nhPriv, int* _exit_code)
+  : argc(_argc), argv(_argv), nodeName(_nodeName), nhPriv(_nhPriv), exit_code(_exit_code) 
+  {
+    generic_laser_thread = new std::thread(&GenericLaserCallable::mainGenericLaserCb, this);
+  }
+  void mainGenericLaserCb(void)
+  {
+    mainGenericLaserInternal(argc, argv, nodeName, nhPriv, false, *exit_code);
+  }
+  void join(void)
+  {
+    generic_laser_thread->join();
+  }
+  int argc; 
+  char** argv; 
+  std::string nodeName; 
+  rosNodePtr nhPriv; 
+  int* exit_code;
+  std::thread* generic_laser_thread;
+};
+
+static GenericLaserCallable* s_generic_laser_thread = 0;
+
 NodeRunState runState = scanner_init;
 
 /*!
@@ -145,6 +173,12 @@ bool stopScannerAndExit(bool force_immediate_shutdown)
       success = s_scanner->stopScanData(force_immediate_shutdown);
     }
     runState = scanner_finalize;
+    if(s_generic_laser_thread)
+    {
+      s_generic_laser_thread->join();
+      delete s_generic_laser_thread;
+      s_generic_laser_thread = 0;
+    }
   }
   return success;
 }
@@ -244,14 +278,16 @@ bool parseLaunchfileSetParameter(rosNodePtr nhPriv, int argc, char **argv)
 \param argc: Number of Arguments
 \param argv: Argument variable
 \param nodeName name of the ROS-node
-\return exit-code
+\param nhPriv ros node handle
+\param exit_code exit-code
 \sa main
 */
-int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv)
+void mainGenericLaserInternal(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv, bool do_ros_spin, int & exit_code)
 {
   std::string tag;
   std::string val;
 
+  exit_code = sick_scan::ExitSuccess;
   bool doInternalDebug = false;
   bool emulSensor = false;
   for (int i = 0; i < argc; i++)
@@ -284,6 +320,7 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP
   if(!parseLaunchfileSetParameter(nhPriv, argc, argv))
   {
     ROS_ERROR_STREAM("## ERROR sick_generic_laser: parseLaunchfileSetParameter() failed, aborting\n");
+    exit_code = sick_scan::ExitError;
     exit(-1);
   }
 #endif
@@ -359,18 +396,21 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP
 #if defined LDMRS_SUPPORT && LDMRS_SUPPORT > 0
     ROS_INFO("Initializing LDMRS...");
     sick_scan::SickLdmrsNode ldmrs;
-    int result = ldmrs.init(nhPriv, hostname, frame_id);
-    if(result != sick_scan::ExitSuccess)
+    exit_code = ldmrs.init(nhPriv, hostname, frame_id);
+    if(exit_code != sick_scan::ExitSuccess)
     {
       ROS_ERROR("LDMRS initialization failed.");
-      return sick_scan::ExitError;
+      exit_code = sick_scan::ExitError;
+      return;
     }
     ROS_INFO("LDMRS initialized.");
     rosSpin(nhPriv);
-    return sick_scan::ExitSuccess;
+    exit_code = sick_scan::ExitSuccess;
+    return;
 #else
     ROS_ERROR("LDMRS not supported. Please build sick_scan with option LDMRS_SUPPORT");
-    return sick_scan::ExitError;
+    exit_code = sick_scan::ExitError;
+    return;
 #endif
   }
 
@@ -469,7 +509,7 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP
 
   bool start_services = true;
   sick_scan::SickScanServices* services = 0;
-  int result = sick_scan::ExitError;
+  exit_code = sick_scan::ExitError;
 
   //sick_scan::SickScanConfig cfg;
   //std::chrono::system_clock::time_point timestamp_rosOk = std::chrono::system_clock::now();
@@ -501,11 +541,11 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP
         {
           s_scanner->setEmulSensor(true);
         }
-        result = s_scanner->init(nhPriv);
-        if (result == sick_scan::ExitError || result == sick_scan::ExitFatal)
+        exit_code = s_scanner->init(nhPriv);
+        if (exit_code == sick_scan::ExitError || exit_code == sick_scan::ExitFatal)
         {
 		      ROS_ERROR("## ERROR in mainGenericLaser: init failed, retrying..."); // ROS_ERROR("init failed, shutting down");
-          continue; // return result;
+          continue;
         }
 
         // Start ROS services
@@ -520,7 +560,7 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP
         isInitialized = true;
         // signal(SIGINT, SIG_DFL); // change back to standard signal handler after initialising
 
-        if (result == sick_scan::ExitSuccess) // OK -> loop again
+        if (exit_code == sick_scan::ExitSuccess) // OK -> loop again
         {
           if (changeIP)
           {
@@ -535,15 +575,18 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP
         break;
 
       case scanner_run:
-        if (result == sick_scan::ExitSuccess) // OK -> loop again
+        if (exit_code == sick_scan::ExitSuccess) // OK -> loop again
         {
-          rosSpinOnce(nhPriv);
-          result = s_scanner->loopOnce(nhPriv);
+          if(do_ros_spin)
+          {
+            rosSpinOnce(nhPriv);
+          }
+          exit_code = s_scanner->loopOnce(nhPriv);
 
           if(scan_msg_monitor && message_monitoring_enabled) // Monitor scanner messages
           {
-            result = scan_msg_monitor->checkStateReinitOnError(nhPriv, runState, s_scanner, parser, services);
-            if(result != sick_scan::ExitSuccess) // scanner re-init failed after read timeout or tcp error
+            exit_code = scan_msg_monitor->checkStateReinitOnError(nhPriv, runState, s_scanner, parser, services);
+            if(exit_code != sick_scan::ExitSuccess) // scanner re-init failed after read timeout or tcp error
             {
               ROS_ERROR("## ERROR in sick_generic_laser main loop: read timeout, scanner re-init failed");
             }
@@ -569,6 +612,39 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP
   DELETE_PTR(services);
   DELETE_PTR(s_scanner); // close connnect
   DELETE_PTR(parser); // close parser
-  return result;
+  return;
+}
 
+/*!
+\brief Runs mainGenericLaser non-blocking in a new thread
+\param argc: Number of Arguments
+\param argv: Argument variable
+\param nodeName name of the ROS-node
+\param nhPriv ros node handle
+\param exit_code exit-code
+\return true if mainGenericLaser is running, false otherwise
+\sa mainGenericLaser
+*/
+bool startGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv, int* exit_code)
+{
+  if (s_generic_laser_thread == 0)
+  {
+    s_generic_laser_thread = new GenericLaserCallable(argc, argv, nodeName, nhPriv, exit_code);
+  }
+  return (s_generic_laser_thread != 0);
+}
+
+/*!
+\brief Internal Startup routine.
+\param argc: Number of Arguments
+\param argv: Argument variable
+\param nodeName name of the ROS-node
+\return exit-code
+\sa main
+*/
+int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv)
+{
+  int result;
+  mainGenericLaserInternal(argc, argv, nodeName, nhPriv, true, result);
+  return result;
 }

include/sick_scan/sick_generic_laser.h

@@ -14,6 +14,8 @@ enum NodeRunState
 };
 
 
+bool startGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhPriv, int* exit_code);
+
 int mainGenericLaser(int argc, char **argv, std::string scannerName, rosNodePtr nh);
 
 void rosSignalHandler(int signalRecv);

launch/sick_lrs_4xxx.launch

@@ -8,8 +8,8 @@
         <param name="frame_id" type="str" value="cloud"/>
         <param name="port" type="string" value="2112"/>
         <param name="timelimit" type="int" value="5"/>
-        <param name="min_ang" type="double" value="-4.71238898"/> <!-- value from sick_scan2/config/sick_lrs_4xxx.yaml -->
-        <param name="max_ang" type="double" value="1.57079632"/>  <!-- value from sick_scan2/config/sick_lrs_4xxx.yaml -->
+        <param name="min_ang" type="double" value="-3.1415926"/>
+        <param name="max_ang" type="double" value="3.1415926"/>
         <param name="use_binary_protocol" type="bool" value="true"/>
         <param name="range_max" type="double" value="100.0"/>
         <param name="intensity" type="bool" value="True"/>