Automatically reboot scanner if it reports an error code. (#44)

Author: dbking77

cfg/SickTim.cfg

@@ -46,5 +46,6 @@ gen.add("intensity",      bool_t,   0, "Whether or not to return intensity value
 gen.add("skip",           int_t,    0, "The number of scans to skip between each measured scan.",                     0,          0,         9)
 gen.add("frame_id",       str_t,    0, "The TF frame in which laser scans will be returned.",                        "laser")
 gen.add("time_offset",    double_t, 0, "An offset to add to the time stamp before publication of a scan [s].",       -0.001,     -0.25, 0.25)
+gen.add("auto_reboot",    bool_t,   0, "Whether or not to reboot laser if it reports an error",                      True)
 
 exit(gen.generate(PACKAGE, "sick_tim", "SickTim"))

include/sick_tim/sick_tim_common.h

@@ -41,6 +41,7 @@
 
 #include <stdio.h>
 #include <stdlib.h>
+#include <string>
 #include <string.h>
 #include <vector>
 
@@ -71,6 +72,12 @@ class SickTimCommon
 
   double get_expected_frequency() const { return expectedFrequency_; }
 
+  /// Send a SOPAS command to the scanner that should cause a soft reset
+  /**
+   * \returns true if reboot command was accepted, false otherwise
+   */
+  virtual bool rebootScanner();
+
 protected:
   virtual int init_device() = 0;
   virtual int init_scanner();
@@ -92,6 +99,13 @@ class SickTimCommon
    */
   virtual int get_datagram(unsigned char* receiveBuffer, int bufferSize, int* actual_length) = 0;
 
+  /// Converts reply from sendSOPASCommand to string
+  /**
+   * \param [in] reply reply from sendSOPASCommand
+   * \returns reply as string with special characters stripped out
+   */
+  static std::string replyToString(const std::vector<unsigned char> &reply);
+
   bool isCompatibleDevice(const std::string identStr) const;
 
 protected:

src/sick_tim_common.cpp

@@ -86,6 +86,54 @@ int SickTimCommon::stop_scanner()
   return result;
 }
 
+bool SickTimCommon::rebootScanner()
+{
+  /*
+   * Set Maintenance access mode to allow reboot to be sent
+   */
+  std::vector<unsigned char> access_reply;
+  int result = sendSOPASCommand("\x02sMN SetAccessMode 03 F4724744\x03\0", &access_reply);
+  if (result != 0)
+  {
+    ROS_ERROR("SOPAS - Error setting access mode");
+    diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error setting access mode.");
+    return false;
+  }
+  std::string access_reply_str = replyToString(access_reply);
+  if (access_reply_str != "sAN SetAccessMode 1")
+  {
+    ROS_ERROR_STREAM("SOPAS - Error setting access mode, unexpected response : " << access_reply_str);
+    diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error setting access mode.");
+    return false;
+  }
+
+  /*
+   * Send reboot command
+   */
+  std::vector<unsigned char> reboot_reply;
+  result = sendSOPASCommand("\x02sMN mSCreboot\x03\0", &reboot_reply);
+  if (result != 0)
+  {
+    ROS_ERROR("SOPAS - Error rebooting scanner");
+    diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error rebooting device.");
+    return false;
+  }
+  std::string reboot_reply_str = replyToString(reboot_reply);
+  if (reboot_reply_str != "sAN mSCreboot")
+  {
+    ROS_ERROR_STREAM("SOPAS - Error rebooting scanner, unexpected response : " << reboot_reply_str);
+    diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error setting access mode.");
+    return false;
+  }
+
+  ROS_INFO("SOPAS - Rebooted scanner");
+
+  // Wait a few seconds after rebooting
+  ros::Duration(15.0).sleep();
+
+  return true;
+}
+
 SickTimCommon::~SickTimCommon()
 {
   delete diagnosticPub_;
@@ -135,20 +183,8 @@ int SickTimCommon::init_scanner()
   }
 
   // set hardware ID based on DeviceIdent and SerialNumber
-  identReply.push_back(0);  // add \0 to convert to string
-  serialReply.push_back(0);
-  std::string identStr;
-  for (std::vector<unsigned char>::iterator it = identReply.begin(); it != identReply.end(); it++)
-  {
-    if (*it > 13) // filter control characters for display
-      identStr.push_back(*it);
-  }
-  std::string serialStr;
-  for (std::vector<unsigned char>::iterator it = serialReply.begin(); it != serialReply.end(); it++)
-  {
-    if (*it > 13)
-      serialStr.push_back(*it);
-  }
+  std::string identStr = replyToString(identReply);
+  std::string serialStr = replyToString(serialReply);
   diagnostics_.setHardwareID(identStr + " " + serialStr);
 
   if (!isCompatibleDevice(identStr))
@@ -165,6 +201,44 @@ int SickTimCommon::init_scanner()
     diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'FirmwareVersion'.");
   }
 
+  /*
+   * Read Device State
+   */
+  const char requestDeviceState[] = {"\x02sRN SCdevicestate\x03\0"};
+  std::vector<unsigned char> deviceStateReply;
+  result = sendSOPASCommand(requestDeviceState, &deviceStateReply);
+  if (result != 0)
+  {
+    ROS_ERROR("SOPAS - Error reading variable 'devicestate'.");
+    diagnostics_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "SOPAS - Error reading variable 'devicestate'.");
+  }
+  std::string deviceStateReplyStr = replyToString(deviceStateReply);
+
+  /*
+   * Process device state, 0=Busy, 1=Ready, 2=Error
+   * If configuration parameter is set, try resetting device in error state
+   */
+  if (deviceStateReplyStr == "sRA SCdevicestate 0")
+  {
+    ROS_WARN("Laser is busy");
+  }
+  else if (deviceStateReplyStr == "sRA SCdevicestate 1")
+  {
+    ROS_DEBUG("Laser is ready");
+  }
+  else if (deviceStateReplyStr == "sRA SCdevicestate 2")
+  {
+    ROS_ERROR_STREAM("Laser reports error state : " << deviceStateReplyStr);
+    if (config_.auto_reboot)
+    {
+      rebootScanner();
+    }
+  }
+  else
+  {
+    ROS_WARN_STREAM("Laser reports unknown devicestate : " << deviceStateReplyStr);
+  }
+
   /*
    * Start streaming 'LMDscandata'.
    */
@@ -180,6 +254,19 @@ int SickTimCommon::init_scanner()
   return ExitSuccess;
 }
 
+std::string sick_tim::SickTimCommon::replyToString(const std::vector<unsigned char> &reply)
+{
+  std::string reply_str;
+  for (std::vector<unsigned char>::const_iterator it = reply.begin(); it != reply.end(); it++)
+  {
+    if (*it > 13) // filter control characters for display
+    {
+      reply_str.push_back(*it);
+    }
+  }
+  return reply_str;
+}
+
 bool sick_tim::SickTimCommon::isCompatibleDevice(const std::string identStr) const
 {
   char device_string[7];