Add version detection

include/abb_libegm/egm_base_interface.h

@@ -196,6 +196,13 @@ class EGMBaseInterface : public AbstractUDPServerInterface
     bool statesOk() const;
 
   private:
+    /**
+     * \brief Detect RobotWare and EGM protocol versions from a received EGM message.
+     *
+     * Note: Only a rough version detection is possible based on whether certain fields are present or not.
+     */
+    void detectRWAndEGMVersions();
+
     /**
      * \brief Estimate the sample time.
      *

proto/egm_wrapper.proto

@@ -25,9 +25,28 @@ message Header
     DATA      = 1; // Message contains data sent by the robot controller.
   }
 
+  // Note: EGM messages received during runtime are inspected to detect the RobotWare version.
+  enum RWVersion
+  {
+    RW_UNKNOWN                  = 0;
+    RW_6_10_AND_NEWER           = 1; // Selected if all the fields mentioned below exist.
+    RW_BETWEEN_6_AND_6_06_03    = 2; // Selected if no time field exist (was added in RW6.07).
+    RW_BETWEEN_6_07_AND_6_09_02 = 3; // Selected if no utilization rate field exist (was added in RW6.10).
+  }
+
+  // Note: EGM messages received during runtime are inspected to detect the EGM version.
+  enum EGMVersion
+  {
+    EGM_UNKNOWN = 0;
+    EGM_1_0     = 1; // Selected if no time field exist (was added in 6.07).
+    EGM_1_1     = 2; // Selected if the field mentioned below exist.
+  }
+
   optional uint32      sequence_number = 1; // Sequence number (to be able to detect lost messages).
   optional uint32      time_stamp      = 2; // Time stamp in milliseconds.
   optional MessageType message_type    = 3 [default = UNDEFINED];
+  optional RWVersion   rw_version      = 4 [default = RW_UNKNOWN];
+  optional EGMVersion  egm_version     = 5 [default = EGM_UNKNOWN];
 }
 
 // Note: Status of the robot controller, and the active EGM motion.

src/egm_base_interface.cpp

@@ -84,6 +84,8 @@ bool EGMBaseInterface::InputContainer::extractParsedInformation(const RobotAxes&
 {
   bool success = false;
 
+  detectRWAndEGMVersions();
+
   if (has_new_data_ &&
       parse(current_.mutable_header(), egm_robot_.header()) &&
       parse(current_.mutable_feedback(), egm_robot_.feedback(), axes) &&
@@ -121,6 +123,48 @@ bool EGMBaseInterface::InputContainer::statesOk() const
  * Auxiliary methods
  */
 
+void EGMBaseInterface::InputContainer::detectRWAndEGMVersions()
+{
+  if(has_new_data_)
+  {
+    // Time field was added in RobotWare '6.07', as well as fix of inconsistent units (e.g. radians and degrees).
+    if(egm_robot_.feedback().has_time())
+    {
+      // If time field present:
+      // - RW greater than or equal to '6.07'.
+      // - EGM protocol '1.1' (with consistent units).
+      current_.mutable_header()->set_egm_version(wrapper::Header_EGMVersion_EGM_1_1);
+
+      // Utilization field was added in RobotWare '6.10'.
+      if(egm_robot_.has_utilizationrate())
+      {
+        // If utilization field present:
+        // - RW greater than or equal to '6.10'.
+        current_.mutable_header()->set_rw_version(wrapper::Header_RWVersion_RW_6_10_AND_NEWER);
+      }
+      else
+      {
+        // If utilization field absent:
+        // - RW between '6.07' and '6.09.02'.
+        current_.mutable_header()->set_rw_version(wrapper::Header_RWVersion_RW_BETWEEN_6_07_AND_6_09_02);
+      }
+    }
+    else
+    {
+      // If time field absent:
+      // - RW between '6.0' and '6.06.03'.
+      // - EGM protocol '1.0' (with inconsistent units).
+      current_.mutable_header()->set_rw_version(wrapper::Header_RWVersion_RW_BETWEEN_6_AND_6_06_03);
+      current_.mutable_header()->set_egm_version(wrapper::Header_EGMVersion_EGM_1_0);
+    }
+  }
+  else
+  {
+    current_.mutable_header()->set_rw_version(wrapper::Header_RWVersion_RW_UNKNOWN);
+    current_.mutable_header()->set_egm_version(wrapper::Header_EGMVersion_EGM_UNKNOWN);
+  }
+}
+
 double EGMBaseInterface::InputContainer::estimateSampleTime()
 {
   double estimate = 0.0;