Merge pull request #172 from Yaskawa-Global/debug_tf

Correct `/tf` calculation for base tracks

Author: gavanderhoorn

src/MathConstants.h

@@ -16,7 +16,10 @@
 #define DEGREES_PER_RAD (57.295779513082)
 
 // macro
-#define NANOMETERS_TO_METERS(x)    (x * 0.000001)
+#define MICROMETERS_TO_METERS(x)    (x * 0.000001)
+#define METERS_TO_MICROMETERS(x)    (x * 1000000)
+#define RAD_TO_DEG_0001(x)          (x * DEGREES_PER_RAD * 10000)
+#define DEG_0001_TO_RAD(x)          ((x / 10000) * RAD_PER_DEGREE)
 
 // used for comparing floating point values
 #define EPSILON_TOLERANCE_DOUBLE (0.001)

src/MotoROS.h

@@ -50,6 +50,7 @@
 //============================================
 #include <std_srvs/srv/trigger.h>
 #include <sensor_msgs/msg/joint_state.h>
+#include <geometry_msgs/msg/pose.h>
 #include <geometry_msgs/msg/transform_stamped.h>
 #include <geometry_msgs/msg/quaternion.h>
 #include <tf2_msgs/msg/tf_message.h>
@@ -98,11 +99,14 @@
 #include "RosApiNameConstants.h"
 #include "TimeConversionUtils.h"
 #include "Tests_CtrlGroup.h"
+#include "Tests_TestUtils.h"
+#include "Tests_RosMotoPlusConversionUtils.h"
 #include "FauxCommandLineArgs.h"
 #include "InformCheckerAndGenerator.h"
 #include "MathConstants.h"
 #include "MotoROS_PlatformLib.h"
 #include "Ros_mpGetRobotCalibrationData.h"
+#include "RosMotoPlusConversionUtils.h"
 
 extern void Ros_Sleep(float milliseconds);
 

src/MotoROS2_AllControllers.vcxproj

@@ -344,6 +344,8 @@
     <ClCompile Include="ServiceStartTrajMode.c" />
     <ClCompile Include="ServiceSelectMotionTool.c" />
     <ClCompile Include="Tests_CtrlGroup.c" />
+    <ClCompile Include="Tests_TestUtils.c" />
+    <ClCompile Include="Tests_RosMotoPlusConversionUtils.c" />
     <ClCompile Include="MotionControl.c" />
     <ClCompile Include="ActionServer_FJT.c" />
     <ClCompile Include="main.c" />
@@ -352,6 +354,7 @@
     <ClCompile Include="ServiceResetError.c" />
     <ClCompile Include="FauxCommandLineArgs.c" />
     <ClCompile Include="Ros_mpGetRobotCalibrationData.c" />
+    <ClCompile Include="RosMotoPlusConversionUtils.c" />
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="ConfigFile.h" />
@@ -373,6 +376,8 @@
     <ClInclude Include="ServiceStartTrajMode.h" />
     <ClInclude Include="ServiceSelectMotionTool.h" />
     <ClInclude Include="Tests_CtrlGroup.h" />
+    <ClInclude Include="Tests_TestUtils.h" />
+    <ClInclude Include="Tests_RosMotoPlusConversionUtils.h" />
     <ClInclude Include="TimeConversionUtils.h" />
     <ClInclude Include="MotionControl.h" />
     <ClInclude Include="ActionServer_FJT.h" />
@@ -386,6 +391,7 @@
     <ClInclude Include="MathConstants.h" />
     <ClInclude Include="MotoROS_PlatformLib.h" />
     <ClInclude Include="Ros_mpGetRobotCalibrationData.h" />
+    <ClInclude Include="RosMotoPlusConversionUtils.h" />
   </ItemGroup>
   <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
   <ImportGroup Label="ExtensionTargets">

src/MotoROS2_AllControllers.vcxproj.filters

@@ -306,6 +306,12 @@
     <ClCompile Include="Tests_CtrlGroup.c">
       <Filter>Source Files\Tests</Filter>
     </ClCompile>
