Introduce ROS<->M+ concept conversion functions

Only support MP_COORD -> geometry_msgs/{Pose,Transform} for now, but others can be added later.

Author: gavanderhoorn

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);
 

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">

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>

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);
+}

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