From 5236faf9d9d5e43c4f7749fd9a1db8d0318c5e60 Mon Sep 17 00:00:00 2001
From: gavanderhoorn <g.a.vanderhoorn@tudelft.nl>
Date: Fri, 6 Oct 2023 17:12:22 +0200
Subject: [PATCH] Introduce ROS<->M+ concept conversion functions

Only support MP_COORD -> geometry_msgs/{Pose,Transform} for now, but others can be added later.
---
 src/MotoROS.h                               |  2 +
 src/MotoROS2_AllControllers.vcxproj         |  2 +
 src/MotoROS2_AllControllers.vcxproj.filters |  6 +++
 src/RosMotoPlusConversionUtils.c            | 27 +++++++++++++
 src/RosMotoPlusConversionUtils.h            | 42 +++++++++++++++++++++
 5 files changed, 79 insertions(+)
 create mode 100644 src/RosMotoPlusConversionUtils.c
 create mode 100644 src/RosMotoPlusConversionUtils.h

diff --git a/src/MotoROS.h b/src/MotoROS.h
index 99691ea0..f9cd659d 100644
--- a/src/MotoROS.h
+++ b/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>
@@ -103,6 +104,7 @@
 #include "MathConstants.h"
 #include "MotoROS_PlatformLib.h"
 #include "Ros_mpGetRobotCalibrationData.h"
+#include "RosMotoPlusConversionUtils.h"
 
 extern void Ros_Sleep(float milliseconds);
 
diff --git a/src/MotoROS2_AllControllers.vcxproj b/src/MotoROS2_AllControllers.vcxproj
index 12bc3a9c..b3f365f2 100644
--- a/src/MotoROS2_AllControllers.vcxproj
+++ b/src/MotoROS2_AllControllers.vcxproj
@@ -352,6 +352,7 @@
     <ClCompile Include="ServiceResetError.c" />
     <ClCompile Include="FauxCommandLineArgs.c" />
     <ClCompile Include="Ros_mpGetRobotCalibrationData.c" />
+    <ClCompile Include="RosMotoPlusConversionUtils.c" />
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="ConfigFile.h" />
@@ -386,6 +387,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">
diff --git a/src/MotoROS2_AllControllers.vcxproj.filters b/src/MotoROS2_AllControllers.vcxproj.filters
index bb612e56..b87555e1 100644
--- a/src/MotoROS2_AllControllers.vcxproj.filters
+++ b/src/MotoROS2_AllControllers.vcxproj.filters
@@ -321,6 +321,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">
@@ -401,6 +404,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>
diff --git a/src/RosMotoPlusConversionUtils.c b/src/RosMotoPlusConversionUtils.c
new file mode 100644
index 00000000..2fd19371
--- /dev/null
+++ b/src/RosMotoPlusConversionUtils.c
@@ -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);
+}
diff --git a/src/RosMotoPlusConversionUtils.h b/src/RosMotoPlusConversionUtils.h
new file mode 100644
index 00000000..f8fdef40
--- /dev/null
+++ b/src/RosMotoPlusConversionUtils.h
@@ -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