+    <ClCompile Include="Tests_TestUtils.c">
+      <Filter>Source Files\Tests</Filter>
+    </ClCompile>
+    <ClCompile Include="Tests_RosMotoPlusConversionUtils.c">
+      <Filter>Source Files\Tests</Filter>
+    </ClCompile>
     <ClCompile Include="InformCheckerAndGenerator.c">
       <Filter>Source Files\Robot Controller</Filter>
     </ClCompile>
@@ -321,6 +327,9 @@
     <ClCompile Include="Ros_mpGetRobotCalibrationData.c">
       <Filter>Source Files\Robot Controller</Filter>
     </ClCompile>
+    <ClCompile Include="RosMotoPlusConversionUtils.c">
+      <Filter>Source Files\Utilities</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="MotoROS.h">
@@ -338,6 +347,12 @@
     <ClInclude Include="Tests_CtrlGroup.h">
       <Filter>Header Files\Tests</Filter>
     </ClInclude>
+    <ClInclude Include="Tests_TestUtils.h">
+      <Filter>Header Files\Tests</Filter>
+    </ClInclude>
+    <ClInclude Include="Tests_RosMotoPlusConversionUtils.h">
+      <Filter>Header Files\Tests</Filter>
+    </ClInclude>
     <ClInclude Include="ConfigFile.h">
       <Filter>Header Files\Parameters and Config</Filter>
     </ClInclude>
@@ -401,6 +416,9 @@
     <ClInclude Include="TimeConversionUtils.h">
       <Filter>Header Files\Utilities</Filter>
     </ClInclude>
+    <ClInclude Include="RosMotoPlusConversionUtils.h">
+      <Filter>Header Files\Utilities</Filter>
+    </ClInclude>
     <ClInclude Include="MotoPlusExterns.h">
       <Filter>Header Files\Robot Controller</Filter>
     </ClInclude>

src/ParameterExtraction.dnLib

@@ -295,12 +295,12 @@ void Ros_PositionMonitor_CalculateTransforms(int groupIndex, long* pulsePos_moto
         {
             switch (group->baseTrackInfo.motionType[i])
             {
-            case MOTION_TYPE_X: coordTrackTravel.x = track_pos_meters[i]; break;
-            case MOTION_TYPE_Y: coordTrackTravel.y = track_pos_meters[i]; break;
-            case MOTION_TYPE_Z: coordTrackTravel.z = track_pos_meters[i]; break;
-            case MOTION_TYPE_RX: coordTrackTravel.rx = track_pos_meters[i]; break;
-            case MOTION_TYPE_RY: coordTrackTravel.ry = track_pos_meters[i]; break;
-            case MOTION_TYPE_RZ: coordTrackTravel.rz = track_pos_meters[i]; break;
+            case MOTION_TYPE_X: coordTrackTravel.x = METERS_TO_MICROMETERS(track_pos_meters[i]); break;
+            case MOTION_TYPE_Y: coordTrackTravel.y = METERS_TO_MICROMETERS(track_pos_meters[i]); break;
+            case MOTION_TYPE_Z: coordTrackTravel.z = METERS_TO_MICROMETERS(track_pos_meters[i]); break;
+            case MOTION_TYPE_RX: coordTrackTravel.rx = RAD_TO_DEG_0001(track_pos_meters[i]); break;
+            case MOTION_TYPE_RY: coordTrackTravel.ry = RAD_TO_DEG_0001(track_pos_meters[i]); break;
+            case MOTION_TYPE_RZ: coordTrackTravel.rz = RAD_TO_DEG_0001(track_pos_meters[i]); break;
             }
         }
         mpZYXeulerToFrame(&coordWorldToBase, &frameWorldToTrack);
@@ -314,10 +314,7 @@ void Ros_PositionMonitor_CalculateTransforms(int groupIndex, long* pulsePos_moto
     }
 
     geometry_msgs__msg__Transform* transform = &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * NUMBER_TRANSFORM_LINKS_PER_ROBOT) + tfLink_WorldToBase].transform;
-    transform->translation.x = NANOMETERS_TO_METERS(coordWorldToBase.x);
-    transform->translation.y = NANOMETERS_TO_METERS(coordWorldToBase.y);
-    transform->translation.z = NANOMETERS_TO_METERS(coordWorldToBase.z);
-    QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(coordWorldToBase.rx, coordWorldToBase.ry, coordWorldToBase.rz, &transform->rotation);
+    Ros_MpCoord_To_GeomMsgsTransform(&coordWorldToBase, transform);
 
     //============================
     // Calculate Flange and Tool0
