From f7b6deb1c14fbf5e2046593dfbfe4b49354959c5 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 21 Jun 2022 10:58:09 +0200 Subject: [PATCH 01/23] initial action idl generation --- rcldotnet_common/CMakeLists.txt | 1 + rcldotnet_common/IRosActionDefinition.cs | 27 +++++++ ...generator_dotnet_generate_interfaces.cmake | 20 ++++- rosidl_generator_dotnet/resource/action.c.em | 8 ++ rosidl_generator_dotnet/resource/action.cs.em | 61 ++++++++++++++++ rosidl_generator_dotnet/resource/action.h.em | 33 +++++++++ rosidl_generator_dotnet/resource/idl.c.em | 73 ++++++++++++++++++- rosidl_generator_dotnet/resource/idl.cs.em | 73 ++++++++++++++++++- rosidl_generator_dotnet/resource/idl.h.em | 71 +++++++++++++++++- 9 files changed, 360 insertions(+), 7 deletions(-) create mode 100644 rcldotnet_common/IRosActionDefinition.cs create mode 100644 rosidl_generator_dotnet/resource/action.c.em create mode 100644 rosidl_generator_dotnet/resource/action.cs.em create mode 100644 rosidl_generator_dotnet/resource/action.h.em diff --git a/rcldotnet_common/CMakeLists.txt b/rcldotnet_common/CMakeLists.txt index 246c87b2..e97e99b2 100644 --- a/rcldotnet_common/CMakeLists.txt +++ b/rcldotnet_common/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(DotNETExtra REQUIRED) set(CS_SOURCES DllLoadUtils.cs + IRosActionDefinition.cs IRosMessage.cs IRosServiceDefinition.cs ) diff --git a/rcldotnet_common/IRosActionDefinition.cs b/rcldotnet_common/IRosActionDefinition.cs new file mode 100644 index 00000000..7fcf0104 --- /dev/null +++ b/rcldotnet_common/IRosActionDefinition.cs @@ -0,0 +1,27 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // must be implemented on deriving types, gets called via reflection + // (static abstract interface members are not supported yet.) + // public static abstract IntPtr __GetTypeSupport(); + } +} diff --git a/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake b/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake index 617654ce..52e9aadc 100644 --- a/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake +++ b/rosidl_generator_dotnet/cmake/rosidl_generator_dotnet_generate_interfaces.cmake @@ -68,6 +68,17 @@ foreach(_abs_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) "${_output_path}/${_parent_folder}/${_module_name}.c" ) elseif(_parent_folder STREQUAL "action") + list(APPEND _generated_cs_files + "${_output_path}/${_parent_folder}/${_module_name}.cs" + ) + + list(APPEND _generated_h_files + "${_output_path}/${_parent_folder}/rcldotnet_${_module_name}.h" + ) + + list(APPEND _generated_c_ts_files + "${_output_path}/${_parent_folder}/${_module_name}.c" + ) else() message(FATAL_ERROR "Interface file with unknown parent folder: ${_idl_file}") endif() @@ -87,13 +98,18 @@ endforeach() set(target_dependencies "${rosidl_generator_dotnet_BIN}" ${rosidl_generator_dotnet_GENERATOR_FILES} - "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.h.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/action.c.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/action.cs.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/action.h.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.c.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.cs.em" - "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.h.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/idl.h.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.c.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.cs.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/msg.h.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/srv.c.em" "${rosidl_generator_dotnet_TEMPLATE_DIR}/srv.cs.em" + "${rosidl_generator_dotnet_TEMPLATE_DIR}/srv.h.em" ${rosidl_generate_interfaces_ABS_IDL_FILES} ${_dependency_files}) foreach(dep ${target_dependencies}) diff --git a/rosidl_generator_dotnet/resource/action.c.em b/rosidl_generator_dotnet/resource/action.c.em new file mode 100644 index 00000000..a9b7843d --- /dev/null +++ b/rosidl_generator_dotnet/resource/action.c.em @@ -0,0 +1,8 @@ +@{ +type_name = action.namespaced_type.name +action_typename = '%s__%s' % ('__'.join(action.namespaced_type.namespaces), type_name) +}@ +const void * @(action_typename)__get_typesupport(void) { + const void * ptr = ROSIDL_TYPESUPPORT_INTERFACE__ACTION_SYMBOL_NAME(rosidl_typesupport_c, @(', '.join(action.namespaced_type.namespaced_name())))(); + return ptr; +} diff --git a/rosidl_generator_dotnet/resource/action.cs.em b/rosidl_generator_dotnet/resource/action.cs.em new file mode 100644 index 00000000..7c930458 --- /dev/null +++ b/rosidl_generator_dotnet/resource/action.cs.em @@ -0,0 +1,61 @@ +@{ +from rosidl_generator_dotnet import get_field_name +from rosidl_generator_dotnet import get_dotnet_type +from rosidl_generator_dotnet import get_builtin_dotnet_type +from rosidl_generator_dotnet import constant_value_to_dotnet + +from rosidl_parser.definition import AbstractNestedType +from rosidl_parser.definition import AbstractGenericString +from rosidl_parser.definition import AbstractString +from rosidl_parser.definition import AbstractWString +from rosidl_parser.definition import AbstractSequence +from rosidl_parser.definition import Array +from rosidl_parser.definition import BasicType +from rosidl_parser.definition import NamespacedType + +type_name = action.namespaced_type.name +goal_type_name = action.goal.structure.namespaced_type.name +result_type_name = action.result.structure.namespaced_type.name +feedback_type_name = action.feedback.structure.namespaced_type.name +action_typename = '%s__%s' % ('__'.join(action.namespaced_type.namespaces), type_name) +} +namespace @('.'.join(action.namespaced_type.namespaces)) +{ +@# sealed class with private constructor -> no instance can be created +@# static classes can't implement an interface (or any other basetype), +@# but we need some type that can hold the typesupport and be passed to the Node.CreateActionServce/Client() method. +@# So sealed + private constructor is as static as it gets. +@# static abstract interface members are currently in preview, so maybe we could use the feature in the future. +@# (if hey add support to derive from static only interfaces in static classes) +@# Another option is to not use generics for passing the typesupport, but lets try this until we hit some wall. + public sealed class @(type_name) : global::ROS2.IRosActionDefinition<@(goal_type_name), @(result_type_name), @(feedback_type_name)> + { + private static readonly DllLoadUtils dllLoadUtils; + + private @(type_name)() + { + } + + static @(type_name)() + { + dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativelibrary = dllLoadUtils.LoadLibrary("@(package_name)__dotnetext"); + + IntPtr native_get_typesupport_ptr = dllLoadUtils.GetProcAddress(nativelibrary, "@(action_typename)__get_typesupport"); + + @(type_name).native_get_typesupport = (NativeGetTypeSupportType)Marshal.GetDelegateForFunctionPointer( + native_get_typesupport_ptr, typeof(NativeGetTypeSupportType)); + } + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + private delegate IntPtr NativeGetTypeSupportType(); + + private static NativeGetTypeSupportType native_get_typesupport = null; + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static IntPtr __GetTypeSupport() { + return native_get_typesupport(); + } + } +} diff --git a/rosidl_generator_dotnet/resource/action.h.em b/rosidl_generator_dotnet/resource/action.h.em new file mode 100644 index 00000000..dab48679 --- /dev/null +++ b/rosidl_generator_dotnet/resource/action.h.em @@ -0,0 +1,33 @@ +@{ +type_name = action.namespaced_type.name +action_typename = '%s__%s' % ('__'.join(action.namespaced_type.namespaces), type_name) +action_prefix = "RCLDOTNET_{0}_{1}_{2}".format(package_name, '_'.join(action.namespaced_type.namespaces), type_name).upper() +}@ +#if defined(_MSC_VER) + // Microsoft + #define @(action_prefix)_EXPORT __declspec(dllexport) + #define @(action_prefix)_IMPORT __declspec(dllimport) + #if defined(_M_IX86) + #define @(action_prefix)_CDECL __cdecl + #else + #define @(action_prefix)_CDECL + #endif +#elif defined(__GNUC__) + // GCC + #define @(action_prefix)_EXPORT __attribute__((visibility("default"))) + #define @(action_prefix)_IMPORT + #if defined(__i386__) + #define @(action_prefix)_CDECL __attribute__((__cdecl__)) + #else + #define @(action_prefix)_CDECL + #endif +#else + // do nothing and hope for the best? + #define @(action_prefix)_EXPORT + #define @(action_prefix)_IMPORT + #define @(action_prefix)_CDECL + #pragma warning Unknown dynamic link import/export semantics. +#endif + +@(action_prefix)_EXPORT +const void * @(action_prefix)_CDECL @(action_typename)__get_typesupport(void); diff --git a/rosidl_generator_dotnet/resource/idl.c.em b/rosidl_generator_dotnet/resource/idl.c.em index 14efb40c..2b2f4231 100644 --- a/rosidl_generator_dotnet/resource/idl.c.em +++ b/rosidl_generator_dotnet/resource/idl.c.em @@ -87,5 +87,74 @@ TEMPLATE( @# Handle actions @####################################################################### @ -@#TODO - actions not implemented -@ \ No newline at end of file +@{ +from rosidl_parser.definition import Action +}@ +@[for action in content.get_elements_of_type(Action)]@ +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.goal) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.result) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.feedback) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.c.em', + package_name=package_name, interface_path=interface_path, service=action.send_goal_service) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.c.em', + package_name=package_name, interface_path=interface_path, service=action.get_result_service) +}@ + +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, interface_path=interface_path, message=action.feedback_message) +}@ + +@{ +TEMPLATE( + 'action.c.em', + package_name=package_name, interface_path=interface_path, action=action) +}@ +@[end for]@ +@ diff --git a/rosidl_generator_dotnet/resource/idl.cs.em b/rosidl_generator_dotnet/resource/idl.cs.em index 3cfc1ede..1f18e48d 100644 --- a/rosidl_generator_dotnet/resource/idl.cs.em +++ b/rosidl_generator_dotnet/resource/idl.cs.em @@ -62,5 +62,74 @@ TEMPLATE( @# Handle actions @####################################################################### @ -@#TODO - actions not implemented -@ \ No newline at end of file +@{ +from rosidl_parser.definition import Action +}@ +@[for action in content.get_elements_of_type(Action)]@ +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.goal) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.result) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.feedback) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.cs.em', + package_name=package_name, interface_path=interface_path, service=action.send_goal_service) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.cs.em', + package_name=package_name, interface_path=interface_path, service=action.get_result_service) +}@ + +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, interface_path=interface_path, message=action.feedback_message) +}@ + +@{ +TEMPLATE( + 'action.cs.em', + package_name=package_name, interface_path=interface_path, action=action) +}@ +@[end for]@ +@ diff --git a/rosidl_generator_dotnet/resource/idl.h.em b/rosidl_generator_dotnet/resource/idl.h.em index cf1fc096..e1439b98 100644 --- a/rosidl_generator_dotnet/resource/idl.h.em +++ b/rosidl_generator_dotnet/resource/idl.h.em @@ -68,7 +68,76 @@ TEMPLATE( @# Handle actions @####################################################################### @ -@# TODO - actions not implemented +@{ +from rosidl_parser.definition import Action +}@ +@[for action in content.get_elements_of_type(Action)]@ +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.goal) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.result) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.feedback) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.h.em', + package_name=package_name, interface_path=interface_path, service=action.send_goal_service) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message) +}@ + +@{ +TEMPLATE( + 'srv.h.em', + package_name=package_name, interface_path=interface_path, service=action.get_result_service) +}@ + +@{ +TEMPLATE( + 'msg.h.em', + package_name=package_name, interface_path=interface_path, message=action.feedback_message) +}@ + +@{ +TEMPLATE( + 'action.h.em', + package_name=package_name, interface_path=interface_path, action=action) +}@ +@[end for]@ @ #endif // @(header_guard) From fdba3a6bac0fc3446e5e3c9c00424ea44f897aee Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 21 Jun 2022 13:48:39 +0200 Subject: [PATCH 02/23] extend idl generation to include interfaces for "action wrapper types" With "action wrapper types" this means the ROS messages and services that add the `GoalId`, `TimeStamp` or other fields on top of the user defined types for the action. The introduced methods in `IRosActionDefinition` are needed to work around the missing existential type support in C# (https://github.com/dotnet/csharplang/issues/5556). --- rcldotnet_common/CMakeLists.txt | 5 ++ rcldotnet_common/IRosActionDefinition.cs | 15 ++++ rcldotnet_common/IRosActionFeedbackMessage.cs | 26 ++++++ .../IRosActionGetResultRequest.cs | 23 +++++ .../IRosActionGetResultResponse.cs | 25 ++++++ rcldotnet_common/IRosActionSendGoalRequest.cs | 42 +++++++++ .../IRosActionSendGoalResponse.cs | 25 ++++++ rosidl_generator_dotnet/resource/action.cs.em | 90 ++++++++++++++++++- rosidl_generator_dotnet/resource/idl.cs.em | 23 +++-- rosidl_generator_dotnet/resource/msg.cs.em | 14 ++- rosidl_generator_dotnet/resource/srv.cs.em | 11 ++- .../rosidl_generator_dotnet/__init__.py | 5 ++ 12 files changed, 288 insertions(+), 16 deletions(-) create mode 100644 rcldotnet_common/IRosActionFeedbackMessage.cs create mode 100644 rcldotnet_common/IRosActionGetResultRequest.cs create mode 100644 rcldotnet_common/IRosActionGetResultResponse.cs create mode 100644 rcldotnet_common/IRosActionSendGoalRequest.cs create mode 100644 rcldotnet_common/IRosActionSendGoalResponse.cs diff --git a/rcldotnet_common/CMakeLists.txt b/rcldotnet_common/CMakeLists.txt index e97e99b2..eb2f4387 100644 --- a/rcldotnet_common/CMakeLists.txt +++ b/rcldotnet_common/CMakeLists.txt @@ -11,6 +11,11 @@ find_package(DotNETExtra REQUIRED) set(CS_SOURCES DllLoadUtils.cs IRosActionDefinition.cs + IRosActionFeedbackMessage.cs + IRosActionGetResultRequest.cs + IRosActionGetResultResponse.cs + IRosActionSendGoalRequest.cs + IRosActionSendGoalResponse.cs IRosMessage.cs IRosServiceDefinition.cs ) diff --git a/rcldotnet_common/IRosActionDefinition.cs b/rcldotnet_common/IRosActionDefinition.cs index 7fcf0104..470df3db 100644 --- a/rcldotnet_common/IRosActionDefinition.cs +++ b/rcldotnet_common/IRosActionDefinition.cs @@ -23,5 +23,20 @@ public interface IRosActionDefinition // must be implemented on deriving types, gets called via reflection // (static abstract interface members are not supported yet.) // public static abstract IntPtr __GetTypeSupport(); + + // public static abstract IRosActionSendGoalRequest __CreateSendGoalRequest(); + // public static abstract SafeHandle __CreateSendGoalRequestHandle(); + + // public static abstract IRosActionSendGoalResponse __CreateSendGoalResponse(); + // public static abstract SafeHandle __CreateSendGoalResponseHandle(); + + // public static abstract IRosActionGetResultRequest __CreateGetResultRequest(); + // public static abstract SafeHandle __CreateGetResultRequestHandle(); + + // public static abstract IRosActionGetResultResponse __CreateGetResultResponse(); + // public static abstract SafeHandle __CreateGetResultResponseHandle(); + + // public static abstract IRosActionFeedbackMessage __CreateFeedbackMessage(); + // public static abstract SafeHandle __CreateFeedbackMessageHandle(); } } diff --git a/rcldotnet_common/IRosActionFeedbackMessage.cs b/rcldotnet_common/IRosActionFeedbackMessage.cs new file mode 100644 index 00000000..819b11cf --- /dev/null +++ b/rcldotnet_common/IRosActionFeedbackMessage.cs @@ -0,0 +1,26 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionFeedbackMessage : IRosMessage + where TFeedback : IRosMessage, new() + { + // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` + // unique_identifier_msgs.msg.UUID GoalId { get; set; } + + TFeedback Feedback { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionGetResultRequest.cs b/rcldotnet_common/IRosActionGetResultRequest.cs new file mode 100644 index 00000000..25425a07 --- /dev/null +++ b/rcldotnet_common/IRosActionGetResultRequest.cs @@ -0,0 +1,23 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionGetResultRequest : IRosMessage + { + // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` + // unique_identifier_msgs.msg.UUID GoalId { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionGetResultResponse.cs b/rcldotnet_common/IRosActionGetResultResponse.cs new file mode 100644 index 00000000..283ab298 --- /dev/null +++ b/rcldotnet_common/IRosActionGetResultResponse.cs @@ -0,0 +1,25 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionGetResultResponse : IRosMessage + where TResult : IRosMessage, new() + { + sbyte Status { get; set; } + + TResult Result { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionSendGoalRequest.cs b/rcldotnet_common/IRosActionSendGoalRequest.cs new file mode 100644 index 00000000..0bd87079 --- /dev/null +++ b/rcldotnet_common/IRosActionSendGoalRequest.cs @@ -0,0 +1,42 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionSendGoalRequest : IRosMessage + where TGoal : IRosMessage, new() + { + // NOTICE: This would cause a cyclic reference: + // + // - `unique_identifier_msgs.msg.UUID` in the `unique_identifier_msgs` + // assembly references `IRosMessage` in the `rcldotnet_common` + // assembly. + // - `IRosActionSendGoalRequest` in the `rcldotnet_common` + // assembly references `unique_identifier_msgs.msg.UUID` in the + // `unique_identifier_msgs` assembly. + // + // So we need a workaround: + // - Use reflection later on to get to this. + // - Or use types like `byte[]` (or ValueTuple for + // `builtin_interfaces.msg.Time`) and generate accessor methods that + // convert and use those types. + // - Or provide property `IRosMessage GoalIdRosMessage { get; set; }` + // and cast to the concrete type on usage. + // + // unique_identifier_msgs.msg.UUID GoalId { get; set; } + + TGoal Goal { get; set; } + } +} diff --git a/rcldotnet_common/IRosActionSendGoalResponse.cs b/rcldotnet_common/IRosActionSendGoalResponse.cs new file mode 100644 index 00000000..535d14b3 --- /dev/null +++ b/rcldotnet_common/IRosActionSendGoalResponse.cs @@ -0,0 +1,25 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public interface IRosActionSendGoalResponse : IRosMessage + { + bool Accepted { get; set; } + + // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` + // builtin_interfaces.msg.Time Stamp { get; set; } + } +} diff --git a/rosidl_generator_dotnet/resource/action.cs.em b/rosidl_generator_dotnet/resource/action.cs.em index 7c930458..36bdb2bf 100644 --- a/rosidl_generator_dotnet/resource/action.cs.em +++ b/rosidl_generator_dotnet/resource/action.cs.em @@ -1,6 +1,7 @@ @{ from rosidl_generator_dotnet import get_field_name from rosidl_generator_dotnet import get_dotnet_type +from rosidl_generator_dotnet import get_dotnet_type_for_message from rosidl_generator_dotnet import get_builtin_dotnet_type from rosidl_generator_dotnet import constant_value_to_dotnet @@ -14,9 +15,17 @@ from rosidl_parser.definition import BasicType from rosidl_parser.definition import NamespacedType type_name = action.namespaced_type.name -goal_type_name = action.goal.structure.namespaced_type.name -result_type_name = action.result.structure.namespaced_type.name -feedback_type_name = action.feedback.structure.namespaced_type.name + +goal_type_name = get_dotnet_type_for_message(action.goal) +result_type_name = get_dotnet_type_for_message(action.result) +feedback_type_name = get_dotnet_type_for_message(action.feedback) + +send_goal_request_type_name = get_dotnet_type_for_message(action.send_goal_service.request_message) +send_goal_response_type_name = get_dotnet_type_for_message(action.send_goal_service.response_message) +get_result_request_type_name = get_dotnet_type_for_message(action.get_result_service.request_message) +get_result_response_type_name = get_dotnet_type_for_message(action.get_result_service.response_message) +feedback_message_type_name = get_dotnet_type_for_message(action.feedback_message) + action_typename = '%s__%s' % ('__'.join(action.namespaced_type.namespaces), type_name) } namespace @('.'.join(action.namespaced_type.namespaces)) @@ -28,7 +37,10 @@ namespace @('.'.join(action.namespaced_type.namespaces)) @# static abstract interface members are currently in preview, so maybe we could use the feature in the future. @# (if hey add support to derive from static only interfaces in static classes) @# Another option is to not use generics for passing the typesupport, but lets try this until we hit some wall. - public sealed class @(type_name) : global::ROS2.IRosActionDefinition<@(goal_type_name), @(result_type_name), @(feedback_type_name)> + public sealed class @(type_name) : global::ROS2.IRosActionDefinition< + global::@(goal_type_name), + global::@(result_type_name), + global::@(feedback_type_name)> { private static readonly DllLoadUtils dllLoadUtils; @@ -57,5 +69,75 @@ namespace @('.'.join(action.namespaced_type.namespaces)) public static IntPtr __GetTypeSupport() { return native_get_typesupport(); } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionSendGoalRequest __CreateSendGoalRequest() + { + return new global::@(send_goal_request_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateSendGoalRequestHandle() + { + return global::@(send_goal_request_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionSendGoalResponse __CreateSendGoalResponse() + { + return new global::@(send_goal_response_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateSendGoalResponseHandle() + { + return global::@(send_goal_response_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionGetResultRequest __CreateGetResultRequest() + { + return new global::@(get_result_request_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateGetResultRequestHandle() + { + return global::@(get_result_request_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionGetResultResponse __CreateGetResultResponse() + { + return new global::@(get_result_response_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateGetResultResponseHandle() + { + return global::@(get_result_response_type_name).__CreateMessageHandle(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static global::ROS2.IRosActionFeedbackMessage __CreateFeedbackMessage() + { + return new global::@(feedback_message_type_name)(); + } + +@# This method gets called via reflection as static abstract interface members are not supported yet. + [global::System.ComponentModel.EditorBrowsable(global::System.ComponentModel.EditorBrowsableState.Never)] + public static SafeHandle __CreateFeedbackMessageHandle() + { + return global::@(feedback_message_type_name).__CreateMessageHandle(); + } } } diff --git a/rosidl_generator_dotnet/resource/idl.cs.em b/rosidl_generator_dotnet/resource/idl.cs.em index 1f18e48d..18a9b314 100644 --- a/rosidl_generator_dotnet/resource/idl.cs.em +++ b/rosidl_generator_dotnet/resource/idl.cs.em @@ -1,6 +1,9 @@ // generated from rosidl_generator_dotnet/resource/idl.cs.em // with input from @(package_name):@(interface_path) // generated code does not contain a copyright notice +@{ +from rosidl_generator_dotnet import get_dotnet_type_for_message +}@ @ @####################################################################### @# EmPy template for generating .cs files @@ -87,13 +90,17 @@ TEMPLATE( @{ TEMPLATE( 'msg.cs.em', - package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message) + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.request_message, + action_interface='global::ROS2.IRosActionSendGoalRequest' % get_dotnet_type_for_message(action.goal) +) }@ @{ TEMPLATE( 'msg.cs.em', - package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message) + package_name=package_name, interface_path=interface_path, message=action.send_goal_service.response_message, + action_interface='global::ROS2.IRosActionSendGoalResponse' +) }@ @{ @@ -105,13 +112,17 @@ TEMPLATE( @{ TEMPLATE( 'msg.cs.em', - package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message) + package_name=package_name, interface_path=interface_path, message=action.get_result_service.request_message, + action_interface='global::ROS2.IRosActionGetResultRequest' +) }@ @{ TEMPLATE( 'msg.cs.em', - package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message) + package_name=package_name, interface_path=interface_path, message=action.get_result_service.response_message, + action_interface='global::ROS2.IRosActionGetResultResponse' % get_dotnet_type_for_message(action.result) +) }@ @{ @@ -123,7 +134,9 @@ TEMPLATE( @{ TEMPLATE( 'msg.cs.em', - package_name=package_name, interface_path=interface_path, message=action.feedback_message) + package_name=package_name, interface_path=interface_path, message=action.feedback_message, + action_interface='global::ROS2.IRosActionFeedbackMessage' % get_dotnet_type_for_message(action.feedback) +) }@ @{ diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index 69350cde..d07c9c43 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -17,11 +17,17 @@ from rosidl_parser.definition import NamespacedType type_name = message.structure.namespaced_type.name msg_typename = '%s__%s' % ('__'.join(message.structure.namespaced_type.namespaces), type_name) + +if 'action_interface' not in locals(): + action_interface = None + +additional_interfaces_str = ', ' + action_interface if action_interface is not None else '' + } namespace @('.'.join(message.structure.namespaced_type.namespaces)) { -public class @(type_name) : global::ROS2.IRosMessage { +public class @(type_name) : global::ROS2.IRosMessage@(additional_interfaces_str) { private static readonly DllLoadUtils dllLoadUtils; @[for member in message.structure.members]@ @@ -277,7 +283,7 @@ public class @(type_name) : global::ROS2.IRosMessage { IntPtr pStr_@(get_field_name(type_name, member.name)) = native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @(get_field_name(type_name, member.name))[i__local_variable] = Marshal.PtrToStringAnsi(pStr_@(get_field_name(type_name, member.name))); @[ elif isinstance(member.type.value_type, AbstractWString)]@ - // TODO: Unicode types are not supported + // TODO: Unicode types are not supported @[ else]@ @(get_field_name(type_name, member.name))[i__local_variable] = new @(get_dotnet_type(member.type.value_type))(); @(get_field_name(type_name, member.name))[i__local_variable].__ReadFromHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @@ -296,7 +302,7 @@ public class @(type_name) : global::ROS2.IRosMessage { IntPtr pStr_@(get_field_name(type_name, member.name)) = native_read_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @(get_field_name(type_name, member.name)).Add(Marshal.PtrToStringAnsi(pStr_@(get_field_name(type_name, member.name)))); @[ elif isinstance(member.type.value_type, AbstractWString)]@ - // TODO: Unicode types are not supported + // TODO: Unicode types are not supported @[ else]@ @(get_field_name(type_name, member.name)).Add(new @(get_dotnet_type(member.type.value_type))()); @(get_field_name(type_name, member.name))[i__local_variable].__ReadFromHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @@ -348,7 +354,7 @@ public class @(type_name) : global::ROS2.IRosMessage { @[ if isinstance(member.type.value_type, BasicType) or isinstance(member.type.value_type, AbstractString)]@ native_write_field_@(member.name)(native_get_field_@(member.name)_message(messageHandle, i__local_variable), value__local_variable); @[ elif isinstance(member.type.value_type, AbstractWString)] -// TODO: Unicode types are not supported +// TODO: Unicode types are not supported @[ else]@ value__local_variable.__WriteToHandle(native_get_field_@(member.name)_message(messageHandle, i__local_variable)); @[ end if]@ diff --git a/rosidl_generator_dotnet/resource/srv.cs.em b/rosidl_generator_dotnet/resource/srv.cs.em index bd541280..8231e40f 100644 --- a/rosidl_generator_dotnet/resource/srv.cs.em +++ b/rosidl_generator_dotnet/resource/srv.cs.em @@ -1,6 +1,7 @@ @{ from rosidl_generator_dotnet import get_field_name from rosidl_generator_dotnet import get_dotnet_type +from rosidl_generator_dotnet import get_dotnet_type_for_message from rosidl_generator_dotnet import get_builtin_dotnet_type from rosidl_generator_dotnet import constant_value_to_dotnet @@ -14,8 +15,10 @@ from rosidl_parser.definition import BasicType from rosidl_parser.definition import NamespacedType type_name = service.namespaced_type.name -request_type_name = service.request_message.structure.namespaced_type.name -response_type_name = service.response_message.structure.namespaced_type.name + +request_type_name = get_dotnet_type_for_message(service.request_message) +response_type_name = get_dotnet_type_for_message(service.response_message) + srv_typename = '%s__%s' % ('__'.join(service.namespaced_type.namespaces), type_name) } namespace @('.'.join(service.namespaced_type.namespaces)) @@ -27,7 +30,9 @@ namespace @('.'.join(service.namespaced_type.namespaces)) @# static abstract interface members are currently in preview, so maybe we could use the feature in the future. @# (if hey add support to derive from static only interfaces in static classes) @# Another option is to not use generics for passing the typesupport, but lets try this until we hit some wall. - public sealed class @(type_name) : global::ROS2.IRosServiceDefinition<@(request_type_name), @(response_type_name)> + public sealed class @(type_name) : global::ROS2.IRosServiceDefinition< + global::@(request_type_name), + global::@(response_type_name)> { private static readonly DllLoadUtils dllLoadUtils; diff --git a/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py b/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py index 95310ecc..39b67272 100644 --- a/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py +++ b/rosidl_generator_dotnet/rosidl_generator_dotnet/__init__.py @@ -142,6 +142,11 @@ def get_dotnet_type(type_, use_primitives=True): return get_builtin_dotnet_type(type_.typename, use_primitives=use_primitives) +def get_dotnet_type_for_message(message): + return '%s.%s' % ( + '.'.join(message.structure.namespaced_type.namespaces), + message.structure.namespaced_type.name) + def msg_type_to_c(type_): if isinstance(type_, AbstractString): return 'char *' From 0c2b20af3167a762e8eedbf995c1c3471ddd3211 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 22 Jun 2022 17:12:29 +0200 Subject: [PATCH 03/23] add accessors to `UUID` and `Time` properties for "action wrapper types" This uses the base type `IMessage` to get around the cyclic references. --- rcldotnet_common/IRosActionFeedbackMessage.cs | 4 ++++ .../IRosActionGetResultRequest.cs | 4 ++++ rcldotnet_common/IRosActionSendGoalRequest.cs | 7 +++++++ .../IRosActionSendGoalResponse.cs | 4 ++++ rosidl_generator_dotnet/resource/msg.cs.em | 19 +++++++++++++++++++ 5 files changed, 38 insertions(+) diff --git a/rcldotnet_common/IRosActionFeedbackMessage.cs b/rcldotnet_common/IRosActionFeedbackMessage.cs index 819b11cf..95ce0a5a 100644 --- a/rcldotnet_common/IRosActionFeedbackMessage.cs +++ b/rcldotnet_common/IRosActionFeedbackMessage.cs @@ -21,6 +21,10 @@ public interface IRosActionFeedbackMessage : IRosMessage // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` // unique_identifier_msgs.msg.UUID GoalId { get; set; } + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage GoalIdAsRosMessage { get; set; } + TFeedback Feedback { get; set; } } } diff --git a/rcldotnet_common/IRosActionGetResultRequest.cs b/rcldotnet_common/IRosActionGetResultRequest.cs index 25425a07..6313d6f3 100644 --- a/rcldotnet_common/IRosActionGetResultRequest.cs +++ b/rcldotnet_common/IRosActionGetResultRequest.cs @@ -19,5 +19,9 @@ public interface IRosActionGetResultRequest : IRosMessage { // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` // unique_identifier_msgs.msg.UUID GoalId { get; set; } + + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage GoalIdAsRosMessage { get; set; } } } diff --git a/rcldotnet_common/IRosActionSendGoalRequest.cs b/rcldotnet_common/IRosActionSendGoalRequest.cs index 0bd87079..52af436c 100644 --- a/rcldotnet_common/IRosActionSendGoalRequest.cs +++ b/rcldotnet_common/IRosActionSendGoalRequest.cs @@ -35,8 +35,15 @@ public interface IRosActionSendGoalRequest : IRosMessage // - Or provide property `IRosMessage GoalIdRosMessage { get; set; }` // and cast to the concrete type on usage. // + // The later one was chosen as it gives the most control over object + // references and avoids using of reflection. + // // unique_identifier_msgs.msg.UUID GoalId { get; set; } + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage GoalIdAsRosMessage { get; set; } + TGoal Goal { get; set; } } } diff --git a/rcldotnet_common/IRosActionSendGoalResponse.cs b/rcldotnet_common/IRosActionSendGoalResponse.cs index 535d14b3..c906400d 100644 --- a/rcldotnet_common/IRosActionSendGoalResponse.cs +++ b/rcldotnet_common/IRosActionSendGoalResponse.cs @@ -21,5 +21,9 @@ public interface IRosActionSendGoalResponse : IRosMessage // NOTICE: cyclic reference, see `IRosActionSendGoalRequest` // builtin_interfaces.msg.Time Stamp { get; set; } + + // This will be implemented explicitly so it doesn't collide with fields + // of the same name. + IRosMessage StampAsRosMessage { get; set; } } } diff --git a/rosidl_generator_dotnet/resource/msg.cs.em b/rosidl_generator_dotnet/resource/msg.cs.em index d07c9c43..5d4ca694 100644 --- a/rosidl_generator_dotnet/resource/msg.cs.em +++ b/rosidl_generator_dotnet/resource/msg.cs.em @@ -388,6 +388,25 @@ public class @(type_name) : global::ROS2.IRosMessage@(additional_interfaces_str) @[ end if]@ @[end for]@ +@[if action_interface is not None and ( + action_interface.startswith("global::ROS2.IRosActionSendGoalRequest") or + action_interface.startswith("global::ROS2.IRosActionGetResultRequest") or + action_interface.startswith("global::ROS2.IRosActionFeedbackMessage") +)]@ + global::ROS2.IRosMessage @(action_interface).GoalIdAsRosMessage + { + get => GoalId; + set => GoalId = (global::unique_identifier_msgs.msg.UUID)value; + } +@[end if]@ +@[if action_interface is not None and action_interface.startswith("global::ROS2.IRosActionSendGoalResponse")]@ + global::ROS2.IRosMessage @(action_interface).StampAsRosMessage + { + get => Stamp; + set => Stamp = (global::builtin_interfaces.msg.Time)value; + } +@[end if]@ + private sealed class Safe@(type_name)Handle : global::System.Runtime.InteropServices.SafeHandle { public Safe@(type_name)Handle() : base(global::System.IntPtr.Zero, true) { } From 550e70c07bcaf7ed585bd5b77ad588c222a228e9 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 22 Jun 2022 17:37:29 +0200 Subject: [PATCH 04/23] add ActionDefinitionStaticMemberCache --- .../ActionDefinitionStaticMemberCache.cs | 259 ++++++++++++++++++ rcldotnet/CMakeLists.txt | 1 + 2 files changed, 260 insertions(+) create mode 100644 rcldotnet/ActionDefinitionStaticMemberCache.cs diff --git a/rcldotnet/ActionDefinitionStaticMemberCache.cs b/rcldotnet/ActionDefinitionStaticMemberCache.cs new file mode 100644 index 00000000..c07c3015 --- /dev/null +++ b/rcldotnet/ActionDefinitionStaticMemberCache.cs @@ -0,0 +1,259 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Reflection; +using System.Runtime.InteropServices; + +namespace ROS2 +{ + internal static class ActionDefinitionStaticMemberCache + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + private static readonly IntPtr s_typeSupport; + private static readonly Func> s_createSendGoalRequest; + private static readonly Func s_createSendGoalRequestHandle; + private static readonly Func s_createSendGoalResponse; + private static readonly Func s_createSendGoalResponseHandle; + private static readonly Func s_createGetResultRequest; + private static readonly Func s_createGetResultRequestHandle; + private static readonly Func> s_createGetResultResponse; + private static readonly Func s_createGetResultResponseHandle; + private static readonly Func> s_createFeedbackMessage; + private static readonly Func s_createFeedbackMessageHandle; + + static ActionDefinitionStaticMemberCache() + { + TypeInfo typeInfo = typeof(TAction).GetTypeInfo(); + + MethodInfo getTypeSupport = typeInfo.GetDeclaredMethod("__GetTypeSupport"); + if (getTypeSupport != null) + { + try + { + s_typeSupport = (IntPtr)getTypeSupport.Invoke(null, new object[] { }); + } + catch + { + s_typeSupport = IntPtr.Zero; + } + } + else + { + s_typeSupport = IntPtr.Zero; + } + + s_createSendGoalRequest = CreateDelegateForDeclaredMethod>>( + typeInfo, + "__CreateSendGoalRequest"); + + s_createSendGoalRequestHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateSendGoalRequestHandle"); + + s_createSendGoalResponse = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateSendGoalResponse"); + + s_createSendGoalResponseHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateSendGoalResponseHandle"); + + s_createGetResultRequest = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateGetResultRequest"); + + s_createGetResultRequestHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateGetResultRequestHandle"); + + s_createGetResultResponse = CreateDelegateForDeclaredMethod>>( + typeInfo, + "__CreateGetResultResponse"); + + s_createGetResultResponseHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateGetResultResponseHandle"); + + s_createFeedbackMessage = CreateDelegateForDeclaredMethod>>( + typeInfo, + "__CreateFeedbackMessage"); + + s_createFeedbackMessageHandle = CreateDelegateForDeclaredMethod>( + typeInfo, + "__CreateFeedbackMessageHandle"); + } + + public static IntPtr GetTypeSupport() + { + // This is a Method because it could throw. + if (s_typeSupport == IntPtr.Zero) + { + throw CreateMethodNotDefinedCorrectlyException("__GetTypeSupport"); + } + + return s_typeSupport; + } + + public static IRosActionSendGoalRequest CreateSendGoalRequest() + { + if (s_createSendGoalRequest != null) + { + return s_createSendGoalRequest(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalRequest"); + } + } + + public static SafeHandle CreateSendGoalRequestHandle() + { + if (s_createSendGoalRequestHandle != null) + { + return s_createSendGoalRequestHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalRequestHandle"); + } + } + + public static IRosActionSendGoalResponse CreateSendGoalResponse() + { + if (s_createSendGoalResponse != null) + { + return s_createSendGoalResponse(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalResponse"); + } + } + + public static SafeHandle CreateSendGoalResponseHandle() + { + if (s_createSendGoalResponseHandle != null) + { + return s_createSendGoalResponseHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateSendGoalResponseHandle"); + } + } + + public static IRosActionGetResultRequest CreateGetResultRequest() + { + if (s_createGetResultRequest != null) + { + return s_createGetResultRequest(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultRequest"); + } + } + + public static SafeHandle CreateGetResultRequestHandle() + { + if (s_createGetResultRequestHandle != null) + { + return s_createGetResultRequestHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultRequestHandle"); + } + } + + public static IRosActionGetResultResponse CreateGetResultResponse() + { + if (s_createGetResultResponse != null) + { + return s_createGetResultResponse(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultResponse"); + } + } + + public static SafeHandle CreateGetResultResponseHandle() + { + if (s_createGetResultResponseHandle != null) + { + return s_createGetResultResponseHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateGetResultResponseHandle"); + } + } + + public static IRosActionFeedbackMessage CreateFeedbackMessage() + { + if (s_createFeedbackMessage != null) + { + return s_createFeedbackMessage(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateFeedbackMessage"); + } + } + + public static SafeHandle CreateFeedbackMessageHandle() + { + if (s_createFeedbackMessageHandle != null) + { + return s_createFeedbackMessageHandle(); + } + else + { + throw CreateMethodNotDefinedCorrectlyException("__CreateFeedbackMessageHandle"); + } + } + + private static TDelegate CreateDelegateForDeclaredMethod(TypeInfo typeInfo, string methodName) + where TDelegate : Delegate + { + MethodInfo methodInfo = typeInfo.GetDeclaredMethod(methodName); + if (methodInfo != null) + { + try + { + return (TDelegate)methodInfo.CreateDelegate(typeof(TDelegate)); + } + catch + { + return null; + } + } + else + { + return null; + } + } + + private static Exception CreateMethodNotDefinedCorrectlyException(string methodName) + { + return new InvalidOperationException($"Type '{typeof(TAction).FullName}' did not define a correct {methodName} method."); + } + } +} diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index afbdf6a6..6a9af2e1 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -28,6 +28,7 @@ endif() set(CSHARP_TARGET_FRAMEWORK "netstandard2.0") set(CS_SOURCES + ActionDefinitionStaticMemberCache.cs Client.cs GuardCondition.cs MessageStaticMemberCache.cs From ee82b1932d41e7a15e9fad2d7f325a8893a8da25 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 22 Jun 2022 17:53:15 +0200 Subject: [PATCH 05/23] add public action client API without implementation --- rcldotnet/ActionClient.cs | 55 +++++++++++++++++++++++ rcldotnet/ActionClientGoalHandle.cs | 67 +++++++++++++++++++++++++++++ rcldotnet/ActionGoalStatus.cs | 60 ++++++++++++++++++++++++++ rcldotnet/CMakeLists.txt | 3 ++ rcldotnet/Node.cs | 9 ++++ 5 files changed, 194 insertions(+) create mode 100644 rcldotnet/ActionClient.cs create mode 100644 rcldotnet/ActionClientGoalHandle.cs create mode 100644 rcldotnet/ActionGoalStatus.cs diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs new file mode 100644 index 00000000..2d3c5e62 --- /dev/null +++ b/rcldotnet/ActionClient.cs @@ -0,0 +1,55 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Threading.Tasks; + +namespace ROS2 +{ + public abstract class ActionClient + { + // Only allow internal subclasses. + internal ActionClient() + { + } + } + + public sealed class ActionClient : ActionClient + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // No public constructor. + internal ActionClient() + { + } + + public bool ServiceIsReady() + { + throw new NotImplementedException(); + } + + public Task> SendGoalAsync(TGoal goal) + { + throw new NotImplementedException(); + } + + public Task> SendGoalAsync(TGoal goal, Action feedbackCallback) + { + throw new NotImplementedException(); + } + } +} diff --git a/rcldotnet/ActionClientGoalHandle.cs b/rcldotnet/ActionClientGoalHandle.cs new file mode 100644 index 00000000..df1dc7ee --- /dev/null +++ b/rcldotnet/ActionClientGoalHandle.cs @@ -0,0 +1,67 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Threading.Tasks; +using builtin_interfaces.msg; + +namespace ROS2 +{ + public abstract class ActionClientGoalHandle + { + // Only allow internal subclasses. + internal ActionClientGoalHandle() + { + } + + public abstract Guid GoalId { get; } + + public abstract bool Accepted { get; } + + public abstract Time Stamp { get; } + + public abstract ActionGoalStatus Status { get; } + } + + public sealed class ActionClientGoalHandle : ActionClientGoalHandle + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // No public constructor. + internal ActionClientGoalHandle() + { + } + + public override Guid GoalId { get; } + + public override bool Accepted { get; } + + public override Time Stamp { get; } + + public override ActionGoalStatus Status { get; } + + public Task CancelGoalAsync() + { + throw new NotImplementedException(); + } + + public Task GetResultAsync() + { + throw new NotImplementedException(); + } + } +} diff --git a/rcldotnet/ActionGoalStatus.cs b/rcldotnet/ActionGoalStatus.cs new file mode 100644 index 00000000..52a5d0b8 --- /dev/null +++ b/rcldotnet/ActionGoalStatus.cs @@ -0,0 +1,60 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public enum ActionGoalStatus + { + // see definition here: https://github.com/ros2/rcl_interfaces/blob/master/action_msgs/msg/GoalStatus.msg + + // + // Indicates status has not been properly set. + // + Unknown = 0, + + // + // The goal has been accepted and is awaiting execution. + // + Accepted = 1, + + // + // The goal is currently being executed by the action server. + // + Executing = 2, + + // + // The client has requested that the goal be canceled and the action + // server has accepted the cancel request. + // + Canceling = 3, + + // + // The goal was achieved successfully by the action server. + // + Succeeded = 4, + + // + // The goal was canceled after an external request from an + // action client. + // + Canceled = 5, + + // + // The goal was terminated by the action server without an + // external request. + // + Aborted = 6, + } +} diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 6a9af2e1..33c83548 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -28,7 +28,10 @@ endif() set(CSHARP_TARGET_FRAMEWORK "netstandard2.0") set(CS_SOURCES + ActionClient.cs + ActionClientGoalHandle.cs ActionDefinitionStaticMemberCache.cs + ActionGoalStatus.cs Client.cs GuardCondition.cs MessageStaticMemberCache.cs diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index c0b537e6..0c1de432 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -299,5 +299,14 @@ public GuardCondition CreateGuardCondition(Action callback) _guardConditions.Add(guardCondition); return guardCondition; } + + public ActionClient CreateActionClient(string actionName) + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + throw new NotImplementedException(); + } } } From 1ebcde85f38f5ba6544a6a1b8a1dda13254aa9f8 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 21 Jun 2022 17:03:54 +0200 Subject: [PATCH 06/23] rename `ServiceIsReady()` to `ServerIsReady() `and move to base class --- rcldotnet/ActionClient.cs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index 2d3c5e62..67702314 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -24,6 +24,8 @@ public abstract class ActionClient internal ActionClient() { } + + public abstract bool ServerIsReady(); } public sealed class ActionClient : ActionClient @@ -37,7 +39,7 @@ internal ActionClient() { } - public bool ServiceIsReady() + public override bool ServerIsReady() { throw new NotImplementedException(); } From 2fa3d426972f43fc34eccd78e81b900c1e3a9346 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 23 Jun 2022 08:13:42 +0200 Subject: [PATCH 07/23] add helper for converting guid <-> uuid --- rcldotnet/CMakeLists.txt | 1 + rcldotnet/GuidExtensions.cs | 71 +++++++++++++++++++++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 rcldotnet/GuidExtensions.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 33c83548..75d6d242 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -34,6 +34,7 @@ set(CS_SOURCES ActionGoalStatus.cs Client.cs GuardCondition.cs + GuidExtensions.cs MessageStaticMemberCache.cs Node.cs Publisher.cs diff --git a/rcldotnet/GuidExtensions.cs b/rcldotnet/GuidExtensions.cs new file mode 100644 index 00000000..c75c0a68 --- /dev/null +++ b/rcldotnet/GuidExtensions.cs @@ -0,0 +1,71 @@ +using System; +using System.Linq; +using unique_identifier_msgs.msg; + +namespace ROS2 +{ + public static class GuidExtensions + { + public static UUID ToUuidMsg(this Guid guid) + { + // Microsoft uses some mixed endian representation for Guid in + // Windows and .NET, which the Guid.ToByteArray() does as well. + // Most/All other implementations of UUID/GUIDs use an unique big + // endian representation on the wire. See this StackOverflow answer + // for more context: https://stackoverflow.com/a/9195681 + // + // So there is a need to swap some bytes before converting them to + // the UUID ROS message. + // + // Even on BigEndian machines .NET does the swapping of the first + // three fields to little endian, so it is at least consistent on + // all platforms, no need to check the endianness of the runtime. + // https://github.com/dotnet/runtime/blob/1ba0394d71a4ea6bee7f6b28a22d666b7b56f913/src/libraries/System.Private.CoreLib/src/System/Guid.cs#L819 + var guidBytes = guid.ToByteArray(); + EndianSwap(guidBytes); + + var uuidMsg = new UUID(); + uuidMsg.Uuid = guidBytes; + return uuidMsg; + } + + public static byte[] ToUuidByteArray(this Guid guid) + { + // See comments in `ToUuidMsg` for info on byte swapping. + + var guidBytes = guid.ToByteArray(); + EndianSwap(guidBytes); + + return guidBytes; + } + + public static Guid ToGuid(this UUID uuidMsg) + { + // See comments in `ToUuidMsg` for info on byte swapping. + + var uuidBytesCopy = uuidMsg.Uuid.ToArray(); + EndianSwap(uuidBytesCopy); + + var guid = new Guid(uuidBytesCopy); + return guid; + } + + private static void EndianSwap(byte[] guid) + { + // taken from https://github.com/StephenCleary/Guids/blob/a9bd91835d536636d13249bd1bfbb1cd76c43533/src/Nito.Guids/GuidUtility.cs#L47 + Swap(guid, 0, 3); + Swap(guid, 1, 2); + + Swap(guid, 4, 5); + + Swap(guid, 6, 7); + } + + private static void Swap(byte[] array, int index1, int index2) + { + var temp = array[index1]; + array[index1] = array[index2]; + array[index2] = temp; + } + } +} From c0cf14d64188339f5d01a708317b6cdf7f534a2a Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Fri, 24 Jun 2022 12:59:30 +0200 Subject: [PATCH 08/23] implement basic action client --- rcldotnet/ActionClient.cs | 459 +++++++++++++++++++++++++++- rcldotnet/ActionClientGoalHandle.cs | 42 ++- rcldotnet/CMakeLists.txt | 26 +- rcldotnet/Node.cs | 47 ++- rcldotnet/RCLdotnet.cs | 390 +++++++++++++++++++++++ rcldotnet/SafeActionClientHandle.cs | 75 +++++ rcldotnet/package.xml | 8 + rcldotnet/rcldotnet.c | 104 +++++++ rcldotnet/rcldotnet.h | 37 +++ rcldotnet/rcldotnet_action_client.c | 58 ++++ rcldotnet/rcldotnet_action_client.h | 32 ++ rcldotnet/rcldotnet_node.c | 36 ++- rcldotnet/rcldotnet_node.h | 9 + 13 files changed, 1305 insertions(+), 18 deletions(-) create mode 100644 rcldotnet/SafeActionClientHandle.cs create mode 100644 rcldotnet/rcldotnet_action_client.c create mode 100644 rcldotnet/rcldotnet_action_client.h diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index 67702314..0e058918 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -14,10 +14,76 @@ */ using System; +using System.Collections.Concurrent; +using System.Runtime.InteropServices; using System.Threading.Tasks; +using action_msgs.msg; +using action_msgs.srv; +using builtin_interfaces.msg; +using ROS2.Utils; +using unique_identifier_msgs.msg; namespace ROS2 { + internal static class ActionClientDelegates + { + internal static readonly DllLoadUtils _dllLoadUtils; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendGoalRequestType( + SafeActionClientHandle clientHandle, SafeHandle goalRequestHandle, out long sequenceNumber); + + internal static NativeRCLActionSendGoalRequestType native_rcl_action_send_goal_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendResultRequestType( + SafeActionClientHandle clientHandle, SafeHandle resultRequestHandle, out long sequenceNumber); + + internal static NativeRCLActionSendResultRequestType native_rcl_action_send_result_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendCancelRequestType( + SafeActionClientHandle clientHandle, SafeHandle cancelRequestHandle, out long sequenceNumber); + + internal static NativeRCLActionSendCancelRequestType native_rcl_action_send_cancel_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionServerIsAvailableType( + SafeNodeHandle nodeHandle, SafeActionClientHandle clientHandle, out bool isAvailable); + + internal static NativeRCLActionServerIsAvailableType native_rcl_action_server_is_available = null; + + static ActionClientDelegates() + { + _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); + IntPtr nativeLibrary = _dllLoadUtils.LoadLibrary("rcldotnet"); + + IntPtr native_rcl_action_send_goal_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_goal_request"); + ActionClientDelegates.native_rcl_action_send_goal_request = + (NativeRCLActionSendGoalRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_goal_request_ptr, typeof(NativeRCLActionSendGoalRequestType)); + + IntPtr native_rcl_action_send_result_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_result_request"); + ActionClientDelegates.native_rcl_action_send_result_request = + (NativeRCLActionSendResultRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_result_request_ptr, typeof(NativeRCLActionSendResultRequestType)); + + IntPtr native_rcl_action_send_cancel_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_cancel_request"); + ActionClientDelegates.native_rcl_action_send_cancel_request = + (NativeRCLActionSendCancelRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_cancel_request_ptr, typeof(NativeRCLActionSendCancelRequestType)); + + IntPtr native_rcl_action_server_is_available_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_server_is_available"); + ActionClientDelegates.native_rcl_action_server_is_available = + (NativeRCLActionServerIsAvailableType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_server_is_available_ptr, typeof(NativeRCLActionServerIsAvailableType)); + } + } + public abstract class ActionClient { // Only allow internal subclasses. @@ -25,7 +91,36 @@ internal ActionClient() { } + // ActionClient does intentionally (for now) not implement IDisposable + // as this needs some extra consideration how the type works after its + // internal handle is disposed. By relying on the GC/Finalizer of + // SafeHandle the handle only gets Disposed if the action client is not + // live anymore. + internal abstract SafeActionClientHandle Handle { get; } + public abstract bool ServerIsReady(); + + internal abstract IRosMessage CreateSendGoalResponse(); + + internal abstract SafeHandle CreateSendGoalResponseHandle(); + + internal abstract IRosMessage CreateGetResultResponse(); + + internal abstract SafeHandle CreateGetResultResponseHandle(); + + internal abstract IRosMessage CreateFeedbackMessage(); + + internal abstract SafeHandle CreateFeedbackMessageHandle(); + + internal abstract void HandleFeedbackMessage(IRosMessage feedbackMessage); + + internal abstract void HandleStatusMessage(GoalStatusArray statusMessage); + + internal abstract void HandleGoalResponse(long sequenceNumber, IRosMessage goalResponse); + + internal abstract void HandleCancelResponse(long sequenceNumber, CancelGoal_Response cancelResponse); + + internal abstract void HandleResultResponse(long sequenceNumber, IRosMessage resultResponse); } public sealed class ActionClient : ActionClient @@ -34,24 +129,380 @@ public sealed class ActionClient : ActionCli where TResult : IRosMessage, new() where TFeedback : IRosMessage, new() { + // ros2_java uses a WeakReference here. Not sure if its needed or not. + private readonly Node _node; + + // TODO: (sh) rclpy uses week references to the goal handles here? Is this needed? + private readonly ConcurrentDictionary> _goalHandles + = new ConcurrentDictionary>(); + + private readonly ConcurrentDictionary _pendingGoalRequests + = new ConcurrentDictionary(); + + private readonly ConcurrentDictionary _pendingCancelRequests + = new ConcurrentDictionary(); + + private readonly ConcurrentDictionary _pendingResultRequests + = new ConcurrentDictionary(); + // No public constructor. - internal ActionClient() + internal ActionClient(SafeActionClientHandle handle, Node node) { + Handle = handle; + _node = node; } + internal override SafeActionClientHandle Handle { get; } + public override bool ServerIsReady() { - throw new NotImplementedException(); + RCLRet ret = ActionClientDelegates.native_rcl_action_server_is_available(_node.Handle, Handle, out var serverIsReady); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_server_is_available)}() failed."); + + return serverIsReady; } public Task> SendGoalAsync(TGoal goal) { - throw new NotImplementedException(); + return SendGoalAsyncInternal(goal, null); } public Task> SendGoalAsync(TGoal goal, Action feedbackCallback) { - throw new NotImplementedException(); + return SendGoalAsyncInternal(goal, feedbackCallback); + } + + private Task> SendGoalAsyncInternal(TGoal goal, Action feedbackCallback) + { + long sequenceNumber; + + var goalId = Guid.NewGuid(); + var goalRequest = ActionDefinitionStaticMemberCache.CreateSendGoalRequest(); + goalRequest.GoalIdAsRosMessage = goalId.ToUuidMsg(); + goalRequest.Goal = goal; + + using (var goalRequestHandle = ActionDefinitionStaticMemberCache.CreateSendGoalRequestHandle()) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + goalRequestHandle.DangerousAddRef(ref mustRelease); + goalRequest.__WriteToHandle(goalRequestHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + goalRequestHandle.DangerousRelease(); + } + } + + RCLRet ret = ActionClientDelegates.native_rcl_action_send_goal_request(Handle, goalRequestHandle, out sequenceNumber); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_goal_request)}() failed."); + } + + var taskCompletionSource = new TaskCompletionSource>(); + var pendingGoalRequest = new PendingGoalRequest(goalId, taskCompletionSource, feedbackCallback); + if (!_pendingGoalRequests.TryAdd(sequenceNumber, pendingGoalRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber already taken. + } + + return taskCompletionSource.Task; + } + + // TODO: (sh) Add CancelAllGoalsAsync(), like rclcpp Client.async_cancel_all_goals() + // TODO: (sh) Add CancelGoalsBeforeAsync(), like rclcpp Client.async_cancel_goals_before() + + // TODO: (sh) Maybe make this public. + // (but then check if goalHandle belongs to this actionClient is needed) + internal Task CancelGoalAsync(ActionClientGoalHandle goalHandle) + { + long sequenceNumber; + + var cancelRequest = new CancelGoal_Request(); + cancelRequest.GoalInfo.GoalId.Uuid = goalHandle.GoalId.ToUuidByteArray(); + + using (var cancelRequestHandle = CancelGoal_Request.__CreateMessageHandle()) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + cancelRequestHandle.DangerousAddRef(ref mustRelease); + cancelRequest.__WriteToHandle(cancelRequestHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + cancelRequestHandle.DangerousRelease(); + } + } + + RCLRet ret = ActionClientDelegates.native_rcl_action_send_cancel_request(Handle, cancelRequestHandle, out sequenceNumber); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_cancel_request)}() failed."); + } + + var taskCompletionSource = new TaskCompletionSource(); + var pendingCancelRequest = new PendingCancelRequest(taskCompletionSource); + if (!_pendingCancelRequests.TryAdd(sequenceNumber, pendingCancelRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber already taken. + } + + return taskCompletionSource.Task; + } + + internal Task GetResultAsync( + ActionClientGoalHandle goalHandle, + TaskCompletionSource resultTaskCompletionSource) + { + try + { + long sequenceNumber; + + var resultRequest = ActionDefinitionStaticMemberCache.CreateGetResultRequest(); + resultRequest.GoalIdAsRosMessage = goalHandle.GoalId.ToUuidMsg(); + + using (var resultRequestHandle = ActionDefinitionStaticMemberCache.CreateGetResultRequestHandle()) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + resultRequestHandle.DangerousAddRef(ref mustRelease); + resultRequest.__WriteToHandle(resultRequestHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + resultRequestHandle.DangerousRelease(); + } + } + + RCLRet ret = ActionClientDelegates.native_rcl_action_send_result_request(Handle, resultRequestHandle, out sequenceNumber); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_result_request)}() failed."); + } + + var pendingResultRequest = new PendingResultRequest(resultTaskCompletionSource); + if (!_pendingResultRequests.TryAdd(sequenceNumber, pendingResultRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber already taken. + } + + } + catch (Exception exception) + { + // Don't directly return Task.FromException() so that the tasks + // returned from other threads get the exception as well. + resultTaskCompletionSource.TrySetException(exception); + } + + return resultTaskCompletionSource.Task; + } + + internal override IRosMessage CreateSendGoalResponse() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponse(); + } + + internal override SafeHandle CreateSendGoalResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponseHandle(); + } + + internal override IRosMessage CreateGetResultResponse() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponse(); + } + + internal override SafeHandle CreateGetResultResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponseHandle(); + } + + internal override IRosMessage CreateFeedbackMessage() + { + return ActionDefinitionStaticMemberCache.CreateFeedbackMessage(); + } + + internal override SafeHandle CreateFeedbackMessageHandle() + { + return ActionDefinitionStaticMemberCache.CreateFeedbackMessageHandle(); + } + + internal override void HandleFeedbackMessage(IRosMessage feedbackMessage) + { + var feedbackMessageCasted = (IRosActionFeedbackMessage)feedbackMessage; + + var goalIdUuid = (UUID)feedbackMessageCasted.GoalIdAsRosMessage; + var goalId = goalIdUuid.ToGuid(); + + if (!_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + // TODO: (sh) Add error handling/logging. GoalId not found. + return; + } + + goalHandle.FeedbackCallback?.Invoke(feedbackMessageCasted.Feedback); + } + + internal override void HandleStatusMessage(GoalStatusArray statusMessage) + { + foreach (var statusMessage1 in statusMessage.StatusList) + { + var goalId = statusMessage1.GoalInfo.GoalId.ToGuid(); + if (_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + var status = statusMessage1.Status; + goalHandle.Status = ConvertGoalStatus(status); + + // rclpy does this, rclcpp does not. + if (status == GoalStatus.STATUS_SUCCEEDED + || status == GoalStatus.STATUS_CANCELED + || status == GoalStatus.STATUS_ABORTED) + { + // Remove "done" goals from the dictionary. + // TODO: (sh) Should we remove goalHandles that are not in the status list as well? After a timeout? + _goalHandles.TryRemove(goalId, out _); + } + } + } + } + + private static ActionGoalStatus ConvertGoalStatus(sbyte status) + { + switch (status) + { + case GoalStatus.STATUS_UNKNOWN: + return ActionGoalStatus.Unknown; + + case GoalStatus.STATUS_ACCEPTED: + return ActionGoalStatus.Accepted; + + case GoalStatus.STATUS_EXECUTING: + return ActionGoalStatus.Executing; + + case GoalStatus.STATUS_CANCELING: + return ActionGoalStatus.Canceling; + + case GoalStatus.STATUS_SUCCEEDED: + return ActionGoalStatus.Succeeded; + + case GoalStatus.STATUS_CANCELED: + return ActionGoalStatus.Canceled; + + case GoalStatus.STATUS_ABORTED: + return ActionGoalStatus.Aborted; + + default: + throw new ArgumentException("Unknown status.", nameof(status)); + } + } + + internal override void HandleGoalResponse(long sequenceNumber, IRosMessage goalResponse) + { + if (!_pendingGoalRequests.TryRemove(sequenceNumber, out var pendingGoalRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber not found. + return; + } + + var goalId = pendingGoalRequest.GoalId; + var goalResponseCasted = (IRosActionSendGoalResponse)goalResponse; + var accepted = goalResponseCasted.Accepted; + var stamp = (Time)goalResponseCasted.StampAsRosMessage; + + var goalHandle = new ActionClientGoalHandle(this, goalId, accepted, stamp, pendingGoalRequest.FeedbackCallback); + + if (accepted) + { + if (!_goalHandles.TryAdd(goalId, goalHandle)) + { + var exception = new Exception($"Two goals were accepted with the same Id '{goalId}'."); + pendingGoalRequest.TaskCompletionSource.TrySetException(exception); + return; + }; + } + + pendingGoalRequest.TaskCompletionSource.TrySetResult(goalHandle); + } + + internal override void HandleCancelResponse(long sequenceNumber, CancelGoal_Response cancelResponse) + { + if (!_pendingCancelRequests.TryRemove(sequenceNumber, out var pendingCancelRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber not found. + return; + } + + pendingCancelRequest.TaskCompletionSource.TrySetResult(cancelResponse); + } + + internal override void HandleResultResponse(long sequenceNumber, IRosMessage resultResponse) + { + if (!_pendingResultRequests.TryRemove(sequenceNumber, out var pendingResultRequest)) + { + // TODO: (sh) Add error handling/logging. SequenceNumber not found. + return; + } + + var resultResponseCasted = (IRosActionGetResultResponse)resultResponse; + var result = resultResponseCasted.Result; + + pendingResultRequest.TaskCompletionSource.TrySetResult(result); + } + + private sealed class PendingGoalRequest + { + public PendingGoalRequest( + Guid goalId, + TaskCompletionSource> taskCompletionSource, + Action feedbackCallback) + { + GoalId = goalId; + TaskCompletionSource = taskCompletionSource; + FeedbackCallback = feedbackCallback; + } + + public Guid GoalId { get; } + public TaskCompletionSource> TaskCompletionSource { get; } + public Action FeedbackCallback { get; } + } + + private sealed class PendingCancelRequest + { + public PendingCancelRequest(TaskCompletionSource taskCompletionSource) + { + TaskCompletionSource = taskCompletionSource; + } + + public TaskCompletionSource TaskCompletionSource { get; } + } + + private sealed class PendingResultRequest + { + public PendingResultRequest(TaskCompletionSource taskCompletionSource) + { + TaskCompletionSource = taskCompletionSource; + } + + public TaskCompletionSource TaskCompletionSource { get; } } } } diff --git a/rcldotnet/ActionClientGoalHandle.cs b/rcldotnet/ActionClientGoalHandle.cs index df1dc7ee..bdae9f8b 100644 --- a/rcldotnet/ActionClientGoalHandle.cs +++ b/rcldotnet/ActionClientGoalHandle.cs @@ -14,6 +14,7 @@ */ using System; +using System.Threading; using System.Threading.Tasks; using builtin_interfaces.msg; @@ -32,7 +33,7 @@ internal ActionClientGoalHandle() public abstract Time Stamp { get; } - public abstract ActionGoalStatus Status { get; } + public abstract ActionGoalStatus Status { get; internal set; } } public sealed class ActionClientGoalHandle : ActionClientGoalHandle @@ -41,9 +42,23 @@ public sealed class ActionClientGoalHandle : where TResult : IRosMessage, new() where TFeedback : IRosMessage, new() { + private readonly ActionClient _actionClient; + + private TaskCompletionSource _resultTaskCompletionSource; + // No public constructor. - internal ActionClientGoalHandle() + internal ActionClientGoalHandle( + ActionClient actionClient, + Guid goalId, + bool accepted, + Time stamp, + Action feedbackCallback) { + GoalId = goalId; + Accepted = accepted; + Stamp = stamp; + FeedbackCallback = feedbackCallback; + _actionClient = actionClient; } public override Guid GoalId { get; } @@ -52,16 +67,33 @@ internal ActionClientGoalHandle() public override Time Stamp { get; } - public override ActionGoalStatus Status { get; } + public override ActionGoalStatus Status { get; internal set; } + + internal Action FeedbackCallback { get; } + // TODO: (sh) should we return the CancelGoal_Response? Wrap it in type with dotnet enum/Guid? public Task CancelGoalAsync() { - throw new NotImplementedException(); + return _actionClient.CancelGoalAsync(this); } public Task GetResultAsync() { - throw new NotImplementedException(); + // Fast case to avoid allocation and calling Interlocked.CompareExchange. + if (_resultTaskCompletionSource != null) + { + return _resultTaskCompletionSource.Task; + } + + var resultTaskCompletionSource = new TaskCompletionSource(); + + if (Interlocked.CompareExchange(ref _resultTaskCompletionSource, resultTaskCompletionSource, null) != null) + { + // Some other thread was first. + return _resultTaskCompletionSource.Task; + } + + return _actionClient.GetResultAsync(this, resultTaskCompletionSource); } } } diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 75d6d242..87ec0f20 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -6,7 +6,11 @@ find_package(ament_cmake_export_assemblies REQUIRED) find_package(ament_cmake REQUIRED) find_package(rcl REQUIRED) +find_package(rcl_action REQUIRED) +find_package(action_msgs REQUIRED) find_package(rcl_interfaces REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(unique_identifier_msgs REQUIRED) find_package(rmw REQUIRED) find_package(rmw_implementation REQUIRED) find_package(rmw_implementation_cmake REQUIRED) @@ -42,6 +46,7 @@ set(CS_SOURCES RCLdotnet.cs RCLExceptionHelper.cs RCLRet.cs + SafeActionClientHandle.cs SafeClientHandle.cs SafeGuardConditionHandle.cs SafeNodeHandle.cs @@ -57,21 +62,26 @@ set(CS_SOURCES ) find_package(rcldotnet_common REQUIRED) -foreach(_assembly_dep ${rcldotnet_common_ASSEMBLIES_DLL}) - list(APPEND _assembly_deps_dll "${_assembly_dep}") -endforeach() + +set(_assemblies_dep_dlls + ${action_msgs_ASSEMBLIES_DLL} + ${builtin_interfaces_ASSEMBLIES_DLL} + ${rcldotnet_common_ASSEMBLIES_DLL} + ${unique_identifier_msgs_ASSEMBLIES_DLL} +) add_dotnet_library(${PROJECT_NAME}_assemblies SOURCES ${CS_SOURCES} INCLUDE_DLLS - ${_assembly_deps_dll} + ${_assemblies_dep_dlls} ) install_dotnet(${PROJECT_NAME}_assemblies DESTINATION lib/${PROJECT_NAME}/dotnet) ament_export_assemblies_dll("lib/${PROJECT_NAME}/dotnet/${PROJECT_NAME}_assemblies.dll") add_library(${PROJECT_NAME}_native SHARED + rcldotnet_action_client.c rcldotnet_client.c rcldotnet_guard_condition.c rcldotnet_node.c @@ -80,8 +90,11 @@ add_library(${PROJECT_NAME}_native SHARED ) ament_target_dependencies(${PROJECT_NAME}_native + "action_msgs" "builtin_interfaces" + "unique_identifier_msgs" "rcl" + "rcl_action" "rosidl_generator_c" "rosidl_typesupport_c" ) @@ -109,11 +122,9 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() - set(_assemblies_dep_dlls - ${rcldotnet_common_ASSEMBLIES_DLL} + set(_test_assemblies_dep_dlls ${std_msgs_ASSEMBLIES_DLL} ${test_msgs_ASSEMBLIES_DLL} - ${builtin_interfaces_ASSEMBLIES_DLL} ) # Used by CI to test the different target frameworks in a matrix. @@ -133,6 +144,7 @@ if(BUILD_TESTING) test/test_services.cs INCLUDE_DLLS ${_assemblies_dep_dlls} + ${_test_assemblies_dep_dlls} INCLUDE_REFERENCES "Microsoft.NET.Test.Sdk=15.9.0" "xunit=2.4.1" diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 0c1de432..476d0c4b 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -72,6 +72,18 @@ internal delegate RCLRet NativeRCLDestroyClientHandleType( internal static NativeRCLDestroyClientHandleType native_rcl_destroy_client_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionCreateClientHandleType( + ref SafeActionClientHandle actionClientHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string actionName, IntPtr typesupportHandle); + + internal static NativeRCLActionCreateClientHandleType native_rcl_action_create_client_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionDestroyClientHandleType( + IntPtr actionClientHandle, SafeNodeHandle nodeHandle); + + internal static NativeRCLActionDestroyClientHandleType native_rcl_action_destroy_client_handle = null; + static NodeDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); @@ -132,6 +144,20 @@ static NodeDelegates() NodeDelegates.native_rcl_destroy_client_handle = (NativeRCLDestroyClientHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_destroy_client_handle_ptr, typeof(NativeRCLDestroyClientHandleType)); + + IntPtr native_rcl_action_create_client_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_action_create_client_handle"); + + NodeDelegates.native_rcl_action_create_client_handle = + (NativeRCLActionCreateClientHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_create_client_handle_ptr, typeof(NativeRCLActionCreateClientHandleType)); + + IntPtr native_rcl_action_destroy_client_handle_ptr = _dllLoadUtils.GetProcAddress( + nativeLibrary, "native_rcl_action_destroy_client_handle"); + + NodeDelegates.native_rcl_action_destroy_client_handle = + (NativeRCLActionDestroyClientHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_destroy_client_handle_ptr, typeof(NativeRCLActionDestroyClientHandleType)); } } @@ -145,6 +171,8 @@ public sealed class Node private readonly IList _guardConditions; + private readonly IList _actionClients; + internal Node(SafeNodeHandle handle) { Handle = handle; @@ -152,6 +180,7 @@ internal Node(SafeNodeHandle handle) _services = new List(); _clients = new List(); _guardConditions = new List(); + _actionClients = new List(); } public IList Subscriptions => _subscriptions; @@ -165,6 +194,8 @@ internal Node(SafeNodeHandle handle) public IList GuardConditions => _guardConditions; + public IList ActionClients => _actionClients; + // Node does intentionaly (for now) not implement IDisposable as this // needs some extra consideration how the type works after its // internal handle is disposed. @@ -306,7 +337,21 @@ public ActionClient CreateActionClient.GetTypeSupport(); + + var actionClientHandle = new SafeActionClientHandle(); + RCLRet ret = NodeDelegates.native_rcl_action_create_client_handle(ref actionClientHandle, Handle, actionName, typeSupport); + actionClientHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + actionClientHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_action_create_client_handle)}() failed."); + } + + // TODO: (sh) Add actionName to ActionClient. + var actionClient = new ActionClient(actionClientHandle, this); + _actionClients.Add(actionClient); + return actionClient; } } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 3cc56ac1..9280796b 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -15,6 +15,8 @@ using System; using System.Runtime.InteropServices; +using action_msgs.msg; +using action_msgs.srv; using ROS2.Utils; namespace ROS2 @@ -172,6 +174,54 @@ internal delegate RCLRet NativeRCLCreateRequestIdHandleType( internal static NativeRCLTakeResponseType native_rcl_take_response = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionClientWaitSetGetNumEntriesType( + SafeActionClientHandle actionClientHandle, out int numSubscriptions, out int numGuardConditions, out int numTimers, out int numClients, out int numServices); + + internal static NativeRCLActionClientWaitSetGetNumEntriesType native_rcl_action_client_wait_set_get_num_entries = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionWaitSetAddActionClientType( + SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle); + + internal static NativeRCLActionWaitSetAddActionClientType native_rcl_action_wait_set_add_action_client = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionClientWaitSetGetEntitiesReadyType( + SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle, out bool isFeedbackReady, out bool isStatusReady, out bool isGoalResponseReady, out bool isCancelResponseReady, out bool isResultResponseReady); + + internal static NativeRCLActionClientWaitSetGetEntitiesReadyType native_rcl_action_client_wait_set_get_entities_ready = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeFeedbackType( + SafeActionClientHandle actionClientHandle, SafeHandle feedbackMessageHandle); + + internal static NativeRCLActionTakeFeedbackType native_rcl_action_take_feedback = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeStatusType( + SafeActionClientHandle actionClientHandle, SafeHandle statusMessageHandle); + + internal static NativeRCLActionTakeStatusType native_rcl_action_take_status = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeGoalResponseType( + SafeActionClientHandle actionClientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle goalResponseHandle); + + internal static NativeRCLActionTakeGoalResponseType native_rcl_action_take_goal_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeCancelResponseType( + SafeActionClientHandle actionClientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle cancelResponseHandle); + + internal static NativeRCLActionTakeCancelResponseType native_rcl_action_take_cancel_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeResultResponseType( + SafeActionClientHandle actionClientHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle resultResponseHandle); + + internal static NativeRCLActionTakeResultResponseType native_rcl_action_take_result_response = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLCreateQosProfileHandleType(ref SafeQosProfileHandle qosProfileHandle); internal static NativeRCLCreateQosProfileHandleType native_rcl_create_qos_profile_handle = null; @@ -371,6 +421,54 @@ static RCLdotnetDelegates() (NativeRCLTakeResponseType)Marshal.GetDelegateForFunctionPointer( native_rcl_take_response_ptr, typeof(NativeRCLTakeResponseType)); + IntPtr native_rcl_action_client_wait_set_get_num_entries_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_client_wait_set_get_num_entries"); + RCLdotnetDelegates.native_rcl_action_client_wait_set_get_num_entries = + (NativeRCLActionClientWaitSetGetNumEntriesType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_client_wait_set_get_num_entries_ptr, typeof(NativeRCLActionClientWaitSetGetNumEntriesType)); + + IntPtr native_rcl_action_wait_set_add_action_client_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_wait_set_add_action_client"); + RCLdotnetDelegates.native_rcl_action_wait_set_add_action_client = + (NativeRCLActionWaitSetAddActionClientType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_wait_set_add_action_client_ptr, typeof(NativeRCLActionWaitSetAddActionClientType)); + + IntPtr native_rcl_action_client_wait_set_get_entities_ready_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_client_wait_set_get_entities_ready"); + RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready = + (NativeRCLActionClientWaitSetGetEntitiesReadyType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_client_wait_set_get_entities_ready_ptr, typeof(NativeRCLActionClientWaitSetGetEntitiesReadyType)); + + IntPtr native_rcl_action_take_feedback_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_feedback"); + RCLdotnetDelegates.native_rcl_action_take_feedback + = (NativeRCLActionTakeFeedbackType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_feedback_ptr, typeof(NativeRCLActionTakeFeedbackType)); + + IntPtr native_rcl_action_take_status_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_status"); + RCLdotnetDelegates.native_rcl_action_take_status = + (NativeRCLActionTakeStatusType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_status_ptr, typeof(NativeRCLActionTakeStatusType)); + + IntPtr native_rcl_action_take_goal_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_goal_response"); + RCLdotnetDelegates.native_rcl_action_take_goal_response = + (NativeRCLActionTakeGoalResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_goal_response_ptr, typeof(NativeRCLActionTakeGoalResponseType)); + + IntPtr native_rcl_action_take_cancel_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_cancel_response"); + RCLdotnetDelegates.native_rcl_action_take_cancel_response = + (NativeRCLActionTakeCancelResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_cancel_response_ptr, typeof(NativeRCLActionTakeCancelResponseType)); + + IntPtr native_rcl_action_take_result_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_result_response"); + RCLdotnetDelegates.native_rcl_action_take_result_response = + (NativeRCLActionTakeResultResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_result_response_ptr, typeof(NativeRCLActionTakeResultResponseType)); + IntPtr native_rcl_create_qos_profile_handle_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_create_qos_profile_handle"); RCLdotnetDelegates.native_rcl_create_qos_profile_handle = @@ -479,6 +577,12 @@ private static void WaitSetAddGuardCondition(SafeWaitSetHandle waitSetHandle, Sa RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_wait_set_add_guard_condition)}() failed."); } + private static void WaitSetAddActionClient(SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_wait_set_add_action_client(waitSetHandle, actionClientHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_wait_set_add_action_client)}() failed."); + } + /// /// Block until the wait set is ready or until the timeout has been exceeded. /// @@ -626,6 +730,196 @@ private static bool TakeResponse(Client client, SafeRequestIdHandle requestHeade } } + private static bool TakeFeedbackMessage(ActionClient actionClient, IRosMessage feedbackMessage) + { + using (var feedbackMessageHandle = actionClient.CreateFeedbackMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_feedback(actionClient.Handle, feedbackMessageHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + feedbackMessageHandle.DangerousAddRef(ref mustRelease); + feedbackMessage.__ReadFromHandle(feedbackMessageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + feedbackMessageHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.SubscriptionTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_feedback)}() failed."); + } + } + } + + private static bool TakeStatusMessage(ActionClient actionClient, IRosMessage statusMessage) + { + using (var statusMessageHandle = GoalStatusArray.__CreateMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_status(actionClient.Handle, statusMessageHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + statusMessageHandle.DangerousAddRef(ref mustRelease); + statusMessage.__ReadFromHandle(statusMessageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + statusMessageHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.SubscriptionTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_status)}() failed."); + } + } + } + + private static bool TakeGoalResponse(ActionClient actionClient, SafeRequestIdHandle requestHeaderHandle, IRosMessage goalResponse) + { + using (var goalResponseHandle = actionClient.CreateSendGoalResponseHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_goal_response(actionClient.Handle, requestHeaderHandle, goalResponseHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + goalResponseHandle.DangerousAddRef(ref mustRelease); + goalResponse.__ReadFromHandle(goalResponseHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + goalResponseHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.ClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_goal_response)}() failed."); + } + } + } + + private static bool TakeCancelResponse(ActionClient actionClient, SafeRequestIdHandle requestHeaderHandle, IRosMessage cancelResponse) + { + using (var cancelResponseHandle = CancelGoal_Response.__CreateMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_cancel_response(actionClient.Handle, requestHeaderHandle, cancelResponseHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + cancelResponseHandle.DangerousAddRef(ref mustRelease); + cancelResponse.__ReadFromHandle(cancelResponseHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + cancelResponseHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.ClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_cancel_response)}() failed."); + } + } + } + + private static bool TakeResultResponse(ActionClient actionClient, SafeRequestIdHandle requestHeaderHandle, IRosMessage resultResponse) + { + using (var resultResponseHandle = actionClient.CreateGetResultResponseHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_result_response(actionClient.Handle, requestHeaderHandle, resultResponseHandle); + switch (ret) + { + case RCLRet.Ok: + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be + // handled in generated code across multiple assemblies. + // Array and collection indexing would need to create SafeHandles everywhere. + // It's not worth it, especially considering the extra allocations for SafeHandles in + // arrays or collections that don't really represent their own native resource. + resultResponseHandle.DangerousAddRef(ref mustRelease); + resultResponse.__ReadFromHandle(resultResponseHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + resultResponseHandle.DangerousRelease(); + } + } + + return true; + + case RCLRet.ClientTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_result_response)}() failed."); + } + } + } + private static void SendResponse(Service service, SafeRequestIdHandle requestHeaderHandle, IRosMessage response) { using (var responseHandle = service.CreateResponseHandle()) @@ -666,6 +960,25 @@ public static void SpinOnce(Node node, long timeout) int numberOfServices = node.Services.Count; int numberOfEvents = 0; + foreach (var actionClient in node.ActionClients) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_client_wait_set_get_num_entries( + actionClient.Handle, + out int acNumberOfSubscriptions, + out int acNumberOfGuardConditions, + out int acNumberOfTimers, + out int acNumberOfClients, + out int acNumberOfServices); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_client_wait_set_get_num_entries)}() failed."); + + numberOfSubscriptions += acNumberOfSubscriptions; + numberOfGuardConditions += acNumberOfGuardConditions; + numberOfTimers += acNumberOfTimers; + numberOfClients += acNumberOfClients; + numberOfServices += acNumberOfServices; + } + bool waitSetEmpty = numberOfSubscriptions == 0 && numberOfGuardConditions == 0 && numberOfTimers == 0 @@ -711,6 +1024,11 @@ public static void SpinOnce(Node node, long timeout) WaitSetAddGuardCondition(waitSetHandle, guardCondition.Handle); } + foreach (var actionClient in node.ActionClients) + { + WaitSetAddActionClient(waitSetHandle, actionClient.Handle); + } + bool ready = Wait(waitSetHandle, timeout); if (!ready) { @@ -773,6 +1091,78 @@ public static void SpinOnce(Node node, long timeout) clientIndex++; } + + foreach (var actionClient in node.ActionClients) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready( + waitSetHandle, + actionClient.Handle, + out bool isFeedbackReady, + out bool isStatusReady, + out bool isGoalResponseReady, + out bool isCancelResponseReady, + out bool isResultResponseReady); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready)}() failed."); + + if (isFeedbackReady) + { + var feedbackMessage = actionClient.CreateFeedbackMessage(); + + var result = TakeFeedbackMessage(actionClient, feedbackMessage); + if (result) + { + actionClient.HandleFeedbackMessage(feedbackMessage); + } + } + + if (isStatusReady) + { + var statusMessage = new GoalStatusArray(); + + var result = TakeStatusMessage(actionClient, statusMessage); + if (result) + { + actionClient.HandleStatusMessage(statusMessage); + } + } + + if (isGoalResponseReady) + { + var goalResponse = actionClient.CreateSendGoalResponse(); + + var result = TakeGoalResponse(actionClient, requestIdHandle, goalResponse); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + actionClient.HandleGoalResponse(sequenceNumber, goalResponse); + } + } + + if (isCancelResponseReady) + { + var cancelResponse = new CancelGoal_Response(); + + var result = TakeCancelResponse(actionClient, requestIdHandle, cancelResponse); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + actionClient.HandleCancelResponse(sequenceNumber, cancelResponse); + } + } + + if (isResultResponseReady) + { + var resultResponse = actionClient.CreateGetResultResponse(); + + var result = TakeResultResponse(actionClient, requestIdHandle, resultResponse); + if (result) + { + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + actionClient.HandleResultResponse(sequenceNumber, resultResponse); + } + } + } } int guardConditionIndex = 0; diff --git a/rcldotnet/SafeActionClientHandle.cs b/rcldotnet/SafeActionClientHandle.cs new file mode 100644 index 00000000..92391fc0 --- /dev/null +++ b/rcldotnet/SafeActionClientHandle.cs @@ -0,0 +1,75 @@ +using System.Diagnostics; +using Microsoft.Win32.SafeHandles; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_action_client_t + /// + /// + /// Since we need to delete the action client handle before the node is released we need to actually hold a + /// pointer to a rcl_action_client_t unmanaged structure whose destructor decrements a refCount. Only when + /// the node refCount is 0 it is deleted. This way, we loose a race in the critical finalization + /// of the client handle and node handle. + /// + internal sealed class SafeActionClientHandle : SafeHandleZeroOrMinusOneIsInvalid + { + // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 + // Commit from early 2016, but still in current .NET as of september 2021: + // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs + // + // Finding docs about SafeHandle is difficult, but the implementation and examples + // in github.com/dotnet/runtime (in combination with the discussions in the PRs) + // should be good enougth. At least are there people that know what they do ;) + // + // Needed to change the exact order of statements in ReleaseOrder() as rcl_client_fini() + // needs the parent handle as well. As far as I understand this there should be no new + // race conditions. + + private SafeNodeHandle _parent; + + public SafeActionClientHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + var parent = _parent; + _parent = null; + + bool successfullyFreed = false; + if (parent != null) + { + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + RCLRet ret = NodeDelegates.native_rcl_action_destroy_client_handle(handle, parent); + successfullyFreed = ret == RCLRet.Ok; + + parent.DangerousRelease(); + } + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + + internal void SetParent(SafeNodeHandle parent) + { + if (IsInvalid || IsClosed) + { + return; + } + + Debug.Assert(_parent == null); + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + _parent = parent; + + bool ignored = false; + _parent.DangerousAddRef(ref ignored); + } + } +} diff --git a/rcldotnet/package.xml b/rcldotnet/package.xml index 5aa04a4b..db01ebe6 100644 --- a/rcldotnet/package.xml +++ b/rcldotnet/package.xml @@ -19,9 +19,17 @@ rmw_implementation_cmake rcl + rcl_actions rmw + action_msgs + builtin_interfaces + unique_identifier_msgs rcl + rcl_actions + action_msgs + builtin_interfaces + unique_identifier_msgs std_msgs test_msgs diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 04ffc51c..4a7ae58e 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "rosidl_runtime_c/message_type_support_struct.h" @@ -174,6 +175,47 @@ int32_t native_rcl_wait_set_add_guard_condition(void *wait_set_handle, void *gua return ret; } +int32_t native_rcl_action_client_wait_set_get_num_entries( + void *action_client_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services) +{ + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + + size_t num_subscriptions_as_size_t; + size_t num_guard_conditions_as_size_t; + size_t num_timers_as_size_t; + size_t num_clients_as_size_t; + size_t num_services_as_size_t; + + rcl_ret_t ret = rcl_action_client_wait_set_get_num_entities( + action_client, + &num_subscriptions_as_size_t, + &num_guard_conditions_as_size_t, + &num_timers_as_size_t, + &num_clients_as_size_t, + &num_services_as_size_t); + + *num_subscriptions = (int32_t)num_subscriptions_as_size_t; + *num_guard_conditions = (int32_t)num_guard_conditions_as_size_t; + *num_timers = (int32_t)num_timers_as_size_t; + *num_clients = (int32_t)num_clients_as_size_t; + *num_services = (int32_t)num_services_as_size_t; + + return ret; +} + +int32_t native_rcl_action_wait_set_add_action_client(void *wait_set_handle, void *action_client_handle) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + rcl_ret_t ret = rcl_action_wait_set_add_action_client(wait_set, action_client, NULL, NULL); + + return ret; +} + int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_ret_t ret = rcl_wait(wait_set, timeout); @@ -229,6 +271,30 @@ int32_t native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t return result ? 1 : 0; } +int32_t native_rcl_action_client_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_client_handle, + bool *is_feedback_ready, + bool *is_status_ready, + bool *is_goal_response_ready, + bool *is_cancel_response_ready, + bool *is_result_response_ready) +{ + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( + wait_set, + action_client, + is_feedback_ready, + is_status_ready, + is_goal_response_ready, + is_cancel_response_ready, + is_result_response_ready); + + return ret; +} + int32_t native_rcl_take(void *subscription_handle, void *message_handle) { rcl_subscription_t * subscription = (rcl_subscription_t *)subscription_handle; @@ -278,6 +344,44 @@ int32_t native_rcl_take_response(void *client_handle, void *request_header_handl return ret; } +int32_t native_rcl_action_take_feedback(void *action_client_handle, void *feedback_message_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_take_feedback(action_client, feedback_message_handle); + return ret; +} + +int32_t native_rcl_action_take_status(void *action_client_handle, void *status_message_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_take_status(action_client, status_message_handle); + return ret; +} + +int32_t native_rcl_action_take_goal_response(void *action_client_handle, void *request_header_handle, void *goal_response_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_goal_response(action_client, request_header, goal_response_handle); + return ret; +} + +int32_t native_rcl_action_take_cancel_response(void *action_client_handle, void *request_header_handle, void *cancel_response_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_cancel_response(action_client, request_header, cancel_response_handle); + return ret; +} + +int32_t native_rcl_action_take_result_response(void *action_client_handle, void *request_header_handle, void *result_response_handle) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_result_response(action_client, request_header, result_response_handle); + return ret; +} + int32_t native_rcl_create_qos_profile_handle(void **qos_profile_handle) { rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)malloc(sizeof(rmw_qos_profile_t)); diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index 288e134e..f0ea55fe 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -72,6 +72,18 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_client(void *wait_set_handle, vo RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_add_guard_condition_handle(void *wait_set_handle, void *guard_condition_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_num_entries( + void *action_client_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_wait_set_add_action_client(void *wait_set_handle, void *action_client_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t); @@ -87,6 +99,16 @@ int32_t RCLDOTNET_CDECL native_rcl_wait_set_service_ready(void *wait_set_handle, RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t index); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_client_handle, + bool *is_feedback_ready, + bool *is_status_ready, + bool *is_goal_response_ready, + bool *is_cancel_response_ready, + bool *is_result_response_ready); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take(void *, void *); @@ -108,6 +130,21 @@ int32_t RCLDOTNET_CDECL native_rcl_send_response(void *service_handle, void *req RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take_response(void *client_handle, void *request_header_handle, void *response_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_feedback(void *action_client_handle, void *feedback_message_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_status(void *action_client_handle, void *status_message_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_goal_response(void *action_client_handle, void *request_header_handle, void *goal_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_cancel_response(void *action_client_handle, void *request_header_handle, void *cancel_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_result_response(void *action_client_handle, void *request_header_handle, void *result_response_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_qos_profile_handle(void **qos_profile_handle); diff --git a/rcldotnet/rcldotnet_action_client.c b/rcldotnet/rcldotnet_action_client.c new file mode 100644 index 00000000..a03e6491 --- /dev/null +++ b/rcldotnet/rcldotnet_action_client.c @@ -0,0 +1,58 @@ +// Copyright 2021 Stefan Hoffmann +// +// 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 +#include +#include +#include +#include + +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "rcldotnet_client.h" + +int32_t native_rcl_action_send_goal_request(void *action_client_handle, void *goal_request_handle, int64_t *sequence_number) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_send_goal_request(action_client, goal_request_handle, sequence_number); + return ret; +} + +int32_t native_rcl_action_send_result_request(void *action_client_handle, void *result_request_handle, int64_t *sequence_number) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_send_result_request(action_client, result_request_handle, sequence_number); + return ret; +} + +int32_t native_rcl_action_send_cancel_request(void *action_client_handle, void *cancel_request_handle, int64_t *sequence_number) { + rcl_action_client_t * action_client = (rcl_action_client_t *)action_client_handle; + + rcl_ret_t ret = rcl_action_send_cancel_request(action_client, cancel_request_handle, sequence_number); + return ret; +} + +int32_t native_rcl_action_server_is_available(void *node_handle, void *client_handle, bool *is_available) { + rcl_node_t * node = (rcl_node_t *)node_handle; + rcl_action_client_t * client = (rcl_action_client_t *)client_handle; + + rcl_ret_t ret = rcl_action_server_is_available(node, client, is_available); + return ret; +} diff --git a/rcldotnet/rcldotnet_action_client.h b/rcldotnet/rcldotnet_action_client.h new file mode 100644 index 00000000..93d06d4c --- /dev/null +++ b/rcldotnet/rcldotnet_action_client.h @@ -0,0 +1,32 @@ +// Copyright 2021 Stefan Hoffmann +// +// 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_CLIENT_H +#define RCLDOTNET_CLIENT_H + +#include "rcldotnet_macros.h" + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_goal_request(void *action_client_handle, void *goal_request_handle, int64_t *sequence_number); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_result_request(void *action_client_handle, void *result_request_handle, int64_t *sequence_number); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_cancel_request(void *action_client_handle, void *cancel_request_handle, int64_t *sequence_number); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_server_is_available(void *node_handle, void *action_client_handle, bool *is_available); + +#endif // RCLDOTNET_CLIENT_H diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index f430ec9e..d298a24b 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -20,6 +20,7 @@ #include #include #include +#include #include "rosidl_runtime_c/message_type_support_struct.h" @@ -153,7 +154,7 @@ int32_t native_rcl_create_client_handle(void **client_handle, rcl_client_options_t client_ops = rcl_client_get_default_options(); - rcl_ret_t ret = + rcl_ret_t ret = rcl_client_init(client, node, ts, service_name, &client_ops); *client_handle = (void *)client; @@ -170,3 +171,36 @@ int32_t native_rcl_destroy_client_handle(void *client_handle, void *node_handle) return ret; } + +int32_t native_rcl_action_create_client_handle(void **action_client_handle, + void *node_handle, + const char *action_name, + void *typesupport) { + rcl_node_t *node = (rcl_node_t *)node_handle; + + rosidl_action_type_support_t *ts = + (rosidl_action_type_support_t *)typesupport; + + rcl_action_client_t *action_client = + (rcl_action_client_t *)malloc(sizeof(rcl_action_client_t)); + *action_client = rcl_action_get_zero_initialized_client(); + rcl_action_client_options_t action_client_ops = + rcl_action_client_get_default_options(); + + rcl_ret_t ret = + rcl_action_client_init(action_client, node, ts, action_name, &action_client_ops); + + *action_client_handle = (void *)action_client; + + return ret; +} + +int32_t native_rcl_action_destroy_client_handle(void *action_client_handle, void *node_handle) { + rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + rcl_node_t *node = (rcl_node_t *)node_handle; + + rcl_ret_t ret = rcl_action_client_fini(action_client, node); + free(action_client); + + return ret; +} diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h index c640a30a..ca263e36 100644 --- a/rcldotnet/rcldotnet_node.h +++ b/rcldotnet/rcldotnet_node.h @@ -53,4 +53,13 @@ int32_t RCLDOTNET_CDECL native_rcl_create_client_handle(void **client_handle, RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_destroy_client_handle(void *client_handle, void *node_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_create_client_handle(void **action_client_handle, + void *node_handle, + const char *action_name, + void *typesupport); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_destroy_client_handle(void *action_client_handle, void *node_handle); + #endif // RCLDOTNET_NODE_H From 8bd8eddb9a7f462bc5d4617a608c2bd4825d7a67 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 29 Jun 2022 14:13:43 +0200 Subject: [PATCH 09/23] add public action server API without implementation --- rcldotnet/ActionServer.cs | 88 ++++++++++++++++++++++++++++ rcldotnet/ActionServerGoalHandle.cs | 90 +++++++++++++++++++++++++++++ rcldotnet/CMakeLists.txt | 2 + rcldotnet/Node.cs | 19 ++++++ 4 files changed, 199 insertions(+) create mode 100644 rcldotnet/ActionServer.cs create mode 100644 rcldotnet/ActionServerGoalHandle.cs diff --git a/rcldotnet/ActionServer.cs b/rcldotnet/ActionServer.cs new file mode 100644 index 00000000..c6179589 --- /dev/null +++ b/rcldotnet/ActionServer.cs @@ -0,0 +1,88 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + /// + /// A response returned by an action server callback when a goal is requested. + /// + public enum GoalResponse + { + /// + /// Invalid default value. + /// + Default = 0, + + /// + /// The goal is rejected and will not be executed. + /// + Reject = 1, + + // `rclpy` doesn't provide the option to defer the the execution (unless you override the `acceptCallback`) + // Accept = 2, + // Alternative from `rclcpp`: + + /// + /// The server accepts the goal, and is going to begin execution immediately. + /// + AcceptAndExecute = 2, + + /// + /// The server accepts the goal, and is going to execute it later. + /// + AcceptAndDefer = 3, + } + + /// + /// A response returned by an action server callback when a goal has been asked to be canceled. + /// + public enum CancelResponse + { + /// + /// Invalid default value. + /// + Default = 0, + + /// + /// The server will not try to cancel the goal. + /// + Reject = 1, + + /// + /// The server has agreed to try to cancel the goal. + /// + Accept = 2, + } + + public abstract class ActionServer + { + // Only allow internal subclasses. + internal ActionServer() + { + } + } + + public sealed class ActionServer : ActionServer + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // No public constructor. + internal ActionServer() + { + } + } +} diff --git a/rcldotnet/ActionServerGoalHandle.cs b/rcldotnet/ActionServerGoalHandle.cs new file mode 100644 index 00000000..6daca496 --- /dev/null +++ b/rcldotnet/ActionServerGoalHandle.cs @@ -0,0 +1,90 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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 +{ + public abstract class ActionServerGoalHandle + { + // Only allow internal subclasses. + internal ActionServerGoalHandle() + { + } + + public abstract Guid GoalId { get; } + + public abstract bool IsActive { get; } + + public abstract bool IsCancelRequested { get; } + + public abstract ActionGoalStatus Status { get; } + + // In `rclpy` this calls the `executeCallback` after setting the state to executing. + // In `rclcpp` this does not call the `executeCallback` (as there is none) but sets the state for the explicit `AcceptAndDefer` `GoalResponse`. + public void Execute() + { + throw new NotImplementedException(); + } + } + + public sealed class ActionServerGoalHandle : ActionServerGoalHandle + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + // No public constructor. + internal ActionServerGoalHandle() + { + } + + // `rclpy` uses the name `Request`, but the name from `rclcpp` `Goal` fits better. + public TGoal Goal { get; } + + public override Guid GoalId => throw new NotImplementedException(); + + public override bool IsActive => throw new NotImplementedException(); + + public override bool IsCancelRequested => throw new NotImplementedException(); + + public override ActionGoalStatus Status => throw new NotImplementedException(); + + public void PublishFeedback(TFeedback feedback) => throw new NotImplementedException(); + + + // TODO: (sh) Decide which (or both?) of these methods should be exposed. + // // "rclpy style" + // public void Succeed() => throw new NotImplementedException(); + // public void Abort() => throw new NotImplementedException(); + // public void Canceled() => throw new NotImplementedException(); + + // "rclcpp style" + public void Succeed(TResult result) + { + throw new NotImplementedException(); + } + + public void Abort(TResult result) + { + throw new NotImplementedException(); + } + + public void Canceled(TResult result) + { + throw new NotImplementedException(); + } + } +} diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 87ec0f20..329831aa 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -36,6 +36,8 @@ set(CS_SOURCES ActionClientGoalHandle.cs ActionDefinitionStaticMemberCache.cs ActionGoalStatus.cs + ActionServer.cs + ActionServerGoalHandle.cs Client.cs GuardCondition.cs GuidExtensions.cs diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 476d0c4b..3d28161c 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -353,5 +353,24 @@ public ActionClient CreateActionClient`) + // TODO: (sh) Make acceptedCallback an async callback which returns the `TResult` (`Func>`)? + // Add as overload? + // In that case this would be more or less an `executeCallback` from `rclpy`. + public ActionServer CreateActionServer( + string actionName, + Action> acceptedCallback, + // This has an additional `goalId` parameter compared to `rclpy`. + Func goalCallback = null, + Func, CancelResponse> cancelCallback = null + ) + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + throw new NotImplementedException(); + } } } From c7da1fc41f98efe6f5d8bd51dab3088ce5eaf5c2 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 29 Jun 2022 15:18:33 +0200 Subject: [PATCH 10/23] add missing license header --- rcldotnet/SafeActionClientHandle.cs | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/rcldotnet/SafeActionClientHandle.cs b/rcldotnet/SafeActionClientHandle.cs index 92391fc0..ba42db11 100644 --- a/rcldotnet/SafeActionClientHandle.cs +++ b/rcldotnet/SafeActionClientHandle.cs @@ -1,3 +1,18 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Diagnostics; using Microsoft.Win32.SafeHandles; From b356a2502304ebed5fc583d0c279916f6612492c Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Jul 2022 12:51:15 +0200 Subject: [PATCH 11/23] change IsCancelRequested to IsCanceling, add IsExecuting --- rcldotnet/ActionServerGoalHandle.cs | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/rcldotnet/ActionServerGoalHandle.cs b/rcldotnet/ActionServerGoalHandle.cs index 6daca496..3a4198c6 100644 --- a/rcldotnet/ActionServerGoalHandle.cs +++ b/rcldotnet/ActionServerGoalHandle.cs @@ -26,9 +26,23 @@ internal ActionServerGoalHandle() public abstract Guid GoalId { get; } + /// + /// Indicate if goal is pending or executing. + /// + /// False if goal has reached a terminal state. public abstract bool IsActive { get; } - public abstract bool IsCancelRequested { get; } + /// + /// Indicate if client has requested this goal be cancelled. + /// + /// True if a cancellation request has been accepted for this goal. + public abstract bool IsCanceling { get; } + + /// + /// Indicate if goal is executing. + /// + /// True only if the goal is in an executing state. + public abstract bool IsExecuting { get; } public abstract ActionGoalStatus Status { get; } @@ -58,7 +72,9 @@ internal ActionServerGoalHandle() public override bool IsActive => throw new NotImplementedException(); - public override bool IsCancelRequested => throw new NotImplementedException(); + public override bool IsCanceling => throw new NotImplementedException(); + + public override bool IsExecuting => throw new NotImplementedException(); public override ActionGoalStatus Status => throw new NotImplementedException(); From 28790e89915d96e6667cf86dc1501415e441924b Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Jul 2022 13:41:42 +0200 Subject: [PATCH 12/23] add ReadFromMessageHandle/WriteToMessageHandle helper mehtods --- rcldotnet/ActionClient.cs | 57 +--------- rcldotnet/Client.cs | 19 +--- rcldotnet/Publisher.cs | 19 +--- rcldotnet/RCLdotnet.cs | 232 ++++++++++---------------------------- 4 files changed, 63 insertions(+), 264 deletions(-) diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index 0e058918..a8f52c08 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -183,24 +183,7 @@ private Task> SendGoa using (var goalRequestHandle = ActionDefinitionStaticMemberCache.CreateSendGoalRequestHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - goalRequestHandle.DangerousAddRef(ref mustRelease); - goalRequest.__WriteToHandle(goalRequestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - goalRequestHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(goalRequest, goalRequestHandle); RCLRet ret = ActionClientDelegates.native_rcl_action_send_goal_request(Handle, goalRequestHandle, out sequenceNumber); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_goal_request)}() failed."); @@ -230,24 +213,7 @@ internal Task CancelGoalAsync(ActionClientGoalHandle goalHa using (var cancelRequestHandle = CancelGoal_Request.__CreateMessageHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - cancelRequestHandle.DangerousAddRef(ref mustRelease); - cancelRequest.__WriteToHandle(cancelRequestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - cancelRequestHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(cancelRequest, cancelRequestHandle); RCLRet ret = ActionClientDelegates.native_rcl_action_send_cancel_request(Handle, cancelRequestHandle, out sequenceNumber); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_cancel_request)}() failed."); @@ -276,24 +242,7 @@ internal Task GetResultAsync( using (var resultRequestHandle = ActionDefinitionStaticMemberCache.CreateGetResultRequestHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - resultRequestHandle.DangerousAddRef(ref mustRelease); - resultRequest.__WriteToHandle(resultRequestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - resultRequestHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(resultRequest, resultRequestHandle); RCLRet ret = ActionClientDelegates.native_rcl_action_send_result_request(Handle, resultRequestHandle, out sequenceNumber); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_send_result_request)}() failed."); diff --git a/rcldotnet/Client.cs b/rcldotnet/Client.cs index 643552d6..f0ff7ad0 100644 --- a/rcldotnet/Client.cs +++ b/rcldotnet/Client.cs @@ -108,24 +108,7 @@ public Task SendRequestAsync(TRequest request) using (var requestHandle = MessageStaticMemberCache.CreateMessageHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - requestHandle.DangerousAddRef(ref mustRelease); - request.__WriteToHandle(requestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - requestHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(request, requestHandle); RCLRet ret = ClientDelegates.native_rcl_send_request(Handle, requestHandle, out sequenceNumber); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ClientDelegates.native_rcl_send_request)}() failed."); diff --git a/rcldotnet/Publisher.cs b/rcldotnet/Publisher.cs index ef99966d..02198057 100644 --- a/rcldotnet/Publisher.cs +++ b/rcldotnet/Publisher.cs @@ -73,24 +73,7 @@ public void Publish(T message) { using (var messageHandle = MessageStaticMemberCache.CreateMessageHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - messageHandle.DangerousAddRef(ref mustRelease); - message.__WriteToHandle(messageHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - messageHandle.DangerousRelease(); - } - } + RCLdotnet.WriteToMessageHandle(message, messageHandle); RCLRet ret = PublisherDelegates.native_rcl_publish(Handle, messageHandle); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(PublisherDelegates.native_rcl_publish)}() failed."); diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 9280796b..4ffe3983 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -624,25 +624,7 @@ private static bool Take(Subscription subscription, IRosMessage message) switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - messageHandle.DangerousAddRef(ref mustRelease); - message.__ReadFromHandle(messageHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - messageHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(message, messageHandle); return true; case RCLRet.SubscriptionTakeFailed: @@ -662,25 +644,7 @@ private static bool TakeRequest(Service service, SafeRequestIdHandle requestHead switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - requestHandle.DangerousAddRef(ref mustRelease); - request.__ReadFromHandle(requestHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - requestHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(request, requestHandle); return true; case RCLRet.ServiceTakeFailed: @@ -700,25 +664,7 @@ private static bool TakeResponse(Client client, SafeRequestIdHandle requestHeade switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - responseHandle.DangerousAddRef(ref mustRelease); - response.__ReadFromHandle(responseHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - responseHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(response, responseHandle); return true; case RCLRet.ClientTakeFailed: @@ -738,25 +684,7 @@ private static bool TakeFeedbackMessage(ActionClient actionClient, IRosMessage f switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - feedbackMessageHandle.DangerousAddRef(ref mustRelease); - feedbackMessage.__ReadFromHandle(feedbackMessageHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - feedbackMessageHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(feedbackMessage, feedbackMessageHandle); return true; case RCLRet.SubscriptionTakeFailed: @@ -776,25 +704,7 @@ private static bool TakeStatusMessage(ActionClient actionClient, IRosMessage sta switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - statusMessageHandle.DangerousAddRef(ref mustRelease); - statusMessage.__ReadFromHandle(statusMessageHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - statusMessageHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(statusMessage, statusMessageHandle); return true; case RCLRet.SubscriptionTakeFailed: @@ -814,25 +724,7 @@ private static bool TakeGoalResponse(ActionClient actionClient, SafeRequestIdHan switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - goalResponseHandle.DangerousAddRef(ref mustRelease); - goalResponse.__ReadFromHandle(goalResponseHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - goalResponseHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(goalResponse, goalResponseHandle); return true; case RCLRet.ClientTakeFailed: @@ -852,25 +744,7 @@ private static bool TakeCancelResponse(ActionClient actionClient, SafeRequestIdH switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - cancelResponseHandle.DangerousAddRef(ref mustRelease); - cancelResponse.__ReadFromHandle(cancelResponseHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - cancelResponseHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(cancelResponse, cancelResponseHandle); return true; case RCLRet.ClientTakeFailed: @@ -890,25 +764,7 @@ private static bool TakeResultResponse(ActionClient actionClient, SafeRequestIdH switch (ret) { case RCLRet.Ok: - bool mustRelease = false; - try - { - // Using SafeHandles for __ReadFromHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywhere. - // It's not worth it, especially considering the extra allocations for SafeHandles in - // arrays or collections that don't really represent their own native resource. - resultResponseHandle.DangerousAddRef(ref mustRelease); - resultResponse.__ReadFromHandle(resultResponseHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - resultResponseHandle.DangerousRelease(); - } - } - + ReadFromMessageHandle(resultResponse, resultResponseHandle); return true; case RCLRet.ClientTakeFailed: @@ -924,30 +780,10 @@ private static void SendResponse(Service service, SafeRequestIdHandle requestHea { using (var responseHandle = service.CreateResponseHandle()) { - bool mustRelease = false; - try - { - // Using SafeHandles for __WriteToHandle() is very tedious as this needs to be - // handled in generated code across multiple assemblies. - // Array and collection indexing would need to create SafeHandles everywere. - // It's not worth it, especialy considering the extra allocations for SafeHandles in - // arrays or collections that don't realy represent their own native recource. - responseHandle.DangerousAddRef(ref mustRelease); - response.__WriteToHandle(responseHandle.DangerousGetHandle()); - } - finally - { - if (mustRelease) - { - responseHandle.DangerousRelease(); - } - } + WriteToMessageHandle(response, responseHandle); RCLRet ret = RCLdotnetDelegates.native_rcl_send_response(service.Handle, requestHeaderHandle, responseHandle); - if (ret != RCLRet.Ok) - { - throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); - } + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_send_response)}() failed."); } } @@ -1197,5 +1033,53 @@ public static string GetRMWIdentifier() string rmw_identifier = Marshal.PtrToStringAnsi(ptr); return rmw_identifier; } + + internal static void ReadFromMessageHandle(IRosMessage message, SafeHandle messageHandle) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __ReadFromHandle() is very tedious as + // this needs to be handled in generated code across multiple + // assemblies. Array and collection indexing would need to + // create SafeHandles everywhere. It's not worth it, especially + // considering the extra allocations for SafeHandles in arrays + // or collections that don't really represent their own native + // resource. + messageHandle.DangerousAddRef(ref mustRelease); + message.__ReadFromHandle(messageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } + } + + internal static void WriteToMessageHandle(IRosMessage message, SafeHandle messageHandle) + { + bool mustRelease = false; + try + { + // Using SafeHandles for __WriteToHandle() is very tedious as + // this needs to be handled in generated code across multiple + // assemblies. Array and collection indexing would need to + // create SafeHandles everywhere. It's not worth it, especially + // considering the extra allocations for SafeHandles in arrays + // or collections that don't really represent their own native + // resource. + messageHandle.DangerousAddRef(ref mustRelease); + message.__WriteToHandle(messageHandle.DangerousGetHandle()); + } + finally + { + if (mustRelease) + { + messageHandle.DangerousRelease(); + } + } + } } } From 8ed025678f5a37fe6afb3f166b51b3d7948bc59a Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Jul 2022 14:14:50 +0200 Subject: [PATCH 13/23] implement basic action server --- rcldotnet/ActionServer.cs | 392 +++++++++++++++++++++++++++- rcldotnet/ActionServerGoalHandle.cs | 104 +++++++- rcldotnet/CMakeLists.txt | 2 + rcldotnet/Node.cs | 70 ++++- rcldotnet/RCLdotnet.cs | 362 +++++++++++++++++++++++++ rcldotnet/SafeActionGoalHandle.cs | 45 ++++ rcldotnet/SafeActionServerHandle.cs | 90 +++++++ rcldotnet/rcldotnet.c | 269 +++++++++++++++++++ rcldotnet/rcldotnet.h | 71 +++++ rcldotnet/rcldotnet_node.c | 36 +++ rcldotnet/rcldotnet_node.h | 9 + 11 files changed, 1434 insertions(+), 16 deletions(-) create mode 100644 rcldotnet/SafeActionGoalHandle.cs create mode 100644 rcldotnet/SafeActionServerHandle.cs diff --git a/rcldotnet/ActionServer.cs b/rcldotnet/ActionServer.cs index c6179589..6db438b5 100644 --- a/rcldotnet/ActionServer.cs +++ b/rcldotnet/ActionServer.cs @@ -13,6 +13,16 @@ * limitations under the License. */ +using System; +using System.Collections.Concurrent; +using System.Collections.Generic; +using System.Diagnostics; +using System.Runtime.InteropServices; +using System.Threading.Tasks; +using action_msgs.msg; +using action_msgs.srv; +using unique_identifier_msgs.msg; + namespace ROS2 { /// @@ -66,12 +76,44 @@ public enum CancelResponse Accept = 2, } + internal enum ActionGoalEvent + { + // These values need to mirror rcl_action_goal_event_t. + + Execute = 0, + CancelGoal, + Succeed, + Abort, + Canceled, + } + public abstract class ActionServer { // Only allow internal subclasses. internal ActionServer() { } + + // ActionServer does intentionally (for now) not implement IDisposable + // as this needs some extra consideration how the type works after its + // internal handle is disposed. By relying on the GC/Finalizer of + // SafeHandle the handle only gets Disposed if the action client is not + // live anymore. + internal abstract SafeActionServerHandle Handle { get; } + + internal abstract IRosMessage CreateSendGoalRequest(); + internal abstract SafeHandle CreateSendGoalRequestHandle(); + + internal abstract IRosActionGetResultRequest CreateGetResultRequest(); + internal abstract SafeHandle CreateGetResultRequestHandle(); + + internal abstract void HandleGoalRequest(SafeRequestIdHandle requestIdHandle, IRosMessage goalRequest); + + internal abstract void HandleCancelRequest(SafeRequestIdHandle requestIdHandle, CancelGoal_Request cancelRequest); + + internal abstract void HandleResultRequest(SafeRequestIdHandle requestIdHandle, IRosActionGetResultRequest resultRequest); + + internal abstract void HandleGoalExpired(); } public sealed class ActionServer : ActionServer @@ -80,9 +122,357 @@ public sealed class ActionServer : ActionSer where TResult : IRosMessage, new() where TFeedback : IRosMessage, new() { + private readonly Action> _acceptedCallback; + + private readonly Func _goalCallback; + + private readonly Func, CancelResponse> _cancelCallback; + + private readonly ConcurrentDictionary> _goalHandles + = new ConcurrentDictionary>(); + // No public constructor. - internal ActionServer() + internal ActionServer( + SafeActionServerHandle handle, + Action> acceptedCallback, + Func goalCallback, + Func, CancelResponse> cancelCallback) + { + Handle = handle; + _acceptedCallback = acceptedCallback; + _goalCallback = goalCallback; + _cancelCallback = cancelCallback; + } + + internal override SafeActionServerHandle Handle { get; } + + internal override IRosMessage CreateSendGoalRequest() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalRequest(); + } + + internal override SafeHandle CreateSendGoalRequestHandle() { + return ActionDefinitionStaticMemberCache.CreateSendGoalRequestHandle(); + } + + internal override IRosActionGetResultRequest CreateGetResultRequest() + { + return ActionDefinitionStaticMemberCache.CreateGetResultRequest(); + } + + internal override SafeHandle CreateGetResultRequestHandle() + { + return ActionDefinitionStaticMemberCache.CreateGetResultRequestHandle(); + } + + internal IRosActionGetResultResponse CreateGetResultResponse() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponse(); + } + + private SafeHandle CreateGetResultResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateGetResultResponseHandle(); + } + + private IRosActionSendGoalResponse CreateSendGoalResponse() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponse(); + } + + private SafeHandle CreateSendGoalResponseHandle() + { + return ActionDefinitionStaticMemberCache.CreateSendGoalResponseHandle(); + } + + private SafeHandle CreateFeedbackMessageHandle() + { + return ActionDefinitionStaticMemberCache.CreateFeedbackMessageHandle(); + } + + internal override void HandleGoalRequest(SafeRequestIdHandle requestIdHandle, IRosMessage goalRequest) + { + var goalRequestCasted = (IRosActionSendGoalRequest)goalRequest; + + var goal = goalRequestCasted.Goal; + var uuid = (UUID)goalRequestCasted.GoalIdAsRosMessage; + var goalId = uuid.ToGuid(); + + // TODO: check if a goalHandle for the goalId already exists. + // + // rclpy does this before calling the goalCallback, rclcpp doesn't + // do this and relies on rcl_action_accept_new_goal to return NULL. + // In the rclpy case goalCallback doesn't get called, in rclcpp it + // does. + // rclpy logs this, sends a not accepted response. + // rclcpp throws an error without logging and sending an response. + // Currently this behaves like rclcpp. + + var goalResponse = _goalCallback(goalId, goal); + + var accepted = + goalResponse == GoalResponse.AcceptAndExecute || + goalResponse == GoalResponse.AcceptAndDefer; + + ActionServerGoalHandle goalHandle = null; + + // goalInfo.Stamp will be filled in by native_rcl_action_accept_new_goal(). + GoalInfo goalInfo = null; + + if (accepted) + { + goalInfo = new GoalInfo(); + + goalInfo.GoalId.Uuid = goalId.ToUuidByteArray(); + + using (var goalInfoHandle = GoalInfo.__CreateMessageHandle()) + { + RCLdotnet.WriteToMessageHandle(goalInfo, goalInfoHandle); + + var actionGoalHandle = new SafeActionGoalHandle(); + RCLRet ret = RCLdotnetDelegates.native_rcl_action_accept_new_goal(ref actionGoalHandle, Handle, goalInfoHandle); + if (ret != RCLRet.Ok) + { + actionGoalHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_accept_new_goal)}() failed."); + } + + // Read back goalInfo.Stamp. + RCLdotnet.ReadFromMessageHandle(goalInfo, goalInfoHandle); + + goalHandle = new ActionServerGoalHandle(actionGoalHandle, this, goalId, goal); + + if (!_goalHandles.TryAdd(goalId, goalHandle)) + { + throw new Exception($"Two goals were accepted with the same Id '{goalId}'."); + }; + + if (goalResponse == GoalResponse.AcceptAndExecute) + { + ret = RCLdotnetDelegates.native_rcl_action_update_goal_state(goalHandle.Handle, ActionGoalEvent.Execute); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_update_goal_state)}() failed."); + } + } + } + + var response = CreateSendGoalResponse(); + response.Accepted = accepted; + + if (accepted) + { + Debug.Assert(goalInfo != null); + + // TODO: (sh) File Issues for different handling of this in rclcpp or rclpy? + // rclpy fills in the response.Stamp but with a different timestamp than rcl_action_accept_new_goal() restamps. + // rclcpp doesn't fill in the stamp at all. + + // Return the stamp that native_rcl_action_accept_new_goal filled in. + response.StampAsRosMessage = goalInfo.Stamp; + } + + SendGoalResponse(requestIdHandle, response); + + if (accepted) + { + Debug.Assert(goalHandle != null); + + PublishStatus(); + + _acceptedCallback(goalHandle); + } + } + + private void SendGoalResponse(SafeRequestIdHandle requestHeaderHandle, IRosMessage goalResponse) + { + using (var goalResponseHandle = CreateSendGoalResponseHandle()) + { + RCLdotnet.WriteToMessageHandle(goalResponse, goalResponseHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_send_goal_response(Handle, requestHeaderHandle, goalResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_send_goal_response)}() failed."); + } + } + + internal override void HandleCancelRequest(SafeRequestIdHandle requestIdHandle, CancelGoal_Request cancelRequest) + { + var cancelResponse = new CancelGoal_Response(); + + using (var cancelRequestHandle = CancelGoal_Request.__CreateMessageHandle()) + using (var cancelResponseHandle = CancelGoal_Response.__CreateMessageHandle()) + { + RCLdotnet.WriteToMessageHandle(cancelRequest, cancelRequestHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_process_cancel_request(Handle, cancelRequestHandle, cancelResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_process_cancel_request)}() failed."); + + RCLdotnet.ReadFromMessageHandle(cancelResponse, cancelResponseHandle); + + var goalsCancelingFromRcl = cancelResponse.GoalsCanceling; + + // Create and fill in new list for the goals the callback accepts to cancel. + cancelResponse.GoalsCanceling = new List(); + + foreach (var goalInfo in goalsCancelingFromRcl) + { + var goalId = goalInfo.GoalId.ToGuid(); + + if (_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + var response = _cancelCallback(goalHandle); + + if (response == CancelResponse.Accept) + { + ret = RCLdotnetDelegates.native_rcl_action_update_goal_state(goalHandle.Handle, ActionGoalEvent.CancelGoal); + if (ret == RCLRet.Ok) + { + cancelResponse.GoalsCanceling.Add(goalInfo); + } + else + { + // If the goal's just succeeded after user cancel callback + // that will generate an exception from invalid transition. + // -> Remove from response since goal has been succeeded. + + // TODO: (sh) Logging. + + // Don't add to goalsForResponse. + } + } + } + else + { + // Possibly the user doesn't care to track the goal handle + // Remove from response. + // or: + // We can't cancel a goal for a goalHandle we don't have. + + // Don't add to goalsForResponse. + } + } + + // TODO: (sh) rclpy doesn't set this? -> Create issue there? + // If the user rejects all individual requests to cancel goals, + // then we consider the top-level cancel request as rejected. + if (goalsCancelingFromRcl.Count > 0 && cancelResponse.GoalsCanceling.Count == 0) + { + cancelResponse.ReturnCode = CancelGoal_Response.ERROR_REJECTED; + } + + if (cancelResponse.GoalsCanceling.Count > 0) + { + // At least one goal state changed, publish a new status message. + PublishStatus(); + } + + RCLdotnet.WriteToMessageHandle(cancelResponse, cancelResponseHandle); + + ret = RCLdotnetDelegates.native_rcl_action_send_cancel_response(Handle, requestIdHandle, cancelResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_send_cancel_response)}() failed."); + } + } + + internal override void HandleResultRequest(SafeRequestIdHandle requestIdHandle, IRosActionGetResultRequest resultRequest) + { + var uuid = (UUID)resultRequest.GoalIdAsRosMessage; + var goalId = uuid.ToGuid(); + + if (_goalHandles.TryGetValue(goalId, out var goalHandle)) + { + // Using Task.ContinueWith here should deal with all the multi-threaded race conditions. + goalHandle.ResultTask.ContinueWith(ResultTaskContinuationAction, requestIdHandle, TaskContinuationOptions.ExecuteSynchronously); + } + else + { + try + { + var resultResponse = CreateGetResultResponse(); + resultResponse.Status = GoalStatus.STATUS_UNKNOWN; + SendResultResponse(requestIdHandle, resultResponse); + } + finally + { + requestIdHandle.Dispose(); + } + } + } + + private void ResultTaskContinuationAction(Task> resultTask, object state) + { + var requestIdHandle = (SafeRequestIdHandle)state; + + try + { + SendResultResponse(requestIdHandle, resultTask.Result); + } + finally + { + requestIdHandle.Dispose(); + } + } + + private void SendResultResponse(SafeRequestIdHandle requestHeaderHandle, IRosActionGetResultResponse resultResponse) + { + using (var resultResponseHandle = CreateGetResultResponseHandle()) + { + RCLdotnet.WriteToMessageHandle(resultResponse, resultResponseHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_send_result_response(Handle, requestHeaderHandle, resultResponseHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_send_result_response)}() failed."); + } + } + + internal override void HandleGoalExpired() + { + // This expects only one goal to expire at a time (rclcpp does the same). + // Loop until no new goals are expired. + var goalInfo = new GoalInfo(); + int numExpired; + + using (var goalInfoHandle = GoalInfo.__CreateMessageHandle()) + { + do + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_expire_goals(Handle, goalInfoHandle, out numExpired); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_expire_goals)}() failed."); + + if (numExpired > 0) + { + RCLdotnet.ReadFromMessageHandle(goalInfo, goalInfoHandle); + Guid goalId = goalInfo.GoalId.ToGuid(); + + _goalHandles.TryRemove(goalId, out _); + } + } while (numExpired > 0); + } + } + + internal void HandleTerminalState( + ActionServerGoalHandle actionServerGoalHandle, + IRosActionGetResultResponse response) + { + actionServerGoalHandle.ResultTaskCompletionSource.SetResult(response); + PublishStatus(); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_notify_goal_done(Handle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_notify_goal_done)}() failed."); + } + + internal void PublishStatus() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_publish_status(Handle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_publish_status)}() failed."); + } + + internal void PublishFeedbackMessage(IRosMessage feedbackMessage) + { + using (var feedbackMessageHandle = CreateFeedbackMessageHandle()) + { + RCLdotnet.WriteToMessageHandle(feedbackMessage, feedbackMessageHandle); + + RCLRet ret = RCLdotnetDelegates.native_rcl_action_publish_feedback(Handle, feedbackMessageHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_publish_feedback)}() failed."); + } } } } diff --git a/rcldotnet/ActionServerGoalHandle.cs b/rcldotnet/ActionServerGoalHandle.cs index 3a4198c6..f5d3e626 100644 --- a/rcldotnet/ActionServerGoalHandle.cs +++ b/rcldotnet/ActionServerGoalHandle.cs @@ -14,6 +14,9 @@ */ using System; +using System.Threading.Tasks; +using action_msgs.msg; +using unique_identifier_msgs.msg; namespace ROS2 { @@ -46,12 +49,11 @@ internal ActionServerGoalHandle() public abstract ActionGoalStatus Status { get; } + internal abstract SafeActionGoalHandle Handle { get; } + // In `rclpy` this calls the `executeCallback` after setting the state to executing. // In `rclcpp` this does not call the `executeCallback` (as there is none) but sets the state for the explicit `AcceptAndDefer` `GoalResponse`. - public void Execute() - { - throw new NotImplementedException(); - } + public abstract void Execute(); } public sealed class ActionServerGoalHandle : ActionServerGoalHandle @@ -60,26 +62,79 @@ public sealed class ActionServerGoalHandle : where TResult : IRosMessage, new() where TFeedback : IRosMessage, new() { + private readonly ActionServer _actionServer; + // No public constructor. - internal ActionServerGoalHandle() + internal ActionServerGoalHandle( + SafeActionGoalHandle handle, + ActionServer actionServer, + Guid goalId, + TGoal goal) { + Handle = handle; + _actionServer = actionServer; + Goal = goal; + GoalId = goalId; + ResultTaskCompletionSource = new TaskCompletionSource>(); } // `rclpy` uses the name `Request`, but the name from `rclcpp` `Goal` fits better. public TGoal Goal { get; } - public override Guid GoalId => throw new NotImplementedException(); + public override Guid GoalId { get; } + + public override bool IsActive + { + get + { + bool isActive = RCLdotnetDelegates.native_rcl_action_goal_handle_is_active(Handle); + return isActive; + } + } + + public override bool IsCanceling => Status == ActionGoalStatus.Canceling; + + public override bool IsExecuting => Status == ActionGoalStatus.Executing; + + public override ActionGoalStatus Status + { + get + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_goal_handle_get_status(Handle, out byte status); + + // In .NET properties should not throw exceptions. Should this + // be converted to a method for this reason? -> No as the rcl + // methods only do argument null checks for values which + // rcldotnet should ensure that they are not null. + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_goal_handle_get_status)}() failed."); + + return (ActionGoalStatus)status; + } + } + + internal override SafeActionGoalHandle Handle { get; } - public override bool IsActive => throw new NotImplementedException(); + internal TaskCompletionSource> ResultTaskCompletionSource { get; } - public override bool IsCanceling => throw new NotImplementedException(); + internal Task> ResultTask => ResultTaskCompletionSource.Task; - public override bool IsExecuting => throw new NotImplementedException(); + public override void Execute() + { + UpdateGoalState(ActionGoalEvent.Execute); - public override ActionGoalStatus Status => throw new NotImplementedException(); + _actionServer.PublishStatus(); + } - public void PublishFeedback(TFeedback feedback) => throw new NotImplementedException(); + public void PublishFeedback(TFeedback feedback) + { + IRosActionFeedbackMessage feedbackMessage = + ActionDefinitionStaticMemberCache.CreateFeedbackMessage(); + var goalId = (UUID)feedbackMessage.GoalIdAsRosMessage; + goalId.Uuid = GoalId.ToUuidByteArray(); + feedbackMessage.Feedback = feedback; + _actionServer.PublishFeedbackMessage(feedbackMessage); + } // TODO: (sh) Decide which (or both?) of these methods should be exposed. // // "rclpy style" @@ -90,17 +145,38 @@ internal ActionServerGoalHandle() // "rclcpp style" public void Succeed(TResult result) { - throw new NotImplementedException(); + UpdateGoalState(ActionGoalEvent.Succeed); + + var response = _actionServer.CreateGetResultResponse(); + response.Status = GoalStatus.STATUS_SUCCEEDED; + response.Result = result; + _actionServer.HandleTerminalState(this, response); } public void Abort(TResult result) { - throw new NotImplementedException(); + UpdateGoalState(ActionGoalEvent.Abort); + + var response = _actionServer.CreateGetResultResponse(); + response.Status = GoalStatus.STATUS_ABORTED; + response.Result = result; + _actionServer.HandleTerminalState(this, response); } public void Canceled(TResult result) { - throw new NotImplementedException(); + UpdateGoalState(ActionGoalEvent.Canceled); + + var response = _actionServer.CreateGetResultResponse(); + response.Status = GoalStatus.STATUS_CANCELED; + response.Result = result; + _actionServer.HandleTerminalState(this, response); + } + + private void UpdateGoalState(ActionGoalEvent actionGoalEvent) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_update_goal_state(Handle, actionGoalEvent); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_update_goal_state)}() failed."); } } } diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 329831aa..12bf48b9 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -49,6 +49,8 @@ set(CS_SOURCES RCLExceptionHelper.cs RCLRet.cs SafeActionClientHandle.cs + SafeActionGoalHandle.cs + SafeActionServerHandle.cs SafeClientHandle.cs SafeGuardConditionHandle.cs SafeNodeHandle.cs diff --git a/rcldotnet/Node.cs b/rcldotnet/Node.cs index 3d28161c..1439d56b 100644 --- a/rcldotnet/Node.cs +++ b/rcldotnet/Node.cs @@ -84,6 +84,18 @@ internal delegate RCLRet NativeRCLActionDestroyClientHandleType( internal static NativeRCLActionDestroyClientHandleType native_rcl_action_destroy_client_handle = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionCreateServerHandleType( + ref SafeActionServerHandle actionServerHandle, SafeNodeHandle nodeHandle, [MarshalAs(UnmanagedType.LPStr)] string actionName, IntPtr typesupportHandle); + + internal static NativeRCLActionCreateServerHandleType native_rcl_action_create_server_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionDestroyServerHandleType( + IntPtr actionServerHandle, SafeNodeHandle nodeHandle); + + internal static NativeRCLActionDestroyServerHandleType native_rcl_action_destroy_server_handle = null; + static NodeDelegates() { _dllLoadUtils = DllLoadUtilsFactory.GetDllLoadUtils(); @@ -158,6 +170,18 @@ static NodeDelegates() NodeDelegates.native_rcl_action_destroy_client_handle = (NativeRCLActionDestroyClientHandleType)Marshal.GetDelegateForFunctionPointer( native_rcl_action_destroy_client_handle_ptr, typeof(NativeRCLActionDestroyClientHandleType)); + + IntPtr native_rcl_action_create_server_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_create_server_handle"); + NodeDelegates.native_rcl_action_create_server_handle = + (NativeRCLActionCreateServerHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_create_server_handle_ptr, typeof(NativeRCLActionCreateServerHandleType)); + + IntPtr native_rcl_action_destroy_server_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_destroy_server_handle"); + NodeDelegates.native_rcl_action_destroy_server_handle = + (NativeRCLActionDestroyServerHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_destroy_server_handle_ptr, typeof(NativeRCLActionDestroyServerHandleType)); } } @@ -173,6 +197,8 @@ public sealed class Node private readonly IList _actionClients; + private readonly IList _actionServers; + internal Node(SafeNodeHandle handle) { Handle = handle; @@ -181,6 +207,7 @@ internal Node(SafeNodeHandle handle) _clients = new List(); _guardConditions = new List(); _actionClients = new List(); + _actionServers = new List(); } public IList Subscriptions => _subscriptions; @@ -196,6 +223,8 @@ internal Node(SafeNodeHandle handle) public IList ActionClients => _actionClients; + public IList ActionServers => _actionServers; + // Node does intentionaly (for now) not implement IDisposable as this // needs some extra consideration how the type works after its // internal handle is disposed. @@ -370,7 +399,46 @@ public ActionServer CreateActionServer.GetTypeSupport(); + + var actionServerHandle = new SafeActionServerHandle(); + RCLRet ret = NodeDelegates.native_rcl_action_create_server_handle(ref actionServerHandle, Handle, actionName, typeSupport); + actionServerHandle.SetParent(Handle); + if (ret != RCLRet.Ok) + { + actionServerHandle.Dispose(); + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(NodeDelegates.native_rcl_action_create_server_handle)}() failed."); + } + + // TODO: (sh) Add actionName to ActionServer. + var actionServer = new ActionServer( + actionServerHandle, + acceptedCallback, + goalCallback, + cancelCallback); + + _actionServers.Add(actionServer); + return actionServer; + } + + private static GoalResponse DefaultGoalCallback(Guid goalId, TGoal goal) + { + return GoalResponse.AcceptAndExecute; + } + + private static CancelResponse DefaultCancelCallback(ActionServerGoalHandle goalHandle) + { + return CancelResponse.Reject; } } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 4ffe3983..5ba5674e 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -192,6 +192,24 @@ internal delegate RCLRet NativeRCLActionClientWaitSetGetEntitiesReadyType( internal static NativeRCLActionClientWaitSetGetEntitiesReadyType native_rcl_action_client_wait_set_get_entities_ready = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionServerWaitSetGetNumEntriesType( + SafeActionServerHandle actionServerHandle, out int numSubscriptions, out int numGuardConditions, out int numTimers, out int numClients, out int numServices); + + internal static NativeRCLActionServerWaitSetGetNumEntriesType native_rcl_action_server_wait_set_get_num_entries = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionWaitSetAddActionServerType( + SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle); + + internal static NativeRCLActionWaitSetAddActionServerType native_rcl_action_wait_set_add_action_server = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionServerWaitSetGetEntitiesReadyType( + SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle, out bool isGoalRequestReady, out bool isCancelRequestReady, out bool isResultRequestReady, out bool isGoalExpired); + + internal static NativeRCLActionServerWaitSetGetEntitiesReadyType native_rcl_action_server_wait_set_get_entities_ready = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLActionTakeFeedbackType( SafeActionClientHandle actionClientHandle, SafeHandle feedbackMessageHandle); @@ -222,6 +240,91 @@ internal delegate RCLRet NativeRCLActionTakeResultResponseType( internal static NativeRCLActionTakeResultResponseType native_rcl_action_take_result_response = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeGoalRequestType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle goalRequestHandle); + + internal static NativeRCLActionTakeGoalRequestType native_rcl_action_take_goal_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendGoalResponseType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle gaolResponseHandle); + + internal static NativeRCLActionSendGoalResponseType native_rcl_action_send_goal_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionAcceptNewGoalType( + ref SafeActionGoalHandle actionGoalHandleHandle, SafeActionServerHandle actionServerHandle, SafeHandle goalInfoHandle); + + internal static NativeRCLActionAcceptNewGoalType native_rcl_action_accept_new_goal = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionDestroyGoalHandleType(IntPtr actionGoalHandleHandle); + + internal static NativeRCLActionDestroyGoalHandleType native_rcl_action_destroy_goal_handle = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionUpdateGoalStateType( + SafeActionGoalHandle actionGoalHandleHandle, ActionGoalEvent goalEvent); + + internal static NativeRCLActionUpdateGoalStateType native_rcl_action_update_goal_state = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionPublishStatusType(SafeActionServerHandle actionServerHandle); + internal static NativeRCLActionPublishStatusType native_rcl_action_publish_status = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionPublishFeedbackType(SafeActionServerHandle actionServerHandle, SafeHandle feedbackMessageHandle); + internal static NativeRCLActionPublishFeedbackType native_rcl_action_publish_feedback = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeCancelRequestType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle cancelRequestHandle); + + internal static NativeRCLActionTakeCancelRequestType native_rcl_action_take_cancel_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionProcessCancelRequestType( + SafeActionServerHandle actionServerHandle, SafeHandle cancelRequestHandle, SafeHandle cancelResponseHandle); + + internal static NativeRCLActionProcessCancelRequestType native_rcl_action_process_cancel_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendCancelResponseType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle cancelResponseHandle); + + internal static NativeRCLActionSendCancelResponseType native_rcl_action_send_cancel_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionTakeResultRequestType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle resultRequestHandle); + + internal static NativeRCLActionTakeResultRequestType native_rcl_action_take_result_request = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionSendResultResponseType( + SafeActionServerHandle actionServerHandle, SafeRequestIdHandle requestHeaderHandle, SafeHandle resultResponseHandle); + + internal static NativeRCLActionSendResultResponseType native_rcl_action_send_result_response = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionNotifyGoalDoneType(SafeActionServerHandle actionServerHandle); + internal static NativeRCLActionNotifyGoalDoneType native_rcl_action_notify_goal_done = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionExpireGoalsType( + SafeActionServerHandle actionServerHandle, SafeHandle goalInfoHandle, out int numExpired); + + internal static NativeRCLActionExpireGoalsType native_rcl_action_expire_goals = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate bool NativeRCLActionGoalHandleIsActiveType(SafeActionGoalHandle actionGoalHandleHandle); + internal static NativeRCLActionGoalHandleIsActiveType native_rcl_action_goal_handle_is_active = null; + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + internal delegate RCLRet NativeRCLActionGoalHandleGetStatusType(SafeActionGoalHandle actionGoalHandleHandle, out byte status); + internal static NativeRCLActionGoalHandleGetStatusType native_rcl_action_goal_handle_get_status = null; + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLCreateQosProfileHandleType(ref SafeQosProfileHandle qosProfileHandle); internal static NativeRCLCreateQosProfileHandleType native_rcl_create_qos_profile_handle = null; @@ -439,6 +542,24 @@ static RCLdotnetDelegates() (NativeRCLActionClientWaitSetGetEntitiesReadyType)Marshal.GetDelegateForFunctionPointer( native_rcl_action_client_wait_set_get_entities_ready_ptr, typeof(NativeRCLActionClientWaitSetGetEntitiesReadyType)); + IntPtr native_rcl_action_server_wait_set_get_num_entries_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_server_wait_set_get_num_entries"); + RCLdotnetDelegates.native_rcl_action_server_wait_set_get_num_entries = + (NativeRCLActionServerWaitSetGetNumEntriesType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_server_wait_set_get_num_entries_ptr, typeof(NativeRCLActionServerWaitSetGetNumEntriesType)); + + IntPtr native_rcl_action_wait_set_add_action_server_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_wait_set_add_action_server"); + RCLdotnetDelegates.native_rcl_action_wait_set_add_action_server = + (NativeRCLActionWaitSetAddActionServerType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_wait_set_add_action_server_ptr, typeof(NativeRCLActionWaitSetAddActionServerType)); + + IntPtr native_rcl_action_server_wait_set_get_entities_ready_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_server_wait_set_get_entities_ready"); + RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready = + (NativeRCLActionServerWaitSetGetEntitiesReadyType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_server_wait_set_get_entities_ready_ptr, typeof(NativeRCLActionServerWaitSetGetEntitiesReadyType)); + IntPtr native_rcl_action_take_feedback_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_feedback"); RCLdotnetDelegates.native_rcl_action_take_feedback @@ -469,6 +590,102 @@ static RCLdotnetDelegates() (NativeRCLActionTakeResultResponseType)Marshal.GetDelegateForFunctionPointer( native_rcl_action_take_result_response_ptr, typeof(NativeRCLActionTakeResultResponseType)); + IntPtr native_rcl_action_take_goal_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_goal_request"); + RCLdotnetDelegates.native_rcl_action_take_goal_request = + (NativeRCLActionTakeGoalRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_goal_request_ptr, typeof(NativeRCLActionTakeGoalRequestType)); + + IntPtr native_rcl_action_send_goal_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_goal_response"); + RCLdotnetDelegates.native_rcl_action_send_goal_response = + (NativeRCLActionSendGoalResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_goal_response_ptr, typeof(NativeRCLActionSendGoalResponseType)); + + IntPtr native_rcl_action_accept_new_goal_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_accept_new_goal"); + RCLdotnetDelegates.native_rcl_action_accept_new_goal = + (NativeRCLActionAcceptNewGoalType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_accept_new_goal_ptr, typeof(NativeRCLActionAcceptNewGoalType)); + + IntPtr native_rcl_action_destroy_goal_handle_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_destroy_goal_handle"); + RCLdotnetDelegates.native_rcl_action_destroy_goal_handle = + (NativeRCLActionDestroyGoalHandleType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_destroy_goal_handle_ptr, typeof(NativeRCLActionDestroyGoalHandleType)); + + IntPtr native_rcl_action_update_goal_state_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_update_goal_state"); + RCLdotnetDelegates.native_rcl_action_update_goal_state = + (NativeRCLActionUpdateGoalStateType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_update_goal_state_ptr, typeof(NativeRCLActionUpdateGoalStateType)); + + IntPtr native_rcl_action_publish_status_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_publish_status"); + RCLdotnetDelegates.native_rcl_action_publish_status = + (NativeRCLActionPublishStatusType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_publish_status_ptr, typeof(NativeRCLActionPublishStatusType)); + + IntPtr native_rcl_action_publish_feedback_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_publish_feedback"); + RCLdotnetDelegates.native_rcl_action_publish_feedback = + (NativeRCLActionPublishFeedbackType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_publish_feedback_ptr, typeof(NativeRCLActionPublishFeedbackType)); + + IntPtr native_rcl_action_take_cancel_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_cancel_request"); + RCLdotnetDelegates.native_rcl_action_take_cancel_request = + (NativeRCLActionTakeCancelRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_cancel_request_ptr, typeof(NativeRCLActionTakeCancelRequestType)); + + IntPtr native_rcl_action_process_cancel_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_process_cancel_request"); + RCLdotnetDelegates.native_rcl_action_process_cancel_request = + (NativeRCLActionProcessCancelRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_process_cancel_request_ptr, typeof(NativeRCLActionProcessCancelRequestType)); + + IntPtr native_rcl_action_send_cancel_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_cancel_response"); + RCLdotnetDelegates.native_rcl_action_send_cancel_response = + (NativeRCLActionSendCancelResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_cancel_response_ptr, typeof(NativeRCLActionSendCancelResponseType)); + + IntPtr native_rcl_action_take_result_request_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_take_result_request"); + RCLdotnetDelegates.native_rcl_action_take_result_request = + (NativeRCLActionTakeResultRequestType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_take_result_request_ptr, typeof(NativeRCLActionTakeResultRequestType)); + + IntPtr native_rcl_action_send_result_response_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_send_result_response"); + RCLdotnetDelegates.native_rcl_action_send_result_response = + (NativeRCLActionSendResultResponseType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_send_result_response_ptr, typeof(NativeRCLActionSendResultResponseType)); + + IntPtr native_rcl_action_notify_goal_done_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_notify_goal_done"); + RCLdotnetDelegates.native_rcl_action_notify_goal_done = + (NativeRCLActionNotifyGoalDoneType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_notify_goal_done_ptr, typeof(NativeRCLActionNotifyGoalDoneType)); + + IntPtr native_rcl_action_expire_goals_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_expire_goals"); + RCLdotnetDelegates.native_rcl_action_expire_goals = + (NativeRCLActionExpireGoalsType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_expire_goals_ptr, typeof(NativeRCLActionExpireGoalsType)); + + IntPtr native_rcl_action_goal_handle_is_active_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_goal_handle_is_active"); + RCLdotnetDelegates.native_rcl_action_goal_handle_is_active = + (NativeRCLActionGoalHandleIsActiveType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_goal_handle_is_active_ptr, typeof(NativeRCLActionGoalHandleIsActiveType)); + + IntPtr native_rcl_action_goal_handle_get_status_ptr = + _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_action_goal_handle_get_status"); + RCLdotnetDelegates.native_rcl_action_goal_handle_get_status = + (NativeRCLActionGoalHandleGetStatusType)Marshal.GetDelegateForFunctionPointer( + native_rcl_action_goal_handle_get_status_ptr, typeof(NativeRCLActionGoalHandleGetStatusType)); + IntPtr native_rcl_create_qos_profile_handle_ptr = _dllLoadUtils.GetProcAddress(nativeLibrary, "native_rcl_create_qos_profile_handle"); RCLdotnetDelegates.native_rcl_create_qos_profile_handle = @@ -583,6 +800,12 @@ private static void WaitSetAddActionClient(SafeWaitSetHandle waitSetHandle, Safe RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_wait_set_add_action_client)}() failed."); } + private static void WaitSetAddActionServer(SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_wait_set_add_action_server(waitSetHandle, actionServerHandle); + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_wait_set_add_action_server)}() failed."); + } + /// /// Block until the wait set is ready or until the timeout has been exceeded. /// @@ -787,6 +1010,66 @@ private static void SendResponse(Service service, SafeRequestIdHandle requestHea } } + private static bool TakeGoalRequest(ActionServer actionServer, SafeRequestIdHandle requestHeaderHandle, IRosMessage goalRequest) + { + using (var goalRequestHandle = actionServer.CreateSendGoalRequestHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_goal_request(actionServer.Handle, requestHeaderHandle, goalRequestHandle); + switch (ret) + { + case RCLRet.Ok: + ReadFromMessageHandle(goalRequest, goalRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_goal_request)}() failed."); + } + } + } + + private static bool TakeCancelRequest(ActionServer actionServer, SafeRequestIdHandle requestHeaderHandle, IRosMessage cancelRequest) + { + using (var cancelRequestHandle = CancelGoal_Request.__CreateMessageHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_cancel_request(actionServer.Handle, requestHeaderHandle, cancelRequestHandle); + switch (ret) + { + case RCLRet.Ok: + ReadFromMessageHandle(cancelRequest, cancelRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_cancel_request)}() failed."); + } + } + } + + private static bool TakeResultRequest(ActionServer actionServer, SafeRequestIdHandle requestHeaderHandle, IRosMessage resultRequest) + { + using (var resultRequestHandle = actionServer.CreateGetResultRequestHandle()) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_take_result_request(actionServer.Handle, requestHeaderHandle, resultRequestHandle); + switch (ret) + { + case RCLRet.Ok: + ReadFromMessageHandle(resultRequest, resultRequestHandle); + return true; + + case RCLRet.ServiceTakeFailed: + return false; + + default: + throw RCLExceptionHelper.CreateFromReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_take_result_request)}() failed."); + } + } + } + public static void SpinOnce(Node node, long timeout) { int numberOfSubscriptions = node.Subscriptions.Count; @@ -815,6 +1098,25 @@ public static void SpinOnce(Node node, long timeout) numberOfServices += acNumberOfServices; } + foreach (var actionServer in node.ActionServers) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_server_wait_set_get_num_entries( + actionServer.Handle, + out int asNumberOfSubscriptions, + out int asNumberOfGuardConditions, + out int asNumberOfTimers, + out int asNumberOfClients, + out int asNumberOfServices); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_server_wait_set_get_num_entries)}() failed."); + + numberOfSubscriptions += asNumberOfSubscriptions; + numberOfGuardConditions += asNumberOfGuardConditions; + numberOfTimers += asNumberOfTimers; + numberOfClients += asNumberOfClients; + numberOfServices += asNumberOfServices; + } + bool waitSetEmpty = numberOfSubscriptions == 0 && numberOfGuardConditions == 0 && numberOfTimers == 0 @@ -865,6 +1167,11 @@ public static void SpinOnce(Node node, long timeout) WaitSetAddActionClient(waitSetHandle, actionClient.Handle); } + foreach (var actionServer in node.ActionServers) + { + WaitSetAddActionServer(waitSetHandle, actionServer.Handle); + } + bool ready = Wait(waitSetHandle, timeout); if (!ready) { @@ -999,6 +1306,61 @@ public static void SpinOnce(Node node, long timeout) } } } + + foreach (var actionServer in node.ActionServers) + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready( + waitSetHandle, + actionServer.Handle, + out bool isGoalRequestReady, + out bool isCancelRequestReady, + out bool isResultRequestReady, + out bool isGoalExpired); + + RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready)}() failed."); + + if (isGoalRequestReady) + { + var goalRequest = actionServer.CreateSendGoalRequest(); + + var result = TakeGoalRequest(actionServer, requestIdHandle, goalRequest); + if (result) + { + actionServer.HandleGoalRequest(requestIdHandle, goalRequest); + } + } + + if (isCancelRequestReady) + { + var cancelRequest = new CancelGoal_Request(); + + var result = TakeCancelRequest(actionServer, requestIdHandle, cancelRequest); + if (result) + { + actionServer.HandleCancelRequest(requestIdHandle, cancelRequest); + } + } + + if (isResultRequestReady) + { + var resultRequest = actionServer.CreateGetResultRequest(); + + // We can't reuse requestIdHandle here as it is needed later when the result is ready. + // Create a new one for each GetResultRequest. + SafeRequestIdHandle resultRequestIdHandle = CreateRequestId(); + + var result = TakeResultRequest(actionServer, resultRequestIdHandle, resultRequest); + if (result) + { + actionServer.HandleResultRequest(resultRequestIdHandle, resultRequest); + } + } + + if (isGoalExpired) + { + actionServer.HandleGoalExpired(); + } + } } int guardConditionIndex = 0; diff --git a/rcldotnet/SafeActionGoalHandle.cs b/rcldotnet/SafeActionGoalHandle.cs new file mode 100644 index 00000000..838083c5 --- /dev/null +++ b/rcldotnet/SafeActionGoalHandle.cs @@ -0,0 +1,45 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Diagnostics; +using Microsoft.Win32.SafeHandles; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_action_goal_handle_t + /// This is only used to implement the action server. + /// It is not used for the action client. + /// + // Could be called SafeActionGoalHandleHandle as well: + // Safe${c_type}Handle where c_type = ActionGoalHandle ;) + internal sealed class SafeActionGoalHandle : SafeHandleZeroOrMinusOneIsInvalid + { + public SafeActionGoalHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + RCLRet ret = RCLdotnetDelegates.native_rcl_action_destroy_goal_handle(handle); + bool successfullyFreed = ret == RCLRet.Ok; + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + } +} diff --git a/rcldotnet/SafeActionServerHandle.cs b/rcldotnet/SafeActionServerHandle.cs new file mode 100644 index 00000000..36831444 --- /dev/null +++ b/rcldotnet/SafeActionServerHandle.cs @@ -0,0 +1,90 @@ +/* Copyright 2022 Stefan Hoffmann + * + * 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.Diagnostics; +using Microsoft.Win32.SafeHandles; + +namespace ROS2 +{ + /// + /// Safe handle representing a rcl_action_server_t + /// + /// + /// Since we need to delete the action client handle before the node is released we need to actually hold a + /// pointer to a rcl_action_server_t unmanaged structure whose destructor decrements a refCount. Only when + /// the node refCount is 0 it is deleted. This way, we loose a race in the critical finalization + /// of the client handle and node handle. + /// + internal sealed class SafeActionServerHandle : SafeHandleZeroOrMinusOneIsInvalid + { + // Trick with parent handles taken from https://github.com/dotnet/corefx/pull/6366 + // Commit from early 2016, but still in current .NET as of september 2021: + // https://github.com/dotnet/runtime/blob/57bfe474518ab5b7cfe6bf7424a79ce3af9d6657/src/libraries/Common/src/Interop/Windows/Advapi32/SafeKeyHandle.cs + // + // Finding docs about SafeHandle is difficult, but the implementation and examples + // in github.com/dotnet/runtime (in combination with the discussions in the PRs) + // should be good enougth. At least are there people that know what they do ;) + // + // Needed to change the exact order of statements in ReleaseOrder() as rcl_client_fini() + // needs the parent handle as well. As far as I understand this there should be no new + // race conditions. + + private SafeNodeHandle _parent; + + public SafeActionServerHandle() + : base(ownsHandle: true) + { + } + + protected override bool ReleaseHandle() + { + var parent = _parent; + _parent = null; + + bool successfullyFreed = false; + if (parent != null) + { + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + RCLRet ret = NodeDelegates.native_rcl_action_destroy_server_handle(handle, parent); + successfullyFreed = ret == RCLRet.Ok; + + parent.DangerousRelease(); + } + + Debug.Assert(successfullyFreed); + + return successfullyFreed; + } + + internal void SetParent(SafeNodeHandle parent) + { + if (IsInvalid || IsClosed) + { + return; + } + + Debug.Assert(_parent == null); + Debug.Assert(!parent.IsClosed); + Debug.Assert(!parent.IsInvalid); + + _parent = parent; + + bool ignored = false; + _parent.DangerousAddRef(ref ignored); + } + } +} diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index 4a7ae58e..a7a590f7 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -26,6 +26,7 @@ #include "rcldotnet.h" static rcl_context_t context; +static rcl_clock_t clock; int32_t native_rcl_init() { // TODO(esteve): parse args @@ -39,9 +40,18 @@ int32_t native_rcl_init() { } const char ** arg_values = NULL; ret = rcl_init(num_args, arg_values, &init_options, &context); + if (ret != RCL_RET_OK) { + return ret; + } + + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); return ret; } +rcl_clock_t *native_rcl_get_default_clock() { + return &clock; +} + const char *native_rcl_get_rmw_identifier() { return rmw_get_implementation_identifier(); } @@ -216,6 +226,47 @@ int32_t native_rcl_action_wait_set_add_action_client(void *wait_set_handle, void return ret; } +int32_t native_rcl_action_server_wait_set_get_num_entries( + void *action_server_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services) +{ + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + + size_t num_subscriptions_as_size_t; + size_t num_guard_conditions_as_size_t; + size_t num_timers_as_size_t; + size_t num_clients_as_size_t; + size_t num_services_as_size_t; + + rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( + action_server, + &num_subscriptions_as_size_t, + &num_guard_conditions_as_size_t, + &num_timers_as_size_t, + &num_clients_as_size_t, + &num_services_as_size_t); + + *num_subscriptions = (int32_t)num_subscriptions_as_size_t; + *num_guard_conditions = (int32_t)num_guard_conditions_as_size_t; + *num_timers = (int32_t)num_timers_as_size_t; + *num_clients = (int32_t)num_clients_as_size_t; + *num_services = (int32_t)num_services_as_size_t; + + return ret; +} + +int32_t native_rcl_action_wait_set_add_action_server(void *wait_set_handle, void *action_server_handle) { + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + rcl_ret_t ret = rcl_action_wait_set_add_action_server(wait_set, action_server, NULL); + + return ret; +} + int32_t native_rcl_wait(void *wait_set_handle, int64_t timeout) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_ret_t ret = rcl_wait(wait_set, timeout); @@ -295,6 +346,28 @@ int32_t native_rcl_action_client_wait_set_get_entities_ready( return ret; } +int32_t native_rcl_action_server_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_server_handle, + bool *is_goal_request_ready, + bool *is_cancel_request_ready, + bool *is_result_request_ready, + bool *is_goal_expired) +{ + rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + wait_set, + action_server, + is_goal_request_ready, + is_cancel_request_ready, + is_result_request_ready, + is_goal_expired); + + return ret; +} + int32_t native_rcl_take(void *subscription_handle, void *message_handle) { rcl_subscription_t * subscription = (rcl_subscription_t *)subscription_handle; @@ -382,6 +455,202 @@ int32_t native_rcl_action_take_result_response(void *action_client_handle, void return ret; } +int32_t native_rcl_action_take_goal_request(void *action_server_handle, void *request_header_handle, void *goal_request_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_goal_request(action_server, request_header, goal_request_handle); + return ret; +} + +int32_t native_rcl_action_send_goal_response(void *action_server_handle, void *request_header_handle, void *goal_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_send_goal_response(action_server, request_header, goal_response_handle); + return ret; +} + +int32_t native_rcl_action_accept_new_goal(void **action_goal_handle_handle, void *action_server_handle, void *goal_info_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rcl_action_goal_info_t * goal_info = (rcl_action_goal_info_t *)goal_info_handle; + + rcl_action_goal_handle_t *action_goal_handle = + (rcl_action_goal_handle_t *)malloc(sizeof(rcl_action_goal_handle_t)); + + *action_goal_handle = rcl_action_get_zero_initialized_goal_handle(); + + rcl_action_goal_handle_t *rcl_action_goal_handle = rcl_action_accept_new_goal(action_server, goal_info); + rcl_ret_t ret; + if (rcl_action_goal_handle == NULL) + { + ret = RCL_RET_ERROR; + } + else + { + // Copy out goal handle since action server storage disappears when it is fini'd. + *action_goal_handle = *rcl_action_goal_handle; + + // Get the goal_info from the goal_handle to return the stamp back to native code. + ret = rcl_action_goal_handle_get_info(action_goal_handle, goal_info); + } + + *action_goal_handle_handle = (void *)action_goal_handle; + + return ret; +} + +int32_t native_rcl_action_destroy_goal_handle(void *action_goal_handle) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle; + + rcl_ret_t ret = rcl_action_goal_handle_fini(goal_handle); + free(goal_handle); + + return ret; +} + +int32_t native_rcl_action_update_goal_state(void *action_goal_handle_handle, int32_t goal_event) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle_handle; + + rcl_ret_t ret = rcl_action_update_goal_state(goal_handle, (rcl_action_goal_event_t)goal_event); + return ret; +} + +int32_t native_rcl_action_publish_status(void *action_server_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + rcl_action_goal_status_array_t status_message = + rcl_action_get_zero_initialized_goal_status_array(); + + rcl_ret_t ret = rcl_action_get_goal_status_array(action_server, &status_message); + if (RCL_RET_OK != ret) { + return ret; + } + + ret = rcl_action_publish_status(action_server, &status_message); + + rcl_ret_t cleanup_ret; + cleanup_ret = rcl_action_goal_status_array_fini(&status_message); + if (cleanup_ret != RCL_RET_OK) + { + // If we got two unexpected errors, return the earlier error. + if (ret == RCL_RET_OK) { + // Error message already set. + ret = cleanup_ret; + } + } + + return ret; +} + +int32_t native_rcl_action_publish_feedback(void *action_server_handle, void *feedback_message_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + rcl_ret_t ret = rcl_action_publish_feedback(action_server, feedback_message_handle); + return ret; +} + +int32_t native_rcl_action_take_cancel_request(void *action_server_handle, void *request_header_handle, void *cancel_request_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_cancel_request(action_server, request_header, cancel_request_handle); + return ret; +} + +int32_t native_rcl_action_process_cancel_request(void *action_server_handle, void *cancel_request_handle, void *cancel_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + // the rcl_action_cancel_request_t is a direct typedef to + // action_msgs__srv__CancelGoal_Request + + // rcl_action_cancel_response_t is a wrapper struct around + // action_msgs__srv__CancelGoal_Response with an additional allocator field. + // -> Don't cast in this case! + + rcl_action_cancel_response_t tmp_cancel_response = rcl_action_get_zero_initialized_cancel_response(); + + rcl_ret_t ret = rcl_action_process_cancel_request(action_server, cancel_request_handle, &tmp_cancel_response); + + // TODO: (sh) would be better to copy the list over element by element? + + // HACK: Don't deallocate but instead move reference to data into incoming cancel_response_handle. + // rcl_action_cancel_response_fini(&cancel_response); + action_msgs__srv__CancelGoal_Response * cancel_response = (action_msgs__srv__CancelGoal_Response *)cancel_response_handle; + *cancel_response = tmp_cancel_response.msg; + + // HACK: as rcl_action_cancel_response_init doesn't fill in capacity... + cancel_response->goals_canceling.capacity = cancel_response->goals_canceling.size; + + return ret; +} + +int32_t native_rcl_action_send_cancel_response(void *action_server_handle, void *request_header_handle, void *cancel_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_send_cancel_response(action_server, request_header, cancel_response_handle); + return ret; +} + +int32_t native_rcl_action_take_result_request(void *action_server_handle, void *request_header_handle, void *result_request_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_take_result_request(action_server, request_header, result_request_handle); + return ret; +} + +int32_t native_rcl_action_send_result_response(void *action_server_handle, void *request_header_handle, void *result_response_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + rmw_request_id_t * request_header = (rmw_request_id_t *)request_header_handle; + + rcl_ret_t ret = rcl_action_send_result_response(action_server, request_header, result_response_handle); + return ret; +} + +int32_t native_rcl_action_notify_goal_done(void *action_server_handle) { + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + rcl_ret_t ret = rcl_action_notify_goal_done(action_server); + return ret; +} + +int32_t native_rcl_action_expire_goals(void *action_server_handle, void *goal_info_handle, int32_t *num_expired) +{ + rcl_action_server_t * action_server = (rcl_action_server_t *)action_server_handle; + + size_t num_expired_as_size_t; + + // Only provide one goal_info to the underlying function. + // rclcpp does the same as only one goal is expected to expire at the same time. + // The out parameter num_expired can be used with a loop to expire all goals. + // This does also help to avoid implementing a new SafeHandle and accessor methods for a list of `GoalHandle`s. + rcl_ret_t ret = rcl_action_expire_goals(action_server, goal_info_handle, 1, &num_expired_as_size_t); + + *num_expired = (int32_t)num_expired_as_size_t; + + return ret; +} + +bool native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle_handle; + + return rcl_action_goal_handle_is_active(goal_handle); +} + +int32_t native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle, int8_t *status) { + rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle_handle; + + rcl_action_goal_state_t status_as_rcl_type; + + rcl_ret_t ret = rcl_action_goal_handle_get_status(goal_handle, &status_as_rcl_type); + + *status = (int8_t)status_as_rcl_type; + + return ret; +} + int32_t native_rcl_create_qos_profile_handle(void **qos_profile_handle) { rmw_qos_profile_t *qos_profile = (rmw_qos_profile_t *)malloc(sizeof(rmw_qos_profile_t)); diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index f0ea55fe..ba8b9d1e 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -20,6 +20,8 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_init(); +rcl_clock_t *native_rcl_get_default_clock(); + RCLDOTNET_EXPORT const char * RCLDOTNET_CDECL native_rcl_get_rmw_identifier(); @@ -84,6 +86,18 @@ int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_num_entries( RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_wait_set_add_action_client(void *wait_set_handle, void *action_client_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_server_wait_set_get_num_entries( + void *action_server_handle, + int32_t *num_subscriptions, + int32_t *num_guard_conditions, + int32_t *num_timers, + int32_t *num_clients, + int32_t *num_services); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_wait_set_add_action_server(void *wait_set_handle, void *action_server_handle); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_wait(void *, int64_t); @@ -109,6 +123,15 @@ int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_entities_ready( bool *is_cancel_response_ready, bool *is_result_response_ready); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_server_wait_set_get_entities_ready( + void *wait_set_handle, + void *action_server_handle, + bool *is_goal_request_ready, + bool *is_cancel_request_ready, + bool *is_result_request_ready, + bool *is_goal_expired); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take(void *, void *); @@ -145,6 +168,54 @@ int32_t RCLDOTNET_CDECL native_rcl_action_take_cancel_response(void *action_clie RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_take_result_response(void *action_client_handle, void *request_header_handle, void *result_response_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_goal_request(void *action_server_handle, void *request_header_handle, void *goal_request_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_goal_response(void *action_server_handle, void *request_header_handle, void *goal_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_accept_new_goal(void **action_goal_handle_handle, void *action_server_handle, void *goal_info_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_destroy_goal_handle(void *action_goal_handle_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_update_goal_state(void *action_goal_handle_handle, int32_t goal_event); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_publish_status(void *action_server_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_publish_feedback(void *action_server_handle, void *feedback_message_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_cancel_request(void *action_server_handle, void *request_header_handle, void *cancel_request_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_process_cancel_request(void *action_server_handle, void *cancel_request_handle, void *cancel_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_cancel_response(void *action_server_handle, void *request_header_handle, void *cancel_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_take_result_request(void *action_server_handle, void *request_header_handle, void *result_request_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_send_result_response(void *action_server_handle, void *request_header_handle, void *result_response_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_notify_goal_done(void *action_server_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_expire_goals(void *action_server_handle, void *goal_info_handle, int32_t *num_expired); + +RCLDOTNET_EXPORT +bool RCLDOTNET_CDECL native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle, int8_t *status); + RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_create_qos_profile_handle(void **qos_profile_handle); diff --git a/rcldotnet/rcldotnet_node.c b/rcldotnet/rcldotnet_node.c index d298a24b..3dfa900a 100644 --- a/rcldotnet/rcldotnet_node.c +++ b/rcldotnet/rcldotnet_node.c @@ -24,6 +24,7 @@ #include "rosidl_runtime_c/message_type_support_struct.h" +#include "rcldotnet.h" #include "rcldotnet_node.h" int32_t native_rcl_create_publisher_handle(void **publisher_handle, @@ -204,3 +205,38 @@ int32_t native_rcl_action_destroy_client_handle(void *action_client_handle, void return ret; } + +int32_t native_rcl_action_create_server_handle(void **action_server_handle, + void *node_handle, + const char *action_name, + void *typesupport) { + rcl_node_t *node = (rcl_node_t *)node_handle; + + rosidl_action_type_support_t *ts = + (rosidl_action_type_support_t *)typesupport; + + rcl_action_server_t *action_server = + (rcl_action_server_t *)malloc(sizeof(rcl_action_server_t)); + *action_server = rcl_action_get_zero_initialized_server(); + rcl_action_server_options_t action_server_ops = + rcl_action_server_get_default_options(); + + rcl_clock_t *clock = native_rcl_get_default_clock(); + + rcl_ret_t ret = + rcl_action_server_init(action_server, node, clock, ts, action_name, &action_server_ops); + + *action_server_handle = (void *)action_server; + + return ret; +} + +int32_t native_rcl_action_destroy_server_handle(void *action_server_handle, void *node_handle) { + rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + rcl_node_t *node = (rcl_node_t *)node_handle; + + rcl_ret_t ret = rcl_action_server_fini(action_server, node); + free(action_server); + + return ret; +} diff --git a/rcldotnet/rcldotnet_node.h b/rcldotnet/rcldotnet_node.h index ca263e36..f28da07e 100644 --- a/rcldotnet/rcldotnet_node.h +++ b/rcldotnet/rcldotnet_node.h @@ -62,4 +62,13 @@ int32_t RCLDOTNET_CDECL native_rcl_action_create_client_handle(void **action_cli RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_destroy_client_handle(void *action_client_handle, void *node_handle); +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_create_server_handle(void **action_server_handle, + void *node_handle, + const char *action_name, + void *typesupport); + +RCLDOTNET_EXPORT +int32_t RCLDOTNET_CDECL native_rcl_action_destroy_server_handle(void *action_server_handle, void *node_handle); + #endif // RCLDOTNET_NODE_H From b4503f9af877f9bbd29fa0f0eabfb7a9d0371759 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 19 Jul 2022 15:58:29 +0200 Subject: [PATCH 14/23] add example action client and server --- rcldotnet_examples/CMakeLists.txt | 18 +++++ rcldotnet_examples/RCLDotnetActionClient.cs | 87 +++++++++++++++++++++ rcldotnet_examples/RCLDotnetActionServer.cs | 70 +++++++++++++++++ rcldotnet_examples/package.xml | 4 + 4 files changed, 179 insertions(+) create mode 100644 rcldotnet_examples/RCLDotnetActionClient.cs create mode 100644 rcldotnet_examples/RCLDotnetActionServer.cs diff --git a/rcldotnet_examples/CMakeLists.txt b/rcldotnet_examples/CMakeLists.txt index a5518444..7b95e6cc 100644 --- a/rcldotnet_examples/CMakeLists.txt +++ b/rcldotnet_examples/CMakeLists.txt @@ -7,8 +7,10 @@ find_package(rcldotnet REQUIRED) find_package(rcldotnet_common REQUIRED) +find_package(builtin_interfaces REQUIRED) find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(test_msgs REQUIRED) find_package(rcldotnet REQUIRED) find_package(dotnet_cmake_module REQUIRED) @@ -19,8 +21,10 @@ find_package(DotNETExtra REQUIRED) set(_assemblies_dep_dlls ${rcldotnet_common_ASSEMBLIES_DLL} ${rcldotnet_ASSEMBLIES_DLL} + ${builtin_interfaces_ASSEMBLIES_DLL} ${std_msgs_ASSEMBLIES_DLL} ${std_srvs_ASSEMBLIES_DLL} + ${test_msgs_ASSEMBLIES_DLL} ) add_dotnet_executable(rcldotnet_talker @@ -51,10 +55,24 @@ add_dotnet_executable(rcldotnet_guard_condition ${_assemblies_dep_dlls} ) +add_dotnet_executable(rcldotnet_example_action_server + RCLDotnetActionServer.cs + INCLUDE_DLLS + ${_assemblies_dep_dlls} +) + +add_dotnet_executable(rcldotnet_example_action_client + RCLDotnetActionClient.cs + INCLUDE_DLLS + ${_assemblies_dep_dlls} +) + install_dotnet(rcldotnet_talker DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_listener DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_example_service DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_example_client DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(rcldotnet_guard_condition DESTINATION lib/${PROJECT_NAME}/dotnet) +install_dotnet(rcldotnet_example_action_server DESTINATION lib/${PROJECT_NAME}/dotnet) +install_dotnet(rcldotnet_example_action_client DESTINATION lib/${PROJECT_NAME}/dotnet) ament_package() diff --git a/rcldotnet_examples/RCLDotnetActionClient.cs b/rcldotnet_examples/RCLDotnetActionClient.cs new file mode 100644 index 00000000..e90475f3 --- /dev/null +++ b/rcldotnet_examples/RCLDotnetActionClient.cs @@ -0,0 +1,87 @@ +using System; +using System.Threading; +using System.Threading.Tasks; +using ROS2; +using test_msgs.action; + +namespace ConsoleApplication +{ + public static class RCLDotnetActionClient + { + public static void Main(string[] args) + { + RCLdotnet.Init(); + var node = RCLdotnet.CreateNode("action_client"); + + var actionClient = node.CreateActionClient("fibonacci"); + + var cts = new CancellationTokenSource(); + var task = DoWorkAsync(actionClient, cts.Token); + + while (RCLdotnet.Ok()) + { + RCLdotnet.SpinOnce(node, 500); + + if (task.IsCompletedSuccessfully) + { + break; + } + else if (task.IsFaulted) + { + Console.WriteLine($"Task faulted. Exception {task.Exception}"); + break; + } + else if (task.IsCanceled) + { + Console.WriteLine("Task canceled."); + break; + } + } + + cts.Cancel(); + task.GetAwaiter().GetResult(); + } + + private static async Task DoWorkAsync( + ActionClient actionClient, + CancellationToken cancellationToken) + { + while (!actionClient.ServerIsReady()) + { + cancellationToken.ThrowIfCancellationRequested(); + + // NOTE: This causes the code to resume in an background worker Thread. + // Consider this when copying code from the example if additional synchronization is needed. + await Task.Delay(1000, cancellationToken); + } + + var goal = new Fibonacci_Goal(); + goal.Order = 10; + + Console.WriteLine("SendGoal"); + + ActionClientGoalHandle goalHandleForCallback = null; + + var goalHandle = await actionClient.SendGoalAsync(goal, (Fibonacci_Feedback feedback) => + { + Console.WriteLine($"Feedback: {string.Join(", ", feedback.Sequence)}"); + Console.WriteLine($"Status after Feedback: {goalHandleForCallback.Status}"); + }); + + goalHandleForCallback = goalHandle; + + if (goalHandle.Accepted) + { + Console.WriteLine("Goal accepted."); + + var result = await goalHandle.GetResultAsync(); + Console.WriteLine($"Result: {string.Join(", ", result.Sequence)}"); + Console.WriteLine($"Status: {goalHandle.Status}"); + } + else + { + Console.WriteLine("Goal not accepted."); + } + } + } +} diff --git a/rcldotnet_examples/RCLDotnetActionServer.cs b/rcldotnet_examples/RCLDotnetActionServer.cs new file mode 100644 index 00000000..5cb9f576 --- /dev/null +++ b/rcldotnet_examples/RCLDotnetActionServer.cs @@ -0,0 +1,70 @@ +using System; +using System.Collections.Generic; +using System.Threading; +using System.Threading.Tasks; +using ROS2; +using test_msgs.action; + +namespace ConsoleApplication +{ + public static class RCLDotnetActionServer + { + public static void Main(string[] args) + { + RCLdotnet.Init(); + var node = RCLdotnet.CreateNode("action_server"); + + var actionServer = node.CreateActionServer("fibonacci", HandleAccepted, cancelCallback: HandleCancel); + + RCLdotnet.Spin(node); + } + + private static void HandleAccepted(ActionServerGoalHandle goalHandle) + { + // Don't block in the callback. + // -> Don't wait for the returned Task. + _ = DoWorkWithGoal(goalHandle); + } + + private static CancelResponse HandleCancel(ActionServerGoalHandle goalHandle) + { + return CancelResponse.Accept; + } + + private static async Task DoWorkWithGoal(ActionServerGoalHandle goalHandle) + { + Console.WriteLine("Executing goal..."); + var feedback = new Fibonacci_Feedback(); + + feedback.Sequence = new List { 0, 1 }; + + for (int i = 1; i < goalHandle.Goal.Order; i++) + { + if (goalHandle.IsCanceling) + { + var cancelResult = new Fibonacci_Result(); + cancelResult.Sequence = feedback.Sequence; + + Console.WriteLine($"Canceled Result: {string.Join(", ", cancelResult.Sequence)}"); + goalHandle.Canceled(cancelResult); + return; + } + + feedback.Sequence.Add(feedback.Sequence[i] + feedback.Sequence[i - 1]); + + Console.WriteLine($"Feedback: {string.Join(", ", feedback.Sequence)}"); + goalHandle.PublishFeedback(feedback); + + // NOTE: This causes the code to resume in an background worker Thread. + // Consider this when copying code from the example if additional synchronization is needed. + await Task.Delay(1000); + } + + var result = new Fibonacci_Result(); + result.Sequence = feedback.Sequence; + + Console.WriteLine($"Result: {string.Join(", ", result.Sequence)}"); + goalHandle.Succeed(result); + } + } +} diff --git a/rcldotnet_examples/package.xml b/rcldotnet_examples/package.xml index 1a4ef781..602e7a01 100644 --- a/rcldotnet_examples/package.xml +++ b/rcldotnet_examples/package.xml @@ -13,18 +13,22 @@ rcldotnet_common rosidl_cmake + builtin_interfaces example_interfaces rcldotnet std_msgs std_srvs sensor_msgs + test_msgs rcldotnet_common + builtin_interfaces example_interfaces rcldotnet std_msgs std_srvs sensor_msgs + test_msgs ament_cmake From 9522815109faf439408b0f409cd1ffa90da7daae Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Fri, 22 Jul 2022 09:36:11 +0200 Subject: [PATCH 15/23] make local variable name more obvious --- rcldotnet/ActionClient.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index a8f52c08..f7982795 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -313,12 +313,12 @@ internal override void HandleFeedbackMessage(IRosMessage feedbackMessage) internal override void HandleStatusMessage(GoalStatusArray statusMessage) { - foreach (var statusMessage1 in statusMessage.StatusList) + foreach (var statusMessageEntry in statusMessage.StatusList) { - var goalId = statusMessage1.GoalInfo.GoalId.ToGuid(); + var goalId = statusMessageEntry.GoalInfo.GoalId.ToGuid(); if (_goalHandles.TryGetValue(goalId, out var goalHandle)) { - var status = statusMessage1.Status; + var status = statusMessageEntry.Status; goalHandle.Status = ConvertGoalStatus(status); // rclpy does this, rclcpp does not. From 41ed59228f6f64518a2883491f9db1b5a0c40699 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Fri, 22 Jul 2022 09:43:06 +0200 Subject: [PATCH 16/23] use return value from Interlocked.CompareExchange The field access after Interlocked.CompareExchange might not reflect the updated value. Or does it? Anyways this makes it more clear. --- rcldotnet/ActionClientGoalHandle.cs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rcldotnet/ActionClientGoalHandle.cs b/rcldotnet/ActionClientGoalHandle.cs index bdae9f8b..34ca9baa 100644 --- a/rcldotnet/ActionClientGoalHandle.cs +++ b/rcldotnet/ActionClientGoalHandle.cs @@ -87,10 +87,11 @@ public Task GetResultAsync() var resultTaskCompletionSource = new TaskCompletionSource(); - if (Interlocked.CompareExchange(ref _resultTaskCompletionSource, resultTaskCompletionSource, null) != null) + var oldResultTcs = Interlocked.CompareExchange(ref _resultTaskCompletionSource, resultTaskCompletionSource, null); + if (oldResultTcs != null) { // Some other thread was first. - return _resultTaskCompletionSource.Task; + return oldResultTcs.Task; } return _actionClient.GetResultAsync(this, resultTaskCompletionSource); From 23f5e460a70d9ef2506f4ab0bcf19094cc9f2b56 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 28 Jul 2022 09:00:06 +0200 Subject: [PATCH 17/23] reorder HandleGoalResponse, HandleStatusMessage and HandleFeedbackMessage --- rcldotnet/RCLdotnet.cs | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 5ba5674e..8d747351 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -1248,17 +1248,24 @@ public static void SpinOnce(Node node, long timeout) RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready)}() failed."); - if (isFeedbackReady) + // Check isGoalResponseReady so that a new goalHandle is + // already created if the status or feedback for the new + // goal is received. + if (isGoalResponseReady) { - var feedbackMessage = actionClient.CreateFeedbackMessage(); + var goalResponse = actionClient.CreateSendGoalResponse(); - var result = TakeFeedbackMessage(actionClient, feedbackMessage); + var result = TakeGoalResponse(actionClient, requestIdHandle, goalResponse); if (result) { - actionClient.HandleFeedbackMessage(feedbackMessage); + var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); + actionClient.HandleGoalResponse(sequenceNumber, goalResponse); } } + // Check isStatusReady before isFeedbackReady so that + // the feedback callback already has the newest status + // information if updates are received at the same time. if (isStatusReady) { var statusMessage = new GoalStatusArray(); @@ -1270,15 +1277,14 @@ public static void SpinOnce(Node node, long timeout) } } - if (isGoalResponseReady) + if (isFeedbackReady) { - var goalResponse = actionClient.CreateSendGoalResponse(); + var feedbackMessage = actionClient.CreateFeedbackMessage(); - var result = TakeGoalResponse(actionClient, requestIdHandle, goalResponse); + var result = TakeFeedbackMessage(actionClient, feedbackMessage); if (result) { - var sequenceNumber = RCLdotnetDelegates.native_rcl_request_id_get_sequence_number(requestIdHandle); - actionClient.HandleGoalResponse(sequenceNumber, goalResponse); + actionClient.HandleFeedbackMessage(feedbackMessage); } } From 12da3eece16754a5b5fe9f1b641f052951b0c384 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 28 Jul 2022 09:28:40 +0200 Subject: [PATCH 18/23] handle action specific take failed error codes --- rcldotnet/RCLRet.cs | 4 +++- rcldotnet/RCLdotnet.cs | 8 ++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/rcldotnet/RCLRet.cs b/rcldotnet/RCLRet.cs index 95c2f6de..2f529d2a 100644 --- a/rcldotnet/RCLRet.cs +++ b/rcldotnet/RCLRet.cs @@ -50,6 +50,8 @@ internal enum RCLRet InvalidParamRule = 1010, InvalidLogLevelRule = 1020, EventInvalid = 2000, - EventTakeFailed = 2001 + EventTakeFailed = 2001, + ActionClientTakeFailed = 2103, + ActionServerTakeFailed = 2201, } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 8d747351..8d74c990 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -911,6 +911,7 @@ private static bool TakeFeedbackMessage(ActionClient actionClient, IRosMessage f return true; case RCLRet.SubscriptionTakeFailed: + case RCLRet.ActionClientTakeFailed: return false; default: @@ -931,6 +932,7 @@ private static bool TakeStatusMessage(ActionClient actionClient, IRosMessage sta return true; case RCLRet.SubscriptionTakeFailed: + case RCLRet.ActionClientTakeFailed: return false; default: @@ -951,6 +953,7 @@ private static bool TakeGoalResponse(ActionClient actionClient, SafeRequestIdHan return true; case RCLRet.ClientTakeFailed: + case RCLRet.ActionClientTakeFailed: return false; default: @@ -971,6 +974,7 @@ private static bool TakeCancelResponse(ActionClient actionClient, SafeRequestIdH return true; case RCLRet.ClientTakeFailed: + case RCLRet.ActionClientTakeFailed: return false; default: @@ -991,6 +995,7 @@ private static bool TakeResultResponse(ActionClient actionClient, SafeRequestIdH return true; case RCLRet.ClientTakeFailed: + case RCLRet.ActionClientTakeFailed: return false; default: @@ -1022,6 +1027,7 @@ private static bool TakeGoalRequest(ActionServer actionServer, SafeRequestIdHand return true; case RCLRet.ServiceTakeFailed: + case RCLRet.ActionServerTakeFailed: return false; default: @@ -1042,6 +1048,7 @@ private static bool TakeCancelRequest(ActionServer actionServer, SafeRequestIdHa return true; case RCLRet.ServiceTakeFailed: + case RCLRet.ActionServerTakeFailed: return false; default: @@ -1062,6 +1069,7 @@ private static bool TakeResultRequest(ActionServer actionServer, SafeRequestIdHa return true; case RCLRet.ServiceTakeFailed: + case RCLRet.ActionServerTakeFailed: return false; default: From 5a504ff22e781c6567b74e6376caf7778d0f0784 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Thu, 28 Jul 2022 09:33:42 +0200 Subject: [PATCH 19/23] publish status before sending the result response --- rcldotnet/ActionServer.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcldotnet/ActionServer.cs b/rcldotnet/ActionServer.cs index 6db438b5..18a1caca 100644 --- a/rcldotnet/ActionServer.cs +++ b/rcldotnet/ActionServer.cs @@ -451,8 +451,8 @@ internal void HandleTerminalState( ActionServerGoalHandle actionServerGoalHandle, IRosActionGetResultResponse response) { - actionServerGoalHandle.ResultTaskCompletionSource.SetResult(response); PublishStatus(); + actionServerGoalHandle.ResultTaskCompletionSource.SetResult(response); RCLRet ret = RCLdotnetDelegates.native_rcl_action_notify_goal_done(Handle); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_notify_goal_done)}() failed."); From 0416620ba76b7d29f7db330f2667accfe8ab43e0 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Wed, 17 Aug 2022 14:25:28 +0200 Subject: [PATCH 20/23] fix typo in dependency --- rcldotnet/package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcldotnet/package.xml b/rcldotnet/package.xml index db01ebe6..88be5f0f 100644 --- a/rcldotnet/package.xml +++ b/rcldotnet/package.xml @@ -19,14 +19,14 @@ rmw_implementation_cmake rcl - rcl_actions + rcl_action rmw action_msgs builtin_interfaces unique_identifier_msgs rcl - rcl_actions + rcl_action action_msgs builtin_interfaces unique_identifier_msgs From 3187f5a1a64841e26e372ccb660f8e13d1ad84f3 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Fri, 19 May 2023 21:28:20 +0200 Subject: [PATCH 21/23] fix bool pInvokes in actions --- rcldotnet/ActionClient.cs | 6 +-- rcldotnet/ActionServerGoalHandle.cs | 4 +- rcldotnet/RCLdotnet.cs | 42 +++++++++---------- rcldotnet/rcldotnet.c | 63 ++++++++++++++++++++--------- rcldotnet/rcldotnet.h | 20 ++++----- rcldotnet/rcldotnet_action_client.c | 7 +++- rcldotnet/rcldotnet_action_client.h | 2 +- 7 files changed, 85 insertions(+), 59 deletions(-) diff --git a/rcldotnet/ActionClient.cs b/rcldotnet/ActionClient.cs index f7982795..9f9e362e 100644 --- a/rcldotnet/ActionClient.cs +++ b/rcldotnet/ActionClient.cs @@ -49,7 +49,7 @@ internal delegate RCLRet NativeRCLActionSendCancelRequestType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLActionServerIsAvailableType( - SafeNodeHandle nodeHandle, SafeActionClientHandle clientHandle, out bool isAvailable); + SafeNodeHandle nodeHandle, SafeActionClientHandle clientHandle, out int isAvailable); internal static NativeRCLActionServerIsAvailableType native_rcl_action_server_is_available = null; @@ -156,10 +156,10 @@ internal ActionClient(SafeActionClientHandle handle, Node node) public override bool ServerIsReady() { - RCLRet ret = ActionClientDelegates.native_rcl_action_server_is_available(_node.Handle, Handle, out var serverIsReady); + RCLRet ret = ActionClientDelegates.native_rcl_action_server_is_available(_node.Handle, Handle, out int serverIsReady); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(ActionClientDelegates.native_rcl_action_server_is_available)}() failed."); - return serverIsReady; + return serverIsReady != 0; } public Task> SendGoalAsync(TGoal goal) diff --git a/rcldotnet/ActionServerGoalHandle.cs b/rcldotnet/ActionServerGoalHandle.cs index f5d3e626..cd87d68c 100644 --- a/rcldotnet/ActionServerGoalHandle.cs +++ b/rcldotnet/ActionServerGoalHandle.cs @@ -87,8 +87,8 @@ public override bool IsActive { get { - bool isActive = RCLdotnetDelegates.native_rcl_action_goal_handle_is_active(Handle); - return isActive; + int isActive = RCLdotnetDelegates.native_rcl_action_goal_handle_is_active(Handle); + return isActive != 0; } } diff --git a/rcldotnet/RCLdotnet.cs b/rcldotnet/RCLdotnet.cs index 8d74c990..3a1136a1 100644 --- a/rcldotnet/RCLdotnet.cs +++ b/rcldotnet/RCLdotnet.cs @@ -188,7 +188,7 @@ internal delegate RCLRet NativeRCLActionWaitSetAddActionClientType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLActionClientWaitSetGetEntitiesReadyType( - SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle, out bool isFeedbackReady, out bool isStatusReady, out bool isGoalResponseReady, out bool isCancelResponseReady, out bool isResultResponseReady); + SafeWaitSetHandle waitSetHandle, SafeActionClientHandle actionClientHandle, out int isFeedbackReady, out int isStatusReady, out int isGoalResponseReady, out int isCancelResponseReady, out int isResultResponseReady); internal static NativeRCLActionClientWaitSetGetEntitiesReadyType native_rcl_action_client_wait_set_get_entities_ready = null; @@ -206,7 +206,7 @@ internal delegate RCLRet NativeRCLActionWaitSetAddActionServerType( [UnmanagedFunctionPointer(CallingConvention.Cdecl)] internal delegate RCLRet NativeRCLActionServerWaitSetGetEntitiesReadyType( - SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle, out bool isGoalRequestReady, out bool isCancelRequestReady, out bool isResultRequestReady, out bool isGoalExpired); + SafeWaitSetHandle waitSetHandle, SafeActionServerHandle actionServerHandle, out int isGoalRequestReady, out int isCancelRequestReady, out int isResultRequestReady, out int isGoalExpired); internal static NativeRCLActionServerWaitSetGetEntitiesReadyType native_rcl_action_server_wait_set_get_entities_ready = null; @@ -318,7 +318,7 @@ internal delegate RCLRet NativeRCLActionExpireGoalsType( internal static NativeRCLActionExpireGoalsType native_rcl_action_expire_goals = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - internal delegate bool NativeRCLActionGoalHandleIsActiveType(SafeActionGoalHandle actionGoalHandleHandle); + internal delegate int NativeRCLActionGoalHandleIsActiveType(SafeActionGoalHandle actionGoalHandleHandle); internal static NativeRCLActionGoalHandleIsActiveType native_rcl_action_goal_handle_is_active = null; [UnmanagedFunctionPointer(CallingConvention.Cdecl)] @@ -1248,18 +1248,18 @@ public static void SpinOnce(Node node, long timeout) RCLRet ret = RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready( waitSetHandle, actionClient.Handle, - out bool isFeedbackReady, - out bool isStatusReady, - out bool isGoalResponseReady, - out bool isCancelResponseReady, - out bool isResultResponseReady); + out int isFeedbackReady, + out int isStatusReady, + out int isGoalResponseReady, + out int isCancelResponseReady, + out int isResultResponseReady); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_client_wait_set_get_entities_ready)}() failed."); // Check isGoalResponseReady so that a new goalHandle is // already created if the status or feedback for the new // goal is received. - if (isGoalResponseReady) + if (isGoalResponseReady != 0) { var goalResponse = actionClient.CreateSendGoalResponse(); @@ -1274,7 +1274,7 @@ public static void SpinOnce(Node node, long timeout) // Check isStatusReady before isFeedbackReady so that // the feedback callback already has the newest status // information if updates are received at the same time. - if (isStatusReady) + if (isStatusReady != 0) { var statusMessage = new GoalStatusArray(); @@ -1285,7 +1285,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isFeedbackReady) + if (isFeedbackReady != 0) { var feedbackMessage = actionClient.CreateFeedbackMessage(); @@ -1296,7 +1296,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isCancelResponseReady) + if (isCancelResponseReady != 0) { var cancelResponse = new CancelGoal_Response(); @@ -1308,7 +1308,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isResultResponseReady) + if (isResultResponseReady != 0) { var resultResponse = actionClient.CreateGetResultResponse(); @@ -1326,14 +1326,14 @@ public static void SpinOnce(Node node, long timeout) RCLRet ret = RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready( waitSetHandle, actionServer.Handle, - out bool isGoalRequestReady, - out bool isCancelRequestReady, - out bool isResultRequestReady, - out bool isGoalExpired); + out int isGoalRequestReady, + out int isCancelRequestReady, + out int isResultRequestReady, + out int isGoalExpired); RCLExceptionHelper.CheckReturnValue(ret, $"{nameof(RCLdotnetDelegates.native_rcl_action_server_wait_set_get_entities_ready)}() failed."); - if (isGoalRequestReady) + if (isGoalRequestReady != 0) { var goalRequest = actionServer.CreateSendGoalRequest(); @@ -1344,7 +1344,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isCancelRequestReady) + if (isCancelRequestReady != 0) { var cancelRequest = new CancelGoal_Request(); @@ -1355,7 +1355,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isResultRequestReady) + if (isResultRequestReady != 0) { var resultRequest = actionServer.CreateGetResultRequest(); @@ -1370,7 +1370,7 @@ public static void SpinOnce(Node node, long timeout) } } - if (isGoalExpired) + if (isGoalExpired != 0) { actionServer.HandleGoalExpired(); } diff --git a/rcldotnet/rcldotnet.c b/rcldotnet/rcldotnet.c index a7a590f7..ffd268dd 100644 --- a/rcldotnet/rcldotnet.c +++ b/rcldotnet/rcldotnet.c @@ -325,23 +325,35 @@ int32_t native_rcl_wait_set_guard_condition_ready(void *wait_set_handle, int32_t int32_t native_rcl_action_client_wait_set_get_entities_ready( void *wait_set_handle, void *action_client_handle, - bool *is_feedback_ready, - bool *is_status_ready, - bool *is_goal_response_ready, - bool *is_cancel_response_ready, - bool *is_result_response_ready) + int32_t *is_feedback_ready, + int32_t *is_status_ready, + int32_t *is_goal_response_ready, + int32_t *is_cancel_response_ready, + int32_t *is_result_response_ready) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_action_client_t *action_client = (rcl_action_client_t *)action_client_handle; + bool is_feedback_ready_as_bool; + bool is_status_ready_as_bool; + bool is_goal_response_ready_as_bool; + bool is_cancel_response_ready_as_bool; + bool is_result_response_ready_as_bool; + rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( wait_set, action_client, - is_feedback_ready, - is_status_ready, - is_goal_response_ready, - is_cancel_response_ready, - is_result_response_ready); + &is_feedback_ready_as_bool, + &is_status_ready_as_bool, + &is_goal_response_ready_as_bool, + &is_cancel_response_ready_as_bool, + &is_result_response_ready_as_bool); + + *is_feedback_ready = is_feedback_ready_as_bool ? 1 : 0; + *is_status_ready = is_status_ready_as_bool ? 1 : 0; + *is_goal_response_ready = is_goal_response_ready_as_bool ? 1 : 0; + *is_cancel_response_ready = is_cancel_response_ready_as_bool ? 1 : 0; + *is_result_response_ready = is_result_response_ready_as_bool ? 1 : 0; return ret; } @@ -349,21 +361,31 @@ int32_t native_rcl_action_client_wait_set_get_entities_ready( int32_t native_rcl_action_server_wait_set_get_entities_ready( void *wait_set_handle, void *action_server_handle, - bool *is_goal_request_ready, - bool *is_cancel_request_ready, - bool *is_result_request_ready, - bool *is_goal_expired) + int32_t *is_goal_request_ready, + int32_t *is_cancel_request_ready, + int32_t *is_result_request_ready, + int32_t *is_goal_expired) { rcl_wait_set_t *wait_set = (rcl_wait_set_t *)wait_set_handle; rcl_action_server_t *action_server = (rcl_action_server_t *)action_server_handle; + bool is_goal_request_ready_as_bool; + bool is_cancel_request_ready_as_bool; + bool is_result_request_ready_as_bool; + bool is_goal_expired_as_bool; + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( wait_set, action_server, - is_goal_request_ready, - is_cancel_request_ready, - is_result_request_ready, - is_goal_expired); + &is_goal_request_ready_as_bool, + &is_cancel_request_ready_as_bool, + &is_result_request_ready_as_bool, + &is_goal_expired_as_bool); + + *is_goal_request_ready = is_goal_request_ready_as_bool ? 1 : 0; + *is_cancel_request_ready = is_cancel_request_ready_as_bool ? 1 : 0; + *is_result_request_ready = is_result_request_ready_as_bool ? 1 : 0; + *is_goal_expired = is_goal_expired_as_bool ? 1 : 0; return ret; } @@ -633,10 +655,11 @@ int32_t native_rcl_action_expire_goals(void *action_server_handle, void *goal_in return ret; } -bool native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle) { +int32_t native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle) { rcl_action_goal_handle_t *goal_handle = (rcl_action_goal_handle_t *)action_goal_handle_handle; - return rcl_action_goal_handle_is_active(goal_handle); + bool result = rcl_action_goal_handle_is_active(goal_handle); + return result ? 1 : 0; } int32_t native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle, int8_t *status) { diff --git a/rcldotnet/rcldotnet.h b/rcldotnet/rcldotnet.h index ba8b9d1e..6c4252ff 100644 --- a/rcldotnet/rcldotnet.h +++ b/rcldotnet/rcldotnet.h @@ -117,20 +117,20 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_client_wait_set_get_entities_ready( void *wait_set_handle, void *action_client_handle, - bool *is_feedback_ready, - bool *is_status_ready, - bool *is_goal_response_ready, - bool *is_cancel_response_ready, - bool *is_result_response_ready); + int32_t *is_feedback_ready, + int32_t *is_status_ready, + int32_t *is_goal_response_ready, + int32_t *is_cancel_response_ready, + int32_t *is_result_response_ready); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_server_wait_set_get_entities_ready( void *wait_set_handle, void *action_server_handle, - bool *is_goal_request_ready, - bool *is_cancel_request_ready, - bool *is_result_request_ready, - bool *is_goal_expired); + int32_t *is_goal_request_ready, + int32_t *is_cancel_request_ready, + int32_t *is_result_request_ready, + int32_t *is_goal_expired); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_take(void *, void *); @@ -211,7 +211,7 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_expire_goals(void *action_server_handle, void *goal_info_handle, int32_t *num_expired); RCLDOTNET_EXPORT -bool RCLDOTNET_CDECL native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle); +int32_t RCLDOTNET_CDECL native_rcl_action_goal_handle_is_active(void *action_goal_handle_handle); RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_goal_handle_get_status(void *action_goal_handle_handle, int8_t *status); diff --git a/rcldotnet/rcldotnet_action_client.c b/rcldotnet/rcldotnet_action_client.c index a03e6491..25d4ea52 100644 --- a/rcldotnet/rcldotnet_action_client.c +++ b/rcldotnet/rcldotnet_action_client.c @@ -49,10 +49,13 @@ int32_t native_rcl_action_send_cancel_request(void *action_client_handle, void * return ret; } -int32_t native_rcl_action_server_is_available(void *node_handle, void *client_handle, bool *is_available) { +int32_t native_rcl_action_server_is_available(void *node_handle, void *client_handle, int32_t *is_available) { rcl_node_t * node = (rcl_node_t *)node_handle; rcl_action_client_t * client = (rcl_action_client_t *)client_handle; - rcl_ret_t ret = rcl_action_server_is_available(node, client, is_available); + bool is_available_as_bool; + rcl_ret_t ret = rcl_action_server_is_available(node, client, &is_available_as_bool); + *is_available = is_available_as_bool ? 1 : 0; + return ret; } diff --git a/rcldotnet/rcldotnet_action_client.h b/rcldotnet/rcldotnet_action_client.h index 93d06d4c..4fbd873f 100644 --- a/rcldotnet/rcldotnet_action_client.h +++ b/rcldotnet/rcldotnet_action_client.h @@ -27,6 +27,6 @@ RCLDOTNET_EXPORT int32_t RCLDOTNET_CDECL native_rcl_action_send_cancel_request(void *action_client_handle, void *cancel_request_handle, int64_t *sequence_number); RCLDOTNET_EXPORT -int32_t RCLDOTNET_CDECL native_rcl_action_server_is_available(void *node_handle, void *action_client_handle, bool *is_available); +int32_t RCLDOTNET_CDECL native_rcl_action_server_is_available(void *node_handle, void *action_client_handle, int32_t *is_available); #endif // RCLDOTNET_CLIENT_H From ac13d48dc2604e50aa93579d3b06115b857f06d1 Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Mon, 29 May 2023 11:11:09 +0200 Subject: [PATCH 22/23] add unittests for actions --- rcldotnet/CMakeLists.txt | 1 + rcldotnet/test/test_actions.cs | 240 +++++++++++++++++++++++++++++++++ 2 files changed, 241 insertions(+) create mode 100644 rcldotnet/test/test_actions.cs diff --git a/rcldotnet/CMakeLists.txt b/rcldotnet/CMakeLists.txt index 12bf48b9..883bbe85 100644 --- a/rcldotnet/CMakeLists.txt +++ b/rcldotnet/CMakeLists.txt @@ -143,6 +143,7 @@ if(BUILD_TESTING) ${RCLDOTNET_TEST_TARGET_FRAMEWORK} SOURCES ${CS_SOURCES} + test/test_actions.cs test/test_guard_conditions.cs test/test_messages.cs test/test_services.cs diff --git a/rcldotnet/test/test_actions.cs b/rcldotnet/test/test_actions.cs new file mode 100644 index 00000000..78acf162 --- /dev/null +++ b/rcldotnet/test/test_actions.cs @@ -0,0 +1,240 @@ +/* Copyright 2023 Stefan Hoffmann + * + * 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.Threading.Tasks; +using ROS2; +using test_msgs.action; +using Xunit; + +namespace RCLdotnetTests +{ + // TODO: (sh) Notes for extended tests: + // - Add tests for: + // - ActionClientGoalHandle.Stamp + // - ActionServerGoalHandle.IsActive, .IsExecuting, .Status, .Execute() + // - Node.CreateActionServer() that overrides goalCallback + // - Node.CreateActionServer() that overrides cancelCallback that doesn't + // allow cancel. + // - Extend test coverage for: + // - ActionClientGoalHandle.Status + // - Test that invalid state-transitions throw exceptions. + // (e.g. ActionServerGoalHandle.Canceled() when the cancelCallback didn't + // allow cancellation) + // - Test like rclpy (look into rclcpp as well) with mock server/client to + // test the cases with different callback orderings. + // - https://github.com/ros2/rclpy/blob/rolling/rclpy/test/test_action_client.py + // - https://github.com/ros2/rclpy/blob/rolling/rclpy/test/test_action_server.py + + public sealed class TestActions + { + private readonly Node _serverNode; + private readonly Node _clientNode; + + // Each test method get's its own instance of this class, so no need to + // reset the fields here. + private Guid _serverGoalId; + private int _serverGoalOrder; + private List _clientFeedbackSequence = null; + + public TestActions() + { + RCLdotnet.Init(); + _serverNode = RCLdotnet.CreateNode("action_server_node"); + _clientNode = RCLdotnet.CreateNode("action_client_node"); + } + + [Fact] + public void TestActionClientServerIsReady() + { + var actionClient = _clientNode.CreateActionClient("unittest_dotnet_fibonacci"); + + SpinEachNodeOnce(); + Assert.False(actionClient.ServerIsReady()); + + var actionServer = _serverNode.CreateActionServer("unittest_dotnet_fibonacci", HandleAccepted); + + SpinEachNodeOnce(); + Assert.True(actionClient.ServerIsReady()); + } + + [Fact] + public void TestActionSucceed() + { + var actionServer = _serverNode.CreateActionServer("unittest_dotnet_fibonacci", HandleAccepted); + var actionClient = _clientNode.CreateActionClient("unittest_dotnet_fibonacci"); + + var goal = new Fibonacci_Goal(); + goal.Order = 3; + var goalHandleTask = actionClient.SendGoalAsync(goal, HandleFeedback); + + SpinEachNodeUntilTaskCompleted(goalHandleTask); + Assert.Equal(3, _serverGoalOrder); + Assert.NotNull(_clientFeedbackSequence); + Assert.Equal(new List { 0, 1 }, _clientFeedbackSequence); + + var goalHandle = goalHandleTask.GetAwaiter().GetResult(); + Assert.True(goalHandle.Accepted); + Assert.NotEqual(Guid.Empty, goalHandle.GoalId); + Assert.Equal(_serverGoalId, goalHandle.GoalId); + + var resultTask = goalHandle.GetResultAsync(); + SpinEachNodeUntilTaskCompleted(resultTask); + var result = resultTask.GetAwaiter().GetResult(); + Assert.Equal(new List { 0, 1 }, result.Sequence); + Assert.Equal(ActionGoalStatus.Succeeded, goalHandle.Status); + } + + [Fact] + public void TestActionAbort() + { + var actionServer = _serverNode.CreateActionServer("unittest_dotnet_fibonacci", HandleAcceptedWithAbort); + var actionClient = _clientNode.CreateActionClient("unittest_dotnet_fibonacci"); + + var goal = new Fibonacci_Goal(); + goal.Order = 23; + var goalHandleTask = actionClient.SendGoalAsync(goal); + + SpinEachNodeUntilTaskCompleted(goalHandleTask); + Assert.Equal(23, _serverGoalOrder); + + var goalHandle = goalHandleTask.GetAwaiter().GetResult(); + Assert.True(goalHandle.Accepted); + Assert.NotEqual(Guid.Empty, goalHandle.GoalId); + Assert.Equal(_serverGoalId, goalHandle.GoalId); + + var resultTask = goalHandle.GetResultAsync(); + SpinEachNodeUntilTaskCompleted(resultTask); + var result = resultTask.GetAwaiter().GetResult(); + Assert.Equal(new List { -1 }, result.Sequence); + Assert.Equal(ActionGoalStatus.Aborted, goalHandle.Status); + } + + [Fact] + public void TestActionCanceled() + { + var actionServer = _serverNode.CreateActionServer("unittest_dotnet_fibonacci", HandleAcceptedWithCanceled, cancelCallback: HandleCancel); + var actionClient = _clientNode.CreateActionClient("unittest_dotnet_fibonacci"); + + var goal = new Fibonacci_Goal(); + goal.Order = int.MaxValue; + var goalHandleTask = actionClient.SendGoalAsync(goal); + + SpinEachNodeUntilTaskCompleted(goalHandleTask); + Assert.Equal(int.MaxValue, _serverGoalOrder); + + var goalHandle = goalHandleTask.GetAwaiter().GetResult(); + Assert.True(goalHandle.Accepted); + Assert.NotEqual(Guid.Empty, goalHandle.GoalId); + Assert.Equal(_serverGoalId, goalHandle.GoalId); + + var cancelGoalTask = goalHandle.CancelGoalAsync(); + SpinEachNodeUntilTaskCompleted(cancelGoalTask); + cancelGoalTask.GetAwaiter().GetResult(); + + var resultTask = goalHandle.GetResultAsync(); + SpinEachNodeUntilTaskCompleted(resultTask); + var result = resultTask.GetAwaiter().GetResult(); + Assert.Equal(new List { 42 }, result.Sequence); + Assert.Equal(ActionGoalStatus.Canceled, goalHandle.Status); + } + + private void HandleAccepted(ActionServerGoalHandle goalHandle) + { + _serverGoalId = goalHandle.GoalId; + _serverGoalOrder = goalHandle.Goal.Order; + + var feedback = new Fibonacci_Feedback(); + feedback.Sequence = new List { 0, 1 }; + goalHandle.PublishFeedback(feedback); + + SpinEachNodeOnce(); + + var result = new Fibonacci_Result(); + result.Sequence = feedback.Sequence; + goalHandle.Succeed(result); + } + + private void HandleAcceptedWithAbort(ActionServerGoalHandle goalHandle) + { + _serverGoalId = goalHandle.GoalId; + _serverGoalOrder = goalHandle.Goal.Order; + + var result = new Fibonacci_Result(); + result.Sequence = new List { -1 }; + goalHandle.Abort(result); + } + + private void HandleAcceptedWithCanceled(ActionServerGoalHandle goalHandle) + { + _ = HandleAcceptedWithCanceledAsync(goalHandle); + } + + private async Task HandleAcceptedWithCanceledAsync(ActionServerGoalHandle goalHandle) + { + try + { + _serverGoalId = goalHandle.GoalId; + _serverGoalOrder = goalHandle.Goal.Order; + + var result = new Fibonacci_Result(); + result.Sequence = new List { 42 }; + + for (int i = 0; i < 20; i++) + { + if (goalHandle.IsCanceling) + { + goalHandle.Canceled(result); + return; + } + + await Task.Delay(50); + } + + // didn't get to the `IsCanceling` state -> abort that the final assertion fails + goalHandle.Abort(result); + } + catch (Exception exception) + { + Console.WriteLine(exception.Message); + } + } + + private CancelResponse HandleCancel(ActionServerGoalHandle goalHandle) + { + return CancelResponse.Accept; + } + + private void HandleFeedback(Fibonacci_Feedback feedback) + { + _clientFeedbackSequence = feedback.Sequence; + } + + private void SpinEachNodeUntilTaskCompleted(Task task) + { + while (!task.IsCompleted) + { + SpinEachNodeOnce(); + } + } + + private void SpinEachNodeOnce() + { + RCLdotnet.SpinOnce(_serverNode, 50); + RCLdotnet.SpinOnce(_clientNode, 50); + } + } +} From fe7a78148d37c51da0e83f54e36f296fc558e94a Mon Sep 17 00:00:00 2001 From: Stefan Hoffmann Date: Tue, 15 Aug 2023 13:50:52 +0200 Subject: [PATCH 23/23] update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 1fbe9fb3..c1aeeeef 100644 --- a/README.md +++ b/README.md @@ -21,6 +21,7 @@ The current set of features include: - Generation of all builtin ROS types - Support for publishers and subscriptions - Support for clients and services +- Support action clients and servers - Cross-platform support (Linux, Windows, Windows IoT Core, UWP) What's missing? @@ -30,7 +31,6 @@ Lots of things! - Unicode types - String constants (specifically BoundedString) - Component nodes -- Actions - Tests - Documentation - More examples (e.g. IoT, VB, UWP, HoloLens, etc.)