Implemented parameter handling

rcldotnet/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(ament_cmake REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rcl_action REQUIRED)
 find_package(action_msgs REQUIRED)
+find_package(rosgraph_msgs REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(unique_identifier_msgs REQUIRED)
@@ -68,13 +69,23 @@ set(CS_SOURCES
   ServiceDefinitionStaticMemberCache.cs
   Subscription.cs
   Timer.cs
+  ParameterHandling/ParameterDelegates.cs
+  ParameterHandling/ParameterHandler.cs
+  ParameterHandling/SafeRclParamsHandle.cs
+  ParameterHandling/Exceptions/InvalidParameterTypeException.cs
+  ParameterHandling/Exceptions/ParameterException.cs
+  ParameterHandling/Exceptions/ParameterImmutableException.cs
+  ParameterHandling/Exceptions/ParameterNotDeclaredException.cs
+  ParameterHandling/Exceptions/ParameterTypeMismatchException.cs
 )
 
 find_package(rcldotnet_common REQUIRED)
 
 set(_assemblies_dep_dlls
   ${action_msgs_ASSEMBLIES_DLL}
+  ${rosgraph_msgs_ASSEMBLIES_DLL}
   ${builtin_interfaces_ASSEMBLIES_DLL}
+  ${rcl_interfaces_ASSEMBLIES_DLL}
   ${rcldotnet_common_ASSEMBLIES_DLL}
   ${unique_identifier_msgs_ASSEMBLIES_DLL}
 )
@@ -98,11 +109,13 @@ add_library(${PROJECT_NAME}_native SHARED
   rcldotnet_publisher.c
   rcldotnet_timer.c
   rcldotnet_qos_profile.c
+  rcldotnet_params.c
   rcldotnet.c
 )
 
 ament_target_dependencies(${PROJECT_NAME}_native
   "action_msgs"
+  "rosgraph_msgs"
   "builtin_interfaces"
   "unique_identifier_msgs"
   "rcl"

rcldotnet/Node.cs

@@ -16,6 +16,7 @@
 using System;
 using System.Collections.Generic;
 using System.Runtime.InteropServices;
+using rcl_interfaces.msg;
 using ROS2.Utils;
 
 namespace ROS2
@@ -127,6 +128,8 @@ static NodeDelegates()
 
     public sealed class Node
     {
+        private const string ParameterNameSimulatedTime = "use_sim_time";
+
         private readonly Clock _clock;
 
         private readonly IList<Subscription> _subscriptions;
@@ -143,6 +146,8 @@ public sealed class Node
 
         private readonly IList<Timer> _timers;
 
+        private readonly ParameterHandler _parameterHandler;
+
         internal Node(SafeNodeHandle handle)
         {
             Handle = handle;
@@ -156,6 +161,10 @@ internal Node(SafeNodeHandle handle)
             _actionClients = new List<ActionClient>();
             _actionServers = new List<ActionServer>();
             _timers = new List<Timer>();
+
+            _parameterHandler = new ParameterHandler(this);
+            _parameterHandler.AddOnSetParameterCallback(OnSetParameters);
+            _parameterHandler.DeclareParameter(ParameterNameSimulatedTime, false);
         }
 
         public string Name => RCLdotnet.GetStringFromNativeDelegate(NodeDelegates.native_rcl_node_get_name_handle, Handle);
@@ -190,6 +199,36 @@ internal Node(SafeNodeHandle handle)
         // Disposed if the node is not live anymore.
         internal SafeNodeHandle Handle { get; }
 
+        private Subscription<rosgraph_msgs.msg.Clock> ClockSubscription { get; set; }
+
+        private void OnSetParameters(List<Parameter> parameters)
+        {
+            Parameter simulatedTimeParameter = parameters.Find(parameter => parameter.Name == ParameterNameSimulatedTime);
+            if (simulatedTimeParameter == null) return;
+
+            // Update clock setup if applicable.
+            bool subscribedToClock = ClockSubscription != null;
+            bool useSimulatedTime = simulatedTimeParameter.Value.BoolValue;
+            if (useSimulatedTime == subscribedToClock) return;
+
+            if (useSimulatedTime)
+            {
+                ClockSubscription = CreateSubscription<rosgraph_msgs.msg.Clock>("/clock", OnClockMessage, QosProfile.ClockProfile);
+                _clock.EnableRosTimeOverride();
+            }
+            else
+            {
+                DestroySubscription(ClockSubscription);
+                ClockSubscription = null;
+                _clock.DisableRosTimeOverride();
+            }
+        }
+
+        private void OnClockMessage(rosgraph_msgs.msg.Clock message)
+        {
+            _clock.SetRosTimeOverride(TimePoint.FromMsg(message.Clock_).nanoseconds);
+        }
+
         public Publisher<T> CreatePublisher<T>(string topic, QosProfile qosProfile = null) where T : IRosMessage
         {
             if (qosProfile != null)
@@ -259,6 +298,15 @@ private Publisher<T> CreatePublisherInner<T>(string topic, SafeQosProfileHandle
             return subscription;
         }
 
+        internal bool DestroySubscription(Subscription subscription)
+        {
+            if (!_subscriptions.Contains(subscription)) return false;
+
+            _subscriptions.Remove(subscription);
+            subscription.Handle.Dispose();
+            return true;
+        }
+
         public Service<TService, TRequest, TResponse> CreateService<TService, TRequest, TResponse>(string serviceName, Action<TRequest, TResponse> callback)
             where TService : IRosServiceDefinition<TRequest, TResponse>
             where TRequest : IRosMessage, new()
@@ -414,5 +462,64 @@ private static CancelResponse DefaultCancelCallback(ActionServerGoalHandle goalH
         {
             return CancelResponse.Reject;
         }
+
+        #region Parameter Handling Passthroughs
+
+        public void DeclareParameter(string name, bool defaultValue = false, ParameterDescriptor descriptor = null) =>
+            _parameterHandler.DeclareParameter(name, defaultValue, descriptor);
+
+        public void DeclareParameter(string name, int defaultValue = 0, ParameterDescriptor descriptor = null) =>
+            _parameterHandler.DeclareParameter(name, defaultValue, descriptor);
+
+        public void DeclareParameter(string name, long defaultValue = 0L, ParameterDescriptor descriptor = null) =>
+            _parameterHandler.DeclareParameter(name, defaultValue, descriptor);
+
+        public void DeclareParameter(string name, float defaultValue = 0.0f, ParameterDescriptor descriptor = null) =>
+            _parameterHandler.DeclareParameter(name, defaultValue, descriptor);
+
+        public void DeclareParameter(string name, double defaultValue = 0.0, ParameterDescriptor descriptor = null) =>
+            _parameterHandler.DeclareParameter(name, defaultValue, descriptor);
+
+        public void DeclareParameter(string name, string defaultValue = "", ParameterDescriptor descriptor = null) =>
+            _parameterHandler.DeclareParameter(name, defaultValue, descriptor);
+
+        public void DeclareParameter(string name, IEnumerable<byte> defaultValue = null, ParameterDescriptor descriptor = null) =>
+            _parameterHandler.DeclareParameter(name, defaultValue, descriptor);
+
+        public void DeclareParameter(string name, IEnumerable<bool> defaultValue = null, ParameterDescriptor descriptor = null) =>
+            _parameterHandler.DeclareParameter(name, defaultValue, descriptor);
+
+        public void DeclareParameter(string name, IEnumerable<long> defaultValue = null, ParameterDescriptor descriptor = null) =>
+            _parameterHandler.DeclareParameter(name, defaultValue, descriptor);
+
+        public void DeclareParameter(string name, IEnumerable<double> defaultValue = null, ParameterDescriptor descriptor = null) =>
+            _parameterHandler.DeclareParameter(name, defaultValue, descriptor);
+
+        public void DeclareParameter(string name, IEnumerable<string> defaultValue = null, ParameterDescriptor descriptor = null) =>
+            _parameterHandler.DeclareParameter(name, defaultValue, descriptor);
+
+        public void UndeclareParameter(string name) => _parameterHandler.UndeclareParameter(name);
+
+        public List<ParameterValue> GetParameters(IEnumerable<string> names) => _parameterHandler.GetParameters(names);
+
+        public ParameterValue GetParameter(string name) => _parameterHandler.GetParameter(name);
+
+        public List<SetParametersResult> SetParameters(List<Parameter> parameters) =>
+            _parameterHandler.SetParameters(parameters);
+
+        public SetParametersResult SetParametersAtomically(List<Parameter> parameters) =>
+            _parameterHandler.SetParametersAtomically(parameters);
+
+        public SetParametersResult SetParameter(Parameter parameter) => _parameterHandler.SetParameter(parameter);
+
+        public bool HasParameter(string name) => _parameterHandler.HasParameter(name);
+
+        public void AddOnSetParameterCallback(Action<List<Parameter>> callback) =>
+            _parameterHandler.AddOnSetParameterCallback(callback);
+
+        public void RemoveOnSetParameterCallback(Action<List<Parameter>> callback) =>
+            _parameterHandler.RemoveOnSetParameterCallback(callback);
+
+        #endregion
     }
 }

rcldotnet/ParameterHandling/Exceptions/InvalidParameterTypeException.cs

@@ -0,0 +1,30 @@
+/* Copyright 2023 Queensland University of Technology.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using System;
+
+namespace ROS2.ParameterHandling.Exceptions
+{
+    public class InvalidParameterTypeException : ParameterException
+    {
+        public InvalidParameterTypeException(byte typeCode) : base($"TypeCode: \"{typeCode}\" is not a valid parameter type!")
+        {
+        }
+
+        public InvalidParameterTypeException(Type type) : base($"\"{type.Name}\" is not a valid parameter type!")
+        {
+        }
+    }
+}

rcldotnet/ParameterHandling/Exceptions/ParameterException.cs

@@ -0,0 +1,26 @@
+/* Copyright 2023 Queensland University of Technology.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using System;
+
+namespace ROS2.ParameterHandling.Exceptions
+{
+    public abstract class ParameterException : Exception
+    {
+        protected ParameterException(string message) : base(message)
+        {
+        }
+    }
+}

rcldotnet/ParameterHandling/Exceptions/ParameterImmutableException.cs

@@ -0,0 +1,24 @@
+/* Copyright 2023 Queensland University of Technology.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+namespace ROS2.ParameterHandling.Exceptions
+{
+    public class ParameterImmutableException : ParameterException
+    {
+        public ParameterImmutableException(string name) : base($"Parameter \"{name}\" was declared as read-only!")
+        {
+        }
+    }
+}

rcldotnet/ParameterHandling/Exceptions/ParameterNotDeclaredException.cs

@@ -0,0 +1,24 @@
+/* Copyright 2023 Queensland University of Technology.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+namespace ROS2.ParameterHandling.Exceptions
+{
+    public class ParameterNotDeclaredException : ParameterException
+    {
+        public ParameterNotDeclaredException(string name) : base($"Parameter with name \"{name}\" was not declared!")
+        {
+        }
+    }
+}

rcldotnet/ParameterHandling/Exceptions/ParameterTypeMismatchException.cs

@@ -0,0 +1,32 @@
+/* Copyright 2023 Queensland University of Technology.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using System;
+using rcl_interfaces.msg;
+
+namespace ROS2.ParameterHandling.Exceptions
+{
+    public class ParameterTypeMismatchException : ParameterException
+    {
+        public ParameterTypeMismatchException(string message) : base(message)
+        {
+        }
+
+        public ParameterTypeMismatchException(string name, Type parameterType, Type requestedType) : this(
+            $"Parameter with name \"{name}\" is of type \"{parameterType.Name}\" it cannot be set with a value of type \"{requestedType.Name}\"!")
+        {
+        }
+    }
+}

rcldotnet/ParameterHandling/ParameterDelegates.cs

@@ -0,0 +1,44 @@
+/* Copyright 2023 Queensland University of Technology.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using System;
+using System.Runtime.InteropServices;
+using ROS2.Utils;
+
+namespace ROS2.ParameterHandling {
+    internal static class ParameterDelegates
+    {
+        private static readonly DllLoadUtils _dllLoadUtils;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
+        internal delegate void NativeRCLDestroyRclParamsType(IntPtr paramsHandle);
+
+        internal static NativeRCLDestroyRclParamsType native_rcl_destroy_rcl_params = null;
+
+        [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
+        internal delegate int NativeRCLTryGetParameterType(SafeHandle parameterValueHandle, SafeRclParamsHandle paramsHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string name);
+
+        internal static NativeRCLTryGetParameterType native_rcl_try_get_parameter = null;
+
+        static ParameterDelegates()
+        {
+            _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils();
+            IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet");
+
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_rcl_params), out native_rcl_destroy_rcl_params);
+            _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_try_get_parameter), out native_rcl_try_get_parameter);
+        }
+    }
+}