@@ -374,22 +371,13 @@ void Ros_PositionMonitor_CalculateTransforms(int groupIndex, long* pulsePos_moto
     //=======================
 
     transform = &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * NUMBER_TRANSFORM_LINKS_PER_ROBOT) + tfLink_BaseToFlange].transform;
-    transform->translation.x = NANOMETERS_TO_METERS(coordBaseToFlange.x);
-    transform->translation.y = NANOMETERS_TO_METERS(coordBaseToFlange.y);
-    transform->translation.z = NANOMETERS_TO_METERS(coordBaseToFlange.z);
-    QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(coordBaseToFlange.rx, coordBaseToFlange.ry, coordBaseToFlange.rz, &transform->rotation);
+    Ros_MpCoord_To_GeomMsgsTransform(&coordBaseToFlange, transform);
 
     transform = &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * NUMBER_TRANSFORM_LINKS_PER_ROBOT) + tfLink_FlangeToTool0].transform;
-    transform->translation.x = NANOMETERS_TO_METERS(coordFlangeToTool0.x);
-    transform->translation.y = NANOMETERS_TO_METERS(coordFlangeToTool0.y);
-    transform->translation.z = NANOMETERS_TO_METERS(coordFlangeToTool0.z);
-    QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(coordFlangeToTool0.rx, coordFlangeToTool0.ry, coordFlangeToTool0.rz, &transform->rotation);
+    Ros_MpCoord_To_GeomMsgsTransform(&coordFlangeToTool0, transform);
 
     transform = &g_messages_PositionMonitor.transform->transforms.data[(groupIndex * NUMBER_TRANSFORM_LINKS_PER_ROBOT) + tfLink_FlangeToTcp].transform;
-    transform->translation.x = NANOMETERS_TO_METERS(coordToolData.x);
-    transform->translation.y = NANOMETERS_TO_METERS(coordToolData.y);
-    transform->translation.z = NANOMETERS_TO_METERS(coordToolData.z);
-    QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(coordToolData.rx, coordToolData.ry, coordToolData.rz, &transform->rotation);
+    Ros_MpCoord_To_GeomMsgsTransform(&coordToolData, transform);
 }
 
 void Ros_PositionMonitor_UpdateLocation()

src/ParameterExtraction.yrcLib

@@ -0,0 +1,27 @@
+//RosMotoPlusConversionUtils.c
+
+// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc.
+// SPDX-FileCopyrightText: 2023, Delft University of Technology
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#include "MotoROS.h"
+
+
+void Ros_MpCoord_To_GeomMsgsPose(MP_COORD const* const mp_coord, geometry_msgs__msg__Pose* const ros_pose)
+{
+    ros_pose->position.x = MICROMETERS_TO_METERS(mp_coord->x);
+    ros_pose->position.y = MICROMETERS_TO_METERS(mp_coord->y);
+    ros_pose->position.z = MICROMETERS_TO_METERS(mp_coord->z);
+    QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(
+        mp_coord->rx, mp_coord->ry, mp_coord->rz, &ros_pose->orientation);
+}
+
+void Ros_MpCoord_To_GeomMsgsTransform(MP_COORD const* const mp_coord, geometry_msgs__msg__Transform* const ros_transform)
+{
+    ros_transform->translation.x = MICROMETERS_TO_METERS(mp_coord->x);
+    ros_transform->translation.y = MICROMETERS_TO_METERS(mp_coord->y);
+    ros_transform->translation.z = MICROMETERS_TO_METERS(mp_coord->z);
+    QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion(
+        mp_coord->rx, mp_coord->ry, mp_coord->rz, &ros_transform->rotation);
+}

src/ParameterExtraction.yrcmLib

