Merge pull request #25 from SICKAG/feature/lidar_stop_exit

Fix issue #24 (stop scanner at exit), new ros service SickScanExit to…

Author: rostest

CHANGELOG.md

@@ -8,6 +8,10 @@ features that will be removed in future versions **Removed** for deprecated feat
 
 ## Released ##
 
+### v2.5.0 - 
+  - **Fixed** Issue #24 (stop scanner at exit)
+  - **Added** new ros service SickScanExit to stop scanner and exit
+
 ### v2.4.6 - 
   - **Fixed** Corrected angle shift parameter for LMS-4xxx
   - **Changed** Typo corrected

CMakeLists.txt

@@ -149,6 +149,7 @@ if(ROS_VERSION EQUAL 1)
         SCdevicestateSrv.srv
         SCrebootSrv.srv
         SCsoftresetSrv.srv
+        SickScanExitSrv.srv
     )
 endif(ROS_VERSION EQUAL 1)
 
@@ -158,7 +159,10 @@ if(ROS_VERSION EQUAL 2)
     set(FastRTPS_LIBRARY_RELEASE /opt/ros/$ENV{ROS_DISTRO}/lib/libfastrtps.so)
 
     find_package(ament_cmake REQUIRED)
-    find_package(diagnostic_updater REQUIRED)
+    if(NOT WIN32)
+        set(diagnostic_updater_pkg diagnostic_updater)
+        find_package(${diagnostic_updater_pkg} REQUIRED)
+    endif()
     find_package(rclcpp REQUIRED)
     find_package(sensor_msgs REQUIRED)
     find_package(std_msgs REQUIRED)
@@ -257,6 +261,7 @@ if(ROS_VERSION EQUAL 2)
         "srv/SCdevicestateSrv.srv"
         "srv/SCrebootSrv.srv"
         "srv/SCsoftresetSrv.srv"
+        "srv/SickScanExitSrv.srv"
         ${ROSIDL_EMULATOR_FILES}
         DEPENDENCIES builtin_interfaces std_msgs nav_msgs geometry_msgs sensor_msgs
     )   
@@ -363,7 +368,7 @@ if(ROS_VERSION EQUAL 1)
     )
 
     catkin_package(
-        CATKIN_DEPENDS message_runtime roscpp sensor_msgs diagnostic_updater dynamic_reconfigure pcl_conversions pcl_ros tf tf2
+        CATKIN_DEPENDS message_runtime roscpp sensor_msgs ${diagnostic_updater_pkg} dynamic_reconfigure pcl_conversions pcl_ros tf tf2
         LIBRARIES sick_scan_lib sick_scan_shared_lib
         INCLUDE_DIRS include
         # DEPENDS Boost
@@ -527,7 +532,7 @@ if(ROS_VERSION EQUAL 2)
             "nav_msgs"
             "visualization_msgs"
             "tf2_ros"
-            "diagnostic_updater"
+            "${diagnostic_updater_pkg}"
         ${LDMRS_TARGET_DEPENDENCIES}
     )
 
@@ -542,7 +547,7 @@ if(ROS_VERSION EQUAL 2)
             "nav_msgs"
             "visualization_msgs"
             "tf2_ros"
-            "diagnostic_updater"
+            "${diagnostic_updater_pkg}"
             ${LDMRS_TARGET_DEPENDENCIES}
     )
 
@@ -557,7 +562,7 @@ if(ROS_VERSION EQUAL 2)
             "nav_msgs"
             "visualization_msgs"
             "tf2_ros"
-            "diagnostic_updater"
+            "${diagnostic_updater_pkg}"
         ${LDMRS_TARGET_DEPENDENCIES}
     )
 

README.md

