diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 257b2751..a7743e72 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) @@ -68,13 +69,23 @@ set(CS_SOURCES ServiceDefinitionStaticMemberCache.cs Subscription.cs Timer.cs + ParameterHandling/ParameterDelegates.cs + ParameterHandling/ParameterHandler.cs + ParameterHandling/SafeRclParamsHandle.cs + ParameterHandling/Exceptions/InvalidParameterTypeException.cs + ParameterHandling/Exceptions/ParameterException.cs + ParameterHandling/Exceptions/ParameterImmutableException.cs + ParameterHandling/Exceptions/ParameterNotDeclaredException.cs + ParameterHandling/Exceptions/ParameterTypeMismatchException.cs ) find_package(rcldotnet_common REQUIRED) set(_assemblies_dep_dlls ${action_msgs_ASSEMBLIES_DLL} + ${rosgraph_msgs_ASSEMBLIES_DLL} ${builtin_interfaces_ASSEMBLIES_DLL} + ${rcl_interfaces_ASSEMBLIES_DLL} ${rcldotnet_common_ASSEMBLIES_DLL} ${unique_identifier_msgs_ASSEMBLIES_DLL} ) @@ -98,11 +109,13 @@ add_library(${PROJECT_NAME}_native SHARED rcldotnet_publisher.c rcldotnet_timer.c rcldotnet_qos_profile.c + rcldotnet_params.c rcldotnet.c ) ament_target_dependencies(${PROJECT_NAME}_native "action_msgs" + "rosgraph_msgs" "builtin_interfaces" "unique_identifier_msgs" "rcl" diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 80b5549a..8c22e20a 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -16,6 +16,7 @@ using System; using System.Collections.Generic; using System.Runtime.InteropServices; +using rcl_interfaces.msg; using ROS2.Utils; namespace ROS2 @@ -127,6 +128,8 @@ static NodeDelegates() public sealed class Node { + private const string ParameterNameSimulatedTime = "use_sim_time"; + private readonly Clock _clock; private readonly IList _subscriptions; @@ -143,6 +146,8 @@ public sealed class Node private readonly IList _timers; + private readonly ParameterHandler _parameterHandler; + internal Node(SafeNodeHandle handle) { Handle = handle; @@ -156,6 +161,10 @@ internal Node(SafeNodeHandle handle) _actionClients = new List(); _actionServers = new List(); _timers = new List(); + + _parameterHandler = new ParameterHandler(this); + _parameterHandler.AddOnSetParameterCallback(OnSetParameters); + _parameterHandler.DeclareParameter(ParameterNameSimulatedTime, false); } public string Name => RCLdotnet.GetStringFromNativeDelegate(NodeDelegates.native_rcl_node_get_name_handle, Handle); @@ -190,6 +199,36 @@ internal Node(SafeNodeHandle handle) // Disposed if the node is not live anymore. internal SafeNodeHandle Handle { get; } + private Subscription ClockSubscription { get; set; } + + private void OnSetParameters(List parameters) + { + Parameter simulatedTimeParameter = parameters.Find(parameter => parameter.Name == ParameterNameSimulatedTime); + if (simulatedTimeParameter == null) return; + + // Update clock setup if applicable. + bool subscribedToClock = ClockSubscription != null; + bool useSimulatedTime = simulatedTimeParameter.Value.BoolValue; + if (useSimulatedTime == subscribedToClock) return; + + if (useSimulatedTime) + { + ClockSubscription = CreateSubscription("/clock", OnClockMessage, QosProfile.ClockProfile); + _clock.EnableRosTimeOverride(); + } + else + { + DestroySubscription(ClockSubscription); + ClockSubscription = null; + _clock.DisableRosTimeOverride(); + } + } + + private void OnClockMessage(rosgraph_msgs.msg.Clock message) + { + _clock.SetRosTimeOverride(TimePoint.FromMsg(message.Clock_).nanoseconds); + } + public Publisher CreatePublisher(string topic, QosProfile qosProfile = null) where T : IRosMessage { if (qosProfile != null) @@ -259,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() @@ -414,5 +462,64 @@ private static CancelResponse DefaultCancelCallback(ActionServerGoalHandle goalH { return CancelResponse.Reject; } + + #region Parameter Handling Passthroughs + + public void DeclareParameter(string name, bool defaultValue = false, ParameterDescriptor descriptor = null) => + _parameterHandler.DeclareParameter(name, defaultValue, descriptor); + + public void DeclareParameter(string name, int defaultValue = 0, ParameterDescriptor descriptor = null) => + _parameterHandler.DeclareParameter(name, defaultValue, descriptor); + + public void DeclareParameter(string name, long defaultValue = 0L, ParameterDescriptor descriptor = null) => + _parameterHandler.DeclareParameter(name, defaultValue, descriptor); + + public void DeclareParameter(string name, float defaultValue = 0.0f, ParameterDescriptor descriptor = null) => + _parameterHandler.DeclareParameter(name, defaultValue, descriptor); + + public void DeclareParameter(string name, double defaultValue = 0.0, ParameterDescriptor descriptor = null) => + _parameterHandler.DeclareParameter(name, defaultValue, descriptor); + + public void DeclareParameter(string name, string defaultValue = "", ParameterDescriptor descriptor = null) => + _parameterHandler.DeclareParameter(name, defaultValue, descriptor); + + public void DeclareParameter(string name, IEnumerable 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 } } 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/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/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/ParameterHandling/Exceptions/ParameterTypeMismatchException.cs b/rcldotnet/ParameterHandling/Exceptions/ParameterTypeMismatchException.cs new file mode 100644 index 00000000..fa05e665 --- /dev/null +++ b/rcldotnet/ParameterHandling/Exceptions/ParameterTypeMismatchException.cs @@ -0,0 +1,32 @@ +/* Copyright 2023 Queensland University of Technology. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +using System; +using rcl_interfaces.msg; + +namespace ROS2.ParameterHandling.Exceptions +{ + public class ParameterTypeMismatchException : ParameterException + { + public ParameterTypeMismatchException(string message) : base(message) + { + } + + public ParameterTypeMismatchException(string name, Type parameterType, Type requestedType) : this( + $"Parameter with name \"{name}\" is of type \"{parameterType.Name}\" it cannot be set with a value of type \"{requestedType.Name}\"!") + { + } + } +} diff --git a/rcldotnet/ParameterHandling/ParameterDelegates.cs b/rcldotnet/ParameterHandling/ParameterDelegates.cs new file mode 100644 index 00000000..cf801e3b --- /dev/null +++ b/rcldotnet/ParameterHandling/ParameterDelegates.cs @@ -0,0 +1,44 @@ +/* Copyright 2023 Queensland University of Technology. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +using System; +using System.Runtime.InteropServices; +using ROS2.Utils; + +namespace ROS2.ParameterHandling { + internal static class ParameterDelegates + { + private static readonly DllLoadUtils _dllLoadUtils; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate void NativeRCLDestroyRclParamsType(IntPtr paramsHandle); + + internal static NativeRCLDestroyRclParamsType native_rcl_destroy_rcl_params = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl, CharSet = CharSet.Ansi)] + internal delegate int NativeRCLTryGetParameterType(SafeHandle parameterValueHandle, SafeRclParamsHandle paramsHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string name); + + internal static NativeRCLTryGetParameterType native_rcl_try_get_parameter = null; + + static ParameterDelegates() + { + _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); + + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_destroy_rcl_params), out native_rcl_destroy_rcl_params); + _dllLoadUtils.RegisterNativeFunction(nativeLibrary, nameof(native_rcl_try_get_parameter), out native_rcl_try_get_parameter); + } + } +} diff --git a/rcldotnet/ParameterHandling/ParameterHandler.cs b/rcldotnet/ParameterHandling/ParameterHandler.cs new file mode 100644 index 00000000..e643dbb5 --- /dev/null +++ b/rcldotnet/ParameterHandling/ParameterHandler.cs @@ -0,0 +1,479 @@ +/* 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 System.Runtime.InteropServices; +using rcl_interfaces.msg; +using rcl_interfaces.srv; +using ROS2.ParameterHandling; +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 Node _node; + private readonly IDictionary _parameters = new Dictionary(); + private readonly IDictionary _descriptors = new Dictionary(); + + private readonly Publisher _publisherEvent; + + private Action> _onSetParameterCallback; + + internal ParameterHandler(Node node) + { + _node = node; + _publisherEvent = node.CreatePublisher("/parameter_events", QosProfile.ParameterEventsProfile); + + 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; + } + + public void RemoveOnSetParameterCallback(Action> callback) + { + _onSetParameterCallback -= callback; + } + + private ParameterEvent GenerateParameterEventMessage() + { + return new ParameterEvent + { + Node = _node.FullyQualifiedName, + Stamp = _node.Clock.Now() + }; + } + + private void PublishParametersDeclaredEvent(List parameters) + { + ParameterEvent parameterEvent = GenerateParameterEventMessage(); + parameterEvent.NewParameters.AddRange(parameters); + _publisherEvent.Publish(parameterEvent); + _onSetParameterCallback?.Invoke(parameters); + } + + private void PublishParametersChangedEvent(IEnumerable parameters) + { + ParameterEvent parameterEvent = GenerateParameterEventMessage(); + parameterEvent.ChangedParameters.AddRange(parameters); + _publisherEvent.Publish(parameterEvent); + } + + private void PublishParametersDeletedEvent(IEnumerable parameters) + { + ParameterEvent parameterEvent = GenerateParameterEventMessage(); + parameterEvent.DeletedParameters.AddRange(parameters); + _publisherEvent.Publish(parameterEvent); + } + + private bool TryGetParameterOverride(string name, ref ParameterValue parameterOverride) + { + 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) != 0; + RCLdotnet.ReadFromMessageHandle(parameterOverride, messageHandle); + } + + return overrideExists; + } + + private void DeclareParameter(string name, Type type, Action assignDefaultCallback, 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: Should we update the description if it doesn't match or throw an error? + return; + } + + ParameterValue declaredValue = new ParameterValue { Type = typeCode }; + Parameter declaredParameter = new Parameter { Name = name, Value = declaredValue }; + if (!TryGetParameterOverride(name, ref declaredValue)) + { + assignDefaultCallback?.Invoke(declaredParameter.Value); + } + + _parameters.Add(name, declaredParameter); + _descriptors.Add(name, descriptor); + + PublishParametersDeclaredEvent(new List { declaredParameter }); + } + + public void DeclareParameter(string name, bool defaultValue = false, ParameterDescriptor descriptor = null) + { + DeclareParameter(name, typeof(bool), value => { value.BoolValue = defaultValue; }, descriptor); + } + + public void DeclareParameter(string name, int defaultValue = 0, ParameterDescriptor descriptor = null) => DeclareParameter(name, (long)defaultValue, descriptor); + + public void DeclareParameter(string name, long defaultValue = 0L, ParameterDescriptor descriptor = null) + { + DeclareParameter(name, typeof(long), value => { value.IntegerValue = defaultValue; }, descriptor); + } + + 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, ParameterDescriptor descriptor = null) + { + DeclareParameter(name, typeof(double), value => { value.DoubleValue = defaultValue; }, descriptor); + } + + public void DeclareParameter(string name, string defaultValue = "", ParameterDescriptor descriptor = null) + { + DeclareParameter(name, typeof(string), value => { value.StringValue = defaultValue; }, descriptor); + } + + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) + { + DeclareParameter(name, typeof(List), value => + { + if (defaultValue != null) value.ByteArrayValue.AddRange(defaultValue); + }, descriptor); + } + + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) + { + DeclareParameter(name, typeof(List), value => + { + if (defaultValue != null) value.BoolArrayValue.AddRange(defaultValue); + }, descriptor); + } + + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) + { + DeclareParameter(name, typeof(List), value => + { + if (defaultValue != null) value.IntegerArrayValue.AddRange(defaultValue); + }, descriptor); + } + + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) + { + DeclareParameter(name, typeof(List), value => + { + if (defaultValue != null) value.DoubleArrayValue.AddRange(defaultValue); + }, descriptor); + } + + public void DeclareParameter(string name, IEnumerable defaultValue = null, ParameterDescriptor descriptor = null) + { + DeclareParameter(name, typeof(List), value => + { + if (defaultValue != null) value.StringArrayValue.AddRange(defaultValue); + }, descriptor); + } + + public void UndeclareParameter(string name) + { + if (!_descriptors.TryGetValue(name, out ParameterDescriptor descriptor)) + { + throw new ParameterNotDeclaredException(name); + } + + 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) + { + 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 ParameterValue GetParameter(string name) + { + if (_parameters.TryGetValue(name, out Parameter parameter)) + { + return CloneParameterValue(parameter.Value); + } + + throw new ParameterNotDeclaredException(name); + } + + 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 SetParametersResult CheckParameterCompatibility(Parameter update) + { + SetParametersResult result = new SetParametersResult(); + if (!_descriptors.TryGetValue(update.Name, out ParameterDescriptor descriptor)) + { + result.Successful = false; + result.Reason = "Parameter was not declared!"; + } + else if (descriptor.ReadOnly) + { + result.Successful = false; + result.Reason = "Parameter is read-only!"; + } + else if (update.Value.Type != descriptor.Type) + { + result.Successful = false; + result.Reason = $"Parameter type mismatch: {descriptor.Type} != {update.Value.Type}!"; + } + // TODO: Check value compatibility against ParameterDescriptor constraints. + 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 SetParametersResult SetParameter(Parameter parameter) + { + return SetParametersAtomically(new List { parameter }); + } + + public List SetParameters(List parameters) + { + List results = new List(); + + foreach (Parameter source in parameters) + { + results.Add(SetParametersAtomically(new List { source })); + } + + return results; + } + + 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); + } + + PublishParametersChangedEvent(parameters); + _onSetParameterCallback?.Invoke(parameters); + + return result; + } + + public bool HasParameter(string name) => _parameters.ContainsKey(name); + } +} diff --git a/rcldotnet/ParameterHandling/SafeRclParamsHandle.cs b/rcldotnet/ParameterHandling/SafeRclParamsHandle.cs new file mode 100644 index 00000000..dce0d8c9 --- /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() + { + ParameterDelegates.native_rcl_destroy_rcl_params(handle); + return true; + } + } +} diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index eeefea8a..0327d8e9 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,11 @@ 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, CharSet = CharSet.Ansi)] internal delegate RCLRet NativeRCLCreateNodeHandleType( ref SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string nodeName, [MarshalAs(UnmanagedType.LPStr)] string nodeNamespace); @@ -729,6 +735,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 +750,9 @@ public static class RCLdotnet private static bool initialized = false; private static readonly object syncLock = new object(); + internal static bool HasGlobalParameterOverrides { get; private set; } + internal static SafeRclParamsHandle GlobalParameterOverrideHandle { get; private set; } + public static bool Ok() { return RCLdotnetDelegates.native_rcl_ok() != 0; @@ -1479,6 +1490,19 @@ 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."); + } + + HasGlobalParameterOverrides = !globalParamsHandle.IsInvalid; + GlobalParameterOverrideHandle = globalParamsHandle; + initialized = true; } } 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 diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 40be6421..94ed2c77 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -43,6 +43,12 @@ 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; +} + 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 +788,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..c39dd080 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -20,6 +20,9 @@ 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 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..4c9fae5d --- /dev/null +++ b/rcldotnet/rcldotnet_params.c @@ -0,0 +1,178 @@ +// 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 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; + + if (dest->capacity != capacity) { + if (dest->capacity != 0) { + free(dest->data); + } + + dest->capacity = capacity; + dest->data = malloc(capacity); + } + + memcpy(dest->data, src, capacity); +} + +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; + + 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 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) { + + 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]; + rcldotnet_params_copy_to_rosidl_runtime_c__String(&(dest->data[i]), src->data[i]); + } +} + +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) { + break; + } + } + + if (param_index >= node_params->num_params) { + return false; + } + + rcl_variant_t *rcl_param_value = &node_params->parameter_values[param_index]; + + 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: + 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. + //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: + 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: + 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: + 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: + rcldotnet_params_copy_yaml_string_array_to_parameter_string_array(¶m_value->string_array_value, rcl_param_value->string_array_value); + break; + } + + return true; +} + +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; + 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 (rcldotnet_params_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 (rcldotnet_params_try_get_parameter_from_node_params(&rcl_params->params[i], name, param_value)) { + return true; + } + } + } + + return false; +} \ No newline at end of file diff --git a/rcldotnet/rcldotnet_params.h b/rcldotnet/rcldotnet_params.h new file mode 100644 index 00000000..c409fa35 --- /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 +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 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..5b8e8e45 100644 --- a/rcldotnet_examples/RCLDotnetTalker.cs +++ b/rcldotnet_examples/RCLDotnetTalker.cs @@ -15,6 +15,7 @@ private RCLDotnetTalker() { RCLdotnet.Init(); _node = RCLdotnet.CreateNode("talker"); + _node.DeclareParameter("publish_string_prefix", "Hello World"); _chatterPub = _node.CreatePublisher("chatter"); @@ -23,7 +24,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);