Feature/image performance

NOTICE

@@ -3,3 +3,4 @@ Copyright 2016-2018 Esteve Fernandez ([email protected])
 
 This product includes software developed by
 Esteve Fernandez ([email protected])
+Stefan Hoffmann ([email protected])

rcldotnet/CMakeLists.txt

@@ -15,6 +15,8 @@ find_package(rmw REQUIRED)
 find_package(rmw_implementation REQUIRED)
 find_package(rmw_implementation_cmake REQUIRED)
 find_package(rosidl_generator_c REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 
 find_package(dotnet_cmake_module REQUIRED)
 find_package(DotNETExtra REQUIRED)
@@ -76,6 +78,8 @@ set(_assemblies_dep_dlls
   ${builtin_interfaces_ASSEMBLIES_DLL}
   ${rcldotnet_common_ASSEMBLIES_DLL}
   ${unique_identifier_msgs_ASSEMBLIES_DLL}
+  ${sensor_msgs_ASSEMBLIES_DLL}
+  ${std_msgs_ASSEMBLIES_DLL}
 )
 
 add_dotnet_library(${PROJECT_NAME}_assemblies
@@ -141,7 +145,7 @@ if(BUILD_TESTING)
   if(DEFINED ENV{RCLDOTNET_TEST_TARGET_FRAMEWORK})
     set(RCLDOTNET_TEST_TARGET_FRAMEWORK $ENV{RCLDOTNET_TEST_TARGET_FRAMEWORK})
   else()
-    set(RCLDOTNET_TEST_TARGET_FRAMEWORK "net6.0")
+    set(RCLDOTNET_TEST_TARGET_FRAMEWORK "net7.0")
   endif()
 
   add_dotnet_test(test_messages

rcldotnet/RCLdotnet.cs

@@ -18,6 +18,9 @@
 using action_msgs.msg;
 using action_msgs.srv;
 using ROS2.Utils;
+using sensor_msgs.msg;
+using System.Collections.Generic;
+using System.Diagnostics;
 
 namespace ROS2
 {
@@ -898,7 +901,21 @@ private static bool Take(Subscription subscription, IRosMessage message)
                 switch (ret)
                 {
                     case RCLRet.Ok:
-                        ReadFromMessageHandle(message, messageHandle);
+                        if (message is sensor_msgs.msg.Image)
+                        {
+                            HackedReadFromImageMessageHandle(subscription, message, messageHandle);
+
+                        }
+                        else if (message is sensor_msgs.msg.PointCloud2)
+                        {
+                            // Console.WriteLine("PointCloud2");
+                            HackedReadFromPointCloudMessageHandle(subscription, message, messageHandle);
+                        }
+                        else
+                        {
+                            ReadFromMessageHandle(message, messageHandle);
+                        }
+
                         return true;
 
                     case RCLRet.SubscriptionTakeFailed:
@@ -1485,6 +1502,217 @@ public static string GetRMWIdentifier()
             return rmw_identifier;
         }
 
+
+        // WORKAROUND IMAGES
+        // public static Dictionary<Subscription, sensor_msgs.msg.Image> lastImages =new Dictionary<Subscription, sensor_msgs.msg.Image>();
+
+        public static Dictionary<Subscription, sensor_msgs.msg.PointCloud2> pointCloudSubscription = new Dictionary<Subscription, sensor_msgs.msg.PointCloud2>();
+
+        public static Dictionary<Subscription, sensor_msgs.msg.Image> imageSubscription = new Dictionary<Subscription, sensor_msgs.msg.Image>();
+
+        public static Dictionary<sensor_msgs.msg.Image, byte[]> lastImagesBytes = new Dictionary<Image, byte[]>();
+
+        public static Dictionary<sensor_msgs.msg.PointCloud2, byte[]> lastPointCloudsBytes = new Dictionary<sensor_msgs.msg.PointCloud2, byte[]>();
+
+        public static object IMG_DATA_LOCK = new object();
+        public static object POINTCLOUD_DATA_LOCK = new object();
+
+
+        public static void GetLastPointCloudCopy(sensor_msgs.msg.PointCloud2 pointCloud2, ref byte[] pointCloudData)
+        {
+            lock (POINTCLOUD_DATA_LOCK)
+            {
+                if (lastPointCloudsBytes.ContainsKey(pointCloud2))
+                {
+                    var cloudData = lastPointCloudsBytes[pointCloud2];
+
+                    // cloudData = lastPointCloudsBytes[pointCloud2];
+                    if(pointCloudData == null || pointCloudData.Length != cloudData.Length)
+                    {
+                        pointCloudData = new byte[lastPointCloudsBytes[pointCloud2].Length];
+                    }
+
+                    Buffer.BlockCopy(cloudData, 0, pointCloudData, 0, cloudData.Length);
+                    // Marshal.Copy(lastPointCloudsBytes[pointCloud2], 0, pointCloudData, cloudData.Length);
+
+                }
+                else
+                {
+                    pointCloudData = null;
+                }   
+            }
+        }
+
+        // public static byte[] GetLastPointCloudBytes(sensor_msgs.msg.PointCloud2 pointCloud2)
+        // {
+        //     lock (POINTCLOUD_DATA_LOCK)
+        //     {
+        //         if (lastPointCloudsBytes.ContainsKey(pointCloud2))
+        //         {
+        //             return lastPointCloudsBytes[pointCloud2];
+        //         }   
+        //         return null;
+        //     }
+        // }
+
+        public static void HackedPointCloud__ReadFromHandle(Subscription subscription, global::System.IntPtr messageHandle, sensor_msgs.msg.PointCloud2 pointCloud2)
+        {
+            Debug.WriteLine("HackedPointCloud__ReadFromHandle");
+            var sw = System.Diagnostics.Stopwatch.StartNew();
+            lock(POINTCLOUD_DATA_LOCK)
+            {
+                pointCloud2.Header.__ReadFromHandle(sensor_msgs.msg.PointCloud2.native_get_field_header_HANDLE(messageHandle));
+                pointCloud2.Height = sensor_msgs.msg.PointCloud2.native_read_field_height(messageHandle);
+                pointCloud2.Width = sensor_msgs.msg.PointCloud2.native_read_field_width(messageHandle);
+                pointCloud2.PointStep = sensor_msgs.msg.PointCloud2.native_read_field_point_step(messageHandle);
+                pointCloud2.RowStep = sensor_msgs.msg.PointCloud2.native_read_field_row_step(messageHandle);
+                pointCloud2.IsBigendian = sensor_msgs.msg.PointCloud2.native_read_field_is_bigendian(messageHandle);
+                {
+                    int size = sensor_msgs.msg.PointCloud2.native_getsize_field_data_message(messageHandle);
+                    HackedPointCloudMemoryCopy(subscription, messageHandle, pointCloud2, size);
+                }
+            }
+        }
+
+        public static void HackedImage__ReadFromHandle(Subscription subscription, global::System.IntPtr messageHandle, sensor_msgs.msg.Image image)  
+        {
+            // Debug.WriteLine("HackedImage__ReadFromHandle");
+            // var sw = System.Diagnostics.Stopwatch.StartNew();
+
+            lock(IMG_DATA_LOCK)
+            {
+            image.Header.__ReadFromHandle(sensor_msgs.msg.Image.native_get_field_header_HANDLE(messageHandle));
+            image.Height = sensor_msgs.msg.Image.native_read_field_height(messageHandle);
+            image.Width = sensor_msgs.msg.Image.native_read_field_width(messageHandle);
+            IntPtr pStr_Encoding = sensor_msgs.msg.Image.native_read_field_encoding(messageHandle);
+            image.Encoding = Marshal.PtrToStringAnsi(pStr_Encoding);
+
+            image.IsBigendian = sensor_msgs.msg.Image.native_read_field_is_bigendian(messageHandle);
+            image.Step = sensor_msgs.msg.Image.native_read_field_step(messageHandle);
+                {
+                    int size__local_variable = sensor_msgs.msg.Image.native_getsize_field_data_message(messageHandle);
+
+                    HackedMemoryCopy(subscription, messageHandle, image, size__local_variable);
+
+                    // sw.Stop();
+
+                    // var sw2 = System.Diagnostics.Stopwatch.StartNew();
+
+                    // image.Data = new System.Collections.Generic.List<byte>(size__local_variable);
+                    // for (int i__local_variable = 0; i__local_variable < size__local_variable; i__local_variable++)
+                    // {
+                    //     image.Data.Add(sensor_msgs.msg.Image.native_read_field_data(sensor_msgs.msg.Image.native_get_field_data_message(messageHandle, i__local_variable)));
+                    // }
+
+                    // sw2.Stop();
+                    // Console.WriteLine($"copying image took: {sw.ElapsedTicks} ticks, {sw2.ElapsedTicks} ticks: {(float)(sw.ElapsedTicks - sw2.ElapsedTicks)/(float)sw.ElapsedTicks}");
+
+                }
+            }
+
+            // sw.Stop();
+            // Debug.WriteLine($"HackedImage__ReadFromHandle END: {sw.ElapsedMilliseconds} ms");
+        }
+
+        private static void HackedPointCloudMemoryCopy(Subscription subscription, IntPtr messageHandle, sensor_msgs.msg.PointCloud2 pointCloud2, int size__local_variable)
+        {
+            IntPtr first = sensor_msgs.msg.PointCloud2.native_get_field_data_message(messageHandle, 0);
+            byte[] bytes = null;
+            if (pointCloudSubscription.ContainsKey(subscription))
+            {
+                var lastPointCloud = pointCloudSubscription[subscription];
+                bytes = lastPointCloudsBytes[lastPointCloud];
+                lastPointCloudsBytes.Remove(lastPointCloud);
+            }
+
+            if (bytes == null || bytes.Length != size__local_variable)
+            {
+                bytes = new byte[size__local_variable];
+                lastPointCloudsBytes[pointCloud2] = bytes;
+            }
+
+            Marshal.Copy(first, bytes, 0, (int)(size__local_variable));
+            lastPointCloudsBytes[pointCloud2] = bytes;
+            pointCloudSubscription[subscription] = pointCloud2;
+        }
+
+        private static void HackedMemoryCopy(Subscription subscription, IntPtr messageHandle, sensor_msgs.msg.Image image, int size__local_variable)
+        {
+            IntPtr first = sensor_msgs.msg.Image.native_get_field_data_message(messageHandle, 0);
+
+            // lastImagesHandles[image] = new Tuple<IntPtr, uint>(first, (uint)size__local_variable);
+
+            byte[] bytes = null;
+            if (imageSubscription.ContainsKey(subscription))
+            {
+                var lastImage = imageSubscription[subscription];
+                bytes = lastImagesBytes[lastImage];
+                lastImagesBytes.Remove(lastImage);
+            }
+
+            if (bytes == null || bytes.Length != size__local_variable)
+            {
+                // Debug.WriteLine("HackedImage__ReadFromHandle: new byte buffer created");
+
+                bytes = new byte[size__local_variable];
+                // Console.WriteLine($"makmustReleaseing new size for image subscription: {size__local_variable} subscription: {subscription.GetHashCode()}");
+            }
+
+
+            Marshal.Copy(first,bytes, 0, (int)(size__local_variable));
+
+            lastImagesBytes[image] = bytes;
+            imageSubscription[subscription] = image;
+        }
+
+        internal static void HackedReadFromPointCloudMessageHandle(Subscription subscription, IRosMessage message, SafeHandle messageHandle)
+        {
+            bool mustRelease = false;
+            try
+            {
+                messageHandle.DangerousAddRef(ref mustRelease);
+                HackedPointCloud__ReadFromHandle(subscription, messageHandle.DangerousGetHandle(),message as sensor_msgs.msg.PointCloud2);
+            }
+            finally
+            {
+                if (mustRelease)
+                {
+                    messageHandle.DangerousRelease();
+                }
+            }
+        }
+
+
+
+        internal static void HackedReadFromImageMessageHandle(Subscription subscription, IRosMessage message, SafeHandle messageHandle)
+        {
+            bool mustRelease = false;
+            try
+            {
+                // Using SafeHandles for __ReadFromHandle() is very tedious as
+                // this needs to be handled in generated code across multiple
+                // assemblies. Array and collection indexing would need to
+                // create SafeHandles everywhere. It's not worth it, especially
+                // considering the extra allocations for SafeHandles in arrays
+                // or collections that don't really represent their own native
+                // resource.
+                messageHandle.DangerousAddRef(ref mustRelease);
+                HackedImage__ReadFromHandle(subscription, messageHandle.DangerousGetHandle(),message as sensor_msgs.msg.Image);
+                //message.__ReadFromHandle(messageHandle.DangerousGetHandle());
+            }
+            catch(Exception e)
+            {
+                // Debug.WriteLine("HackedImage__ReadFromHandle: exception");
+            }
+            finally
+            {
+                if (mustRelease)
+                {
+                    messageHandle.DangerousRelease();
+                }
+            }
+        }
+
         internal static void ReadFromMessageHandle(IRosMessage message, SafeHandle messageHandle)
         {
             bool mustRelease = false;

rcldotnet/SafeSubscriptionHandle.cs

@@ -28,7 +28,7 @@ namespace ROS2
     /// of the subscription handle and node handle. This also applies to publisher, service and client handles,
     /// which point to a rcl_publisher_t, rcl_service_t or rcl_client_t.
     /// </summary>
-    internal sealed class SafeSubscriptionHandle : SafeHandleZeroOrMinusOneIsInvalid
+    public sealed class SafeSubscriptionHandle : SafeHandleZeroOrMinusOneIsInvalid
     {
         // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366
         // Commit from early 2016, but still in current .NET as of september 2021:

rcldotnet/Subscription.cs

@@ -34,7 +34,7 @@ internal Subscription()
         // internal handle is disposed.
         // By relying on the GC/Finalizer of SafeHandle the handle only gets
         // Disposed if the subscription is not live anymore.
-        internal abstract SafeSubscriptionHandle Handle { get; }
+        public abstract SafeSubscriptionHandle Handle { get; }
 
         internal abstract IRosMessage CreateMessage();
 
@@ -54,7 +54,7 @@ internal Subscription(SafeSubscriptionHandle handle, Action<T> callback)
             _callback = callback;
         }
 
-        internal override SafeSubscriptionHandle Handle { get; }
+        public override SafeSubscriptionHandle Handle { get; }
 
         internal override IRosMessage CreateMessage() => (IRosMessage)new T();
 

rcldotnet/package.xml

@@ -16,6 +16,7 @@
   <exec_depend>rmw_implementation_cmake</exec_depend>
   <exec_depend>rosidl_generator_c</exec_depend>
   <exec_depend>rosidl_parser</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
 
   <build_depend>rmw_implementation_cmake</build_depend>
   <build_depend>rcl</build_depend>
@@ -25,13 +26,16 @@
   <build_depend>action_msgs</build_depend>
   <build_depend>builtin_interfaces</build_depend>
   <build_depend>unique_identifier_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
 
   <exec_depend>rcl</exec_depend>
   <exec_depend>rcl_interfaces</exec_depend>
   <exec_depend>rcl_action</exec_depend>
   <exec_depend>action_msgs</exec_depend>
   <exec_depend>builtin_interfaces</exec_depend>
   <exec_depend>unique_identifier_msgs</exec_depend>
+  <build_depend>unique_identifier_msgs</build_depend>
 
   <test_depend>std_msgs</test_depend>
   <test_depend>test_msgs</test_depend>

rcldotnet/rcldotnet.c

@@ -26,6 +26,7 @@
 #include "rcldotnet.h"
 
 static rcl_context_t context;
+static rcl_clock_t clock;
 
 int32_t native_rcl_init(int argc, const char *argv[]) {
   context = rcl_get_zero_initialized_context();
@@ -40,6 +41,7 @@ int32_t native_rcl_init(int argc, const char *argv[]) {
     return ret;
   }
 
+  ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
   return ret;
 }
 

rcldotnet/rcldotnet_node.c

@@ -223,7 +223,6 @@ int32_t native_rcl_action_create_server_handle(void **action_server_handle,
   rcl_action_server_options_t action_server_ops =
     rcl_action_server_get_default_options();
 
-
   rcl_ret_t ret =
     rcl_action_server_init(action_server, node, clock, ts, action_name, &action_server_ops);
 
@@ -258,4 +257,4 @@ const char * native_rcl_node_get_fully_qualified_name_handle(void *node_handle)
   rcl_node_t *node = (rcl_node_t *)node_handle;
 
   return rcl_node_get_fully_qualified_name(node);
-}
+}