Add remapping back to iron

doc/troubleshooting.md

@@ -613,7 +613,7 @@ ALARM 8011
 [xx]
 ```
 
-Where `[xx]` is a subcode in the ranges `[23 - 54]` or `[56 - 58]`.
+Where `[xx]` is a subcode in the ranges `[23 - 54]`, `[56 - 58]`, or `[64]`.
 
 *Solution:*
 These alarms are often caused by version incompatibilities between ROS 2 (on the client PC), micro-ROS (as part of MotoROS2) and/or the micro-ROS Agent.

src/CommunicationExecutor.c

@@ -143,7 +143,6 @@ void Ros_Communication_Initialize()
     motoRosAssert(ret == RCL_RET_OK, SUBCODE_FAIL_SUPPORT_INIT);
 
     rcl_node_options_t node_options = rcl_node_get_default_options();
-#ifndef MOTOPLUS_LIBMICROROS_ROS2_IS_IRON
     //construct a faux command line to pass to rcl_parse_arguments(..)
     char* faux_argv[MAX_REMAP_RULE_NUM];
     size_t faux_argc = 0;
@@ -172,7 +171,6 @@ void Ros_Communication_Initialize()
             node_options = rcl_node_get_default_options();
         }
     }
-#endif
 
     //init node with the remap rules and other options from the config file
     ret = rclc_node_init_with_options(
@@ -182,11 +180,13 @@ void Ros_Communication_Initialize()
     Ros_Debug_BroadcastMsg("rclc_node_init_with_options = %d", (int)ret);
     motoRosAssert(ret == RCL_RET_OK, SUBCODE_FAIL_NODE_INIT);
 
+#ifdef MOTOPLUS_LIBMICROROS_ROS2_IS_IRON
+    ret = rcl_node_type_cache_init(&g_microRosNodeInfo.node);
+    motoRosAssert(ret == RCL_RET_OK, SUBCODE_FAIL_NODE_TYPE_CACHE_INIT);
+#endif // MOTOPLUS_LIBMICROROS_ROS2_IS_IRON
     //we're done with it
     ret = rcl_node_options_fini(&node_options); RCL_UNUSED(ret);
-#ifndef MOTOPLUS_LIBMICROROS_ROS2_IS_IRON
     Ros_CleanupFauxArgv(faux_argv, faux_argc);
-#endif
 
     MOTOROS2_MEM_TRACE_REPORT(comm_exec_init);
 }
@@ -196,6 +196,12 @@ void Ros_Communication_Cleanup()
     MOTOROS2_MEM_TRACE_START(comm_exec_fini);
     rcl_ret_t ret;
 
+#ifdef MOTOPLUS_LIBMICROROS_ROS2_IS_IRON
+    ret = rcl_node_type_cache_fini(&g_microRosNodeInfo.node);
+    if (ret != RCL_RET_OK)
+        Ros_Debug_BroadcastMsg("Failed cleaning up node type cache: %d", ret);
+#endif
+
     Ros_Debug_BroadcastMsg("Cleanup node");
     ret = rcl_node_fini(&g_microRosNodeInfo.node);
     if (ret != RCL_RET_OK)

src/ErrorHandling.h

@@ -169,7 +169,7 @@ typedef enum
     SUBCODE_CONFIGURATION_FAIL_MP_NICDATA1,
     SUBCODE_FAIL_MP_NICDATA_INIT1,
     SUBCODE_FAIL_INVALID_BASE_TRACK_MOTION_TYPE,
-
+    SUBCODE_FAIL_NODE_TYPE_CACHE_INIT,
 } ALARM_ASSERTION_FAIL_SUBCODE; //8011
 
 typedef enum

src/FauxCommandLineArgs.c

@@ -9,8 +9,6 @@
 #include <string.h>
 #include <rcutils/strdup.h>
 
-#ifndef MOTOPLUS_LIBMICROROS_ROS2_IS_IRON
-
 //TODO: Once https://github.com/ros2/rcl/issues/998 is addressed, this would no longer
 //      be necessary. We could construct rcl_remap_t instances directly without having
 //      to construct a fake argv and go through rcl_parse_arguments(..).
@@ -109,4 +107,3 @@ void Ros_CleanupFauxArgv(char** arr, size_t arr_len)
     }
 }
 
-#endif  // MOTOPLUS_LIBMICROROS_ROS2_IS_IRON

src/MotoROS.h

@@ -23,7 +23,10 @@
 #include <rcl_action/rcl_action.h>
 #include <rcl/error_handling.h>
 #include <rcl/publisher.h>
-
+#ifdef MOTOPLUS_LIBMICROROS_ROS2_IS_IRON
+#include <rcl/arguments.h>
+#include <rcl/node_type_cache.h>
+#endif //MOTOPLUS_LIBMICROROS_ROS2_IS_IRON
 #include <rclc/rclc.h>
 #include <rclc/executor.h>
 #include <rclc_parameter/rclc_parameter.h>

src/MotoROS2_AllControllers.vcxproj

@@ -168,7 +168,7 @@
     <NMakeForcedIncludes />
     <OutDir>$(SolutionDir)bin\$(Configuration)\</OutDir>
     <IntDir />
-    <IncludePath>$(ProjectDir)..\libmicroros_dx200_iron\include;</IncludePath>
+    <IncludePath>$(ProjectDir)..\lib;$(ProjectDir)..\libmicroros_dx200_iron\include;</IncludePath>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='DX200_humble|Win32'">
     <NMakeBuildCommandLine>"$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "mr2_dx2_h" -b "$(SolutionDir)bin\$(Configuration)" -o build -i "$(MP_VS_Install)DX200\inc"</NMakeBuildCommandLine>
@@ -216,7 +216,7 @@
     <NMakeForcedIncludes />
     <OutDir>$(SolutionDir)bin\$(Configuration)\</OutDir>
     <IntDir />
-    <IncludePath>$(ProjectDir)..\libmicroros_yrc1000_iron\include;</IncludePath>
+    <IncludePath>$(ProjectDir)..\lib;$(ProjectDir)..\libmicroros_yrc1000_iron\include;</IncludePath>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='YRC1000_humble|Win32'">
     <NMakeBuildCommandLine>"$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "mr2_yrc1_h" -b "$(SolutionDir)bin\$(Configuration)" -o build -i "$(MP_VS_Install)YRC1000\inc"</NMakeBuildCommandLine>
@@ -264,7 +264,7 @@
     <NMakeForcedIncludes />
     <OutDir>$(SolutionDir)bin\$(Configuration)\</OutDir>
     <IntDir />
-    <IncludePath>$(ProjectDir)..\libmicroros_yrc1000u_iron\include;</IncludePath>
+    <IncludePath>$(ProjectDir)..\lib;$(ProjectDir)..\libmicroros_yrc1000u_iron\include;</IncludePath>
   </PropertyGroup>
   <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='YRC1000u_humble|Win32'">
     <NMakeBuildCommandLine>"$(MP_VS_Install)mpBuilder.exe" -c $(Configuration) -p "$(ProjectDir)\" -n "mr2_yrc1m_h" -b "$(SolutionDir)bin\$(Configuration)" -o build -i "$(MP_VS_Install)YRC1000u\inc"</NMakeBuildCommandLine>
@@ -404,18 +404,24 @@
     <None Include="..\robot_jobs\two_arms\IONAME.DAT" />
     <None Include="DX200_humbleCompilerArguments.mps" />
     <None Include="DX200_humbleLinkerArguments.mps" />
+    <None Include="DX200_ironCompilerArguments.mps" />
+    <None Include="DX200_ironLinkerArguments.mps" />
     <None Include="YRC1000u_foxyCompilerArguments.mps" />
     <None Include="YRC1000u_foxyLinkerArguments.mps" />
     <None Include="YRC1000u_galacticCompilerArguments.mps" />
     <None Include="YRC1000u_galacticLinkerArguments.mps" />
     <None Include="YRC1000u_humbleCompilerArguments.mps" />
     <None Include="YRC1000u_humbleLinkerArguments.mps" />
+    <None Include="YRC1000u_ironCompilerArguments.mps" />
+    <None Include="YRC1000u_ironLinkerArguments.mps" />
     <None Include="YRC1000_foxyCompilerArguments.mps" />
     <None Include="YRC1000_foxyLinkerArguments.mps" />
     <None Include="YRC1000_galacticCompilerArguments.mps" />
     <None Include="YRC1000_galacticLinkerArguments.mps" />
     <None Include="YRC1000_humbleCompilerArguments.mps" />
     <None Include="YRC1000_humbleLinkerArguments.mps" />
+    <None Include="YRC1000_ironCompilerArguments.mps" />
+    <None Include="YRC1000_ironLinkerArguments.mps" />
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="CommunicationExecutor.c" />
@@ -433,6 +439,7 @@
     <ClCompile Include="ServiceStopTrajMode.c" />
     <ClCompile Include="ServiceStartTrajMode.c" />
     <ClCompile Include="ServiceSelectMotionTool.c" />
+    <ClCompile Include="StringConversionUtilities.c" />
     <ClCompile Include="Tests_ActionServer_FJT.c" />
     <ClCompile Include="Tests_ControllerStatusIO.c" />
     <ClCompile Include="Tests_CtrlGroup.c" />
@@ -467,6 +474,7 @@
     <ClInclude Include="ServiceStopTrajMode.h" />
     <ClInclude Include="ServiceStartTrajMode.h" />
     <ClInclude Include="ServiceSelectMotionTool.h" />
+    <ClInclude Include="StringConversionUtils.h" />
     <ClInclude Include="Tests_ActionServer_FJT.h" />
     <ClInclude Include="Tests_ControllerStatusIO.h" />
     <ClInclude Include="Tests_CtrlGroup.h" />

src/MotoROS2_AllControllers.vcxproj.filters

@@ -247,6 +247,27 @@
     <None Include="DX200_galacticLinkerArguments.mps">
       <Filter>Compile Settings</Filter>
     </None>
+    <None Include="..\libmicroros_dx200_iron\libmicroros.dnLib_iron" />
+    <None Include="..\libmicroros_yrc1000u_iron\libmicroros.yrcmLib_iron" />
+    <None Include="..\libmicroros_yrc1000_iron\libmicroros.yrcLib_iron" />
+    <None Include="DX200_ironCompilerArguments.mps">
+      <Filter>Compile Settings</Filter>
+    </None>
+    <None Include="DX200_ironLinkerArguments.mps">
+      <Filter>Compile Settings</Filter>
+    </None>
+    <None Include="YRC1000_ironCompilerArguments.mps">
+      <Filter>Compile Settings</Filter>
+    </None>
+    <None Include="YRC1000_ironLinkerArguments.mps">
+      <Filter>Compile Settings</Filter>
+    </None>
+    <None Include="YRC1000u_ironCompilerArguments.mps">
+      <Filter>Compile Settings</Filter>
+    </None>
+    <None Include="YRC1000u_ironLinkerArguments.mps">
+      <Filter>Compile Settings</Filter>
+    </None>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="main.c">
@@ -333,6 +354,12 @@
     <ClCompile Include="RosMotoPlusConversionUtils.c">
       <Filter>Source Files\Utilities</Filter>
     </ClCompile>
+    <ClCompile Include="Tests_ActionServer_FJT.c">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="StringConversionUtilities.c">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="MotoROS.h">
@@ -425,9 +452,6 @@
     <ClInclude Include="RosMotoPlusConversionUtils.h">
       <Filter>Header Files\Utilities</Filter>
     </ClInclude>
-    <ClInclude Include="MotoPlusExterns.h">
-      <Filter>Header Files\Robot Controller</Filter>
-    </ClInclude>
     <ClInclude Include="InformCheckerAndGenerator.h">
       <Filter>Header Files\Robot Controller</Filter>
     </ClInclude>
@@ -443,5 +467,14 @@
     <ClInclude Include="Ros_mpGetRobotCalibrationData.h">
       <Filter>Header Files\Robot Controller</Filter>
     </ClInclude>
+    <ClInclude Include="..\lib\MotoPlusExterns.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="Tests_ActionServer_FJT.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="StringConversionUtils.h">
+      <Filter>Header Files\Utilities</Filter>
+    </ClInclude>
   </ItemGroup>
-</Project>
+</Project>

src/StringConversionUtilities.c

@@ -0,0 +1,79 @@
+#include "MotoROS.h"
+
+//Implementation taken from https://github.com/gcc-mirror/gcc/blob/master/libiberty/strtoll.c 
+INT64 strtoll(const char* nptr, char** endptr, int base)
+{
+    const char* s = nptr;
+    UINT64 acc;
+    int c;
+    UINT64 cutoff;
+    int neg = 0, any, cutlim;
+    /*
+     * Skip white space and pick up leading +/- sign if any.
+     * If base is 0, allow 0x for hex and 0 for octal, else
+     * assume decimal; if base is already 16, allow 0x.
+     */
+    do {
+        c = *s++;
+    } while (isspace(c));
+    if (c == '-') {
+        neg = 1;
+        c = *s++;
+    }
+    else if (c == '+')
+        c = *s++;
+    if ((base == 0 || base == 16) &&
+        c == '0' && (*s == 'x' || *s == 'X')) {
+        c = s[1];
+        s += 2;
+        base = 16;
+    }
+    if (base == 0)
+        base = c == '0' ? 8 : 10;
+    /*
+     * Compute the cutoff value between legal numbers and illegal
+     * numbers.  That is the largest legal value, divided by the
+     * base.  An input number that is greater than this value, if
+     * followed by a legal input character, is too big.  One that
+     * is equal to this value may be valid or not; the limit
+     * between valid and invalid numbers is then based on the last
+     * digit.  For instance, if the range for longs is
+     * [-2147483648..2147483647] and the input base is 10,
+     * cutoff will be set to 214748364 and cutlim to either
+     * 7 (neg==0) or 8 (neg==1), meaning that if we have accumulated
+     * a value > 214748364, or equal but the next digit is > 7 (or 8),
+     * the number is too big, and we will return a range error.
+     *
+     * Set any if any `digits' consumed; make it negative to indicate
+     * overflow.
+     */
+    cutoff = neg ? -(UINT64) 0: ULONG_LONG_MAX;
+    cutlim = cutoff % (UINT64)base;
+    cutoff /= (UINT64)base;
+    for (acc = 0, any = 0;; c = *s++) {
+        if (isdigit(c))
+            c -= '0';
+        else if (isalpha(c))
+            c -= isupper(c) ? 'A' - 10 : 'a' - 10;
+        else
+            break;
+        if (c >= base)
+            break;
+        if (any < 0 || acc > cutoff || (acc == cutoff && c > cutlim))
+            any = -1;
+        else {
+            any = 1;
+            acc *= base;
+            acc += c;
+        }
+    }
+    if (any < 0) {
+        acc = neg ? LONG_LONG_MIN : LONG_LONG_MAX;
+        errno = ERANGE;
+    }
+    else if (neg)
+        acc = -acc;
+    if (endptr != 0)
+        *endptr = (char*)(any ? s - 1 : nptr);
+    return (acc);
+}

src/StringConversionUtils.h

@@ -0,0 +1,7 @@
+#ifndef MOTOROS2_STRING_CONVERSION_UTILS_H
+#define MOTOROS2_STRING_CONVERSION_UTILS_H
+
+#include "MotoROS.h"
+
+INT64 strtoll(const char* s, char** endptr, int base);
+#endif // MOTOROS2_STRING_CONVERSION_UTILS_H