@@ -0,0 +1,42 @@
+//RosMotoPlusConversionUtils.h
+
+// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc.
+// SPDX-FileCopyrightText: 2023, Delft University of Technology
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#ifndef MOTOROS2_ROS_MOTOPLUS_CONVERSION_UTILS_H
+#define MOTOROS2_ROS_MOTOPLUS_CONVERSION_UTILS_H
+
+
+/**
+ * Converts an M+ `MP_COORD` struct to a ROS `geometry_msgs/msg/Pose`.
+ *
+ * Unit conversion is also performed by this function:
+ *
+ *  - `MP_COORD` positions are converted from micro-meters to meters
+ *  - `MP_COORD` orientations are converted from milli-degrees to radians (and
+ *    subsequently to a `Quaternion`)
+ *
+ * @param mp_coord  Input M+ `MP_COORD` instance
+ * @param ros_pose  Output `geometry_msgs/msg/Pose`
+ */
+extern void Ros_MpCoord_To_GeomMsgsPose(MP_COORD const* const mp_coord, geometry_msgs__msg__Pose* const ros_pose);
+
+
+/**
+ * Converts an M+ `MP_COORD` struct to a ROS `geometry_msgs/msg/Transform`.
+ *
+ * Unit conversion is also performed by this function:
+ *
+ *  - `MP_COORD` positions are converted from micro-meters to meters
+ *  - `MP_COORD` orientations are converted from milli-degrees to radians (and
+ *    subsequently to a `Quaternion`)
+ *
+ * @param mp_coord       Input M+ `MP_COORD` instance
+ * @param ros_transform  Output `geometry_msgs/msg/Pose`
+ */
+extern void Ros_MpCoord_To_GeomMsgsTransform(MP_COORD const* const mp_coord, geometry_msgs__msg__Transform* const ros_transform);
+
+
+#endif  // MOTOROS2_ROS_MOTOPLUS_CONVERSION_UTILS_H

src/PositionMonitor.c

@@ -9,27 +9,6 @@
 
 #include "MotoROS.h"
 
