Merge pull request #32 from jontje/migration_to_latest_rw

Migration to latest rw

include/abb_libegm/egm_common.h

@@ -119,6 +119,11 @@ struct Constants
      * \brief Conversion value from milliseconds to seconds.
      */
     static const double MS_TO_S;
+
+    /**
+     * \brief Conversion value from seconds to microseconds.
+     */
+    static const double S_TO_US;
   };
 };
 

include/abb_libegm/egm_common_auxiliary.h

@@ -321,6 +321,16 @@ bool parse(wrapper::Header* p_target, const EgmHeader& source);
  */
 bool parse(wrapper::Status* p_target, const EgmRobot& source);
 
+/**
+ * \brief Parse an abb::egm::EgmClock object.
+ *
+ * \param p_target for containing the parsed data.
+ * \param source containing data to parse.
+ *
+ * \return bool indicating if the parsing was successful or not.
+ */
+bool parse(wrapper::Clock* p_target, const EgmClock& source);
+
 /**
  * \brief Parse two abb::egm::EgmJoints objects.
  *

proto/egm.proto

@@ -1,4 +1,4 @@
-// Definition of ABB sensor interface V1.0
+// Definition of ABB sensor interface V1.1
 //
 // messages of type EgmRobot are sent out from the robot controller
 // messages of type EgmSensor are sent to the robot controller
@@ -8,20 +8,20 @@ package abb.egm;
 message EgmHeader
 { 
     optional uint32 seqno = 1; // sequence number (to be able to find lost messages)
-    optional uint32 tm = 2;    // time stamp in ms
+    optional uint32 tm = 2;    // controller send time stamp in ms
 
     enum MessageType {
         MSGTYPE_UNDEFINED = 0;
         MSGTYPE_COMMAND = 1;			// for future use
         MSGTYPE_DATA = 2;				// sent by robot controller
-        MSGTYPE_CORRECTION = 3;			// sent by sensor
-        MSGTYPE_PATH_CORRECTION = 4;	// sent by sensor
+        MSGTYPE_CORRECTION = 3;			// sent by sensor for position guidance
+        MSGTYPE_PATH_CORRECTION = 4;	// sent by sensor for path correction
     }
 
     optional MessageType  mtype = 3 [default = MSGTYPE_UNDEFINED];
 }
 
-message EgmCartesian			// Cartesian position in mm, interpreted relative to the sensor frame defined by EGMActPose
+message EgmCartesian			// Cartesian position in mm
 {
     required double x = 1;
     required double y = 2;
@@ -31,22 +31,28 @@ message EgmCartesian			// Cartesian position in mm, interpreted relative to the
 // If you have pose input, i.e. not joint input, you can choose to send orientation data as quaternion or as Euler angles.
 // If both are sent, Euler angles have higher priority.
 
-message EgmQuaternion			// Quaternion orientation interpreted relative to the sensor frame defined by EGMActPose
+message EgmQuaternion			// Quaternion orientation
 {
     required double u0 = 1;
     required double u1 = 2;
     required double u2 = 3;
     required double u3 = 4;
 }
 
-message EgmEuler				// Euler angle orientation in degrees, interpreted relative to the sensor frame defined by EGMActPose
+message EgmEuler				// Euler angle orientation in degrees
 {
     required double x = 1;
     required double y = 2;
     required double z = 3;
 }
 
-message EgmPose					// Pose (i.e. cartesian position and Quaternion orientation) interpreted relative to the sensor frame defined by EGMActPose
+message EgmClock				// Time in seconds and microseconds since 1 Jan 1970
+{
+    required uint64 sec = 1;
+    required uint64 usec = 2;
+}
+
+message EgmPose					// Pose (i.e. cartesian position and Quaternion orientation) relative to the correction frame defined by EGMActPose
 { 
     optional EgmCartesian	pos = 1;
     optional EgmQuaternion	orient = 2;
@@ -63,16 +69,17 @@ message EgmJoints 				// Array of 6 joint values for TCP robot in degrees
     repeated double joints = 1;
 }
 
-message EgmExternalJoints 		// Array of 6 joint values for additional axis in degrees
+message EgmExternalJoints 		// Array of 6 joint values for additional axis, in degrees for rotating axis, in mm for linear axis
 {
     repeated double joints = 1;
 }
 
 message EgmPlanned				// Planned  position for robot (joints or cartesian) and additional axis (array of 6 values)
-{
-    optional EgmJoints joints = 1;
-    optional EgmPose   cartesian = 2;
-    optional EgmJoints externalJoints = 3;
+{								// Is used for position streaming (source: controller) and position guidance (source: sensor)
+    optional EgmJoints	joints = 1;
+    optional EgmPose	cartesian = 2;
+    optional EgmJoints	externalJoints = 3;
+	optional EgmClock	time = 4;
 }
 
 message EgmSpeedRef				// Speed reference values for robot (joint or cartesian) and additional axis (array of 6 values)
@@ -85,16 +92,17 @@ message EgmSpeedRef				// Speed reference values for robot (joint or cartesian)
 
 message EgmPathCorr				// Cartesian path correction and measurment age
 {
-    required EgmCartesian   pos = 1;	// Sensor measurement (x, y, z)
+    required EgmCartesian   pos = 1;	// Sensor measurement (x, y, z) relative the sensor tool coordinate system
 	required uint32			age = 2;	// Sensor measurement age in ms
 }
 
 
 message EgmFeedBack				// Feed back position, i.e. actual measured position for robot (joints or cartesian) and additional axis (array of 6 values)
 {
-    optional EgmJoints    joints = 1;
-    optional EgmPose      cartesian = 2;
-    optional EgmJoints    externalJoints = 3;
+    optional EgmJoints  joints = 1;
+    optional EgmPose    cartesian = 2;
+    optional EgmJoints  externalJoints = 3;
+	optional EgmClock	time = 4;
 }
 
 message EgmMotorState			// Motor state
@@ -136,8 +144,12 @@ message EgmTestSignals			// Test signals
     repeated double signals = 1;
 }
 
+message EgmMeasuredForce 				// Array of 6 force values for a robot
+{
+    repeated double force = 1;
+}
 
-// Robot controller outbound message
+// Robot controller outbound message, sent from the controller to the sensor during position guidance and position streaming
 message EgmRobot
 {
     optional EgmHeader				header = 1;
@@ -146,21 +158,22 @@ message EgmRobot
 
     optional EgmMotorState			motorState = 4;
     optional EgmMCIState			mciState = 5;
-    optional bool				mciConvergenceMet = 6;
+    optional bool					mciConvergenceMet = 6;
     optional EgmTestSignals			testSignals = 7;
-    optional EgmRapidCtrlExecState	        rapidExecState = 8; 
+    optional EgmRapidCtrlExecState	rapidExecState = 8; 
+    optional EgmMeasuredForce		measuredForce = 9;
 }
 
 
-// Robot controller inbound message, sent from sensor
+// Robot controller inbound message, sent from sensor to the controller during position guidance
 message EgmSensor
 {
     optional EgmHeader		header = 1;
     optional EgmPlanned		planned = 2;
     optional EgmSpeedRef	speedRef = 3;
 }
 
-// Robot controller inbound message, sent from sensor
+// Robot controller inbound message, sent from sensor during path correction
 message EgmSensorPathCorr
 {
     optional EgmHeader		header = 1;

proto/egm_wrapper.proto

@@ -61,6 +61,13 @@ message Status
   optional RAPIDExecutionState rapid_execution_state = 4 [default = RAPID_UNDEFINED];
 }
 
+// Note: Time in seconds and microseconds since the epoch (1 Jan 1970)
+message Clock
+{
+    optional uint64 sec  = 1;
+    optional uint64 usec = 2;
+}
+
 //===========================================================
 //
 // Joint space related messages.
@@ -167,13 +174,15 @@ message Feedback
 {  
   optional Robot    robot    = 1;
   optional External external = 2;
+  optional Clock    time     = 3;
 }
 
 // Note: The velocity sub fields are estimated from the actual EGM messages.
 message Planned
 {
   optional Robot    robot    = 1;
   optional External external = 2;
+  optional Clock    time     = 3;
 }
 
 //======================================================================================================================
@@ -196,4 +205,4 @@ message Output
 {
   optional Robot    robot    = 1; // Outputs to the robot.
   optional External external = 2; // Outputs to external axes.
-}
+}

src/egm_base_interface.cpp

@@ -125,13 +125,22 @@ double EGMBaseInterface::InputContainer::estimateSampleTime()
 {
   double estimate = 0.0;
 
-  if (current_.has_header() && previous_.has_header())
-  {
-    double temp = (double) (current_.header().time_stamp() - previous_.header().time_stamp());
-    estimate = temp*Constants::Conversion::MS_TO_S;
+  if (current_.has_feedback() && previous_.has_feedback() &&
+      current_.feedback().has_time() && previous_.feedback().has_time() &&
+      current_.feedback().time().has_sec() && previous_.feedback().time().has_sec() &&
+      current_.feedback().time().has_usec() && previous_.feedback().time().has_usec())
+  {
+    google::protobuf::uint64 diff_s = (current_.feedback().time().sec() - previous_.feedback().time().sec());
+    google::protobuf::uint64 diff_us = (current_.feedback().time().usec() - previous_.feedback().time().usec());
+    if (diff_s > 0)
+    {
+      diff_us += diff_s*((google::protobuf::uint64) Constants::Conversion::S_TO_US);
+    }
+
+    estimate = std::floor(((double) diff_us) * Constants::Conversion::MS_TO_S) * Constants::Conversion::MS_TO_S;
   }
-  
-  if (estimate <= 0.0)
+
+  if (estimate < Constants::RobotController::LOWEST_SAMPLE_TIME)
   {
     estimate = Constants::RobotController::LOWEST_SAMPLE_TIME;
   }

src/egm_common.cpp

@@ -61,6 +61,7 @@ const double Constants::Conversion::RAD_TO_DEG = 180.0 / M_PI;
 const double Constants::Conversion::DEG_TO_RAD = M_PI / 180.0;
 const double Constants::Conversion::MM_TO_M = 0.001;
 const double Constants::Conversion::MS_TO_S = 0.001;
+const double Constants::Conversion::S_TO_US = 1e6;
 
 } // end namespace egm
-} // end namespace abb
+} // end namespace abb

src/egm_common_auxiliary.cpp

@@ -691,6 +691,23 @@ bool parse(wrapper::Status* p_target, const EgmRobot& source)
   return success;
 }
 
+bool parse(wrapper::Clock* p_target, const EgmClock& source)
+{
+  bool success = true;
+
+  if (p_target && source.has_sec() && source.has_usec())
+  {
+    p_target->set_sec(source.sec());
+    p_target->set_usec(source.usec());
+  }
+  else
+  {
+    success = false;
+  }
+
+  return success;
+}
+
 bool parse(wrapper::Joints* p_target_robot,
            wrapper::Joints* p_target_external,
            const EgmJoints& source_robot,
@@ -712,12 +729,12 @@ bool parse(wrapper::Joints* p_target_robot,
         {
           for (int i = 0; i < source_robot.joints_size(); ++i)
           {
-            p_target_robot->add_values(source_robot.joints(i) * Constants::Conversion::RAD_TO_DEG);
+            p_target_robot->add_values(source_robot.joints(i));
           }
 
           for (int i = 0; i < source_external.joints_size(); ++i)
           {
-            p_target_external->add_values(source_external.joints(i) * Constants::Conversion::RAD_TO_DEG);
+            p_target_external->add_values(source_external.joints(i));
           }
 
           success = true;
@@ -731,17 +748,17 @@ bool parse(wrapper::Joints* p_target_robot,
         if (source_robot.joints_size() == Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS &&
             source_external.joints_size() >= 1)
         {
-          p_target_robot->add_values(source_robot.joints(0) * Constants::Conversion::RAD_TO_DEG);
-          p_target_robot->add_values(source_robot.joints(1) * Constants::Conversion::RAD_TO_DEG);
-          p_target_robot->add_values(source_external.joints(0) * Constants::Conversion::RAD_TO_DEG);
-          p_target_robot->add_values(source_robot.joints(2) * Constants::Conversion::RAD_TO_DEG);
-          p_target_robot->add_values(source_robot.joints(3) * Constants::Conversion::RAD_TO_DEG);
-          p_target_robot->add_values(source_robot.joints(4) * Constants::Conversion::RAD_TO_DEG);
-          p_target_robot->add_values(source_robot.joints(5) * Constants::Conversion::RAD_TO_DEG);
+          p_target_robot->add_values(source_robot.joints(0));
+          p_target_robot->add_values(source_robot.joints(1));
+          p_target_robot->add_values(source_external.joints(0));
+          p_target_robot->add_values(source_robot.joints(2));
+          p_target_robot->add_values(source_robot.joints(3));
+          p_target_robot->add_values(source_robot.joints(4));
+          p_target_robot->add_values(source_robot.joints(5));
 
           for (int i = 1; i < source_external.joints_size(); ++i)
           {
-            p_target_external->add_values(source_external.joints(i) * Constants::Conversion::RAD_TO_DEG);
+            p_target_external->add_values(source_external.joints(i));
           }
 
           success = true;
@@ -821,6 +838,11 @@ bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAx
     if (success)
     {
       success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());
+
+      if (success)
+      {
+        success = parse(p_target->mutable_time(), source.time());
+      }
     }
   }
 
@@ -840,8 +862,14 @@ bool parse(wrapper::Planned* p_target, const EgmPlanned& source, const RobotAxes
     if (success)
     {
       success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());
+
+      if (success)
+      {
+        success = parse(p_target->mutable_time(), source.time());
+      }
     }
   }
+
   return success;
 }
 

src/egm_interpolator.cpp

@@ -143,7 +143,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition
     //       E.g. for stopping use K = 0.0.
     //
     //---------------------
-    // Conditons:
+    // Conditions:
     //---------------------
     // 0 < t <= T
     //
@@ -171,7 +171,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition
         // S(t) = A + B*t
         //
         //---------------------
-        // Conditons:
+        // Conditions:
         //---------------------
         // 0 <= t <= T
         //
@@ -194,7 +194,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition
         // S(t) = A + B*t + C*t^2
         //
         //---------------------
-        // Conditons:
+        // Conditions:
         //---------------------
         // 0 <= t <= T
         //
@@ -219,7 +219,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition
         // S(t) = A + B*t + C*t^2 + D*t^3
         //
         //---------------------
-        // Conditons:
+        // Conditions:
         //---------------------
         // 0 <= t <= T
         //
@@ -249,7 +249,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition
         // S(t) = A + B*t + C*t^2 + D*t^3 + E*t^4 + F*t^5
         //
         //---------------------
-        // Conditons:
+        // Conditions:
         //---------------------
         // 0 <= t <= T
         //