Check for firmware versions without range output

Closes #36 .

include/sick_tim/sick_tim_common.h

@@ -55,6 +55,8 @@
 #include <sick_tim/SickTimConfig.h>
 #include <sick_tim/abstract_parser.h>
 
+#define EXIT_FATAL  2
+
 namespace sick_tim
 {
 
@@ -91,6 +93,8 @@ class SickTimCommon
    */
   virtual int get_datagram(unsigned char* receiveBuffer, int bufferSize, int* actual_length) = 0;
 
+  bool isCompatibleDevice(const std::string identStr) const;
+
 protected:
   diagnostic_updater::Updater diagnostics_;
 

src/sick_tim310.cpp

@@ -68,6 +68,9 @@ int main(int argc, char **argv)
       result = s->loopOnce();
     }
 
+    if (result == EXIT_FATAL)
+      return result;
+
     if (ros::ok() && !subscribe_datagram)
       ros::Duration(1.0).sleep(); // Only attempt USB connections once per second
   }

src/sick_tim310_1130000m01.cpp

@@ -68,6 +68,9 @@ int main(int argc, char **argv)
       result = s->loopOnce();
     }
 
+    if (result == EXIT_FATAL)
+      return result;
+
     if (ros::ok() && !subscribe_datagram)
       ros::Duration(1.0).sleep(); // Only attempt USB connections once per second
   }

src/sick_tim310s01.cpp

@@ -68,6 +68,9 @@ int main(int argc, char **argv)
       result = s->loopOnce();
     }
 
+    if (result == EXIT_FATAL)
+      return result;
+
     if (ros::ok() && !subscribe_datagram)
       ros::Duration(1.0).sleep(); // Only attempt USB connections once per second
   }

src/sick_tim551_2050001.cpp

@@ -94,6 +94,9 @@ int main(int argc, char **argv)
       result = s->loopOnce();
     }
 
+    if (result == EXIT_FATAL)
+      return result;
+
     if (ros::ok() && !subscribe_datagram && !useTCP)
       ros::Duration(1.0).sleep(); // Only attempt USB connections once per second
   }

src/sick_tim_common.cpp

@@ -38,6 +38,9 @@
 
 #include <sick_tim/sick_tim_common.h>
 
+#include <cstdio>
+#include <cstring>
+
 namespace sick_tim
 {
 
@@ -135,17 +138,22 @@ int SickTimCommon::init_scanner()
   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);
+  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);
+  for (std::vector<unsigned char>::iterator it = serialReply.begin(); it != serialReply.end(); it++)
+  {
+    if (*it > 13)
+      serialStr.push_back(*it);
   }
   diagnostics_.setHardwareID(identStr + " " + serialStr);
 
+  if (!isCompatibleDevice(identStr))
+    return EXIT_FATAL;
+
   /*
    * Read the SOPAS variable 'FirmwareVersion' by name.
    */
@@ -172,6 +180,26 @@ int SickTimCommon::init_scanner()
   return EXIT_SUCCESS;
 }
 
+bool sick_tim::SickTimCommon::isCompatibleDevice(const std::string identStr) const
+{
+  char device_string[7];
+  int version_major = -1;
+  int version_minor = -1;
+
+  if (sscanf(identStr.c_str(), "sRA 0 6 %6s E V%d.%d", device_string,
+             &version_major, &version_minor) == 3
+      && strncmp("TiM3", device_string, 4) == 0
+      && version_major >= 2 && version_minor >= 50)
+  {
+    ROS_ERROR("This scanner model/firmware combination does not support ranging output!");
+    ROS_ERROR("Supported scanners: TiM5xx: all firmware versions; TiM3xx: firmware versions < V2.50.");
+    ROS_ERROR("This is a %s, firmware version %d.%d", device_string, version_major, version_minor);
+
+    return false;
+  }
+  return true;
+}
+
 int SickTimCommon::loopOnce()
 {
   diagnostics_.update();