-BOOL Ros_Testing_CompareDouble(double a, double b)
-{
-    BOOL ok = fabs(a - b) < EPSILON_TOLERANCE_DOUBLE; //this approach has potential flaws, but it works just fine for these simple tests
-
-    if (!ok)
-        Ros_Debug_BroadcastMsg("Fail: %.04f != %.04f", a, b);
-
-    return ok;
-}
-
-// This is necessary because rounding errors can cause the converted positions to be wrong by a pulse-count or two
-BOOL Ros_Testing_CompareLong(long a, long b)
-{
-    BOOL ok = fabs(a - b) < EPSILON_TOLERANCE_PULSECOUNT; //this approach has potential flaws, but it works just fine for these simple tests
-
-    if (!ok)
-        Ros_Debug_BroadcastMsg("Fail: %d != %d", a, b);
-
-    return ok;
-}
-
 void Ros_Testing_CtrlGroup_MakeFake6dofRobot(CtrlGroup* group)
 {
     bzero(group, sizeof(CtrlGroup));

src/RosMotoPlusConversionUtils.c

@@ -0,0 +1,95 @@
+// Tests_RosMotoPlusConversionUtils.c
+
+// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc.
+// SPDX-FileCopyrightText: 2023, Delft University of Technology
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#ifdef MOTOROS2_TESTING_ENABLE
+
+#include "MotoROS.h"
+
+
+BOOL Ros_Testing_Ros_MpCoord_To_GeomMsgsPose()
+{
+    BOOL bSuccess = TRUE;
+    MP_COORD mp_coord_ = { 0 };
+    geometry_msgs__msg__Pose ros_pose_ = { { 0 } };
+
+    mp_coord_.x  = 1e6;
+    mp_coord_.y  = 2e6;
+    mp_coord_.z  = 3e6;
+    mp_coord_.rx =   4;
+    mp_coord_.ry =   5;
+    mp_coord_.rz =   6;
+
+    Ros_MpCoord_To_GeomMsgsPose(&mp_coord_, &ros_pose_);
+
+    // position is in micro-meters originally, so the ROS type should have it
+    // in meters
+    bSuccess &= Ros_Testing_CompareDouble(1.0, ros_pose_.position.x);
+    bSuccess &= Ros_Testing_CompareDouble(2.0, ros_pose_.position.y);
+    bSuccess &= Ros_Testing_CompareDouble(3.0, ros_pose_.position.z);
+
+    // orientation is a Quaternion (and 'in' radians)
+    // use the inverse (quaternion -> MP_COORD) and compare to the original
+    LONG rx_ = 0, ry_ = 0, rz_ = 0;
+    QuatConversion_GeomMsgsQuaternion_To_MpCoordOrient(
+        &ros_pose_.orientation, &rx_, &ry_, &rz_);
+
+    // NOTE: this is not a strict equals test right now (uses non-zero epsilon)
+    bSuccess &= Ros_Testing_CompareLong(mp_coord_.rx, rx_);
+    bSuccess &= Ros_Testing_CompareLong(mp_coord_.ry, ry_);
+    bSuccess &= Ros_Testing_CompareLong(mp_coord_.rz, rz_);
+
+    Ros_Debug_BroadcastMsg("Testing Ros_MpCoord_To_GeomMsgsPose: %s", bSuccess ? "PASS" : "FAIL");
+    return bSuccess;
+}
+
+BOOL Ros_Testing_Ros_MpCoord_To_GeomMsgsTransform()
+{
+    BOOL bSuccess = TRUE;
+    MP_COORD mp_coord_ = { 0 };
+    geometry_msgs__msg__Transform ros_tf_ = { { 0 } };
+
+    mp_coord_.x  = 4e6;
+    mp_coord_.y  = 5e6;
+    mp_coord_.z  = 6e6;
+    mp_coord_.rx =   1;
+    mp_coord_.ry =   2;
+    mp_coord_.rz =   3;
+
+    Ros_MpCoord_To_GeomMsgsTransform(&mp_coord_, &ros_tf_);
+
+    // translation is in micro-meters originally, so the ROS type should have it
+    // in meters
+    bSuccess &= Ros_Testing_CompareDouble(4.0, ros_tf_.translation.x);
+    bSuccess &= Ros_Testing_CompareDouble(5.0, ros_tf_.translation.y);
+    bSuccess &= Ros_Testing_CompareDouble(6.0, ros_tf_.translation.z);
+
+    // rotation is a Quaternion (and 'in' radians)
+    // use the inverse (quaternion -> MP_COORD) and compare to the original
+    LONG rx_ = 0, ry_ = 0, rz_ = 0;
+    QuatConversion_GeomMsgsQuaternion_To_MpCoordOrient(
+        &ros_tf_.rotation, &rx_, &ry_, &rz_);
+
+    // NOTE: this is not a strict equals test right now (uses non-zero epsilon)
+    bSuccess &= Ros_Testing_CompareLong(mp_coord_.rx, rx_);
+    bSuccess &= Ros_Testing_CompareLong(mp_coord_.ry, ry_);
+    bSuccess &= Ros_Testing_CompareLong(mp_coord_.rz, rz_);
+
+    Ros_Debug_BroadcastMsg("Testing Ros_MpCoord_To_GeomMsgsTransform: %s", bSuccess ? "PASS" : "FAIL");
+    return bSuccess;
+}
+
+BOOL Ros_Testing_RosMotoPlusConversionUtils()
+{
+    BOOL bSuccess = TRUE;
+
+    bSuccess &= Ros_Testing_Ros_MpCoord_To_GeomMsgsPose();
+    bSuccess &= Ros_Testing_Ros_MpCoord_To_GeomMsgsTransform();
+
+    return bSuccess;
+}
+
+#endif //MOTOROS2_TESTING_ENABLE

src/RosMotoPlusConversionUtils.h

@@ -0,0 +1,17 @@
+// Tests_RosMotoPlusConversionUtils.h
+
+// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc.
+// SPDX-FileCopyrightText: 2023, Delft University of Technology
+//
+// SPDX-License-Identifier: Apache-2.0
+
+#ifndef MOTOROS2_TESTS_ROS_MOTO_PLUS_CONVERSION_UTILS_H
+#define MOTOROS2_TESTS_ROS_MOTO_PLUS_CONVERSION_UTILS_H
+
+#ifdef MOTOROS2_TESTING_ENABLE
+
+extern BOOL Ros_Testing_RosMotoPlusConversionUtils();
+
+#endif //MOTOROS2_TESTING_ENABLE
+
+#endif  // MOTOROS2_TESTS_ROS_MOTO_PLUS_CONVERSION_UTILS_H