From 3193916c250948a3b6d67af26811b81fe54ec574 Mon Sep 17 00:00:00 2001 From: Robert Roos Date: Tue, 28 Nov 2023 10:45:03 +0100 Subject: [PATCH 1/5] Added new actionserver example + Stubs for new class --- src/ros2cs/ros2cs_core/ActionServer.cs | 23 ++++++++++ src/ros2cs/ros2cs_core/interfaces/IAction.cs | 27 +++++++++++ src/ros2cs/ros2cs_examples/CMakeLists.txt | 7 +++ .../ros2cs_examples/ROS2ActionServer.cs | 45 +++++++++++++++++++ 4 files changed, 102 insertions(+) create mode 100644 src/ros2cs/ros2cs_core/ActionServer.cs create mode 100644 src/ros2cs/ros2cs_core/interfaces/IAction.cs create mode 100644 src/ros2cs/ros2cs_examples/ROS2ActionServer.cs diff --git a/src/ros2cs/ros2cs_core/ActionServer.cs b/src/ros2cs/ros2cs_core/ActionServer.cs new file mode 100644 index 00000000..b6d0f80b --- /dev/null +++ b/src/ros2cs/ros2cs_core/ActionServer.cs @@ -0,0 +1,23 @@ +// Copyright 2019-2021 Robotec.ai +// +// 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 class ActionServer: IActionServer + where G: Message, new () + where F: Message, new () + where R: Message, new () + { + } +} diff --git a/src/ros2cs/ros2cs_core/interfaces/IAction.cs b/src/ros2cs/ros2cs_core/interfaces/IAction.cs new file mode 100644 index 00000000..5329e628 --- /dev/null +++ b/src/ros2cs/ros2cs_core/interfaces/IAction.cs @@ -0,0 +1,27 @@ +// Copyright 2019-2021 Robotec.ai +// +// 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 +{ + /// Generic base interface for all actions (either server or client) + /// Message Type for the goal + /// Message Type for the feedback + /// Message Type for the result + public interface IAction : IExtendedDisposable + where G: Message + where F: Message + where R: Message + { + } +} diff --git a/src/ros2cs/ros2cs_examples/CMakeLists.txt b/src/ros2cs/ros2cs_examples/CMakeLists.txt index cf8c062f..9fcbaf6a 100644 --- a/src/ros2cs/ros2cs_examples/CMakeLists.txt +++ b/src/ros2cs/ros2cs_examples/CMakeLists.txt @@ -76,11 +76,18 @@ add_dotnet_executable(ros2cs_service ${_assemblies_dep_dlls} ) +add_dotnet_executable(ros2cs_actionserver + ROS2ActionServer.cs + INCLUDE_DLLS + ${_assemblies_dep_dlls} +) + install_dotnet(ros2cs_listener DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(ros2cs_talker DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(ros2cs_performance_talker DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(ros2cs_performance_listener DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(ros2cs_client DESTINATION lib/${PROJECT_NAME}/dotnet) install_dotnet(ros2cs_service DESTINATION lib/${PROJECT_NAME}/dotnet) +install_dotnet(ros2cs_actionserver DESTINATION lib/${PROJECT_NAME}/dotnet) ament_package() diff --git a/src/ros2cs/ros2cs_examples/ROS2ActionServer.cs b/src/ros2cs/ros2cs_examples/ROS2ActionServer.cs new file mode 100644 index 00000000..7bd5f10e --- /dev/null +++ b/src/ros2cs/ros2cs_examples/ROS2ActionServer.cs @@ -0,0 +1,45 @@ +// Copyright 2019-2021 Robotec.ai +// +// 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 ROS2; +using example_interfaces.action; + +namespace Examples +{ + /// A simple action server class to illustrate Ros2cs + public class ROS2ActionServer + { + public static IAction my_action_server; + + public static void Main(string[] args) + { + Console.WriteLine("ActionServer start") + Ros2cs.Init(); + INode node = Ros2cs.CreateNode("action_server"); + my_action_server = node.CreateActionServer( + "fibonacci", + goal_callback + ) + + Ros2cs.Spin(node); + Ros2cs.Shutdown(); + } + + public static bool goal_callback() + { + Console.WriteLine("Receiving new action goal..."); + } + } +} From 938e067b4dfd8a27d559d50d138c63c71d75305d Mon Sep 17 00:00:00 2001 From: Robert Roos Date: Tue, 28 Nov 2023 14:19:26 +0100 Subject: [PATCH 2/5] Started with including Actions in interface --- .../rosidl_generator_cs/resource/action.cs.em | 0 .../rosidl_generator_cs/resource/idl.cs.em | 35 ++++++++++++++++++- 2 files changed, 34 insertions(+), 1 deletion(-) create mode 100644 src/ros2cs/rosidl_generator_cs/resource/action.cs.em diff --git a/src/ros2cs/rosidl_generator_cs/resource/action.cs.em b/src/ros2cs/rosidl_generator_cs/resource/action.cs.em new file mode 100644 index 00000000..e69de29b diff --git a/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em b/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em index 3ac8ffeb..90a88a38 100644 --- a/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em +++ b/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em @@ -82,5 +82,38 @@ TEMPLATE( }@ @[end for]@ @[end if]@ -@# // endif +@####################################################################### +@# Handle action +@####################################################################### +@{ +from rosidl_parser.definition import Action +}@ + +@[if include_parts[1] == "action"]@ + +@[for action in content.get_elements_of_type(Action)]@ + +// Action found + +@{ +TEMPLATE( + 'srv.cs.em', + package_name=package_name, + interface_path=interface_path, + service=action.send_goal_service, + message=action.send_goal_service.request_message, + include_directives=include_directives, + get_dotnet_type=get_dotnet_type, + get_field_name=get_field_name, + constant_value_to_dotnet=constant_value_to_dotnet, + get_c_type=get_c_type, + get_marshal_type=get_marshal_type, + get_marshal_array_type=get_marshal_array_type, + get_csbuild_tool=get_csbuild_tool + ) +}@ + +@[end for]@ + +@[end if]@ From 438f2c9532b7556b180852e9c5a671fc56d321ed Mon Sep 17 00:00:00 2001 From: Robert Roos Date: Fri, 1 Dec 2023 11:58:51 +0100 Subject: [PATCH 3/5] Got Action related stuff to generate source - but new source is not compiled into interface --- src/ros2cs/ros2cs_core/ActionServer.cs | 35 +++++++- src/ros2cs/ros2cs_core/CMakeLists.txt | 2 + src/ros2cs/ros2cs_core/Node.cs | 30 ++++++- .../{IAction.cs => IActionServer.cs} | 18 +++-- src/ros2cs/ros2cs_core/interfaces/INode.cs | 11 +++ .../ros2cs_examples/ROS2ActionServer.cs | 23 ++++-- .../rosidl_generator_cs/resource/action.cs.em | 0 .../rosidl_generator_cs/resource/idl.c.em | 80 ++++++++++++++++++- .../rosidl_generator_cs/resource/idl.cs.em | 78 +++++++++++++++++- 9 files changed, 256 insertions(+), 21 deletions(-) rename src/ros2cs/ros2cs_core/interfaces/{IAction.cs => IActionServer.cs} (64%) delete mode 100644 src/ros2cs/rosidl_generator_cs/resource/action.cs.em diff --git a/src/ros2cs/ros2cs_core/ActionServer.cs b/src/ros2cs/ros2cs_core/ActionServer.cs index b6d0f80b..56f328ee 100644 --- a/src/ros2cs/ros2cs_core/ActionServer.cs +++ b/src/ros2cs/ros2cs_core/ActionServer.cs @@ -12,12 +12,39 @@ // See the License for the specific language governing permissions and // limitations under the License. +using System; +using ROS2.Internal; + namespace ROS2 { - public class ActionServer: IActionServer - where G: Message, new () - where F: Message, new () - where R: Message, new () + public class ActionServer: IActionServer + where TGoalRequest : Message, new() + where TGoalResponse : Message, new() + where TFeedback : Message, new() + where TResultRequest : Message, new() + where TResultResponse : Message, new() { + /// + /// Internal constructor + /// + /// Use to construct new Instances + internal ActionServer(string subTopic, Node node, Func cb, QualityOfServiceProfile qpos = null) + { + + } + + public void Dispose() + { + DestroyActionServer(); + } + + /// + public bool IsDisposed { get { return IsDisposed; } } + private bool disposed = false; + + private void DestroyActionServer() + { + + } } } diff --git a/src/ros2cs/ros2cs_core/CMakeLists.txt b/src/ros2cs/ros2cs_core/CMakeLists.txt index d5f2955a..1b9f50d4 100644 --- a/src/ros2cs/ros2cs_core/CMakeLists.txt +++ b/src/ros2cs/ros2cs_core/CMakeLists.txt @@ -77,6 +77,7 @@ set(CS_INTERFACES interfaces/INode.cs interfaces/IClient.cs interfaces/IService.cs + interfaces/IActionServer.cs interfaces/IPublisher.cs interfaces/ISubscription.cs ) @@ -100,6 +101,7 @@ set(CS_SOURCES Clock.cs Client.cs Service.cs + ActionServer.cs Node.cs Publisher.cs QualityOfServiceProfile.cs diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 6911c2d1..29c163c3 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -65,6 +65,7 @@ internal List Services private HashSet publishers; private HashSet clients; private HashSet services; + private HashSet action_servers; private readonly object mutex = new object(); private bool disposed = false; @@ -82,6 +83,7 @@ internal Node(string nodeName, ref rcl_context_t context) publishers = new HashSet(); clients = new HashSet(); services = new HashSet(); + action_servers = new HashSet(); nodeHandle = NativeRcl.rcl_get_zero_initialized_node(); defaultNodeOptions = NativeRclInterface.rclcs_node_create_default_options(); @@ -187,7 +189,7 @@ public bool RemoveClient(IClientBase client) return null; } - Service service = new Service(topic, this, callback, qos); + Service service = new Service(topic, this, callback, qos); services.Add(service); logger.LogInfo("Created service for topic " + topic); return service; @@ -210,6 +212,32 @@ public bool RemoveService(IServiceBase service) } } + public ActionServer + CreateActionServer( + string topic, Func callback, QualityOfServiceProfile qos = null + ) + where TGoalRequest : Message, new() + where TGoalResponse : Message, new() + where TFeedback : Message, new() + where TResultRequest : Message, new() + where TResultResponse : Message, new() + { + lock (mutex) + { + if (disposed || !Ros2cs.Ok()) + { + logger.LogWarning("Cannot create action server as the class is already disposed or shutdown was called"); + return null; + } + + ActionServer action_server = + new ActionServer(topic, this, callback, qos); + action_servers.Add(action_server); + logger.LogInfo("Created action server for topic " + topic); + return action_server; + } + } + /// Create a publisher for this node for a given topic, qos and message type /// public Publisher CreatePublisher(string topic, QualityOfServiceProfile qos = null) where T : Message, new() diff --git a/src/ros2cs/ros2cs_core/interfaces/IAction.cs b/src/ros2cs/ros2cs_core/interfaces/IActionServer.cs similarity index 64% rename from src/ros2cs/ros2cs_core/interfaces/IAction.cs rename to src/ros2cs/ros2cs_core/interfaces/IActionServer.cs index 5329e628..b7b47f82 100644 --- a/src/ros2cs/ros2cs_core/interfaces/IAction.cs +++ b/src/ros2cs/ros2cs_core/interfaces/IActionServer.cs @@ -14,14 +14,18 @@ namespace ROS2 { + public interface IActionServerBase : IExtendedDisposable + { + + } + /// Generic base interface for all actions (either server or client) - /// Message Type for the goal - /// Message Type for the feedback - /// Message Type for the result - public interface IAction : IExtendedDisposable - where G: Message - where F: Message - where R: Message + public interface IActionServer : IActionServerBase + where TGoalRequest : Message, new() + where TGoalResponse : Message, new() + where TFeedback : Message, new() + where TResultRequest : Message, new() + where TResultResponse : Message, new() { } } diff --git a/src/ros2cs/ros2cs_core/interfaces/INode.cs b/src/ros2cs/ros2cs_core/interfaces/INode.cs index 90b59d4c..682ff0af 100644 --- a/src/ros2cs/ros2cs_core/interfaces/INode.cs +++ b/src/ros2cs/ros2cs_core/interfaces/INode.cs @@ -47,6 +47,17 @@ public interface INode: IExtendedDisposable /// Service for the topic Service CreateService(string topic, Func callback, QualityOfServiceProfile qos = null) where I : Message, new() where O : Message, new(); + /// Create an action server for this node + ActionServer + CreateActionServer( + string topic, Func callback, QualityOfServiceProfile qos = null + ) + where TGoalRequest : Message, new() + where TGoalResponse : Message, new() + where TFeedback : Message, new() + where TResultRequest : Message, new() + where TResultResponse : Message, new(); + /// Remove a service /// Note that this does not call Dispose on Service /// Service created with earlier CreateService call diff --git a/src/ros2cs/ros2cs_examples/ROS2ActionServer.cs b/src/ros2cs/ros2cs_examples/ROS2ActionServer.cs index 7bd5f10e..ec188f9c 100644 --- a/src/ros2cs/ros2cs_examples/ROS2ActionServer.cs +++ b/src/ros2cs/ros2cs_examples/ROS2ActionServer.cs @@ -21,25 +21,38 @@ namespace Examples /// A simple action server class to illustrate Ros2cs public class ROS2ActionServer { - public static IAction my_action_server; + public static IActionServer< + Fibonacci_SendGoal_Request, + Fibonacci_SendGoal_Response, + Fibonacci_Feedback, + Fibonacci_GetResult_Request, + Fibonacci_GetResult_Response + > my_action_server; public static void Main(string[] args) { - Console.WriteLine("ActionServer start") + Console.WriteLine("ActionServer start"); Ros2cs.Init(); INode node = Ros2cs.CreateNode("action_server"); - my_action_server = node.CreateActionServer( + my_action_server = node.CreateActionServer< + Fibonacci_SendGoal_Request, + Fibonacci_SendGoal_Response, + Fibonacci_Feedback, + Fibonacci_GetResult_Request, + Fibonacci_GetResult_Response>( "fibonacci", goal_callback - ) + ); Ros2cs.Spin(node); Ros2cs.Shutdown(); } - public static bool goal_callback() + public static Fibonacci_SendGoal_Response goal_callback(Fibonacci_SendGoal_Request request) { Console.WriteLine("Receiving new action goal..."); + + return new(); } } } diff --git a/src/ros2cs/rosidl_generator_cs/resource/action.cs.em b/src/ros2cs/rosidl_generator_cs/resource/action.cs.em deleted file mode 100644 index e69de29b..00000000 diff --git a/src/ros2cs/rosidl_generator_cs/resource/idl.c.em b/src/ros2cs/rosidl_generator_cs/resource/idl.c.em index 999eb127..8df3c8a8 100644 --- a/src/ros2cs/rosidl_generator_cs/resource/idl.c.em +++ b/src/ros2cs/rosidl_generator_cs/resource/idl.c.em @@ -33,7 +33,6 @@ TEMPLATE( }@ @[end for]@ -@# TODO (adamdbrw): Add services and actions @####################################################################### @# Handle service @@ -60,5 +59,82 @@ TEMPLATE( }@ @[end for]@ @[end if]@ -@# // endif +@####################################################################### +@# Handle action +@####################################################################### +@{ +from rosidl_parser.definition import Action +}@ + +@[if include_parts[1] == "action"]@ + +@[for action in content.get_elements_of_type(Action)]@ + +@# Goal Service Request: +@{ +TEMPLATE( + 'srv.c.em', + package_name=package_name, + interface_path=interface_path, + service=action.send_goal_service, + message=action.send_goal_service.request_message, + include_parts=include_parts, + get_c_type=get_c_type + ) +}@ + +@# Goal Service Response: +@{ +TEMPLATE( + 'srv.c.em', + package_name=package_name, + interface_path=interface_path, + service=action.send_goal_service, + message=action.send_goal_service.response_message, + include_parts=include_parts, + get_c_type=get_c_type + ) +}@ + +@# Feedback Message: +@{ +TEMPLATE( + 'msg.c.em', + package_name=package_name, + interface_path=interface_path, + message=action.feedback, + include_parts=include_parts, + get_c_type=get_c_type + ) +}@ + +@# Result Service Request: +@{ +TEMPLATE( + 'srv.c.em', + package_name=package_name, + interface_path=interface_path, + service=action.get_result_service, + message=action.get_result_service.request_message, + include_parts=include_parts, + get_c_type=get_c_type + ) +}@ + +@# Result Service Response: +@{ +TEMPLATE( + 'srv.c.em', + package_name=package_name, + interface_path=interface_path, + service=action.get_result_service, + message=action.get_result_service.response_message, + include_parts=include_parts, + get_c_type=get_c_type + ) +}@ + +@[end for]@ + +@[end if]@ diff --git a/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em b/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em index 90a88a38..6df7d247 100644 --- a/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em +++ b/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em @@ -94,8 +94,7 @@ from rosidl_parser.definition import Action @[for action in content.get_elements_of_type(Action)]@ -// Action found - +@# Goal Service Request: @{ TEMPLATE( 'srv.cs.em', @@ -114,6 +113,81 @@ TEMPLATE( ) }@ +@# Goal Service Response: +@{ +TEMPLATE( + 'srv.cs.em', + package_name=package_name, + interface_path=interface_path, + service=action.send_goal_service, + message=action.send_goal_service.response_message, + include_directives=include_directives, + get_dotnet_type=get_dotnet_type, + get_field_name=get_field_name, + constant_value_to_dotnet=constant_value_to_dotnet, + get_c_type=get_c_type, + get_marshal_type=get_marshal_type, + get_marshal_array_type=get_marshal_array_type, + get_csbuild_tool=get_csbuild_tool + ) +}@ + +@# Feedback Message: +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, + interface_path=interface_path, + message=action.feedback, + include_directives=include_directives, + get_dotnet_type=get_dotnet_type, + get_field_name=get_field_name, + constant_value_to_dotnet=constant_value_to_dotnet, + get_c_type=get_c_type, + get_marshal_type=get_marshal_type, + get_marshal_array_type=get_marshal_array_type, + get_csbuild_tool=get_csbuild_tool + ) +}@ + +@# Result Service Request: +@{ +TEMPLATE( + 'srv.cs.em', + package_name=package_name, + interface_path=interface_path, + service=action.get_result_service, + message=action.get_result_service.request_message, + include_directives=include_directives, + get_dotnet_type=get_dotnet_type, + get_field_name=get_field_name, + constant_value_to_dotnet=constant_value_to_dotnet, + get_c_type=get_c_type, + get_marshal_type=get_marshal_type, + get_marshal_array_type=get_marshal_array_type, + get_csbuild_tool=get_csbuild_tool + ) +}@ + +@# Result Service Response: +@{ +TEMPLATE( + 'srv.cs.em', + package_name=package_name, + interface_path=interface_path, + service=action.get_result_service, + message=action.get_result_service.response_message, + include_directives=include_directives, + get_dotnet_type=get_dotnet_type, + get_field_name=get_field_name, + constant_value_to_dotnet=constant_value_to_dotnet, + get_c_type=get_c_type, + get_marshal_type=get_marshal_type, + get_marshal_array_type=get_marshal_array_type, + get_csbuild_tool=get_csbuild_tool + ) +}@ + @[end for]@ @[end if]@ From d68b151eecc67d2ab5fc4a9ed4ea364bbba439e1 Mon Sep 17 00:00:00 2001 From: Robert Roos Date: Fri, 1 Dec 2023 14:22:19 +0100 Subject: [PATCH 4/5] Added generated CS to interface build + Included action message types --- ...idl_generator_cs_generate_interfaces.cmake | 3 +- .../rosidl_generator_cs/resource/idl.cs.em | 54 +++++++++++++++---- 2 files changed, 46 insertions(+), 11 deletions(-) diff --git a/src/ros2cs/rosidl_generator_cs/cmake/rosidl_generator_cs_generate_interfaces.cmake b/src/ros2cs/rosidl_generator_cs/cmake/rosidl_generator_cs_generate_interfaces.cmake index b263cb8c..7228fe8e 100644 --- a/src/ros2cs/rosidl_generator_cs/cmake/rosidl_generator_cs_generate_interfaces.cmake +++ b/src/ros2cs/rosidl_generator_cs/cmake/rosidl_generator_cs_generate_interfaces.cmake @@ -65,7 +65,7 @@ foreach(_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) ) list(APPEND _type_support_by_generated_msg_c_files "${_typesupport_impl}") endforeach() - elseif(_parent_folder STREQUAL "srv") + elseif(_parent_folder STREQUAL "srv" OR _parent_folder STREQUAL "action") list(APPEND _generated_srv_cs_files "${_output_path}/${_parent_folder}/${_module_name}.cs" ) @@ -78,7 +78,6 @@ foreach(_idl_file ${rosidl_generate_interfaces_ABS_IDL_FILES}) ) list(APPEND _type_support_by_generated_srv_c_files "${_typesupport_impl}") endforeach() - elseif(_parent_folder STREQUAL "action") else() message(FATAL_ERROR "Interface file with unknown parent folder: ${_idl_file}") endif() diff --git a/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em b/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em index 6df7d247..6633467a 100644 --- a/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em +++ b/src/ros2cs/rosidl_generator_cs/resource/idl.cs.em @@ -94,14 +94,13 @@ from rosidl_parser.definition import Action @[for action in content.get_elements_of_type(Action)]@ -@# Goal Service Request: +@# Goal Message: @{ TEMPLATE( - 'srv.cs.em', + 'msg.cs.em', package_name=package_name, interface_path=interface_path, - service=action.send_goal_service, - message=action.send_goal_service.request_message, + message=action.goal, include_directives=include_directives, get_dotnet_type=get_dotnet_type, get_field_name=get_field_name, @@ -113,14 +112,50 @@ TEMPLATE( ) }@ -@# Goal Service Response: +@# Feedback Message: +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, + interface_path=interface_path, + message=action.feedback, + include_directives=include_directives, + get_dotnet_type=get_dotnet_type, + get_field_name=get_field_name, + constant_value_to_dotnet=constant_value_to_dotnet, + get_c_type=get_c_type, + get_marshal_type=get_marshal_type, + get_marshal_array_type=get_marshal_array_type, + get_csbuild_tool=get_csbuild_tool + ) +}@ + +@# Result Message: +@{ +TEMPLATE( + 'msg.cs.em', + package_name=package_name, + interface_path=interface_path, + message=action.result, + include_directives=include_directives, + get_dotnet_type=get_dotnet_type, + get_field_name=get_field_name, + constant_value_to_dotnet=constant_value_to_dotnet, + get_c_type=get_c_type, + get_marshal_type=get_marshal_type, + get_marshal_array_type=get_marshal_array_type, + get_csbuild_tool=get_csbuild_tool + ) +}@ + +@# Goal Service Request: @{ TEMPLATE( 'srv.cs.em', package_name=package_name, interface_path=interface_path, service=action.send_goal_service, - message=action.send_goal_service.response_message, + message=action.send_goal_service.request_message, include_directives=include_directives, get_dotnet_type=get_dotnet_type, get_field_name=get_field_name, @@ -132,13 +167,14 @@ TEMPLATE( ) }@ -@# Feedback Message: +@# Goal Service Response: @{ TEMPLATE( - 'msg.cs.em', + 'srv.cs.em', package_name=package_name, interface_path=interface_path, - message=action.feedback, + service=action.send_goal_service, + message=action.send_goal_service.response_message, include_directives=include_directives, get_dotnet_type=get_dotnet_type, get_field_name=get_field_name, From 75ee90612685e239d7acbc13b9b26443177d404c Mon Sep 17 00:00:00 2001 From: Robert Roos Date: Mon, 4 Dec 2023 12:05:17 +0100 Subject: [PATCH 5/5] Got an example to compile - but running results in DLL error --- src/ros2cs/ros2cs_core/ActionServer.cs | 95 ++++++++++++++++++- src/ros2cs/ros2cs_core/CMakeLists.txt | 9 ++ src/ros2cs/ros2cs_core/Node.cs | 15 ++- .../ros2cs_core/QualityOfServiceProfile.cs | 3 +- src/ros2cs/ros2cs_core/interfaces/INode.cs | 7 +- .../ros2cs_core/native/rmw_native_interface.c | 5 +- src/ros2cs/ros2cs_examples/CMakeLists.txt | 2 + .../ros2cs_examples/ROS2ActionServer.cs | 36 ++++++- 8 files changed, 163 insertions(+), 9 deletions(-) diff --git a/src/ros2cs/ros2cs_core/ActionServer.cs b/src/ros2cs/ros2cs_core/ActionServer.cs index 56f328ee..e3061f19 100644 --- a/src/ros2cs/ros2cs_core/ActionServer.cs +++ b/src/ros2cs/ros2cs_core/ActionServer.cs @@ -14,9 +14,20 @@ using System; using ROS2.Internal; +using action_msgs.srv; +using action_msgs.msg; namespace ROS2 { + + /// Enum to indicate handling of new goal + public enum ActionGoalResponse + { + REJECT, + ACCEPT_AND_EXECUTE, + ACCEPT_AND_DEFER + } + public class ActionServer: IActionServer where TGoalRequest : Message, new() where TGoalResponse : Message, new() @@ -24,13 +35,65 @@ public class ActionServer Service to cancel action (same for every action) + private Service serviceCancel; + + /// Service to reply to a result request + private Service serviceResult; + + /// Service to register a new goal + private Service serviceGoal; + + /// Topic to which action status is published (same for every action) + private Publisher publisherStatus; + + /// Topic to which action progress is published + private Publisher publisherFeedback; + /// /// Internal constructor /// /// Use to construct new Instances - internal ActionServer(string subTopic, Node node, Func cb, QualityOfServiceProfile qpos = null) + internal ActionServer( + string topic, + Node node, + Func handleGoal, + Func handleCancel, + Action handleAccepted + ) { + string prefix = topic + "_action/"; + + QualityOfServiceProfile qos_service = new QualityOfServiceProfile(QosPresetProfile.SERVICES_DEFAULT); + QualityOfServiceProfile qos_status = new QualityOfServiceProfile(QosPresetProfile.ACTION_STATUS_DEFAULT); + + // Default service and topic: + + this.serviceCancel = node.CreateService( + prefix + "cancel_goal", + handleCancel, + qos_service + ); + + this.publisherStatus = node.CreatePublisher( + prefix + "status", + qos_status + ); + + // Custom: + this.serviceGoal = node.CreateService( + prefix + "send_goal", + onGoalReceive, + qos_service + ); + + this.serviceResult = node.CreateService( + prefix + "get_result", + onResultReceive, + qos_service + ); } public void Dispose() @@ -38,6 +101,36 @@ public void Dispose() DestroyActionServer(); } + /// + /// Callback for the goal service + /// + /// Most of the logic will be in the user defined function, but we wrap it here. + /// + private TGoalResponse onGoalReceive(TGoalRequest request) + { + TGoalResponse response = new TGoalResponse(); + return response; + } + + private TResultResponse onResultReceive(TResultRequest request) + { + TResultResponse response = new TResultResponse(); + return response; + } + + /// + /// Callback for cancel service + /// + private CancelGoal_Response defaultOnCancelReceive(CancelGoal_Request request) + { + CancelGoal_Response response = new CancelGoal_Response() + { + Return_code = CancelGoal_Response.ERROR_REJECTED + }; + return response; + } + + /// public bool IsDisposed { get { return IsDisposed; } } private bool disposed = false; diff --git a/src/ros2cs/ros2cs_core/CMakeLists.txt b/src/ros2cs/ros2cs_core/CMakeLists.txt index 1b9f50d4..00974fcb 100644 --- a/src/ros2cs/ros2cs_core/CMakeLists.txt +++ b/src/ros2cs/ros2cs_core/CMakeLists.txt @@ -25,6 +25,7 @@ if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") endif() find_package(ros2cs_common REQUIRED) +find_package(action_msgs REQUIRED) find_package(ament_cmake_export_assemblies REQUIRED) find_package(ament_cmake REQUIRED) find_package(dotnet_cmake_module REQUIRED) @@ -36,6 +37,7 @@ find_package(DotNETExtra REQUIRED) # Used by ros2cs_native find_package(rcl REQUIRED) +find_package(rcl_action REQUIRED) find_package(rcutils REQUIRED) find_package(rmw REQUIRED) find_package(rosidl_generator_c REQUIRED) @@ -63,6 +65,7 @@ add_library( ament_target_dependencies(ros2cs_native "rcl" + "rcl_action" "rcutils" "rmw" "rosidl_generator_c" @@ -115,6 +118,10 @@ set(_assembly_deps_dll "") foreach(_assembly_dep ${ros2cs_common_ASSEMBLIES_DLL}) list(APPEND _assembly_deps_dll "${_assembly_dep}") endforeach() +foreach(_assembly_dep ${action_msgs_ASSEMBLIES_DLL}) + list(APPEND _assembly_deps_dll "${_assembly_dep}") +endforeach() + add_dotnet_library(${PROJECT_NAME} SOURCES @@ -127,8 +134,10 @@ install_dotnet(${PROJECT_NAME} DESTINATION lib/dotnet) ament_export_assemblies_dll("lib/dotnet/${PROJECT_NAME}.dll") ament_export_dependencies(ros2cs_common) +ament_export_dependencies(action_msgs) ament_export_dependencies(ament_cmake) ament_export_dependencies(rcl) +ament_export_dependencies(rcl_action) ament_export_dependencies(rosidl_generator_c) option(STANDALONE_BUILD "Deploy standalone libraries with build" OFF) diff --git a/src/ros2cs/ros2cs_core/Node.cs b/src/ros2cs/ros2cs_core/Node.cs index 29c163c3..833b0c18 100644 --- a/src/ros2cs/ros2cs_core/Node.cs +++ b/src/ros2cs/ros2cs_core/Node.cs @@ -15,6 +15,8 @@ using System; using System.Linq; using System.Collections.Generic; +using action_msgs.srv; +using action_msgs.msg; namespace ROS2 { @@ -214,7 +216,10 @@ public bool RemoveService(IServiceBase service) public ActionServer CreateActionServer( - string topic, Func callback, QualityOfServiceProfile qos = null + string topic, + Func handleGoal, + Func handleCancel, + Action handleAccepted ) where TGoalRequest : Message, new() where TGoalResponse : Message, new() @@ -231,7 +236,13 @@ public ActionServer action_server = - new ActionServer(topic, this, callback, qos); + new ActionServer( + topic, + this, + handleGoal, + handleCancel, + handleAccepted + ); action_servers.Add(action_server); logger.LogInfo("Created action server for topic " + topic); return action_server; diff --git a/src/ros2cs/ros2cs_core/QualityOfServiceProfile.cs b/src/ros2cs/ros2cs_core/QualityOfServiceProfile.cs index 2c9e281c..3f6498ba 100644 --- a/src/ros2cs/ros2cs_core/QualityOfServiceProfile.cs +++ b/src/ros2cs/ros2cs_core/QualityOfServiceProfile.cs @@ -25,7 +25,8 @@ public enum QosPresetProfile DEFAULT, SERVICES_DEFAULT, PARAMETER_EVENTS, - SYSTEM_DEFAULT + SYSTEM_DEFAULT, + ACTION_STATUS_DEFAULT, } public enum HistoryPolicy diff --git a/src/ros2cs/ros2cs_core/interfaces/INode.cs b/src/ros2cs/ros2cs_core/interfaces/INode.cs index 682ff0af..ae1b6eb6 100644 --- a/src/ros2cs/ros2cs_core/interfaces/INode.cs +++ b/src/ros2cs/ros2cs_core/interfaces/INode.cs @@ -14,6 +14,8 @@ using System; using System.Collections.Generic; +using action_msgs.srv; +using action_msgs.msg; namespace ROS2 { @@ -50,7 +52,10 @@ public interface INode: IExtendedDisposable /// Create an action server for this node ActionServer CreateActionServer( - string topic, Func callback, QualityOfServiceProfile qos = null + string topic, + Func handleGoal, + Func handleCancel, + Action handleAccepted ) where TGoalRequest : Message, new() where TGoalResponse : Message, new() diff --git a/src/ros2cs/ros2cs_core/native/rmw_native_interface.c b/src/ros2cs/ros2cs_core/native/rmw_native_interface.c index a2fe8d6d..a523d3f1 100644 --- a/src/ros2cs/ros2cs_core/native/rmw_native_interface.c +++ b/src/ros2cs/ros2cs_core/native/rmw_native_interface.c @@ -16,6 +16,7 @@ #include #include #include +#include ROSIDL_GENERATOR_C_EXPORT rmw_qos_profile_t * rmw_native_interface_create_qos_profile(int profile) @@ -27,7 +28,8 @@ rmw_qos_profile_t * rmw_native_interface_create_qos_profile(int profile) DEFAULT, SERVICES_DEFAULT, PARAMETER_EVENTS, - SYSTEM_DEFAULT + SYSTEM_DEFAULT, + ACTION_STATUS_DEFAULT }; rmw_qos_profile_t * preset_profile = (rmw_qos_profile_t *)malloc(sizeof(rmw_qos_profile_t)); @@ -40,6 +42,7 @@ rmw_qos_profile_t * rmw_native_interface_create_qos_profile(int profile) case SERVICES_DEFAULT: *preset_profile = rmw_qos_profile_services_default; break; case PARAMETER_EVENTS: *preset_profile = rmw_qos_profile_parameter_events; break; case SYSTEM_DEFAULT: *preset_profile = rmw_qos_profile_system_default; break; + case ACTION_STATUS_DEFAULT: *preset_profile = rcl_action_qos_profile_status_default; break; default: *preset_profile = rmw_qos_profile_unknown; break; } diff --git a/src/ros2cs/ros2cs_examples/CMakeLists.txt b/src/ros2cs/ros2cs_examples/CMakeLists.txt index 9fcbaf6a..e3052eee 100644 --- a/src/ros2cs/ros2cs_examples/CMakeLists.txt +++ b/src/ros2cs/ros2cs_examples/CMakeLists.txt @@ -20,6 +20,7 @@ find_package(ament_cmake REQUIRED) find_package(ros2cs_common REQUIRED) find_package(ros2cs_core REQUIRED) find_package(std_msgs REQUIRED) +find_package(action_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(example_interfaces REQUIRED) find_package(builtin_interfaces REQUIRED) @@ -35,6 +36,7 @@ set(_assemblies_dep_dlls ${ros2cs_common_ASSEMBLIES_DLL} ${ros2cs_core_ASSEMBLIES_DLL} ${std_msgs_ASSEMBLIES_DLL} + ${action_msgs_ASSEMBLIES_DLL} ${sensor_msgs_ASSEMBLIES_DLL} ${example_interfaces_ASSEMBLIES_DLL} ${builtin_interfaces_ASSEMBLIES_DLL} diff --git a/src/ros2cs/ros2cs_examples/ROS2ActionServer.cs b/src/ros2cs/ros2cs_examples/ROS2ActionServer.cs index ec188f9c..eda9b74b 100644 --- a/src/ros2cs/ros2cs_examples/ROS2ActionServer.cs +++ b/src/ros2cs/ros2cs_examples/ROS2ActionServer.cs @@ -13,6 +13,7 @@ // limitations under the License. using System; +using action_msgs.srv; using ROS2; using example_interfaces.action; @@ -41,18 +42,47 @@ public static void Main(string[] args) Fibonacci_GetResult_Request, Fibonacci_GetResult_Response>( "fibonacci", - goal_callback + handle_goal, + handle_cancel, + handle_accepted ); Ros2cs.Spin(node); Ros2cs.Shutdown(); } - public static Fibonacci_SendGoal_Response goal_callback(Fibonacci_SendGoal_Request request) + /// + /// Callback on receiving a goal - must return quickly! + /// + public static ActionGoalResponse handle_goal(Fibonacci_SendGoal_Request request) { Console.WriteLine("Receiving new action goal..."); - return new(); + return ActionGoalResponse.ACCEPT_AND_EXECUTE; + } + + /// + /// Callback on receiving a cancel request - must return quickly! + /// + public static CancelGoal_Response handle_cancel(CancelGoal_Request request) + { + CancelGoal_Response response = new CancelGoal_Response(); + return response; + } + + public static void handle_accepted(Fibonacci_SendGoal_Request request) + { + return; + } + + /// + /// Actual action + /// + /// This function should be called from a different thread and does not need to return any time soon. + /// + public static void execute() + { + return; } } }