@@ -566,6 +566,12 @@ ros2 service call /SCreboot sick_scan/srv/SCrebootSrv "{}"           # execute a
 ros2 service call /SCsoftreset sick_scan/srv/SCsoftresetSrv "{}"     # save current parameter and shut down device
 ```
 
+Use ros service `SickScanExit` to stop the scanner and driver:
+```
+rosservice call /sick_nav_3xx/SickScanExit "{}" # stop scanner and driver on ROS-1
+ros2 service call /SickScanExit sick_scan/srv/SickScanExitSrv "{}" # stop scanner and driver on ROS-2
+```
+
 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.

demo/scan.csv

@@ -80,8 +80,8 @@
 #define MAX_NAME_LEN (1024)
 
 #define SICK_GENERIC_MAJOR_VER "2"
-#define SICK_GENERIC_MINOR_VER "4"
-#define SICK_GENERIC_PATCH_LEVEL "6"
+#define SICK_GENERIC_MINOR_VER "5"
+#define SICK_GENERIC_PATCH_LEVEL "0"
 
 #include <algorithm> // for std::min
 
@@ -155,10 +155,11 @@ int main(int argc, char **argv)
   rosNodePtr node = rclcpp::Node::make_shared("sick_scan", "", node_options);
 #else
   ros::init(argc, argv, scannerName, ros::init_options::NoSigintHandler);  // scannerName holds the node-name
-  signal(SIGINT, rosSignalHandler);
+  // signal(SIGINT, rosSignalHandler);
   ros::NodeHandle nh("~");
   rosNodePtr node = &nh;
 #endif
+  signal(SIGINT, rosSignalHandler);
 
   ROS_INFO_STREAM(versionInfo << "\n");
   for (int i = 0; i < argc_tmp; i++)

demo/scan.jpg

@@ -135,22 +135,27 @@ bool getTagVal(std::string tagVal, std::string &tag, std::string &val)
   return (ret);
 }
 
-
-void rosSignalHandler(int signalRecv)
+bool stopScannerAndExit(bool force_immediate_shutdown)
 {
-  ROS_INFO_STREAM("Caught signal " << signalRecv << "\n");
-  ROS_INFO_STREAM("good bye\n");
-  ROS_INFO_STREAM("You are leaving the following version of this node:\n");
-  ROS_INFO_STREAM(getVersionInfo() << "\n");
+  bool success = true;
   if (s_scanner != NULL)
   {
     if (isInitialized)
     {
-      s_scanner->stopScanData();
+      success = s_scanner->stopScanData(force_immediate_shutdown);
     }
-
     runState = scanner_finalize;
   }
+  return success;
+}
+
+void rosSignalHandler(int signalRecv)
+{
+  ROS_INFO_STREAM("Caught signal " << signalRecv << "\n");
+  ROS_INFO_STREAM("good bye\n");
+  ROS_INFO_STREAM("You are leaving the following version of this node:\n");
+  ROS_INFO_STREAM(getVersionInfo() << "\n");
+  stopScannerAndExit(true);
   rosShutdown();
 }
 
@@ -467,9 +472,15 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP
   int result = sick_scan::ExitError;
 
   //sick_scan::SickScanConfig cfg;
+  //std::chrono::system_clock::time_point timestamp_rosOk = std::chrono::system_clock::now();
 
   while (rosOk() && runState != scanner_finalize)
   {
+    //if (rosOk())
+    //  timestamp_rosOk = std::chrono::system_clock::now();
+    //else if (std::chrono::duration<double>(std::chrono::system_clock::now() - timestamp_rosOk).count() > 2 * 1000) // 2 seconds timeout to stop the scanner 
+    //  runState = scanner_finalize;
+
     switch (runState)
     {
       case scanner_init:
@@ -507,7 +518,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
+        // signal(SIGINT, SIG_DFL); // change back to standard signal handler after initialising
 
         if (result == sick_scan::ExitSuccess) // OK -> loop again
         {
@@ -549,6 +560,7 @@ int mainGenericLaser(int argc, char **argv, std::string nodeName, rosNodePtr nhP
         break;
     }
   }
+  printf("sick_generic_laser: leaving main loop...");
 
   if(pointcloud_monitor)
     pointcloud_monitor->stopPointCloudMonitoring();

driver/src/sick_generic_caller.cpp

@@ -589,38 +589,40 @@ namespace sick_scan
   \brief Stops sending scan data
   \return error code
    */
-  int SickScanCommon::stop_scanner()
+  int SickScanCommon::stop_scanner(bool force_immediate_shutdown)
   {
     /*
      * Stop streaming measurements
      */
-    const char requestScanData0[] = {"\x02sEN LMDscandata 0\x03\0"};
-    int result = sendSOPASCommand(requestScanData0, NULL);
-    if (result != 0)
+    std::vector<std::string> sopas_stop_scanner_cmd = { "\x02sEN LMDscandata 0\x03\0" };
+    if (parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC || parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_LMS5XX_LOGIC)
     {
-      // use printf because we cannot use ROS_ERROR from the destructor
-      printf("\nSOPAS - Error stopping streaming scan data!\n");
+      sopas_stop_scanner_cmd.push_back("\x02sEN LFErec 0\x03"); // TiM781S: deactivate LFErec messages, send "sEN LFErec 0"
+      sopas_stop_scanner_cmd.push_back("\x02sEN LIDoutputstate 0\x03"); // TiM781S: deactivate LIDoutputstate messages, send "sEN LIDoutputstate 0"
+      sopas_stop_scanner_cmd.push_back("\x02sEN LIDinputstate 0\x03"); // TiM781S: deactivate LIDinputstate messages, send "sEN LIDinputstate 0"
     }
-    else
+    sopas_stop_scanner_cmd.push_back("\x02sMN SetAccessMode 3 F4724744\x03\0");
+    sopas_stop_scanner_cmd.push_back("\x02sMN LMCstopmeas\x03\0");
+    // sopas_stop_scanner_cmd.push_back("\x02sMN Run\x03\0");
+
+    setReadTimeOutInMs(1000);
+    ROS_INFO_STREAM("sick_scan_common: stopping scanner ...");
+    int result = ExitSuccess, cmd_result = ExitSuccess;
+    for(int cmd_idx = 0; cmd_idx < sopas_stop_scanner_cmd.size(); cmd_idx++)
     {
-      printf("\nSOPAS - Stopped streaming scan data.\n");
-    }
-
-    if (parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC || parser_->getCurrentParamPtr()->getUseEvalFields() == USE_EVAL_FIELD_LMS5XX_LOGIC)
-    {
-      if(sendSOPASCommand("\x02sEN LFErec 0\x03", NULL) != 0 // TiM781S: deactivate LFErec messages, send "sEN LFErec 0"
-      || sendSOPASCommand("\x02sEN LIDoutputstate 0\x03", NULL) != 0 // TiM781S: deactivate LIDoutputstate messages, send "sEN LIDoutputstate 0"
-      || sendSOPASCommand("\x02sEN LIDinputstate 0\x03", NULL) != 0) // TiM781S: deactivate LIDinputstate messages, send "sEN LIDinputstate 0"
+      std::vector<unsigned char> sopas_reply;
+      cmd_result = convertSendSOPASCommand(sopas_stop_scanner_cmd[cmd_idx], &sopas_reply, (force_immediate_shutdown==false));
+      if (force_immediate_shutdown == false)
       {
-        // use printf because we cannot use ROS_ERROR from the destructor
-        printf("\nSOPAS - Error stopping streaming LFErec, LIDoutputstate and LIDinputstate messages!\n");
+        ROS_INFO_STREAM("sick_scan_common: received sopas reply \"" << replyToString(sopas_reply) << "\"");
       }
-      else
+      if (cmd_result != ExitSuccess)
       {
-        printf("\nSOPAS - Stopped streaming LFErec, LIDoutputstate and LIDinputstate messages\n");
+        ROS_WARN_STREAM("## ERROR sick_scan_common: ERROR sending sopas command \"" << sopas_stop_scanner_cmd[cmd_idx] << "\"");
+        result = ExitError;
       }
+      // std::this_thread::sleep_for(std::chrono::milliseconds((int64_t)100));
     }
-
     return result;
   }
 
@@ -678,9 +680,32 @@ namespace sick_scan
 
   }
 
+  /// Converts a given SOPAS command from ascii to binary (in case of binary communication), sends sopas (ascii or binary) and returns the response (if wait_for_reply:=true)
+  /**
+   * \param [in] request the command to send.
+   * \param [in] cmdLen Length of the Comandstring in bytes used for Binary Mode only
+   */
+  int SickScanCommon::convertSendSOPASCommand(const std::string& sopas_ascii_request, std::vector<unsigned char> *sopas_reply, bool wait_for_reply)
+  {
+    int result = ExitError;
+    if (getProtocolType() == CoLa_B)
+    {
+      std::vector<unsigned char> requestBinary;
+      convertAscii2BinaryCmd(sopas_ascii_request.c_str(), &requestBinary);
+      ROS_INFO_STREAM("sick_scan_common: sending sopas command \"" << stripControl(requestBinary) << "\"");
+      result = sendSOPASCommand((const char*)requestBinary.data(), sopas_reply, requestBinary.size(), wait_for_reply);
+    }
+    else
+    {
+      ROS_INFO_STREAM("sick_scan_common: sending sopas command \"" << sopas_ascii_request << "\"");
+      result = sendSOPASCommand(sopas_ascii_request.c_str(), sopas_reply, sopas_ascii_request.size(), wait_for_reply);
+    }
+    return result;
+  }
+
 
   /*!
-  \brief Reboot scanner (todo: this does not work if the scanner is set to binary mode) Fix me!
+  \brief Reboot scanner
   \return Result of rebooting attempt
   */
   bool SickScanCommon::rebootScanner()
@@ -689,8 +714,10 @@ namespace sick_scan
      * Set Maintenance access mode to allow reboot to be sent
      */
     std::vector<unsigned char> access_reply;
+
+
     // changed from "03" to "3"
-    int result = sendSOPASCommand("\x02sMN SetAccessMode 3 F4724744\x03\0", &access_reply);
+    int result = convertSendSOPASCommand("\x02sMN SetAccessMode 3 F4724744\x03\0", &access_reply);
     if (result != 0)
     {
       ROS_ERROR("SOPAS - Error setting access mode");
@@ -715,7 +742,7 @@ namespace sick_scan
      * Send reboot command
      */
     std::vector<unsigned char> reboot_reply;
-    result = sendSOPASCommand("\x02sMN mSCreboot\x03\0", &reboot_reply);
+    result = convertSendSOPASCommand("\x02sMN mSCreboot\x03\0", &reboot_reply);
     if (result != 0)
     {
       ROS_ERROR("SOPAS - Error rebooting scanner");

driver/src/sick_generic_laser.cpp

@@ -232,7 +232,7 @@ namespace sick_scan
 
   SickScanCommonTcp::~SickScanCommonTcp()
   {
-    // stop_scanner();
+    // stop_scanner(true);
     close_device();
   }
 
@@ -620,10 +620,10 @@ namespace sick_scan
   }
 
 
-  bool SickScanCommonTcp::stopScanData()
+  bool SickScanCommonTcp::stopScanData(bool force_immediate_shutdown)
   {
-    stop_scanner();
-    return (true);
+    int retval = stop_scanner(force_immediate_shutdown);
+    return retval == ExitSuccess;
   }
 
   // void SickScanCommonTcp::handleRead(boost::system::error_code error, size_t bytes_transfered)
@@ -677,7 +677,7 @@ namespace sick_scan
   /**
    * Send a SOPAS command to the device and print out the response to the console.
    */
-  int SickScanCommonTcp::sendSOPASCommand(const char *request, std::vector<unsigned char> *reply, int cmdLen)
+  int SickScanCommonTcp::sendSOPASCommand(const char *request, std::vector<unsigned char> *reply, int cmdLen, bool wait_for_reply)
   {
     int sLen = 0;
     int msgLen = 0;
@@ -744,6 +744,10 @@ namespace sick_scan
         }
       }
     }
+    if(!wait_for_reply)
+    {
+      return ExitSuccess;
+    }
 
     // Set timeout in 5 seconds
     const int BUF_SIZE = 65536;
@@ -765,7 +769,7 @@ namespace sick_scan
           ROS_WARN_STREAM("sendSOPASCommand: no full reply available for read after " << getReadTimeOutInMs() << " ms");
 #endif
 #ifdef USE_DIAGNOSTIC_UPDATER
-          if(diagnostics_)
+          if(diagnostics_ && rosOk())
             diagnostics_->broadcast(getDiagnosticErrorCode(),
                                "sendSOPASCommand: no full reply available for read after timeout.");
 #endif