From 43292937e290a2a8d8cc0d3f5d44886965b213ee Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Tue, 26 Sep 2023 17:45:14 +1000 Subject: [PATCH 01/15] Work in progress implementing node parameters. --- rcldotnet/CMakeLists.txt | 4 + rcldotnet/Node.cs | 5 + .../ParameterInfrastructure/Parameter.cs | 129 ++++++++++++++++++ .../ParameterInfrastructure/ParameterBool.cs | 36 +++++ .../ParameterServer.cs | 128 +++++++++++++++++ 5 files changed, 302 insertions(+) create mode 100644 rcldotnet/ParameterInfrastructure/Parameter.cs create mode 100644 rcldotnet/ParameterInfrastructure/ParameterBool.cs create mode 100644 rcldotnet/ParameterInfrastructure/ParameterServer.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 257b2751..245b3dd6 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -68,6 +68,9 @@ set(CS_SOURCES ServiceDefinitionStaticMemberCache.cs Subscription.cs Timer.cs + ParameterInfrastructure/Parameter.cs + ParameterInfrastructure/ParameterBool.cs + ParameterInfrastructure/ParameterServer.cs ) find_package(rcldotnet_common REQUIRED) @@ -75,6 +78,7 @@ find_package(rcldotnet_common REQUIRED) set(_assemblies_dep_dlls ${action_msgs_ASSEMBLIES_DLL} ${builtin_interfaces_ASSEMBLIES_DLL} + ${rcl_interfaces_ASSEMBLIES_DLL} ${rcldotnet_common_ASSEMBLIES_DLL} ${unique_identifier_msgs_ASSEMBLIES_DLL} ) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 80b5549a..c985eb11 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -16,6 +16,7 @@ using System; using System.Collections.Generic; using System.Runtime.InteropServices; +using ROS2.ParameterInfrastructure; using ROS2.Utils; namespace ROS2 @@ -143,6 +144,8 @@ public sealed class Node private readonly IList _timers; + private readonly ParameterServer _parameterServer; + internal Node(SafeNodeHandle handle) { Handle = handle; @@ -156,6 +159,8 @@ internal Node(SafeNodeHandle handle) _actionClients = new List(); _actionServers = new List(); _timers = new List(); + + _parameterServer = new ParameterServer(this); } public string Name => RCLdotnet.GetStringFromNativeDelegate(NodeDelegates.native_rcl_node_get_name_handle, Handle); diff --git a/rcldotnet/ParameterInfrastructure/Parameter.cs b/rcldotnet/ParameterInfrastructure/Parameter.cs new file mode 100644 index 00000000..2cd5a750 --- /dev/null +++ b/rcldotnet/ParameterInfrastructure/Parameter.cs @@ -0,0 +1,129 @@ +/* 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.Collections.Generic; + +namespace ROS2.ParameterInfrastructure +{ + public class InvalidTypeException : Exception + { + public InvalidTypeException(string message) : base(message) + { + } + } + + public class InvalidGetValueException : Exception + { + public InvalidGetValueException(Type parameterType, Type valueType) : + base($"Unable to retrieve value: {parameterType} does not contain a {valueType}!") + { + } + } + + internal interface ParameterBuilder + { + Parameter BuildParameter(ParameterServer server, string name, object defaultValue); + } + + public abstract class Parameter + { + private static readonly IDictionary _allowedTypes = new Dictionary(); + private static readonly IDictionary _parameterBuilders = new Dictionary(); + + private readonly ParameterServer _server; + + protected Parameter(ParameterServer server, string name) + { + _server = server; + Name = name; + } + + internal abstract byte Type { get; } + + public string Name { get; } + + protected void OnValueChanged() + { + _server.OnParameterValueChanged(this); + } + + public static bool IsOfValidType(Type type) + { + return _allowedTypes.ContainsKey(type); + } + + internal static bool TryGetTypeIndex(Type type, out byte index) + { + return _allowedTypes.TryGetValue(type, out index); + } + + internal static void RegisterBuilder(byte typeIndex, ParameterBuilder builder) where T : struct + { + Type type = typeof(T); + + _allowedTypes.Add(type, typeIndex); + _parameterBuilders.Add(type, builder); + } + + internal static Parameter BuildParameter(ParameterServer server, string name, T defaultValue) where T : struct + { + if (!_parameterBuilders.TryGetValue(typeof(T), out ParameterBuilder builder)) + { + throw new InvalidTypeException($"Unable to build parameter for type: {nameof(T)}. Invalid type!"); + } + + return builder.BuildParameter(server, name, defaultValue); + } + + public abstract T GetValue() where T : struct; + } + + public abstract class ParameterTyped : Parameter where ParameterT : struct + { + private ParameterT? _value; + + public ParameterT? Value + { + get => _value; + set + { + if (_value.Equals(value)) return; + + _value = value; + + OnValueChanged(); + } + } + + protected readonly ParameterT _defaultValue; + + protected ParameterTyped(ParameterServer server, string name, ParameterT defaultValue) : base(server, name) + { + _defaultValue = defaultValue; + } + + public bool IsSet() => _value.HasValue; + + public override T GetValue() + { + if (Value == null && _defaultValue is T outDefaultValue) return outDefaultValue; + + if (Value is T outValue) return outValue; + + throw new InvalidGetValueException(typeof(ParameterTyped), typeof(T)); + } + } +} diff --git a/rcldotnet/ParameterInfrastructure/ParameterBool.cs b/rcldotnet/ParameterInfrastructure/ParameterBool.cs new file mode 100644 index 00000000..7551a3d0 --- /dev/null +++ b/rcldotnet/ParameterInfrastructure/ParameterBool.cs @@ -0,0 +1,36 @@ +/* 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 rcl_interfaces.msg; + +namespace ROS2.ParameterInfrastructure +{ + internal class ParameterBuilderBool : ParameterBuilder + { + public Parameter BuildParameter(ParameterServer server, string name, object defaultValue) + { + return new ParameterBool(server, name, (bool)defaultValue); + } + } + + public class ParameterBool : ParameterTyped + { + internal override byte Type => ParameterType.PARAMETER_BOOL; + + public ParameterBool(ParameterServer server, string name, bool defaultValue) : base(server, name, defaultValue) + { + } + } +} diff --git a/rcldotnet/ParameterInfrastructure/ParameterServer.cs b/rcldotnet/ParameterInfrastructure/ParameterServer.cs new file mode 100644 index 00000000..a1af2f00 --- /dev/null +++ b/rcldotnet/ParameterInfrastructure/ParameterServer.cs @@ -0,0 +1,128 @@ +/* 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.Collections.Generic; +using rcl_interfaces.msg; +using rcl_interfaces.srv; + +namespace ROS2.ParameterInfrastructure +{ + public class ParameterServer + { + private Node _node; + private IDictionary _parameters; + + private Publisher _publisherEvent; + private Service _serviceDescribeParameters; + private Service _serviceGetParameterTypes; + private Service _serviceGetParameters; + private Service _serviceListParameters; + private Service _serviceSetParameters; + private Service _serviceSetParametersAtomically; + + static ParameterServer() + { + Parameter.RegisterBuilder(ParameterType.PARAMETER_BOOL, new ParameterBuilderBool()); + } + + internal ParameterServer(Node node) + { + _node = node; + _parameters = new Dictionary(); + + _publisherEvent = _node.CreatePublisher("/parameter_events"); + _serviceDescribeParameters = _node.CreateService("~/describe_parameters", OnDescribeParameter); + _serviceGetParameterTypes = _node.CreateService("~/get_parameter_types", OnGetParameterTypes); + _serviceGetParameters = _node.CreateService("~/get_parameter", OnGetParameters); + _serviceListParameters = _node.CreateService("~/list_parameters", OnListParameters); + _serviceSetParameters = _node.CreateService("~/set_parameters", OnSetParameters); + _serviceSetParametersAtomically = _node.CreateService("~/set_parameters_atomically", OnSetParametersAtomically); + + DeclareParameter("use_sim_time", false); + } + + internal void OnParameterValueChanged(Parameter parameter) + { + } + + public bool DeclareParameter(string name, ParameterT defaultValue = default) where ParameterT : struct + { + if (!Parameter.IsOfValidType(typeof(ParameterT))) return false; + + if (_parameters.TryGetValue(name, out Parameter existingParameter)) + { + Parameter.TryGetTypeIndex(typeof(ParameterT), out byte type); + + return existingParameter.Type == type; + } + + _parameters.Add(name, Parameter.BuildParameter(this, name, defaultValue)); + return true; + } + + public void SetParameter(string name, ParameterT value) + { + + } + + private void OnDescribeParameter(DescribeParameters_Request request, DescribeParameters_Response response) + { + + } + + private void OnGetParameterTypes(GetParameterTypes_Request request, GetParameterTypes_Response response) + { + + } + + private void OnGetParameters(GetParameters_Request request, GetParameters_Response response) + { + + } + + private void OnListParameters(ListParameters_Request request, ListParameters_Response response) + { + bool hasPrefixes = request.Prefixes.Count != 0; + foreach (Parameter parameter in _parameters.Values) + { + bool matchesCriteria = !hasPrefixes; + + if (hasPrefixes) + { + foreach (string prefix in request.Prefixes) + { + if (parameter.Name.StartsWith(prefix)) + { + matchesCriteria = true; + break; + } + } + } + + if (matchesCriteria) response.Result.Names.Add(parameter.Name); + } + } + + private void OnSetParameters(SetParameters_Request request, SetParameters_Response response) + { + + } + + private void OnSetParametersAtomically(SetParametersAtomically_Request request, SetParametersAtomically_Response response) + { + + } + } +} From 7da4a370b2aff306efb27408a739a665e32c8861 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Wed, 27 Sep 2023 15:45:42 +1000 Subject: [PATCH 02/15] Additional parameter handling implementation. --- rcldotnet/CMakeLists.txt | 8 +- rcldotnet/Node.cs | 18 +- .../InvalidParameterTypeException.cs | 30 ++ .../Exceptions/ParameterException.cs | 26 ++ .../ParameterNotDeclaredException.cs | 24 ++ .../ParameterTypeMismatchException.cs} | 16 +- .../ParameterHandling/ParameterHandler.cs | 357 ++++++++++++++++++ .../ParameterInfrastructure/Parameter.cs | 129 ------- .../ParameterServer.cs | 128 ------- 9 files changed, 463 insertions(+), 273 deletions(-) create mode 100644 rcldotnet/ParameterHandling/Exceptions/InvalidParameterTypeException.cs create mode 100644 rcldotnet/ParameterHandling/Exceptions/ParameterException.cs create mode 100644 rcldotnet/ParameterHandling/Exceptions/ParameterNotDeclaredException.cs rename rcldotnet/{ParameterInfrastructure/ParameterBool.cs => ParameterHandling/Exceptions/ParameterTypeMismatchException.cs} (57%) create mode 100644 rcldotnet/ParameterHandling/ParameterHandler.cs delete mode 100644 rcldotnet/ParameterInfrastructure/Parameter.cs delete mode 100644 rcldotnet/ParameterInfrastructure/ParameterServer.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 245b3dd6..3c9de220 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -68,9 +68,11 @@ set(CS_SOURCES ServiceDefinitionStaticMemberCache.cs Subscription.cs Timer.cs - ParameterInfrastructure/Parameter.cs - ParameterInfrastructure/ParameterBool.cs - ParameterInfrastructure/ParameterServer.cs + ParameterHandling/ParameterHandler.cs + ParameterHandling/Exceptions/ParameterException.cs + ParameterHandling/Exceptions/ParameterNotDeclaredException.cs + ParameterHandling/Exceptions/ParameterTypeMismatchException.cs + ParameterHandling/Exceptions/InvalidParameterTypeException.cs ) find_package(rcldotnet_common REQUIRED) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index c985eb11..f5a7db95 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -16,7 +16,7 @@ using System; using System.Collections.Generic; using System.Runtime.InteropServices; -using ROS2.ParameterInfrastructure; +using rcl_interfaces.msg; using ROS2.Utils; namespace ROS2 @@ -128,6 +128,8 @@ static NodeDelegates() public sealed class Node { + private const string ParameterNameSimulatedTime = "use_sim_time"; + private readonly Clock _clock; private readonly IList _subscriptions; @@ -144,7 +146,7 @@ public sealed class Node private readonly IList _timers; - private readonly ParameterServer _parameterServer; + private readonly ParameterHandler _parameterHandler; internal Node(SafeNodeHandle handle) { @@ -160,7 +162,9 @@ internal Node(SafeNodeHandle handle) _actionServers = new List(); _timers = new List(); - _parameterServer = new ParameterServer(this); + _parameterHandler = new ParameterHandler(this); + _parameterHandler.DeclareParameter(ParameterNameSimulatedTime, false); + _parameterHandler.AddOnSetParameterCallback(OnSetParameters); } public string Name => RCLdotnet.GetStringFromNativeDelegate(NodeDelegates.native_rcl_node_get_name_handle, Handle); @@ -195,6 +199,14 @@ internal Node(SafeNodeHandle handle) // Disposed if the node is not live anymore. internal SafeNodeHandle Handle { get; } + private void OnSetParameters(List parameters) + { + Parameter simulatedTimeParameter = parameters.Find(parameter => parameter.Name == ParameterNameSimulatedTime); + if (simulatedTimeParameter == null) return; + + // TODO: Update clock setup if applicable. + } + public Publisher CreatePublisher(string topic, QosProfile qosProfile = null) where T : IRosMessage { if (qosProfile != null) diff --git a/rcldotnet/ParameterHandling/Exceptions/InvalidParameterTypeException.cs b/rcldotnet/ParameterHandling/Exceptions/InvalidParameterTypeException.cs new file mode 100644 index 00000000..6edb9509 --- /dev/null +++ b/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!") + { + } + } +} diff --git a/rcldotnet/ParameterHandling/Exceptions/ParameterException.cs b/rcldotnet/ParameterHandling/Exceptions/ParameterException.cs new file mode 100644 index 00000000..41d12094 --- /dev/null +++ b/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) + { + } + } +} diff --git a/rcldotnet/ParameterHandling/Exceptions/ParameterNotDeclaredException.cs b/rcldotnet/ParameterHandling/Exceptions/ParameterNotDeclaredException.cs new file mode 100644 index 00000000..9fa509b6 --- /dev/null +++ b/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!") + { + } + } +} diff --git a/rcldotnet/ParameterInfrastructure/ParameterBool.cs b/rcldotnet/ParameterHandling/Exceptions/ParameterTypeMismatchException.cs similarity index 57% rename from rcldotnet/ParameterInfrastructure/ParameterBool.cs rename to rcldotnet/ParameterHandling/Exceptions/ParameterTypeMismatchException.cs index 7551a3d0..fa05e665 100644 --- a/rcldotnet/ParameterInfrastructure/ParameterBool.cs +++ b/rcldotnet/ParameterHandling/Exceptions/ParameterTypeMismatchException.cs @@ -13,23 +13,19 @@ * limitations under the License. */ +using System; using rcl_interfaces.msg; -namespace ROS2.ParameterInfrastructure +namespace ROS2.ParameterHandling.Exceptions { - internal class ParameterBuilderBool : ParameterBuilder + public class ParameterTypeMismatchException : ParameterException { - public Parameter BuildParameter(ParameterServer server, string name, object defaultValue) + public ParameterTypeMismatchException(string message) : base(message) { - return new ParameterBool(server, name, (bool)defaultValue); } - } - - public class ParameterBool : ParameterTyped - { - internal override byte Type => ParameterType.PARAMETER_BOOL; - public ParameterBool(ParameterServer server, string name, bool defaultValue) : base(server, name, defaultValue) + 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}\"!") { } } diff --git a/rcldotnet/ParameterHandling/ParameterHandler.cs b/rcldotnet/ParameterHandling/ParameterHandler.cs new file mode 100644 index 00000000..09ce350e --- /dev/null +++ b/rcldotnet/ParameterHandling/ParameterHandler.cs @@ -0,0 +1,357 @@ +/* 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.Collections.Generic; +using rcl_interfaces.msg; +using rcl_interfaces.srv; +using ROS2.ParameterHandling.Exceptions; + +namespace ROS2 +{ + public class ParameterHandler + { + private static readonly IDictionary _typeToParameterType = new Dictionary + { + {typeof(bool), ParameterType.PARAMETER_BOOL}, + {typeof(long), ParameterType.PARAMETER_INTEGER}, + {typeof(double), ParameterType.PARAMETER_DOUBLE}, + {typeof(string), ParameterType.PARAMETER_STRING}, + {typeof(List), ParameterType.PARAMETER_BYTE_ARRAY}, + {typeof(List), ParameterType.PARAMETER_BOOL_ARRAY}, + {typeof(List), ParameterType.PARAMETER_INTEGER_ARRAY}, + {typeof(List), ParameterType.PARAMETER_DOUBLE_ARRAY}, + {typeof(List), ParameterType.PARAMETER_STRING_ARRAY} + }; + + private readonly IDictionary _parameters; + + // TODO: Implement parameter event publishing + private Publisher _publisherEvent; + + private Action> _onSetParameterCallback; + + internal ParameterHandler(Node node) + { + _parameters = new Dictionary(); + + _publisherEvent = node.CreatePublisher("/parameter_events"); + + node.CreateService("~/describe_parameters", OnDescribeParameter); + node.CreateService("~/get_parameter_types", OnGetParameterTypes); + node.CreateService("~/get_parameters", OnGetParameters); + node.CreateService("~/list_parameters", OnListParameters); + node.CreateService("~/set_parameters", OnSetParameters); + node.CreateService("~/set_parameters_atomically", OnSetParametersAtomically); + } + + public void AddOnSetParameterCallback(Action> callback) + { + _onSetParameterCallback += callback; + } + + public void RemoveOnSetParameterCallback(Action> callback) + { + _onSetParameterCallback -= callback; + } + + private Parameter DeclareParameter(string name, Type type) + { + if (!_typeToParameterType.TryGetValue(type, out byte typeCode)) + { + throw new InvalidParameterTypeException(type); + } + + if (_parameters.TryGetValue(name, out Parameter parameter)) + { + if (parameter.Value.Type != typeCode) + { + throw new ParameterTypeMismatchException( + $"Attempted to redefine parameter \"{name}\" from type {parameter.Value.Type} to {typeCode}!"); + } + + // TODO: Consider updating description + + return parameter; + } + + Parameter declaredParameter = new Parameter { Name = name, Value = { Type = typeCode } }; + _parameters.Add(name, declaredParameter); + return declaredParameter; + } + + public void DeclareParameter(string name, bool defaultValue = false) + { + DeclareParameter(name, typeof(bool)).Value.BoolValue = defaultValue; + } + + public void DeclareParameter(string name, int defaultValue = 0) => DeclareParameter(name, (long)defaultValue); + + public void DeclareParameter(string name, long defaultValue = 0L) + { + DeclareParameter(name, typeof(long)).Value.IntegerValue = defaultValue; + } + + public void DeclareParameter(string name, float defaultValue = 0.0f) => DeclareParameter(name, (double)defaultValue); + + public void DeclareParameter(string name, double defaultValue = 0.0) + { + DeclareParameter(name, typeof(double)).Value.DoubleValue = defaultValue; + } + + public void DeclareParameter(string name, string defaultValue = "") + { + DeclareParameter(name, typeof(string)).Value.StringValue = defaultValue; + } + + public void DeclareParameter(string name, List defaultValue = null) + { + DeclareParameter(name, typeof(List)).Value.ByteArrayValue = defaultValue; + } + + public void DeclareParameter(string name, List defaultValue = null) + { + DeclareParameter(name, typeof(List)).Value.BoolArrayValue = defaultValue; + } + + public void DeclareParameter(string name, List defaultValue = null) + { + DeclareParameter(name, typeof(List)).Value.IntegerArrayValue = defaultValue; + } + + public void DeclareParameter(string name, List defaultValue = null) + { + DeclareParameter(name, typeof(List)).Value.DoubleArrayValue = defaultValue; + } + + public void DeclareParameter(string name, List defaultValue = null) + { + DeclareParameter(name, typeof(List)).Value.StringArrayValue = defaultValue; + } + + private void OnDescribeParameter(DescribeParameters_Request request, DescribeParameters_Response response) + { + // TODO: Implement parameter descriptions. + } + + private void OnGetParameterTypes(GetParameterTypes_Request request, GetParameterTypes_Response response) + { + foreach (Parameter parameter in _parameters.Values) + { + response.Types.Add(parameter.Value.Type); + } + } + + private ParameterValue CloneParameterValue(ParameterValue toClone) + { + byte type = toClone.Type; + ParameterValue clone = new ParameterValue + { + Type = type + }; + + switch (type) + { + case ParameterType.PARAMETER_BOOL: + clone.BoolValue = toClone.BoolValue; + break; + case ParameterType.PARAMETER_INTEGER: + clone.IntegerValue = toClone.IntegerValue; + break; + case ParameterType.PARAMETER_DOUBLE: + clone.DoubleValue = toClone.DoubleValue; + break; + case ParameterType.PARAMETER_STRING: + clone.StringValue = toClone.StringValue; + break; + case ParameterType.PARAMETER_BYTE_ARRAY: + clone.ByteArrayValue.AddRange(toClone.ByteArrayValue); + break; + case ParameterType.PARAMETER_BOOL_ARRAY: + clone.BoolArrayValue.AddRange(toClone.BoolArrayValue); + break; + case ParameterType.PARAMETER_INTEGER_ARRAY: + clone.IntegerArrayValue.AddRange(toClone.IntegerArrayValue); + break; + case ParameterType.PARAMETER_DOUBLE_ARRAY: + clone.DoubleArrayValue.AddRange(toClone.DoubleArrayValue); + break; + case ParameterType.PARAMETER_STRING_ARRAY: + clone.StringArrayValue.AddRange(toClone.StringArrayValue); + break; + default: + throw new InvalidParameterTypeException(type); + } + + return clone; + } + + public List GetParameters(IEnumerable names) + { + List results = new List(); + + foreach (string parameterName in names) + { + if (_parameters.TryGetValue(parameterName, out Parameter parameter)) + { + results.Add(CloneParameterValue(parameter.Value)); + } + } + + return results; + } + + private void OnGetParameters(GetParameters_Request request, GetParameters_Response response) + { + response.Values.AddRange(GetParameters(request.Names)); + } + + public ParameterValue GetParameter(string name) + { + if (_parameters.TryGetValue(name, out Parameter parameter)) + { + return CloneParameterValue(parameter.Value); + } + + throw new ParameterNotDeclaredException(name); + } + + private void OnListParameters(ListParameters_Request request, ListParameters_Response response) + { + bool hasPrefixes = request.Prefixes.Count != 0; + foreach (Parameter parameter in _parameters.Values) + { + bool matchesCriteria = !hasPrefixes; + + if (hasPrefixes) + { + foreach (string prefix in request.Prefixes) + { + if (parameter.Name.StartsWith(prefix)) + { + matchesCriteria = true; + break; + } + } + } + + if (matchesCriteria) response.Result.Names.Add(parameter.Name); + } + } + + private SetParametersResult CheckParameterCompatibility(Parameter update) + { + SetParametersResult result = new SetParametersResult(); + if (!_parameters.TryGetValue(update.Name, out Parameter parameter)) + { + result.Successful = false; + result.Reason = "Parameter was not declared!"; + } + else if (update.Value.Type != parameter.Value.Type) + { + result.Successful = false; + result.Reason = $"Parameter type mismatch: {parameter.Value.Type} != {update.Value.Type}!"; + } + else + { + result.Successful = true; + } + + return result; + } + + private void UpdateParameter(Parameter source) + { + Parameter target = _parameters[source.Name]; + + switch (source.Value.Type) + { + case ParameterType.PARAMETER_BOOL: + target.Value.BoolValue = source.Value.BoolValue; + break; + case ParameterType.PARAMETER_INTEGER: + target.Value.IntegerValue = source.Value.IntegerValue; + break; + case ParameterType.PARAMETER_DOUBLE: + target.Value.DoubleValue = source.Value.DoubleValue; + break; + case ParameterType.PARAMETER_STRING: + target.Value.StringValue = source.Value.StringValue; + break; + case ParameterType.PARAMETER_BYTE_ARRAY: + target.Value.ByteArrayValue = source.Value.ByteArrayValue; + break; + case ParameterType.PARAMETER_BOOL_ARRAY: + target.Value.BoolArrayValue = source.Value.BoolArrayValue; + break; + case ParameterType.PARAMETER_INTEGER_ARRAY: + target.Value.IntegerArrayValue = source.Value.IntegerArrayValue; + break; + case ParameterType.PARAMETER_DOUBLE_ARRAY: + target.Value.DoubleArrayValue = source.Value.DoubleArrayValue; + break; + case ParameterType.PARAMETER_STRING_ARRAY: + target.Value.StringArrayValue = source.Value.StringArrayValue; + break; + default: + throw new InvalidParameterTypeException(source.Value.Type); + } + } + + public List SetParameters(List parameters) + { + List results = new List(); + + foreach (Parameter source in parameters) + { + results.Add(SetParametersAtomically(new List { source })); + } + + return results; + } + + private void OnSetParameters(SetParameters_Request request, SetParameters_Response response) + { + response.Results.AddRange(SetParameters(request.Parameters)); + } + + public SetParametersResult SetParametersAtomically(List parameters) + { + SetParametersResult result = new SetParametersResult(); + + foreach (Parameter source in parameters) + { + result = CheckParameterCompatibility(source); + if (!result.Successful) break; + } + + if (!result.Successful) return result; + + foreach (Parameter source in parameters) + { + UpdateParameter(source); + } + + _onSetParameterCallback?.Invoke(parameters); + + return result; + } + + private void OnSetParametersAtomically(SetParametersAtomically_Request request, SetParametersAtomically_Response response) + { + response.Result = SetParametersAtomically(request.Parameters); + } + } +} diff --git a/rcldotnet/ParameterInfrastructure/Parameter.cs b/rcldotnet/ParameterInfrastructure/Parameter.cs deleted file mode 100644 index 2cd5a750..00000000 --- a/rcldotnet/ParameterInfrastructure/Parameter.cs +++ /dev/null @@ -1,129 +0,0 @@ -/* 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.Collections.Generic; - -namespace ROS2.ParameterInfrastructure -{ - public class InvalidTypeException : Exception - { - public InvalidTypeException(string message) : base(message) - { - } - } - - public class InvalidGetValueException : Exception - { - public InvalidGetValueException(Type parameterType, Type valueType) : - base($"Unable to retrieve value: {parameterType} does not contain a {valueType}!") - { - } - } - - internal interface ParameterBuilder - { - Parameter BuildParameter(ParameterServer server, string name, object defaultValue); - } - - public abstract class Parameter - { - private static readonly IDictionary _allowedTypes = new Dictionary(); - private static readonly IDictionary _parameterBuilders = new Dictionary(); - - private readonly ParameterServer _server; - - protected Parameter(ParameterServer server, string name) - { - _server = server; - Name = name; - } - - internal abstract byte Type { get; } - - public string Name { get; } - - protected void OnValueChanged() - { - _server.OnParameterValueChanged(this); - } - - public static bool IsOfValidType(Type type) - { - return _allowedTypes.ContainsKey(type); - } - - internal static bool TryGetTypeIndex(Type type, out byte index) - { - return _allowedTypes.TryGetValue(type, out index); - } - - internal static void RegisterBuilder(byte typeIndex, ParameterBuilder builder) where T : struct - { - Type type = typeof(T); - - _allowedTypes.Add(type, typeIndex); - _parameterBuilders.Add(type, builder); - } - - internal static Parameter BuildParameter(ParameterServer server, string name, T defaultValue) where T : struct - { - if (!_parameterBuilders.TryGetValue(typeof(T), out ParameterBuilder builder)) - { - throw new InvalidTypeException($"Unable to build parameter for type: {nameof(T)}. Invalid type!"); - } - - return builder.BuildParameter(server, name, defaultValue); - } - - public abstract T GetValue() where T : struct; - } - - public abstract class ParameterTyped : Parameter where ParameterT : struct - { - private ParameterT? _value; - - public ParameterT? Value - { - get => _value; - set - { - if (_value.Equals(value)) return; - - _value = value; - - OnValueChanged(); - } - } - - protected readonly ParameterT _defaultValue; - - protected ParameterTyped(ParameterServer server, string name, ParameterT defaultValue) : base(server, name) - { - _defaultValue = defaultValue; - } - - public bool IsSet() => _value.HasValue; - - public override T GetValue() - { - if (Value == null && _defaultValue is T outDefaultValue) return outDefaultValue; - - if (Value is T outValue) return outValue; - - throw new InvalidGetValueException(typeof(ParameterTyped), typeof(T)); - } - } -} diff --git a/rcldotnet/ParameterInfrastructure/ParameterServer.cs b/rcldotnet/ParameterInfrastructure/ParameterServer.cs deleted file mode 100644 index a1af2f00..00000000 --- a/rcldotnet/ParameterInfrastructure/ParameterServer.cs +++ /dev/null @@ -1,128 +0,0 @@ -/* 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.Collections.Generic; -using rcl_interfaces.msg; -using rcl_interfaces.srv; - -namespace ROS2.ParameterInfrastructure -{ - public class ParameterServer - { - private Node _node; - private IDictionary _parameters; - - private Publisher _publisherEvent; - private Service _serviceDescribeParameters; - private Service _serviceGetParameterTypes; - private Service _serviceGetParameters; - private Service _serviceListParameters; - private Service _serviceSetParameters; - private Service _serviceSetParametersAtomically; - - static ParameterServer() - { - Parameter.RegisterBuilder(ParameterType.PARAMETER_BOOL, new ParameterBuilderBool()); - } - - internal ParameterServer(Node node) - { - _node = node; - _parameters = new Dictionary(); - - _publisherEvent = _node.CreatePublisher("/parameter_events"); - _serviceDescribeParameters = _node.CreateService("~/describe_parameters", OnDescribeParameter); - _serviceGetParameterTypes = _node.CreateService("~/get_parameter_types", OnGetParameterTypes); - _serviceGetParameters = _node.CreateService("~/get_parameter", OnGetParameters); - _serviceListParameters = _node.CreateService("~/list_parameters", OnListParameters); - _serviceSetParameters = _node.CreateService("~/set_parameters", OnSetParameters); - _serviceSetParametersAtomically = _node.CreateService("~/set_parameters_atomically", OnSetParametersAtomically); - - DeclareParameter("use_sim_time", false); - } - - internal void OnParameterValueChanged(Parameter parameter) - { - } - - public bool DeclareParameter(string name, ParameterT defaultValue = default) where ParameterT : struct - { - if (!Parameter.IsOfValidType(typeof(ParameterT))) return false; - - if (_parameters.TryGetValue(name, out Parameter existingParameter)) - { - Parameter.TryGetTypeIndex(typeof(ParameterT), out byte type); - - return existingParameter.Type == type; - } - - _parameters.Add(name, Parameter.BuildParameter(this, name, defaultValue)); - return true; - } - - public void SetParameter(string name, ParameterT value) - { - - } - - private void OnDescribeParameter(DescribeParameters_Request request, DescribeParameters_Response response) - { - - } - - private void OnGetParameterTypes(GetParameterTypes_Request request, GetParameterTypes_Response response) - { - - } - - private void OnGetParameters(GetParameters_Request request, GetParameters_Response response) - { - - } - - private void OnListParameters(ListParameters_Request request, ListParameters_Response response) - { - bool hasPrefixes = request.Prefixes.Count != 0; - foreach (Parameter parameter in _parameters.Values) - { - bool matchesCriteria = !hasPrefixes; - - if (hasPrefixes) - { - foreach (string prefix in request.Prefixes) - { - if (parameter.Name.StartsWith(prefix)) - { - matchesCriteria = true; - break; - } - } - } - - if (matchesCriteria) response.Result.Names.Add(parameter.Name); - } - } - - private void OnSetParameters(SetParameters_Request request, SetParameters_Response response) - { - - } - - private void OnSetParametersAtomically(SetParametersAtomically_Request request, SetParametersAtomically_Response response) - { - - } - } -} From 73c23ca4159b65d53966204d0adab9e17dcd72cd Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Wed, 27 Sep 2023 16:11:23 +1000 Subject: [PATCH 03/15] Partially implemented parameter descriptions. --- .../ParameterHandling/ParameterHandler.cs | 46 ++++++++++++++----- 1 file changed, 34 insertions(+), 12 deletions(-) diff --git a/rcldotnet/ParameterHandling/ParameterHandler.cs b/rcldotnet/ParameterHandling/ParameterHandler.cs index 09ce350e..c22d8819 100644 --- a/rcldotnet/ParameterHandling/ParameterHandler.cs +++ b/rcldotnet/ParameterHandling/ParameterHandler.cs @@ -36,7 +36,8 @@ public class ParameterHandler {typeof(List), ParameterType.PARAMETER_STRING_ARRAY} }; - private readonly IDictionary _parameters; + private readonly IDictionary _parameters = new Dictionary(); + private readonly IDictionary _descriptors = new Dictionary(); // TODO: Implement parameter event publishing private Publisher _publisherEvent; @@ -45,11 +46,9 @@ public class ParameterHandler internal ParameterHandler(Node node) { - _parameters = new Dictionary(); + _publisherEvent = node.CreatePublisher("/parameter_events", QosProfile.ParameterEventsProfile); - _publisherEvent = node.CreatePublisher("/parameter_events"); - - node.CreateService("~/describe_parameters", OnDescribeParameter); + node.CreateService("~/describe_parameters", OnDescribeParameters); node.CreateService("~/get_parameter_types", OnGetParameterTypes); node.CreateService("~/get_parameters", OnGetParameters); node.CreateService("~/list_parameters", OnListParameters); @@ -67,28 +66,39 @@ public void RemoveOnSetParameterCallback(Action> callback) _onSetParameterCallback -= callback; } - private Parameter DeclareParameter(string name, Type type) + private Parameter DeclareParameter(string name, Type type, ParameterDescriptor descriptor = null) { if (!_typeToParameterType.TryGetValue(type, out byte typeCode)) { throw new InvalidParameterTypeException(type); } + if (descriptor == null) + { + descriptor = new ParameterDescriptor + { + Name = name, + Type = typeCode + }; + } + if (_parameters.TryGetValue(name, out Parameter parameter)) { + if (parameter.Value.Type != typeCode) { throw new ParameterTypeMismatchException( $"Attempted to redefine parameter \"{name}\" from type {parameter.Value.Type} to {typeCode}!"); } - // TODO: Consider updating description + // TODO: Should we update the description if it doesn't match or throw an error? return parameter; } Parameter declaredParameter = new Parameter { Name = name, Value = { Type = typeCode } }; _parameters.Add(name, declaredParameter); + _descriptors.Add(name, descriptor); return declaredParameter; } @@ -141,9 +151,15 @@ public void DeclareParameter(string name, List defaultValue = null) DeclareParameter(name, typeof(List)).Value.StringArrayValue = defaultValue; } - private void OnDescribeParameter(DescribeParameters_Request request, DescribeParameters_Response response) + private void OnDescribeParameters(DescribeParameters_Request request, DescribeParameters_Response response) { - // TODO: Implement parameter descriptions. + foreach (string name in request.Names) + { + response.Descriptors.Add( + _descriptors.TryGetValue(name, out ParameterDescriptor descriptor) + ? descriptor + : new ParameterDescriptor()); + } } private void OnGetParameterTypes(GetParameterTypes_Request request, GetParameterTypes_Response response) @@ -254,16 +270,22 @@ private void OnListParameters(ListParameters_Request request, ListParameters_Res private SetParametersResult CheckParameterCompatibility(Parameter update) { SetParametersResult result = new SetParametersResult(); - if (!_parameters.TryGetValue(update.Name, out Parameter parameter)) + if (!_descriptors.TryGetValue(update.Name, out ParameterDescriptor descriptor)) { result.Successful = false; result.Reason = "Parameter was not declared!"; } - else if (update.Value.Type != parameter.Value.Type) + else if (descriptor.ReadOnly) + { + result.Successful = false; + result.Reason = "Parameter is readonly!"; + } + else if (update.Value.Type != descriptor.Type) { result.Successful = false; - result.Reason = $"Parameter type mismatch: {parameter.Value.Type} != {update.Value.Type}!"; + result.Reason = $"Parameter type mismatch: {descriptor.Type} != {update.Value.Type}!"; } + // TODO: Check value compatibility against ParameterDescriptor constraints. else { result.Successful = true; From 17ccf45371efd059cd1009db22735caec4f8e848 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Wed, 27 Sep 2023 17:21:53 +1000 Subject: [PATCH 04/15] Implemented parameter event messaging. --- .../ParameterHandling/ParameterHandler.cs | 94 ++++++++++++++----- 1 file changed, 68 insertions(+), 26 deletions(-) diff --git a/rcldotnet/ParameterHandling/ParameterHandler.cs b/rcldotnet/ParameterHandling/ParameterHandler.cs index c22d8819..d82d2534 100644 --- a/rcldotnet/ParameterHandling/ParameterHandler.cs +++ b/rcldotnet/ParameterHandling/ParameterHandler.cs @@ -36,16 +36,17 @@ public class ParameterHandler {typeof(List), ParameterType.PARAMETER_STRING_ARRAY} }; + private readonly Node _node; private readonly IDictionary _parameters = new Dictionary(); private readonly IDictionary _descriptors = new Dictionary(); - // TODO: Implement parameter event publishing - private Publisher _publisherEvent; + private readonly Publisher _publisherEvent; private Action> _onSetParameterCallback; internal ParameterHandler(Node node) { + _node = node; _publisherEvent = node.CreatePublisher("/parameter_events", QosProfile.ParameterEventsProfile); node.CreateService("~/describe_parameters", OnDescribeParameters); @@ -66,7 +67,30 @@ public void RemoveOnSetParameterCallback(Action> callback) _onSetParameterCallback -= callback; } - private Parameter DeclareParameter(string name, Type type, ParameterDescriptor descriptor = null) + private ParameterEvent GenerateParameterEventMessage() + { + return new ParameterEvent + { + Node = $"{_node.GetNamespace()}{_node.GetName()}", + Stamp = _node.Clock.Now() + }; + } + + private void PublishParametersDeclaredEvent(IEnumerable parameters) + { + ParameterEvent parameterEvent = GenerateParameterEventMessage(); + parameterEvent.NewParameters.AddRange(parameters); + _publisherEvent.Publish(parameterEvent); + } + + private void PublishParametersChangedEvent(IEnumerable parameters) + { + ParameterEvent parameterEvent = GenerateParameterEventMessage(); + parameterEvent.ChangedParameters.AddRange(parameters); + _publisherEvent.Publish(parameterEvent); + } + + private void DeclareParameter(string name, Type type, Action assignDefaultCallback, ParameterDescriptor descriptor = null) { if (!_typeToParameterType.TryGetValue(type, out byte typeCode)) { @@ -92,63 +116,80 @@ private Parameter DeclareParameter(string name, Type type, ParameterDescriptor d } // TODO: Should we update the description if it doesn't match or throw an error? - - return parameter; + return; } Parameter declaredParameter = new Parameter { Name = name, Value = { Type = typeCode } }; _parameters.Add(name, declaredParameter); _descriptors.Add(name, descriptor); - return declaredParameter; + + assignDefaultCallback?.Invoke(declaredParameter.Value); + + PublishParametersDeclaredEvent(new List { declaredParameter }); } - public void DeclareParameter(string name, bool defaultValue = false) + public void DeclareParameter(string name, bool defaultValue = false, ParameterDescriptor descriptor = null) { - DeclareParameter(name, typeof(bool)).Value.BoolValue = defaultValue; + DeclareParameter(name, typeof(bool), value => { value.BoolValue = defaultValue; }, descriptor); } - public void DeclareParameter(string name, int defaultValue = 0) => DeclareParameter(name, (long)defaultValue); + public void DeclareParameter(string name, int defaultValue = 0, ParameterDescriptor descriptor = null) => DeclareParameter(name, (long)defaultValue, descriptor); - public void DeclareParameter(string name, long defaultValue = 0L) + public void DeclareParameter(string name, long defaultValue = 0L, ParameterDescriptor descriptor = null) { - DeclareParameter(name, typeof(long)).Value.IntegerValue = defaultValue; + DeclareParameter(name, typeof(long), value => { value.IntegerValue = defaultValue; }, descriptor); } - public void DeclareParameter(string name, float defaultValue = 0.0f) => DeclareParameter(name, (double)defaultValue); + public void DeclareParameter(string name, float defaultValue = 0.0f, ParameterDescriptor descriptor = null) => DeclareParameter(name, (double)defaultValue, descriptor); - public void DeclareParameter(string name, double defaultValue = 0.0) + public void DeclareParameter(string name, double defaultValue = 0.0, ParameterDescriptor descriptor = null) { - DeclareParameter(name, typeof(double)).Value.DoubleValue = defaultValue; + DeclareParameter(name, typeof(double), value => { value.DoubleValue = defaultValue; }, descriptor); } - public void DeclareParameter(string name, string defaultValue = "") + public void DeclareParameter(string name, string defaultValue = "", ParameterDescriptor descriptor = null) { - DeclareParameter(name, typeof(string)).Value.StringValue = defaultValue; + DeclareParameter(name, typeof(string), value => { value.StringValue = defaultValue; }, descriptor); } - public void DeclareParameter(string name, List defaultValue = null) + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) { - DeclareParameter(name, typeof(List)).Value.ByteArrayValue = defaultValue; + DeclareParameter(name, typeof(List), value => + { + if (defaultValue != null) value.ByteArrayValue.AddRange(defaultValue); + }, descriptor); } - public void DeclareParameter(string name, List defaultValue = null) + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) { - DeclareParameter(name, typeof(List)).Value.BoolArrayValue = defaultValue; + DeclareParameter(name, typeof(List), value => + { + if (defaultValue != null) value.BoolArrayValue.AddRange(defaultValue); + }, descriptor); } - public void DeclareParameter(string name, List defaultValue = null) + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) { - DeclareParameter(name, typeof(List)).Value.IntegerArrayValue = defaultValue; + DeclareParameter(name, typeof(List), value => + { + if (defaultValue != null) value.IntegerArrayValue.AddRange(defaultValue); + }, descriptor); } - public void DeclareParameter(string name, List defaultValue = null) + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) { - DeclareParameter(name, typeof(List)).Value.DoubleArrayValue = defaultValue; + DeclareParameter(name, typeof(List), value => + { + if (defaultValue != null) value.DoubleArrayValue.AddRange(defaultValue); + }, descriptor); } - public void DeclareParameter(string name, List defaultValue = null) + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) { - DeclareParameter(name, typeof(List)).Value.StringArrayValue = defaultValue; + DeclareParameter(name, typeof(List), value => + { + if (defaultValue != null) value.StringArrayValue.AddRange(defaultValue); + }, descriptor); } private void OnDescribeParameters(DescribeParameters_Request request, DescribeParameters_Response response) @@ -366,6 +407,7 @@ public SetParametersResult SetParametersAtomically(List parameters) UpdateParameter(source); } + PublishParametersChangedEvent(parameters); _onSetParameterCallback?.Invoke(parameters); return result; From c9b110d51a98f9853d3a2a0a3934c616ba00ad8d Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 28 Sep 2023 11:10:24 +1000 Subject: [PATCH 05/15] Adding undeclare functionality to parameter handling. --- rcldotnet/CMakeLists.txt | 3 +- .../Exceptions/ParameterImmutableException.cs | 24 +++ .../ParameterHandling/ParameterHandler.cs | 165 +++++++++++------- 3 files changed, 126 insertions(+), 66 deletions(-) create mode 100644 rcldotnet/ParameterHandling/Exceptions/ParameterImmutableException.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 3c9de220..4c8853cb 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -69,10 +69,11 @@ set(CS_SOURCES Subscription.cs Timer.cs ParameterHandling/ParameterHandler.cs + ParameterHandling/Exceptions/InvalidParameterTypeException.cs ParameterHandling/Exceptions/ParameterException.cs + ParameterHandling/Exceptions/ParameterImmutableException.cs ParameterHandling/Exceptions/ParameterNotDeclaredException.cs ParameterHandling/Exceptions/ParameterTypeMismatchException.cs - ParameterHandling/Exceptions/InvalidParameterTypeException.cs ) find_package(rcldotnet_common REQUIRED) diff --git a/rcldotnet/ParameterHandling/Exceptions/ParameterImmutableException.cs b/rcldotnet/ParameterHandling/Exceptions/ParameterImmutableException.cs new file mode 100644 index 00000000..b37acf1b --- /dev/null +++ b/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!") + { + } + } +} diff --git a/rcldotnet/ParameterHandling/ParameterHandler.cs b/rcldotnet/ParameterHandling/ParameterHandler.cs index d82d2534..42396026 100644 --- a/rcldotnet/ParameterHandling/ParameterHandler.cs +++ b/rcldotnet/ParameterHandling/ParameterHandler.cs @@ -49,14 +49,75 @@ internal ParameterHandler(Node node) _node = node; _publisherEvent = node.CreatePublisher("/parameter_events", QosProfile.ParameterEventsProfile); - node.CreateService("~/describe_parameters", OnDescribeParameters); - node.CreateService("~/get_parameter_types", OnGetParameterTypes); - node.CreateService("~/get_parameters", OnGetParameters); - node.CreateService("~/list_parameters", OnListParameters); - node.CreateService("~/set_parameters", OnSetParameters); - node.CreateService("~/set_parameters_atomically", OnSetParametersAtomically); + node.CreateService("~/describe_parameters", OnDescribeParametersServiceRequest); + node.CreateService("~/get_parameter_types", OnGetParameterTypesServiceRequest); + node.CreateService("~/get_parameters", OnGetParametersServiceRequest); + node.CreateService("~/list_parameters", OnListParametersServiceRequest); + node.CreateService("~/set_parameters", OnSetParametersServiceRequest); + node.CreateService("~/set_parameters_atomically", OnSetParametersAtomicallyServiceRequest); } + #region Service Request Handlers + + private void OnDescribeParametersServiceRequest(DescribeParameters_Request request, DescribeParameters_Response response) + { + foreach (string name in request.Names) + { + response.Descriptors.Add( + _descriptors.TryGetValue(name, out ParameterDescriptor descriptor) + ? descriptor + : new ParameterDescriptor()); + } + } + + private void OnGetParameterTypesServiceRequest(GetParameterTypes_Request request, GetParameterTypes_Response response) + { + foreach (Parameter parameter in _parameters.Values) + { + response.Types.Add(parameter.Value.Type); + } + } + + private void OnGetParametersServiceRequest(GetParameters_Request request, GetParameters_Response response) + { + response.Values.AddRange(GetParameters(request.Names)); + } + + private void OnListParametersServiceRequest(ListParameters_Request request, ListParameters_Response response) + { + bool hasPrefixes = request.Prefixes.Count != 0; + foreach (Parameter parameter in _parameters.Values) + { + bool matchesCriteria = !hasPrefixes; + + if (hasPrefixes) + { + foreach (string prefix in request.Prefixes) + { + if (parameter.Name.StartsWith(prefix)) + { + matchesCriteria = true; + break; + } + } + } + + if (matchesCriteria) response.Result.Names.Add(parameter.Name); + } + } + + private void OnSetParametersServiceRequest(SetParameters_Request request, SetParameters_Response response) + { + response.Results.AddRange(SetParameters(request.Parameters)); + } + + private void OnSetParametersAtomicallyServiceRequest(SetParametersAtomically_Request request, SetParametersAtomically_Response response) + { + response.Result = SetParametersAtomically(request.Parameters); + } + + #endregion + public void AddOnSetParameterCallback(Action> callback) { _onSetParameterCallback += callback; @@ -90,6 +151,13 @@ private void PublishParametersChangedEvent(IEnumerable parameters) _publisherEvent.Publish(parameterEvent); } + private void PublishParametersDeletedEvent(IEnumerable parameters) + { + ParameterEvent parameterEvent = GenerateParameterEventMessage(); + parameterEvent.DeletedParameters.AddRange(parameters); + _publisherEvent.Publish(parameterEvent); + } + private void DeclareParameter(string name, Type type, Action assignDefaultCallback, ParameterDescriptor descriptor = null) { if (!_typeToParameterType.TryGetValue(type, out byte typeCode)) @@ -192,23 +260,21 @@ public void DeclareParameter(string name, IEnumerable defaultValue = nul }, descriptor); } - private void OnDescribeParameters(DescribeParameters_Request request, DescribeParameters_Response response) + public void UndeclareParameter(string name) { - foreach (string name in request.Names) + if (!_descriptors.TryGetValue(name, out ParameterDescriptor descriptor)) { - response.Descriptors.Add( - _descriptors.TryGetValue(name, out ParameterDescriptor descriptor) - ? descriptor - : new ParameterDescriptor()); + throw new ParameterNotDeclaredException(name); } - } - private void OnGetParameterTypes(GetParameterTypes_Request request, GetParameterTypes_Response response) - { - foreach (Parameter parameter in _parameters.Values) - { - response.Types.Add(parameter.Value.Type); - } + if (descriptor.ReadOnly) throw new ParameterImmutableException(name); + + Parameter parameter = _parameters[name]; + + _parameters.Remove(name); + _descriptors.Remove(name); + + PublishParametersDeletedEvent(new List { parameter }); } private ParameterValue CloneParameterValue(ParameterValue toClone) @@ -255,26 +321,6 @@ private ParameterValue CloneParameterValue(ParameterValue toClone) return clone; } - public List GetParameters(IEnumerable names) - { - List results = new List(); - - foreach (string parameterName in names) - { - if (_parameters.TryGetValue(parameterName, out Parameter parameter)) - { - results.Add(CloneParameterValue(parameter.Value)); - } - } - - return results; - } - - private void OnGetParameters(GetParameters_Request request, GetParameters_Response response) - { - response.Values.AddRange(GetParameters(request.Names)); - } - public ParameterValue GetParameter(string name) { if (_parameters.TryGetValue(name, out Parameter parameter)) @@ -285,27 +331,19 @@ public ParameterValue GetParameter(string name) throw new ParameterNotDeclaredException(name); } - private void OnListParameters(ListParameters_Request request, ListParameters_Response response) + public List GetParameters(IEnumerable names) { - bool hasPrefixes = request.Prefixes.Count != 0; - foreach (Parameter parameter in _parameters.Values) - { - bool matchesCriteria = !hasPrefixes; + List results = new List(); - if (hasPrefixes) + foreach (string parameterName in names) + { + if (_parameters.TryGetValue(parameterName, out Parameter parameter)) { - foreach (string prefix in request.Prefixes) - { - if (parameter.Name.StartsWith(prefix)) - { - matchesCriteria = true; - break; - } - } + results.Add(CloneParameterValue(parameter.Value)); } - - if (matchesCriteria) response.Result.Names.Add(parameter.Name); } + + return results; } private SetParametersResult CheckParameterCompatibility(Parameter update) @@ -319,7 +357,7 @@ private SetParametersResult CheckParameterCompatibility(Parameter update) else if (descriptor.ReadOnly) { result.Successful = false; - result.Reason = "Parameter is readonly!"; + result.Reason = "Parameter is read-only!"; } else if (update.Value.Type != descriptor.Type) { @@ -373,6 +411,11 @@ private void UpdateParameter(Parameter source) } } + public SetParametersResult SetParameter(Parameter parameter) + { + return SetParametersAtomically(new List { parameter }); + } + public List SetParameters(List parameters) { List results = new List(); @@ -385,11 +428,6 @@ public List SetParameters(List parameters) return results; } - private void OnSetParameters(SetParameters_Request request, SetParameters_Response response) - { - response.Results.AddRange(SetParameters(request.Parameters)); - } - public SetParametersResult SetParametersAtomically(List parameters) { SetParametersResult result = new SetParametersResult(); @@ -413,9 +451,6 @@ public SetParametersResult SetParametersAtomically(List parameters) return result; } - private void OnSetParametersAtomically(SetParametersAtomically_Request request, SetParametersAtomically_Response response) - { - response.Result = SetParametersAtomically(request.Parameters); - } + public bool HasParameter(string name) => _parameters.ContainsKey(name); } } From 10ac1459cf32cea4ff53691a515749179975e8ee Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 28 Sep 2023 11:10:48 +1000 Subject: [PATCH 06/15] Exposing parameter handling to users of node. --- rcldotnet/Node.cs | 59 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index f5a7db95..0a58f772 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -431,5 +431,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 defaultValue = null, ParameterDescriptor descriptor = null) => + _parameterHandler.DeclareParameter(name, defaultValue, descriptor); + + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) => + _parameterHandler.DeclareParameter(name, defaultValue, descriptor); + + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) => + _parameterHandler.DeclareParameter(name, defaultValue, descriptor); + + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) => + _parameterHandler.DeclareParameter(name, defaultValue, descriptor); + + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) => + _parameterHandler.DeclareParameter(name, defaultValue, descriptor); + + public void UndeclareParameter(string name) => _parameterHandler.UndeclareParameter(name); + + public List GetParameters(IEnumerable names) => _parameterHandler.GetParameters(names); + + public ParameterValue GetParameter(string name) => _parameterHandler.GetParameter(name); + + public List SetParameters(List parameters) => + _parameterHandler.SetParameters(parameters); + + public SetParametersResult SetParametersAtomically(List parameters) => + _parameterHandler.SetParametersAtomically(parameters); + + public SetParametersResult SetParameter(Parameter parameter) => _parameterHandler.SetParameter(parameter); + + public bool HasParameter(string name) => _parameterHandler.HasParameter(name); + + public void AddOnSetParameterCallback(Action> callback) => + _parameterHandler.AddOnSetParameterCallback(callback); + + public void RemoveOnSetParameterCallback(Action> callback) => + _parameterHandler.RemoveOnSetParameterCallback(callback); + + #endregion } } From bb717a9977fe2aaa6817f8e2b6ebe0ae5c620605 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 28 Sep 2023 11:11:11 +1000 Subject: [PATCH 07/15] Adding parameter handling to the talker example. --- rcldotnet_examples/CMakeLists.txt | 1 + rcldotnet_examples/RCLDotnetTalker.cs | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/rcldotnet_examples/CMakeLists.txt b/rcldotnet_examples/CMakeLists.txt index 4d273f2b..2c8c0b06 100644 --- a/rcldotnet_examples/CMakeLists.txt +++ b/rcldotnet_examples/CMakeLists.txt @@ -22,6 +22,7 @@ set(_assemblies_dep_dlls ${rcldotnet_common_ASSEMBLIES_DLL} ${rcldotnet_ASSEMBLIES_DLL} ${builtin_interfaces_ASSEMBLIES_DLL} + ${rcl_interfaces_ASSEMBLIES_DLL} ${std_msgs_ASSEMBLIES_DLL} ${std_srvs_ASSEMBLIES_DLL} ${test_msgs_ASSEMBLIES_DLL} diff --git a/rcldotnet_examples/RCLDotnetTalker.cs b/rcldotnet_examples/RCLDotnetTalker.cs index a82e0950..18971d78 100644 --- a/rcldotnet_examples/RCLDotnetTalker.cs +++ b/rcldotnet_examples/RCLDotnetTalker.cs @@ -1,4 +1,5 @@ using System; +using rcl_interfaces.msg; using ROS2; namespace ConsoleApplication @@ -15,6 +16,7 @@ private RCLDotnetTalker() { RCLdotnet.Init(); _node = RCLdotnet.CreateNode("talker"); + _node.DeclareParameter("publish_string_prefix", "Hello World"); _chatterPub = _node.CreatePublisher("chatter"); @@ -23,7 +25,7 @@ private RCLDotnetTalker() private void PublishChatter(TimeSpan elapsed) { - _msg.Data = $"Hello World: {_i}"; + _msg.Data = $"{_node.GetParameter("publish_string_prefix").StringValue}: {_i}"; Console.WriteLine($"Publishing: \"{_msg.Data}\""); _chatterPub.Publish(_msg); From 19c0e07752b01ba6535e4f7fc9da0431464bab5d Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 28 Sep 2023 15:50:43 +1000 Subject: [PATCH 08/15] Implementing use_sim_time functionality. --- rcldotnet/Node.cs | 33 ++++++++++++++++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 0a58f772..1f1d8b3f 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -199,12 +199,34 @@ internal Node(SafeNodeHandle handle) // Disposed if the node is not live anymore. internal SafeNodeHandle Handle { get; } + private Subscription ClockSubscription { get; set; } + private void OnSetParameters(List parameters) { Parameter simulatedTimeParameter = parameters.Find(parameter => parameter.Name == ParameterNameSimulatedTime); if (simulatedTimeParameter == null) return; - // TODO: Update clock setup if applicable. + // Update clock setup if applicable. + bool subscribedToClock = ClockSubscription != null; + bool useSimulatedTime = simulatedTimeParameter.Value.BoolValue; + if (useSimulatedTime == subscribedToClock) return; + + if (useSimulatedTime) + { + ClockSubscription = CreateSubscription("/clock", OnClockMessage, QosProfile.DefaultProfile.WithKeepLast(1)); + _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 CreatePublisher(string topic, QosProfile qosProfile = null) where T : IRosMessage @@ -276,6 +298,15 @@ private Publisher CreatePublisherInner(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 CreateService(string serviceName, Action callback) where TService : IRosServiceDefinition where TRequest : IRosMessage, new() From 0071b2ef40ee653f5bb13933e90cd30426458fbd Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 28 Sep 2023 16:20:02 +1000 Subject: [PATCH 09/15] Addressing problems after merge --- rcldotnet/CMakeLists.txt | 3 +++ rcldotnet/ParameterHandling/ParameterHandler.cs | 2 +- rcldotnet/package.xml | 2 ++ 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 4c8853cb..2d8af2a7 100644 --- a/rcldotnet/CMakeLists.txt +++ b/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) @@ -80,6 +81,7 @@ 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} @@ -110,6 +112,7 @@ add_library(${PROJECT_NAME}_native SHARED ament_target_dependencies(${PROJECT_NAME}_native "action_msgs" + "rosgraph_msgs" "builtin_interfaces" "unique_identifier_msgs" "rcl" diff --git a/rcldotnet/ParameterHandling/ParameterHandler.cs b/rcldotnet/ParameterHandling/ParameterHandler.cs index 42396026..ede93f9f 100644 --- a/rcldotnet/ParameterHandling/ParameterHandler.cs +++ b/rcldotnet/ParameterHandling/ParameterHandler.cs @@ -132,7 +132,7 @@ private ParameterEvent GenerateParameterEventMessage() { return new ParameterEvent { - Node = $"{_node.GetNamespace()}{_node.GetName()}", + Node = _node.FullyQualifiedName, Stamp = _node.Clock.Now() }; } diff --git a/rcldotnet/package.xml b/rcldotnet/package.xml index df264e07..7b7a761d 100644 --- a/rcldotnet/package.xml +++ b/rcldotnet/package.xml @@ -23,6 +23,7 @@ rcl_action rmw action_msgs + rosgraph_msgs builtin_interfaces unique_identifier_msgs @@ -30,6 +31,7 @@ rcl_interfaces rcl_action action_msgs + rosgraph_msgs builtin_interfaces unique_identifier_msgs From 86a962547c9850eb2841c4e1d7e3cacef8338748 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Mon, 9 Oct 2023 13:15:47 +1000 Subject: [PATCH 10/15] Using QoS Clock profile for node clock subscriber --- rcldotnet/Node.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 1f1d8b3f..ccb8976e 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -213,7 +213,7 @@ private void OnSetParameters(List parameters) if (useSimulatedTime) { - ClockSubscription = CreateSubscription("/clock", OnClockMessage, QosProfile.DefaultProfile.WithKeepLast(1)); + ClockSubscription = CreateSubscription("/clock", OnClockMessage, QosProfile.ClockProfile); _clock.EnableRosTimeOverride(); } else From 98a2c4e42201342b6a09043f5ecdc13a99fa28f2 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Tue, 10 Oct 2023 17:19:51 +1000 Subject: [PATCH 11/15] Commenced work implementing parameter overrides --- rcldotnet/CMakeLists.txt | 1 + .../ParameterHandling/ParameterHandler.cs | 15 +++- .../ParameterHandling/SafeRclParamsHandle.cs | 32 +++++++ rcldotnet/RCLdotnet.cs | 27 ++++++ rcldotnet/rcldotnet.c | 88 ++++++++++++++++++- rcldotnet/rcldotnet.h | 9 ++ 6 files changed, 168 insertions(+), 4 deletions(-) create mode 100644 rcldotnet/ParameterHandling/SafeRclParamsHandle.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 2d8af2a7..d8b97af9 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -70,6 +70,7 @@ set(CS_SOURCES Subscription.cs Timer.cs ParameterHandling/ParameterHandler.cs + ParameterHandling/SafeRclParamsHandle.cs ParameterHandling/Exceptions/InvalidParameterTypeException.cs ParameterHandling/Exceptions/ParameterException.cs ParameterHandling/Exceptions/ParameterImmutableException.cs diff --git a/rcldotnet/ParameterHandling/ParameterHandler.cs b/rcldotnet/ParameterHandling/ParameterHandler.cs index ede93f9f..97aa6661 100644 --- a/rcldotnet/ParameterHandling/ParameterHandler.cs +++ b/rcldotnet/ParameterHandling/ParameterHandler.cs @@ -158,6 +158,12 @@ private void PublishParametersDeletedEvent(IEnumerable parameters) _publisherEvent.Publish(parameterEvent); } + private bool TryGetParameterOverride(string name, out Parameter parameterOverride) + { + parameterOverride = null; + return false; + } + private void DeclareParameter(string name, Type type, Action assignDefaultCallback, ParameterDescriptor descriptor = null) { if (!_typeToParameterType.TryGetValue(type, out byte typeCode)) @@ -187,12 +193,15 @@ private void DeclareParameter(string name, Type type, Action ass return; } - Parameter declaredParameter = new Parameter { Name = name, Value = { Type = typeCode } }; + if (!TryGetParameterOverride(name, out Parameter declaredParameter)) + { + declaredParameter = new Parameter { Name = name, Value = { Type = typeCode } }; + assignDefaultCallback?.Invoke(declaredParameter.Value); + } + _parameters.Add(name, declaredParameter); _descriptors.Add(name, descriptor); - assignDefaultCallback?.Invoke(declaredParameter.Value); - PublishParametersDeclaredEvent(new List { declaredParameter }); } diff --git a/rcldotnet/ParameterHandling/SafeRclParamsHandle.cs b/rcldotnet/ParameterHandling/SafeRclParamsHandle.cs new file mode 100644 index 00000000..425b8d25 --- /dev/null +++ b/rcldotnet/ParameterHandling/SafeRclParamsHandle.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 Microsoft.Win32.SafeHandles; + +namespace ROS2.ParameterHandling +{ + internal class SafeRclParamsHandle : SafeHandleZeroOrMinusOneIsInvalid + { + public SafeRclParamsHandle() : base(true) + { + } + + protected override bool ReleaseHandle() + { + RCLdotnetDelegates.native_rcl_destroy_rcl_params(handle); + return true; + } + } +} diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index eeefea8a..043c5e92 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -17,6 +17,7 @@ using System.Runtime.InteropServices; using action_msgs.msg; using action_msgs.srv; +using ROS2.ParameterHandling; using ROS2.Utils; namespace ROS2 @@ -47,6 +48,16 @@ internal delegate void NativeRCLGetErrorStringType( internal static NativeRCLOkType native_rcl_ok = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLArgumentsGetParamOverridesType(ref SafeRclParamsHandle parameterOverrides); + + internal static NativeRCLArgumentsGetParamOverridesType native_rcl_arguments_get_param_overrides = null; + + [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 RCLRet NativeRCLCreateNodeHandleType( ref SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string nodeName, [MarshalAs(UnmanagedType.LPStr)] string nodeNamespace); @@ -729,6 +740,8 @@ static RCLdotnetDelegates() (NativeRCLWriteToQosProfileHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_write_to_qos_profile_handle_ptr, typeof(NativeRCLWriteToQosProfileHandleType)); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_arguments_get_param_overrides), out native_rcl_arguments_get_param_overrides); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_clock_handle), out native_rcl_create_clock_handle); _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_clock_handle), out native_rcl_destroy_clock_handle); _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_create_timer_handle), out native_rcl_create_timer_handle); @@ -742,6 +755,8 @@ public static class RCLdotnet private static bool initialized = false; private static readonly object syncLock = new object(); + internal static SafeRclParamsHandle GlobalParamsHandle { get; private set; } + public static bool Ok() { return RCLdotnetDelegates.native_rcl_ok() != 0; @@ -1479,6 +1494,18 @@ public static void Init() string[] args = System.Environment.GetCommandLineArgs(); RCLRet ret = RCLdotnetDelegates.native_rcl_init(args.Length, args); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_init)}() failed."); + + SafeRclParamsHandle globalParamsHandle = new SafeRclParamsHandle(); + ret = RCLdotnetDelegates.native_rcl_arguments_get_param_overrides(ref globalParamsHandle); + + if (ret != RCLRet.Ok) + { + globalParamsHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_arguments_get_param_overrides)}() failed."); + } + + GlobalParamsHandle = globalParamsHandle; + initialized = true; } } diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 40be6421..486fe73e 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -19,10 +19,14 @@ #include #include #include +#include #include #include "rosidl_runtime_c/message_type_support_struct.h" +#include +#include + #include "rcldotnet.h" static rcl_context_t context; @@ -43,6 +47,88 @@ int32_t native_rcl_init(int argc, const char *argv[]) { return ret; } +int32_t native_rcl_arguments_get_param_overrides(void **parameter_overrides) { + rcl_params_t **global_parameter_overrides = (rcl_params_t **)parameter_overrides; + rcl_ret_t ret = rcl_arguments_get_param_overrides(&context.global_arguments, global_parameter_overrides); + return ret; +} + +void native_rcl_destroy_rcl_params(void *rcl_params) { + rcl_yaml_node_struct_fini((rcl_params_t *)rcl_params); +} + +bool native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name) { + const rcl_params_t *rcl_params = (const rcl_params_t *)params_handle; + const rcl_node_t *node = (const rcl_node_t *)node_handle; + const char *node_name = rcl_node_get_name(node); + + int node_index = 0; + for (; node_index < rcl_params->num_nodes; node_index++) { + if (strcmp(node_name, rcl_params->node_names[node_index])) { + break; + } + } + + if (node_index >= rcl_params->num_nodes) { + return false; + } + + const rcl_node_params_t *node_params = &rcl_params->params[node_index]; + + int param_index = 0; + for (; node_index < node_params->num_params; param_index++) { + if (strcmp(name, node_params->parameter_names[param_index])) { + break; + } + } + + if (param_index >= node_params->num_params) { + return false; + } + + rcl_variant_t *rcl_param_value = &node_params->parameter_values[param_index]; + + rcl_interfaces__msg__ParameterValue *param_value = (rcl_interfaces__msg__ParameterValue *)param_value_handle; + switch (param_value->type) { + case rcl_interfaces__msg__ParameterType__PARAMETER_BOOL: + param_value->bool_value = *rcl_param_value->bool_value; + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER: + param_value->integer_value = *rcl_param_value->integer_value; + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE: + param_value->double_value = *rcl_param_value->double_value; + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_STRING: + { + size_t length = strlen(rcl_param_value->string_value); + size_t capacity = sizeof(char) * length; + param_value->string_value.size = length; + param_value->string_value.capacity = capacity; + param_value->string_value.data = malloc(capacity); + strncpy(param_value->string_value.data, rcl_param_value->string_value, length); + break; + } + case rcl_interfaces__msg__ParameterType__PARAMETER_BYTE_ARRAY: + //TODO + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_BOOL_ARRAY: + //TODO + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER_ARRAY: + //TODO + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE_ARRAY: + //TODO + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_STRING_ARRAY: + //TODO + break; + } + + return true; +} + int32_t native_rcl_create_clock_handle(void **clock_handle, int32_t clock_type) { rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_clock_t *clock = malloc(sizeof(rcl_clock_t)); @@ -782,4 +868,4 @@ int32_t native_rcl_destroy_timer_handle(void *timer_handle) { free(timer); return ret; -} +} \ No newline at end of file diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index c1021a76..325f2951 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -20,6 +20,15 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_init(int argc, const char *argv[]); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_arguments_get_param_overrides(void **parameter_overrides); + +RCLDOTNET_EXPORT +void RCLDOTNET_CDECL native_rcl_destroy_rcl_params(void *rcl_params); + +RCLDOTNET_EXPORT +bool RCLDOTNET_CDECL native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_clock_handle(void **clock_handle, int32_t clock_type); From 639afccd7eca7892717e975f97491b8d6ae2542a Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Wed, 11 Oct 2023 16:07:28 +1000 Subject: [PATCH 12/15] Finished parameter override implementation --- rcldotnet/CMakeLists.txt | 2 + rcldotnet/Node.cs | 2 +- .../ParameterHandling/ParameterDelegates.cs | 45 +++++ .../ParameterHandling/ParameterHandler.cs | 26 ++- .../ParameterHandling/SafeRclParamsHandle.cs | 2 +- rcldotnet/RCLdotnet.cs | 11 +- rcldotnet/rcldotnet.c | 80 --------- rcldotnet/rcldotnet.h | 6 - rcldotnet/rcldotnet_params.c | 169 ++++++++++++++++++ rcldotnet/rcldotnet_params.h | 26 +++ 10 files changed, 268 insertions(+), 101 deletions(-) create mode 100644 rcldotnet/ParameterHandling/ParameterDelegates.cs create mode 100644 rcldotnet/rcldotnet_params.c create mode 100644 rcldotnet/rcldotnet_params.h diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index d8b97af9..a7743e72 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -69,6 +69,7 @@ set(CS_SOURCES ServiceDefinitionStaticMemberCache.cs Subscription.cs Timer.cs + ParameterHandling/ParameterDelegates.cs ParameterHandling/ParameterHandler.cs ParameterHandling/SafeRclParamsHandle.cs ParameterHandling/Exceptions/InvalidParameterTypeException.cs @@ -108,6 +109,7 @@ add_library(${PROJECT_NAME}_native SHARED rcldotnet_publisher.c rcldotnet_timer.c rcldotnet_qos_profile.c + rcldotnet_params.c rcldotnet.c ) diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index ccb8976e..8c22e20a 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -163,8 +163,8 @@ internal Node(SafeNodeHandle handle) _timers = new List(); _parameterHandler = new ParameterHandler(this); - _parameterHandler.DeclareParameter(ParameterNameSimulatedTime, false); _parameterHandler.AddOnSetParameterCallback(OnSetParameters); + _parameterHandler.DeclareParameter(ParameterNameSimulatedTime, false); } public string Name => RCLdotnet.GetStringFromNativeDelegate(NodeDelegates.native_rcl_node_get_name_handle, Handle); diff --git a/rcldotnet/ParameterHandling/ParameterDelegates.cs b/rcldotnet/ParameterHandling/ParameterDelegates.cs new file mode 100644 index 00000000..94ae8ee6 --- /dev/null +++ b/rcldotnet/ParameterHandling/ParameterDelegates.cs @@ -0,0 +1,45 @@ +/* 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 rcl_interfaces.msg; +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 bool NativeRCLTrGetParameterType(SafeHandle parameterValueHandle, SafeRclParamsHandle paramsHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string name); + + internal static NativeRCLTrGetParameterType 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); + } + } +} diff --git a/rcldotnet/ParameterHandling/ParameterHandler.cs b/rcldotnet/ParameterHandling/ParameterHandler.cs index 97aa6661..4b0b8426 100644 --- a/rcldotnet/ParameterHandling/ParameterHandler.cs +++ b/rcldotnet/ParameterHandling/ParameterHandler.cs @@ -15,8 +15,10 @@ using System; using System.Collections.Generic; +using System.Runtime.InteropServices; using rcl_interfaces.msg; using rcl_interfaces.srv; +using ROS2.ParameterHandling; using ROS2.ParameterHandling.Exceptions; namespace ROS2 @@ -137,11 +139,12 @@ private ParameterEvent GenerateParameterEventMessage() }; } - private void PublishParametersDeclaredEvent(IEnumerable parameters) + private void PublishParametersDeclaredEvent(List parameters) { ParameterEvent parameterEvent = GenerateParameterEventMessage(); parameterEvent.NewParameters.AddRange(parameters); _publisherEvent.Publish(parameterEvent); + _onSetParameterCallback?.Invoke(parameters); } private void PublishParametersChangedEvent(IEnumerable parameters) @@ -158,10 +161,20 @@ private void PublishParametersDeletedEvent(IEnumerable parameters) _publisherEvent.Publish(parameterEvent); } - private bool TryGetParameterOverride(string name, out Parameter parameterOverride) + private bool TryGetParameterOverride(string name, ref ParameterValue parameterOverride) { - parameterOverride = null; - return false; + if (!RCLdotnet.HasGlobalParameterOverrides) return false; + + bool overrideExists = false; + + using (SafeHandle messageHandle = MessageStaticMemberCache.CreateMessageHandle()) + { + RCLdotnet.WriteToMessageHandle(parameterOverride, messageHandle); + overrideExists = ParameterDelegates.native_rcl_try_get_parameter(messageHandle, RCLdotnet.GlobalParameterOverrideHandle, _node.Handle, name); + RCLdotnet.ReadFromMessageHandle(parameterOverride, messageHandle); + } + + return overrideExists; } private void DeclareParameter(string name, Type type, Action assignDefaultCallback, ParameterDescriptor descriptor = null) @@ -193,9 +206,10 @@ private void DeclareParameter(string name, Type type, Action ass return; } - if (!TryGetParameterOverride(name, out Parameter declaredParameter)) + ParameterValue declaredValue = new ParameterValue { Type = typeCode }; + Parameter declaredParameter = new Parameter { Name = name, Value = declaredValue }; + if (!TryGetParameterOverride(name, ref declaredValue)) { - declaredParameter = new Parameter { Name = name, Value = { Type = typeCode } }; assignDefaultCallback?.Invoke(declaredParameter.Value); } diff --git a/rcldotnet/ParameterHandling/SafeRclParamsHandle.cs b/rcldotnet/ParameterHandling/SafeRclParamsHandle.cs index 425b8d25..dce0d8c9 100644 --- a/rcldotnet/ParameterHandling/SafeRclParamsHandle.cs +++ b/rcldotnet/ParameterHandling/SafeRclParamsHandle.cs @@ -25,7 +25,7 @@ public SafeRclParamsHandle() : base(true) protected override bool ReleaseHandle() { - RCLdotnetDelegates.native_rcl_destroy_rcl_params(handle); + ParameterDelegates.native_rcl_destroy_rcl_params(handle); return true; } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 043c5e92..0327d8e9 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -53,11 +53,6 @@ internal delegate void NativeRCLGetErrorStringType( internal static NativeRCLArgumentsGetParamOverridesType native_rcl_arguments_get_param_overrides = null; - [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 RCLRet NativeRCLCreateNodeHandleType( ref SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string nodeName, [MarshalAs(UnmanagedType.LPStr)] string nodeNamespace); @@ -755,7 +750,8 @@ public static class RCLdotnet private static bool initialized = false; private static readonly object syncLock = new object(); - internal static SafeRclParamsHandle GlobalParamsHandle { get; private set; } + internal static bool HasGlobalParameterOverrides { get; private set; } + internal static SafeRclParamsHandle GlobalParameterOverrideHandle { get; private set; } public static bool Ok() { @@ -1504,7 +1500,8 @@ public static void Init() throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_arguments_get_param_overrides)}() failed."); } - GlobalParamsHandle = globalParamsHandle; + HasGlobalParameterOverrides = !globalParamsHandle.IsInvalid; + GlobalParameterOverrideHandle = globalParamsHandle; initialized = true; } diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 486fe73e..94ed2c77 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -19,14 +19,10 @@ #include #include #include -#include #include #include "rosidl_runtime_c/message_type_support_struct.h" -#include -#include - #include "rcldotnet.h" static rcl_context_t context; @@ -53,82 +49,6 @@ int32_t native_rcl_arguments_get_param_overrides(void **parameter_overrides) { return ret; } -void native_rcl_destroy_rcl_params(void *rcl_params) { - rcl_yaml_node_struct_fini((rcl_params_t *)rcl_params); -} - -bool native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name) { - const rcl_params_t *rcl_params = (const rcl_params_t *)params_handle; - const rcl_node_t *node = (const rcl_node_t *)node_handle; - const char *node_name = rcl_node_get_name(node); - - int node_index = 0; - for (; node_index < rcl_params->num_nodes; node_index++) { - if (strcmp(node_name, rcl_params->node_names[node_index])) { - break; - } - } - - if (node_index >= rcl_params->num_nodes) { - return false; - } - - const rcl_node_params_t *node_params = &rcl_params->params[node_index]; - - int param_index = 0; - for (; node_index < node_params->num_params; param_index++) { - if (strcmp(name, node_params->parameter_names[param_index])) { - break; - } - } - - if (param_index >= node_params->num_params) { - return false; - } - - rcl_variant_t *rcl_param_value = &node_params->parameter_values[param_index]; - - rcl_interfaces__msg__ParameterValue *param_value = (rcl_interfaces__msg__ParameterValue *)param_value_handle; - switch (param_value->type) { - case rcl_interfaces__msg__ParameterType__PARAMETER_BOOL: - param_value->bool_value = *rcl_param_value->bool_value; - break; - case rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER: - param_value->integer_value = *rcl_param_value->integer_value; - break; - case rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE: - param_value->double_value = *rcl_param_value->double_value; - break; - case rcl_interfaces__msg__ParameterType__PARAMETER_STRING: - { - size_t length = strlen(rcl_param_value->string_value); - size_t capacity = sizeof(char) * length; - param_value->string_value.size = length; - param_value->string_value.capacity = capacity; - param_value->string_value.data = malloc(capacity); - strncpy(param_value->string_value.data, rcl_param_value->string_value, length); - break; - } - case rcl_interfaces__msg__ParameterType__PARAMETER_BYTE_ARRAY: - //TODO - break; - case rcl_interfaces__msg__ParameterType__PARAMETER_BOOL_ARRAY: - //TODO - break; - case rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER_ARRAY: - //TODO - break; - case rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE_ARRAY: - //TODO - break; - case rcl_interfaces__msg__ParameterType__PARAMETER_STRING_ARRAY: - //TODO - break; - } - - return true; -} - int32_t native_rcl_create_clock_handle(void **clock_handle, int32_t clock_type) { rcl_allocator_t allocator = rcl_get_default_allocator(); rcl_clock_t *clock = malloc(sizeof(rcl_clock_t)); diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index 325f2951..c39dd080 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -23,12 +23,6 @@ int32_t RCLDOTNET_CDECL native_rcl_init(int argc, const char *argv[]); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_arguments_get_param_overrides(void **parameter_overrides); -RCLDOTNET_EXPORT -void RCLDOTNET_CDECL native_rcl_destroy_rcl_params(void *rcl_params); - -RCLDOTNET_EXPORT -bool RCLDOTNET_CDECL native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name); - RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_clock_handle(void **clock_handle, int32_t clock_type); diff --git a/rcldotnet/rcldotnet_params.c b/rcldotnet/rcldotnet_params.c new file mode 100644 index 00000000..57a6b15e --- /dev/null +++ b/rcldotnet/rcldotnet_params.c @@ -0,0 +1,169 @@ +// 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. + +#include +#include + +#include + +#include +#include + +#include "rcldotnet_params.h" + +typedef struct rcl_void_array_s { + /// Array with values + void *values; + /// Number of values in the array + size_t size; +} rcl_void_array_t; + +ROSIDL_RUNTIME_C__PRIMITIVE_SEQUENCE(void, void) + +void native_rcl_destroy_rcl_params(void *rcl_params) { + rcl_yaml_node_struct_fini((rcl_params_t *)rcl_params); +} + +void copy_to_rosidl_runtime_c__String(rosidl_runtime_c__String *dest, const char * src) { + size_t length = strlen(src); + size_t capacity = sizeof(char) * (length + 1); + dest->size = length; + + if (dest->capacity != capacity) { + if (dest->capacity != 0) { + free(dest->data); + } + + dest->capacity = capacity; + dest->data = malloc(capacity); + } + + memcpy(dest->data, src, capacity); +} + +void copy_yaml_array_to_parameter_array(rosidl_runtime_c__void__Sequence *dest, const rcl_void_array_t *src, size_t element_size) { + size_t length = src->size; + size_t length_bytes = length * element_size; + dest->size = length; + + if (dest->capacity != length) { + if (dest->capacity > 0) { + free(dest->data); + } + + dest->capacity = length; + dest->data = malloc(length_bytes); + } + + memcpy(dest->data, src->values, length_bytes); +} + +void copy_yaml_string_array_to_parameter_string_array(rosidl_runtime_c__String__Sequence *dest, rcutils_string_array_t *src) { + size_t length = src->size; + + if (dest->capacity != length) { + + if (dest->capacity != 0) { + for (int i = 0; i < dest->capacity; i++) { + free(dest->data->data); + } + + free(dest->data); + } + + dest->capacity = length; + dest->data = malloc(length * sizeof(rosidl_runtime_c__String)); + + // If the elements aren't initialised, free may be run on an uninitialised data pointer. + for (int i = 0; i < length; i++) { + dest->data[i].capacity = 0; + dest->data[i].size = 0; + } + } + + dest->size = length; + + for (int i = 0; i < length; i++) { + rosidl_runtime_c__String *dest_element = &dest->data[i]; + copy_to_rosidl_runtime_c__String(&(dest->data[i]), src->data[i]); + } +} + +bool native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name) { + if (params_handle == NULL) return false; + + const rcl_params_t *rcl_params = (const rcl_params_t *)params_handle; + const rcl_node_t *node = (const rcl_node_t *)node_handle; + const char *node_name = rcl_node_get_name(node); + + int node_index = 0; + for (; node_index < rcl_params->num_nodes; node_index++) { + if (strcmp(node_name, rcl_params->node_names[node_index])) { + break; + } + } + + if (node_index >= rcl_params->num_nodes) { + return false; + } + + const rcl_node_params_t *node_params = &rcl_params->params[node_index]; + + int param_index = 0; + for (; param_index < node_params->num_params; param_index++) { + if (strcmp(name, node_params->parameter_names[param_index]) == 0) { + break; + } + } + + if (param_index >= node_params->num_params) { + return false; + } + + rcl_variant_t *rcl_param_value = &node_params->parameter_values[param_index]; + + rcl_interfaces__msg__ParameterValue *param_value = (rcl_interfaces__msg__ParameterValue *)param_value_handle; + switch (param_value->type) { + case rcl_interfaces__msg__ParameterType__PARAMETER_BOOL: + param_value->bool_value = *rcl_param_value->bool_value; + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER: + param_value->integer_value = *rcl_param_value->integer_value; + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE: + param_value->double_value = *rcl_param_value->double_value; + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_STRING: + copy_to_rosidl_runtime_c__String(¶m_value->string_value, rcl_param_value->string_value); + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_BYTE_ARRAY: + // Byte array parameter loading from YAML not implemented in RCL. + //copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->byte_array_value, (rcl_void_array_t *)rcl_param_value->byte_array_value, sizeof(char)); + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_BOOL_ARRAY: + copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->bool_array_value, (rcl_void_array_t *)rcl_param_value->bool_array_value, sizeof(bool)); + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER_ARRAY: + copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->integer_array_value, (rcl_void_array_t *)rcl_param_value->integer_array_value, sizeof(int64_t)); + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE_ARRAY: + copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->double_array_value, (rcl_void_array_t *)rcl_param_value->double_array_value, sizeof(double)); + break; + case rcl_interfaces__msg__ParameterType__PARAMETER_STRING_ARRAY: + copy_yaml_string_array_to_parameter_string_array(¶m_value->string_array_value, rcl_param_value->string_array_value); + break; + } + + return true; +} \ No newline at end of file diff --git a/rcldotnet/rcldotnet_params.h b/rcldotnet/rcldotnet_params.h new file mode 100644 index 00000000..3881d9a3 --- /dev/null +++ b/rcldotnet/rcldotnet_params.h @@ -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. + +#ifndef RCLDOTNET_PARAMS_H +#define RCLDOTNET_PARAMS_H + +#include "rcldotnet_macros.h" + +RCLDOTNET_EXPORT +void RCLDOTNET_CDECL native_rcl_destroy_rcl_params(void *rcl_params); + +RCLDOTNET_EXPORT +bool RCLDOTNET_CDECL native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name); + +#endif // RCLDOTNET_PARAMS_H From f74b787ae28228c7d1868a69aee9923004c107de Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Wed, 11 Oct 2023 16:54:19 +1000 Subject: [PATCH 13/15] Fixing param override node name searching --- rcldotnet/rcldotnet_params.c | 51 ++++++++++++++++++++---------------- 1 file changed, 29 insertions(+), 22 deletions(-) diff --git a/rcldotnet/rcldotnet_params.c b/rcldotnet/rcldotnet_params.c index 57a6b15e..61a56348 100644 --- a/rcldotnet/rcldotnet_params.c +++ b/rcldotnet/rcldotnet_params.c @@ -100,26 +100,7 @@ void copy_yaml_string_array_to_parameter_string_array(rosidl_runtime_c__String__ } } -bool native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name) { - if (params_handle == NULL) return false; - - const rcl_params_t *rcl_params = (const rcl_params_t *)params_handle; - const rcl_node_t *node = (const rcl_node_t *)node_handle; - const char *node_name = rcl_node_get_name(node); - - int node_index = 0; - for (; node_index < rcl_params->num_nodes; node_index++) { - if (strcmp(node_name, rcl_params->node_names[node_index])) { - break; - } - } - - if (node_index >= rcl_params->num_nodes) { - return false; - } - - const rcl_node_params_t *node_params = &rcl_params->params[node_index]; - +bool try_get_parameter_from_node_params(const rcl_node_params_t *node_params, const char *name, rcl_interfaces__msg__ParameterValue *param_value) { int param_index = 0; for (; param_index < node_params->num_params; param_index++) { if (strcmp(name, node_params->parameter_names[param_index]) == 0) { @@ -133,7 +114,6 @@ bool native_rcl_try_get_parameter(void *param_value_handle, const void *params_h rcl_variant_t *rcl_param_value = &node_params->parameter_values[param_index]; - rcl_interfaces__msg__ParameterValue *param_value = (rcl_interfaces__msg__ParameterValue *)param_value_handle; switch (param_value->type) { case rcl_interfaces__msg__ParameterType__PARAMETER_BOOL: param_value->bool_value = *rcl_param_value->bool_value; @@ -164,6 +144,33 @@ bool native_rcl_try_get_parameter(void *param_value_handle, const void *params_h copy_yaml_string_array_to_parameter_string_array(¶m_value->string_array_value, rcl_param_value->string_array_value); break; } +} + +bool native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name) { + if (params_handle == NULL) return false; + + rcl_interfaces__msg__ParameterValue *param_value = (rcl_interfaces__msg__ParameterValue *)param_value_handle; + const rcl_params_t *rcl_params = (const rcl_params_t *)params_handle; + const rcl_node_t *node = (const rcl_node_t *)node_handle; + const char *node_name = rcl_node_get_fully_qualified_name(node); + + // First check if there is an override which matches the fully qualified node name. + for (int i = 0; i < rcl_params->num_nodes; i++) { + if (strcmp(node_name, rcl_params->node_names[i]) == 0) { + if (try_get_parameter_from_node_params(&rcl_params->params[i], name, param_value)) { + return true; + } + } + } + + // Then check if there is a global override. + for (int i = 0; i < rcl_params->num_nodes; i++) { + if (strcmp("/**", rcl_params->node_names[i]) == 0) { + if (try_get_parameter_from_node_params(&rcl_params->params[i], name, param_value)) { + return true; + } + } + } - return true; + return false; } \ No newline at end of file From 2f53181f2874466d9e41fac2517d21459467d826 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Thu, 12 Oct 2023 16:36:30 +1000 Subject: [PATCH 14/15] Using safer typing and improving style --- .../ParameterHandling/ParameterDelegates.cs | 4 +-- .../ParameterHandling/ParameterHandler.cs | 2 +- rcldotnet/rcldotnet_params.c | 30 ++++++++++--------- rcldotnet/rcldotnet_params.h | 2 +- 4 files changed, 20 insertions(+), 18 deletions(-) diff --git a/rcldotnet/ParameterHandling/ParameterDelegates.cs b/rcldotnet/ParameterHandling/ParameterDelegates.cs index 94ae8ee6..0a955364 100644 --- a/rcldotnet/ParameterHandling/ParameterDelegates.cs +++ b/rcldotnet/ParameterHandling/ParameterDelegates.cs @@ -29,9 +29,9 @@ internal static class ParameterDelegates internal static NativeRCLDestroyRclParamsType native_rcl_destroy_rcl_params = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)] - internal delegate bool NativeRCLTrGetParameterType(SafeHandle parameterValueHandle, SafeRclParamsHandle paramsHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string name); + internal delegate int NativeRCLTryGetParameterType(SafeHandle parameterValueHandle, SafeRclParamsHandle paramsHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string name); - internal static NativeRCLTrGetParameterType native_rcl_try_get_parameter = null; + internal static NativeRCLTryGetParameterType native_rcl_try_get_parameter = null; static ParameterDelegates() { diff --git a/rcldotnet/ParameterHandling/ParameterHandler.cs b/rcldotnet/ParameterHandling/ParameterHandler.cs index 4b0b8426..e643dbb5 100644 --- a/rcldotnet/ParameterHandling/ParameterHandler.cs +++ b/rcldotnet/ParameterHandling/ParameterHandler.cs @@ -170,7 +170,7 @@ private bool TryGetParameterOverride(string name, ref ParameterValue parameterOv using (SafeHandle messageHandle = MessageStaticMemberCache.CreateMessageHandle()) { RCLdotnet.WriteToMessageHandle(parameterOverride, messageHandle); - overrideExists = ParameterDelegates.native_rcl_try_get_parameter(messageHandle, RCLdotnet.GlobalParameterOverrideHandle, _node.Handle, name); + overrideExists = ParameterDelegates.native_rcl_try_get_parameter(messageHandle, RCLdotnet.GlobalParameterOverrideHandle, _node.Handle, name) != 0; RCLdotnet.ReadFromMessageHandle(parameterOverride, messageHandle); } diff --git a/rcldotnet/rcldotnet_params.c b/rcldotnet/rcldotnet_params.c index 61a56348..4c9fae5d 100644 --- a/rcldotnet/rcldotnet_params.c +++ b/rcldotnet/rcldotnet_params.c @@ -35,7 +35,7 @@ void native_rcl_destroy_rcl_params(void *rcl_params) { rcl_yaml_node_struct_fini((rcl_params_t *)rcl_params); } -void copy_to_rosidl_runtime_c__String(rosidl_runtime_c__String *dest, const char * src) { +void rcldotnet_params_copy_to_rosidl_runtime_c__String(rosidl_runtime_c__String *dest, const char * src) { size_t length = strlen(src); size_t capacity = sizeof(char) * (length + 1); dest->size = length; @@ -52,7 +52,7 @@ void copy_to_rosidl_runtime_c__String(rosidl_runtime_c__String *dest, const char memcpy(dest->data, src, capacity); } -void copy_yaml_array_to_parameter_array(rosidl_runtime_c__void__Sequence *dest, const rcl_void_array_t *src, size_t element_size) { +void rcldotnet_params_copy_yaml_array_to_parameter_array(rosidl_runtime_c__void__Sequence *dest, const rcl_void_array_t *src, size_t element_size) { size_t length = src->size; size_t length_bytes = length * element_size; dest->size = length; @@ -69,7 +69,7 @@ void copy_yaml_array_to_parameter_array(rosidl_runtime_c__void__Sequence *dest, memcpy(dest->data, src->values, length_bytes); } -void copy_yaml_string_array_to_parameter_string_array(rosidl_runtime_c__String__Sequence *dest, rcutils_string_array_t *src) { +void rcldotnet_params_copy_yaml_string_array_to_parameter_string_array(rosidl_runtime_c__String__Sequence *dest, rcutils_string_array_t *src) { size_t length = src->size; if (dest->capacity != length) { @@ -96,11 +96,11 @@ void copy_yaml_string_array_to_parameter_string_array(rosidl_runtime_c__String__ for (int i = 0; i < length; i++) { rosidl_runtime_c__String *dest_element = &dest->data[i]; - copy_to_rosidl_runtime_c__String(&(dest->data[i]), src->data[i]); + rcldotnet_params_copy_to_rosidl_runtime_c__String(&(dest->data[i]), src->data[i]); } } -bool try_get_parameter_from_node_params(const rcl_node_params_t *node_params, const char *name, rcl_interfaces__msg__ParameterValue *param_value) { +bool rcldotnet_params_try_get_parameter_from_node_params(const rcl_node_params_t *node_params, const char *name, rcl_interfaces__msg__ParameterValue *param_value) { int param_index = 0; for (; param_index < node_params->num_params; param_index++) { if (strcmp(name, node_params->parameter_names[param_index]) == 0) { @@ -125,28 +125,30 @@ bool try_get_parameter_from_node_params(const rcl_node_params_t *node_params, co param_value->double_value = *rcl_param_value->double_value; break; case rcl_interfaces__msg__ParameterType__PARAMETER_STRING: - copy_to_rosidl_runtime_c__String(¶m_value->string_value, rcl_param_value->string_value); + rcldotnet_params_copy_to_rosidl_runtime_c__String(¶m_value->string_value, rcl_param_value->string_value); break; case rcl_interfaces__msg__ParameterType__PARAMETER_BYTE_ARRAY: // Byte array parameter loading from YAML not implemented in RCL. - //copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->byte_array_value, (rcl_void_array_t *)rcl_param_value->byte_array_value, sizeof(char)); + //rcldotnet_params_copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->byte_array_value, (rcl_void_array_t *)rcl_param_value->byte_array_value, sizeof(char)); break; case rcl_interfaces__msg__ParameterType__PARAMETER_BOOL_ARRAY: - copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->bool_array_value, (rcl_void_array_t *)rcl_param_value->bool_array_value, sizeof(bool)); + rcldotnet_params_copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->bool_array_value, (rcl_void_array_t *)rcl_param_value->bool_array_value, sizeof(bool)); break; case rcl_interfaces__msg__ParameterType__PARAMETER_INTEGER_ARRAY: - copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->integer_array_value, (rcl_void_array_t *)rcl_param_value->integer_array_value, sizeof(int64_t)); + rcldotnet_params_copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->integer_array_value, (rcl_void_array_t *)rcl_param_value->integer_array_value, sizeof(int64_t)); break; case rcl_interfaces__msg__ParameterType__PARAMETER_DOUBLE_ARRAY: - copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->double_array_value, (rcl_void_array_t *)rcl_param_value->double_array_value, sizeof(double)); + rcldotnet_params_copy_yaml_array_to_parameter_array((rosidl_runtime_c__void__Sequence *)¶m_value->double_array_value, (rcl_void_array_t *)rcl_param_value->double_array_value, sizeof(double)); break; case rcl_interfaces__msg__ParameterType__PARAMETER_STRING_ARRAY: - copy_yaml_string_array_to_parameter_string_array(¶m_value->string_array_value, rcl_param_value->string_array_value); + rcldotnet_params_copy_yaml_string_array_to_parameter_string_array(¶m_value->string_array_value, rcl_param_value->string_array_value); break; } + + return true; } -bool native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name) { +int32_t /* bool */ native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name) { if (params_handle == NULL) return false; rcl_interfaces__msg__ParameterValue *param_value = (rcl_interfaces__msg__ParameterValue *)param_value_handle; @@ -157,7 +159,7 @@ bool native_rcl_try_get_parameter(void *param_value_handle, const void *params_h // First check if there is an override which matches the fully qualified node name. for (int i = 0; i < rcl_params->num_nodes; i++) { if (strcmp(node_name, rcl_params->node_names[i]) == 0) { - if (try_get_parameter_from_node_params(&rcl_params->params[i], name, param_value)) { + if (rcldotnet_params_try_get_parameter_from_node_params(&rcl_params->params[i], name, param_value)) { return true; } } @@ -166,7 +168,7 @@ bool native_rcl_try_get_parameter(void *param_value_handle, const void *params_h // Then check if there is a global override. for (int i = 0; i < rcl_params->num_nodes; i++) { if (strcmp("/**", rcl_params->node_names[i]) == 0) { - if (try_get_parameter_from_node_params(&rcl_params->params[i], name, param_value)) { + if (rcldotnet_params_try_get_parameter_from_node_params(&rcl_params->params[i], name, param_value)) { return true; } } diff --git a/rcldotnet/rcldotnet_params.h b/rcldotnet/rcldotnet_params.h index 3881d9a3..c409fa35 100644 --- a/rcldotnet/rcldotnet_params.h +++ b/rcldotnet/rcldotnet_params.h @@ -21,6 +21,6 @@ RCLDOTNET_EXPORT void RCLDOTNET_CDECL native_rcl_destroy_rcl_params(void *rcl_params); RCLDOTNET_EXPORT -bool RCLDOTNET_CDECL native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name); +int32_t /* bool */ RCLDOTNET_CDECL native_rcl_try_get_parameter(void *param_value_handle, const void *params_handle, const void *node_handle, const char *name); #endif // RCLDOTNET_PARAMS_H From 1ba1c52671ccb81ab4bf3803e131c6ff59c48d51 Mon Sep 17 00:00:00 2001 From: Alec Tutin Date: Wed, 17 Jan 2024 09:53:53 +1000 Subject: [PATCH 15/15] Removing unused import --- rcldotnet/ParameterHandling/ParameterDelegates.cs | 1 - rcldotnet_examples/RCLDotnetTalker.cs | 1 - 2 files changed, 2 deletions(-) diff --git a/rcldotnet/ParameterHandling/ParameterDelegates.cs b/rcldotnet/ParameterHandling/ParameterDelegates.cs index 0a955364..cf801e3b 100644 --- a/rcldotnet/ParameterHandling/ParameterDelegates.cs +++ b/rcldotnet/ParameterHandling/ParameterDelegates.cs @@ -15,7 +15,6 @@ using System; using System.Runtime.InteropServices; -using rcl_interfaces.msg; using ROS2.Utils; namespace ROS2.ParameterHandling { diff --git a/rcldotnet_examples/RCLDotnetTalker.cs b/rcldotnet_examples/RCLDotnetTalker.cs index 18971d78..5b8e8e45 100644 --- a/rcldotnet_examples/RCLDotnetTalker.cs +++ b/rcldotnet_examples/RCLDotnetTalker.cs @@ -1,5 +1,4 @@ using System; -using rcl_interfaces.msg; using ROS2; namespace ConsoleApplication