diff --git a/recipes/ros2_csharp/activate.bat b/recipes/ros2_csharp/activate.bat new file mode 100644 index 00000000..5cd8c17d --- /dev/null +++ b/recipes/ros2_csharp/activate.bat @@ -0,0 +1,6 @@ +@echo off +set AMENT_PREFIX_PATH_BACKUP=%AMENT_PREFIX_PATH% +set AMENT_PREFIX_PATH=%CONDA_PREFIX%\Library +set PATH=%CONDA_PREFIX%\Library\bin;%PATH% +set CMAKE_PREFIX_PATH=%CONDA_PREFIX%\Library;%CMAKE_PREFIX_PATH% +set PYTHONPATH=%CONDA_PREFIX%\Lib\site-packages;%PYTHONPATH% \ No newline at end of file diff --git a/recipes/ros2_csharp/deactivate.bat b/recipes/ros2_csharp/deactivate.bat new file mode 100644 index 00000000..b0b09a13 --- /dev/null +++ b/recipes/ros2_csharp/deactivate.bat @@ -0,0 +1,6 @@ +@echo off +set AMENT_PREFIX_PATH=%AMENT_PREFIX_PATH_BACKUP% +set AMENT_PREFIX_PATH_BACKUP= +set PATH=%PATH:%CONDA_PREFIX%\Library\bin;=% +set CMAKE_PREFIX_PATH=%CMAKE_PREFIX_PATH:%CONDA_PREFIX%\Library;=% +set PYTHONPATH=%PYTHONPATH:%CONDA_PREFIX%\Lib\site-packages;=% \ No newline at end of file diff --git a/recipes/ros2_csharp/meta.yaml b/recipes/ros2_csharp/meta.yaml new file mode 100644 index 00000000..d47566bc --- /dev/null +++ b/recipes/ros2_csharp/meta.yaml @@ -0,0 +1,42 @@ +package: + name: ros2_csharp + version: 0.0.1 + +source: + path: {{ 你的安装打包目录 }} + +build: + number: 0 + script: + # 创建基本目录 + - mkdir %PREFIX%\Library\bin 2>nul + - mkdir %PREFIX%\Library\include 2>nul + - mkdir %PREFIX%\Library\lib 2>nul + - mkdir %PREFIX%\Library\share 2>nul + # 复制文件 + - xcopy /E /I /Y %SRC_DIR%\bin %PREFIX%\Library\bin + - xcopy /E /I /Y %SRC_DIR%\include %PREFIX%\Library\include + - xcopy /E /I /Y %SRC_DIR%\lib %PREFIX%\Library\lib + - xcopy /E /I /Y %SRC_DIR%\share %PREFIX%\Library\share + + # 关键：注册ROS 2包 + - mkdir %PREFIX%\Library\share\ament_index\resource_index\packages 2>nul + - copy %RECIPE_DIR%\ros_driver %PREFIX%\Library\share\ament_index\resource_index\packages\ros_driver + + # 设置环境变量 + - mkdir %PREFIX%\etc\conda\activate.d 2>nul + - mkdir %PREFIX%\etc\conda\deactivate.d 2>nul + # activate脚本 + - copy %RECIPE_DIR%\activate.bat %PREFIX%\etc\conda\activate.d\ros_csharp.bat + - copy %RECIPE_DIR%\deactivate.bat %PREFIX%\etc\conda\deactivate.d\ros_csharp.bat + + +requirements: + host: + run: + +about: + home: https://www.ros.org/ + license: BSD-3-Clause + summary: | + Robot Operating System \ No newline at end of file diff --git a/recipes/ros2_csharp/ros_driver b/recipes/ros2_csharp/ros_driver new file mode 100644 index 00000000..e69de29b diff --git a/ros2_csharp/README.md b/ros2_csharp/README.md new file mode 100644 index 00000000..c632bef9 --- /dev/null +++ b/ros2_csharp/README.md @@ -0,0 +1,38 @@ +## ros2 C# 开发指南 + +## 背景 + 该项目是一个示例项目，帮您快速的配置 C# 开发环境。具体硬件设备驱动需要您自行编写。 + + +## C# 开发环境配置 +### 1. 安装 dotnet 环境 +dotnet [下载地址](https://dotnet.microsoft.com/zh-cn/download) +安装完成后执行一下命令，如果安装成功会有如下展示。 +``` + dotnet --version + 8.0.406 +``` + +### 2. ros2-humble 安装 + 请参照官方[安装文档](https://docs.ros.org/en/humble/Installation/Windows-Install-Binary.html) + +### 3. 开发配置 +1. ros2_dotnet 环境配置请参阅[配置文档](https://github.com/ros2-dotnet/ros2_dotnet/blob/main/README.md)。 +2. 参照项目下的 ros_driver 创建您自己的项目。 +3. 编写相应的代码。 +4. 构建流程 + ``` + 打开 windows 搜索 Developer Command Prompt 启动 visual studio + cd {本地ros2-humble} 目录 + call ./local_setup.bat + cd {Uni-Lab-OS 项目目录}/ros_csharp + vcs import ./src < ros2_humble.repos + colcon build --merge-install --packages-up-to {your package name} --event-handlers console_cohesion+ + ``` +5. 拷贝编译完成的包到conda 对应的虚拟环境目录下。 +6. 执行如下命令运行测试 + + ``` + 参照 ros_driver 项目，需要配置 CONFIG_FILE_PATH 环境变量，对应的值是 {Uni-Lab-OS 路径}/unilabos/registry/devices 下的 yaml 文件路径。 + ros2 run {项目包名} {可执行程序名} + ``` \ No newline at end of file diff --git a/ros2_csharp/ros2_humble.repos b/ros2_csharp/ros2_humble.repos new file mode 100644 index 00000000..d2828134 --- /dev/null +++ b/ros2_csharp/ros2_humble.repos @@ -0,0 +1,33 @@ +repositories: + ament_dotnet/ament_cmake_export_assemblies: + type: git + url: https://github.com/ros2-dotnet/ament_cmake_export_assemblies.git + version: main + ros2/common_interfaces: + type: git + url: https://github.com/ros2/common_interfaces.git + version: 4.2.4 + ros2/rcl_interfaces: + type: git + url: https://github.com/ros2/rcl_interfaces.git + version: 1.2.1 + ros2/rosidl_defaults: + type: git + url: https://github.com/ros2/rosidl_defaults.git + version: 1.2.0 + ros2/test_interface_files: + type: git + url: https://github.com/ros2/test_interface_files.git + version: humble + ros2/unique_identifier_msgs: + type: git + url: https://github.com/ros2/unique_identifier_msgs.git + version: 2.2.1 + ros2_dotnet/dotnet_cmake_module: + type: git + url: https://github.com/ros2-dotnet/dotnet_cmake_module.git + version: main + ros2_dotnet/ros2_dotnet: + type: git + url: https://github.com/ros2-dotnet/ros2_dotnet.git + version: main \ No newline at end of file diff --git a/ros2_csharp/src/ros_driver/CMakeLists.txt b/ros2_csharp/src/ros_driver/CMakeLists.txt new file mode 100644 index 00000000..ee0cf131 --- /dev/null +++ b/ros2_csharp/src/ros_driver/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.5) + +project(ros_driver C) + +find_package(ament_cmake REQUIRED) +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(geometry_msgs REQUIRED) +find_package(test_msgs REQUIRED) +find_package(action_msgs REQUIRED) # Add this line +find_package(rcldotnet REQUIRED) +find_package(ilabos_msgs REQUIRED) + + + +find_package(dotnet_cmake_module REQUIRED) + +set(CSHARP_TARGET_FRAMEWORK "netcoreapp6.0") +find_package(DotNETExtra REQUIRED) + + +# 使用相对路径定位 YamlDotNet.dll +set(YAMLDOTNET_DLL "${CMAKE_CURRENT_SOURCE_DIR}/deps/YamlDotNet/YamlDotNet.dll") + +# 检查文件是否存在 +if(NOT EXISTS ${YAMLDOTNET_DLL}) + message(FATAL_ERROR "YamlDotNet.dll not found at: ${YAMLDOTNET_DLL}") +endif() + + +set(_assemblies_dep_dlls + ${rcldotnet_common_ASSEMBLIES_DLL} + ${rcldotnet_ASSEMBLIES_DLL} + ${builtin_interfaces_ASSEMBLIES_DLL} + ${std_msgs_ASSEMBLIES_DLL} + ${std_srvs_ASSEMBLIES_DLL} + ${geometry_msgs_ASSEMBLIES_DLL} + ${test_msgs_ASSEMBLIES_DLL} + ${action_msgs_ASSEMBLIES_DLL} + ${ilabos_msgs_ASSEMBLIES_DLL} + ${YAMLDOTNET_DLL} +) + +add_dotnet_executable(ros_driver + Program.cs + DeviceNode.cs + DeviceActionServer.cs + PropertyPublisher.cs + MockGripper.cs + utils/Yaml.cs + utils/MessageConverter.cs + INCLUDE_DLLS + ${_assemblies_dep_dlls} +) + + +install_dotnet(ros_driver DESTINATION lib/${PROJECT_NAME}/dotnet) + + +ament_package() \ No newline at end of file diff --git a/ros2_csharp/src/ros_driver/DeviceActionServer.cs b/ros2_csharp/src/ros_driver/DeviceActionServer.cs new file mode 100644 index 00000000..907f9ec8 --- /dev/null +++ b/ros2_csharp/src/ros_driver/DeviceActionServer.cs @@ -0,0 +1,133 @@ +using System; +using System.Threading; +using System.Threading.Tasks; +using ROS2; +using test_msgs.action; +using System.Collections.Generic; +using ilabos_msgs.action; +using RosDriver.Utils; + +namespace ros_driver +{ + /// + /// Generic action server that can be used with any ROS2 action type + /// + public class DeviceActionServer : IDisposable + where TAction : IRosActionDefinition + where TGoal : IRosMessage, new() + where TResult : IRosMessage, new() + where TFeedback : IRosMessage, new() + { + private readonly Node _node; + private readonly string _actionName; + private bool _disposed = false; + private ActionValueMapping _actionValue = null; + private readonly object _device = null; + + + public DeviceActionServer( + Node node, + object device, + string actionName, + ActionValueMapping actionValue) + { + _node = node; + _actionName = actionName; + _actionValue = actionValue; + _device = device; + + _node.CreateActionServer( + _actionName, + HandleAccepted, + cancelCallback: HandleCancel); + + Console.WriteLine($"Created Action Server: {_actionName}"); + } + + + private void HandleAccepted(ActionServerGoalHandle goalHandle) + { + // Don't block in the callback. + // -> Don't wait for the returned Task. + _ = DoWorkWithGoal(goalHandle); + } + + private CancelResponse HandleCancel(ActionServerGoalHandle goalHandle) + { + return CancelResponse.Accept; + } + + private async Task DoWorkWithGoal(ActionServerGoalHandle goalHandle) + { + + Console.WriteLine("Executing goal..."); + // 检查 TAction 是否是 Fibonacci 类型 + if (typeof(TAction) == typeof(Fibonacci)) + { + Console.WriteLine("确认 TAction 是 Fibonacci 类型"); + // 注意：这种转换是不安全的，因为泛型参数在编译时已确定 + // 这里仅作为示例，实际使用时需要谨慎 + var fibonacciGoalHandle = goalHandle as ActionServerGoalHandle; + if (fibonacciGoalHandle != null) + { + Console.WriteLine("成功转换为 Fibonacci 类型的 GoalHandle"); + var feedback = new Fibonacci_Feedback(); + + feedback.Sequence = new List { 0, 1 }; + + for (int i = 1; i < fibonacciGoalHandle.Goal.Order; i++) + { + if (fibonacciGoalHandle.IsCanceling) + { + var cancelResult = new Fibonacci_Result(); + cancelResult.Sequence = feedback.Sequence; + + Console.WriteLine($"Canceled Result: {string.Join(", ", cancelResult.Sequence)}"); + fibonacciGoalHandle.Canceled(cancelResult); + return; + } + + feedback.Sequence.Add(feedback.Sequence[i] + feedback.Sequence[i - 1]); + + Console.WriteLine($"Feedback: {string.Join(", ", feedback.Sequence)}"); + fibonacciGoalHandle.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)}"); + fibonacciGoalHandle.Succeed(result); + } + } + else + { + Console.WriteLine($"TAction 不是 Fibonacci 类型，而是 {typeof(TAction).Name}"); + return; + } + } + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + protected virtual void Dispose(bool disposing) + { + if (!_disposed) + { + if (disposing) + { + // Release managed resources if any + } + + _disposed = true; + } + } + } +} diff --git a/ros2_csharp/src/ros_driver/DeviceNode.cs b/ros2_csharp/src/ros_driver/DeviceNode.cs new file mode 100644 index 00000000..aca65281 --- /dev/null +++ b/ros2_csharp/src/ros_driver/DeviceNode.cs @@ -0,0 +1,173 @@ +using System; +using System.Threading.Tasks; +using ROS2; +using test_msgs.action; +using System.Collections.Generic; +using RosDriver.Utils; +using System.Reflection; + + +namespace ros_driver +{ + class DeviceNode : IDisposable + { + private readonly Node _node; + private readonly object _device; + private readonly string _name; + private bool _disposed = false; + + private readonly Dictionary _propertyPublishers = new Dictionary(); + private readonly Dictionary _actionServers = new Dictionary(); + + public DeviceNode(Node node, string name, DeviceInfo deviceInfo) + { + _name = name; + _node = node; + + _device = new MockGripper(); + Type dType = _device.GetType(); + PropertyInfo[] properties = dType.GetProperties( + BindingFlags.Public | BindingFlags.Instance); + + var statusTypes = deviceInfo.StatusTypes; + var actionMappings = deviceInfo.ActionValueMappings; + foreach (var prop in properties) + { + MethodInfo setMethod = prop.GetSetMethod(nonPublic: true); + MethodInfo getMethod = prop.GetGetMethod(nonPublic: true); + + // 如果 setMethod 不是 null 打印方法名和返回值名 + if (setMethod != null) + { + ParameterInfo[] parameters = setMethod.GetParameters(); + Type parameterType = parameters[0].ParameterType; + + // 如果返回值是字符串，则actionMappings 添加一个 ActionValueMapping 对象 + if (setMethod.ReturnType == typeof(string)) + { + var actionValueMapping = new ActionValueMapping + { + Type = "SendCmd", + Goal = new Dictionary { { "command", parameterType.Name } }, + Feedback = new Dictionary { { "status", "status" } }, + Result = new Dictionary(), + }; + actionMappings.Add(prop.Name, new ActionValueMapping()); + } + Console.WriteLine($"Property: {prop.Name}, Setter: {setMethod.Name},Parameter Type: {parameterType.Name}, Return Type: {setMethod.ReturnType.Name}"); + } + + // 如果 getMethod 不是 null 打印方法名和返回值名 + if (getMethod != null) + { + // 判断 prop.Name 不在 statusTypes 中，则添加到 statusTypes 中 + if (!statusTypes.ContainsKey(prop.Name)) + { + statusTypes.Add(prop.Name, getMethod.ReturnType.Name); + } + Console.WriteLine($"Property: {prop.Name}, Getter: {getMethod.Name}, Return Type: {getMethod.ReturnType.Name}"); + } + } + + foreach (var kv in statusTypes) + { + Console.WriteLine($"create publisher: {kv.Key}:"); + CreatePublisher(kv.Key, kv.Value); + } + + foreach (var kv in actionMappings) + { + Console.WriteLine($"create action server: {kv.Key}:"); + CreateActionServer(kv.Key, kv.Value); + } + } + + private void CreatePublisher(string key, string msgType) + { + Type rosMessageType = Converter2RosMessage.GetRosMessageType(msgType); + + // Use reflection to create the appropriate publisher type + var publisherType = typeof(PropertyPublisher<>).MakeGenericType(rosMessageType); + var publisher = Activator.CreateInstance(publisherType, _node, _device, key, 1.0, true); + _propertyPublishers.Add(key, publisher); + } + + private void CreateActionServer(string key, ActionValueMapping actionValue) + { + try + { + List actionTypes = Converter2RosMessage.GetActionMessageType(actionValue.Type); + if (actionTypes.Count != 4) + { + throw new ArgumentException($"Expected 4 action types, but got {actionTypes.Count} for action {actionValue.Type}"); + } + + // 提取类型：Action, Goal, Result, Feedback + Type actionType = actionTypes[0]; + Type goalType = actionTypes[1]; + Type resultType = actionTypes[2]; + Type feedbackType = actionTypes[3]; + + // 使用反射创建正确泛型类型的DeviceActionServer + Type deviceActionServerType = typeof(DeviceActionServer<,,,>).MakeGenericType( + actionType, goalType, resultType, feedbackType); + + // 创建DeviceActionServer实例 + object actionServer = Activator.CreateInstance( + deviceActionServerType, + _node, // 节点 + _device, // 设备对象 + key, // 动作名称 + actionValue // 动作值映射 + ); + + // 将动作服务器存储在字典中 + _actionServers.Add(key, actionServer); + + Console.WriteLine($"成功创建了类型为 {actionValue.Type} 的动作服务器 {key}"); + } + catch (Exception ex) + { + Console.WriteLine($"创建动作服务器 {key} 时出错: {ex.Message}"); + if (ex.InnerException != null) + { + Console.WriteLine($"内部异常: {ex.InnerException.Message}"); + } + Console.WriteLine($"这可能是由于消息定义中缺少或不正确的类型支持。"); + } + } + + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + protected virtual void Dispose(bool disposing) + { + if (!_disposed) + { + if (disposing) + { + // Node doesn't have a Dispose method, so don't try to call it + // _node.Dispose(); + + // Handle other resources that need to be released + foreach (var publisher in _propertyPublishers.Values) + { + if (publisher is IDisposable disposable) + disposable.Dispose(); + } + + foreach (var server in _actionServers.Values) + { + if (server is IDisposable disposable) + disposable.Dispose(); + } + } + // Dispose unmanaged resources if any + _disposed = true; + } + } + } +} \ No newline at end of file diff --git a/ros2_csharp/src/ros_driver/MockGripper.cs b/ros2_csharp/src/ros_driver/MockGripper.cs new file mode 100644 index 00000000..e709d186 --- /dev/null +++ b/ros2_csharp/src/ros_driver/MockGripper.cs @@ -0,0 +1,73 @@ +using System; +using System.Threading.Tasks; + +public class MockGripper +{ + private double _position = 0.0; + public double Position + { + get + { + _position += 1.0; + return _position; + } + private set + { + _position = value; + } + } + + private double _velocity = 2.0; + public double Velocity + { + get + { + _velocity += 1.0; + return _velocity; + } + private set + { + _velocity = value; + } + } + + private double _torque = 0.0; + public double Torque + { + get + { + _torque += 1.0; + return _torque; + } + private set + { + _torque = value; + } + } + + public string Status { get; private set; } = "Idle"; + + public async Task PushToAsync(double position, double torque, double velocity = 0.0) + { + Status = "Running"; + double currentPos = Position; + + if (velocity == 0.0) + { + velocity = Velocity; + } + + double moveTime = Math.Abs(position - currentPos) / velocity; + + for (int i = 0; i < 20; i++) + { + Position = currentPos + (position - currentPos) / 20 * (i + 1); + Torque = torque / (20 - i); + Velocity = velocity; + await Task.Delay((int)(moveTime * 1000 / 20)); // Convert seconds to milliseconds + } + + Torque = torque; + Status = "Idle"; + } +} \ No newline at end of file diff --git a/ros2_csharp/src/ros_driver/Program.cs b/ros2_csharp/src/ros_driver/Program.cs new file mode 100644 index 00000000..487a54f5 --- /dev/null +++ b/ros2_csharp/src/ros_driver/Program.cs @@ -0,0 +1,65 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using ROS2; +using RosDriver.Utils; + +namespace ros_driver +{ + class Program + { + private static void Main(string[] args) + { + RCLdotnet.Init(); + + // Get device configuration from environment variables + string configFilePath = Environment.GetEnvironmentVariable("CONFIG_FILE_PATH"); + if (string.IsNullOrEmpty(configFilePath)) + { + Console.WriteLine("Please set the CONFIG_FILE_PATH environment variable."); + return; + } + + var deviceDict = YamlConfigParser.GetDeviceInfoDict(configFilePath); + + // 创建列表存储所有节点和设备对象 + List nodes = new List(); + List deviceNodes = new List(); + + foreach (var device in deviceDict) + { + string deviceId = device.Key; + DeviceInfo deviceInfo = device.Value; + string nodeName = deviceId.Split('/').Last(); + string nodeNamespace = $"/devices/{deviceId}"; + + Console.WriteLine($"Starting node '{nodeName}' with namespace '{nodeNamespace}'"); + Console.WriteLine($"Using config file: {configFilePath}"); + + var node = RCLdotnet.CreateNode(nodeName, nodeNamespace); + var deviceNode = new DeviceNode(node, deviceId, deviceInfo); + + // 添加到列表中而不是立即spin + nodes.Add(node); + deviceNodes.Add(deviceNode); + } + + while (RCLdotnet.Ok()) + { + foreach (var node in nodes) + { + RCLdotnet.SpinOnce(node, 100); // 100ms超时 + } + } + + // 清理资源 + foreach (var deviceNode in deviceNodes) + { + deviceNode.Dispose(); + } + + RCLdotnet.Shutdown(); + } + } +} \ No newline at end of file diff --git a/ros2_csharp/src/ros_driver/PropertyPublisher.cs b/ros2_csharp/src/ros_driver/PropertyPublisher.cs new file mode 100644 index 00000000..12ca3286 --- /dev/null +++ b/ros2_csharp/src/ros_driver/PropertyPublisher.cs @@ -0,0 +1,217 @@ +using System; +using System.Threading.Tasks; +using ROS2; +using test_msgs.action; +using System.Reflection; +using RosDriver.Utils; + + + +namespace ros_driver +{ + /// + /// A publisher that periodically publishes property values as ROS messages + /// + public class PropertyPublisher where T : IRosMessage, new() + { + private readonly Node _node; + private readonly string _name; + private readonly bool _printPublish; + private readonly object _device = null; + + private T _value; + private Publisher _publisher; + private Timer _timer; + private TimeSpan _timerPeriod; + + + public PropertyPublisher( + Node node, + object device, + string name, + double initialPeriod = 1.0, + bool printPublish = true) + { + _node = node; + _device = device; + _name = name; + _timerPeriod = TimeSpan.FromSeconds(initialPeriod); + _printPublish = printPublish; + + _publisher = node.CreatePublisher(_name); + _timer = node.CreateTimer(_timerPeriod, PublishProperty); + + Console.WriteLine($"创建发布者 {name}, {typeof(T)}"); + + _value = new T(); + InitializeValue(); + } + + private void InitializeValue() + { + if (_value is std_msgs.msg.String strData) + { + strData.Data = "hello world"; + } + + if (_value is std_msgs.msg.Int32 intData) + { + intData.Data = 1; + } + + if (_value is std_msgs.msg.Float64 floatData) + { + floatData.Data = 1.00f; + } + + if (_value is std_msgs.msg.Float64MultiArray floatArrayData) + { + floatArrayData.Data = new System.Collections.Generic.List { 1.0, 2.0, 3.0, 4.0 }; + floatArrayData.Layout = new std_msgs.msg.MultiArrayLayout(); + } + } + + private bool HasMethod(object obj, string methodName) + { + if (obj == null) + return false; + + Type type = obj.GetType(); + return type.GetMethod(methodName) != null; + } + + // 检查对象是否有特定名称的属性 + private bool HasProperty(object obj, string propertyName) + { + if (obj == null) + return false; + + Type type = obj.GetType(); + return type.GetProperty(propertyName) != null; + } + + public object SafeGetPropertyValue(string propertyName) + { + if (!HasProperty(_device, propertyName)) + { + return null; // 或抛出异常，或返回默认值 + } + + PropertyInfo propInfo = _device.GetType().GetProperty(propertyName); + MethodInfo getMethod = propInfo.GetGetMethod(nonPublic: true); + + if (getMethod == null) + return null; // 没有 getter 方法 + + return getMethod.Invoke(_device, null); + } + + private T getProperty() + { + + var result = SafeGetPropertyValue(_name); + if (result != null) + { + Console.WriteLine($"level: info, Property '{_name}' not found in device '{_device.GetType().Name}', result: {result}"); + return Converter2RosMessage.ConvertToRosMessage(result); + } + + return mockData(); + } + + private T mockData() + { + // Get the full name with namespace + string typeName = typeof(T).FullName; + + if (typeName.EndsWith(".Float64")) + return Converter2RosMessage.ConvertToRosMessage(1.0); + else if (typeName.EndsWith(".String")) + return Converter2RosMessage.ConvertToRosMessage("hello world"); + else if (typeName.EndsWith(".Int32")) + return Converter2RosMessage.ConvertToRosMessage(1); + else if (typeName.EndsWith(".Float64MultiArray")) + return Converter2RosMessage.ConvertToRosMessage(new double[] { 1.0, 2.0, 3.0, 4.0 }); + else + throw new NotSupportedException($"Type {typeName} is not supported."); + } + + + private async Task getPropertyAsync() + { + await Task.Yield(); + var result = SafeGetPropertyValue(_name); + if (result != null) + { + // Console.WriteLine($"level: info, Property '{_name}' found in device '{_device.GetType().Name}', result: {result}"); + return Converter2RosMessage.ConvertToRosMessage(result); + } + return mockData(); + } + + /// + /// Gets the property value synchronously + /// + /// The property value + public T GetProperty() + { + + // If async method is available, run it on a task and wait for result + return Task.Run(async () => await getPropertyAsync()).Result; + + + // // Otherwise call the synchronous method + // return getProperty(); + } + + + /// + /// Publishes the property value + /// + private void PublishProperty(TimeSpan elapsed) + { + try + { + T value = GetProperty(); + if (_printPublish) + { + //_node.Logger.Info($"Publishing {typeof(T).Name}: {value}"); + Console.WriteLine($"level: info,prop name: {_name}, {typeof(T).Name}: {value} normal msg: {Converter2RosMessage.ConvertFromRosMessage(value)}"); + } + + if (value != null) + { + // Console.WriteLine($"level: info, Publishing type: {typeof(T)}, value: {value.Data}"); + _publisher.Publish(value); + } + } + catch (Exception ex) + { + //_node.Logger.Error($"Error in PublishProperty: {ex.Message}\n{ex.StackTrace}"); + Console.WriteLine($"level: error, PublishProperty: {ex.Message}\n{ex.StackTrace}"); + } + } + + + + + + + + /// + /// Changes the frequency of publication + /// + /// New period in seconds + public void ChangeFrequency(double period) + { + _timerPeriod = TimeSpan.FromSeconds(period); + // _node.Logger.Info($"Changing {_name} timer period to: {_timerPeriod.TotalSeconds} seconds"); + Console.WriteLine($"level: info, Changing {_name} timer period to: {_timerPeriod.TotalSeconds} seconds"); + + + // Reset timer + _timer.Cancel(); + _timer = _node.CreateTimer(_timerPeriod, PublishProperty); + } + } +} diff --git a/ros2_csharp/src/ros_driver/deps/YamlDotNet/YamlDotNet.dll b/ros2_csharp/src/ros_driver/deps/YamlDotNet/YamlDotNet.dll new file mode 100644 index 00000000..41ad7208 Binary files /dev/null and b/ros2_csharp/src/ros_driver/deps/YamlDotNet/YamlDotNet.dll differ diff --git a/ros2_csharp/src/ros_driver/package.xml b/ros2_csharp/src/ros_driver/package.xml new file mode 100644 index 00000000..b0f732c6 --- /dev/null +++ b/ros2_csharp/src/ros_driver/package.xml @@ -0,0 +1,38 @@ + + + ros_driver + 0.0.0 + Package containing examples of how to use the rcldotnet API. + Esteve Fernandez + Apache License 2.0 + + ament_cmake + ament_cmake_export_assemblies + + dotnet_cmake_module + rcldotnet_common + rosidl_cmake + + builtin_interfaces + example_interfaces + rcldotnet + std_msgs + std_srvs + sensor_msgs + test_msgs + ilabos_msgs + + rcldotnet_common + builtin_interfaces + example_interfaces + rcldotnet + std_msgs + std_srvs + sensor_msgs + test_msgs + ilabos_msgs + + + ament_cmake + + diff --git a/ros2_csharp/src/ros_driver/utils/MessageConverter.cs b/ros2_csharp/src/ros_driver/utils/MessageConverter.cs new file mode 100644 index 00000000..4afde2b7 --- /dev/null +++ b/ros2_csharp/src/ros_driver/utils/MessageConverter.cs @@ -0,0 +1,154 @@ +using System; +using System.Collections.Generic; +using System.Globalization; +using System.Linq; +using ROS2; +using test_msgs.action; +using builtin_interfaces.msg; +using ilabos_msgs.action; + +namespace RosDriver.Utils +{ + // 静态方法1 C# 根据数据std_msg.msg 类型把数据转 std_msg.msg 类型数据。 + + public static class Converter2RosMessage + { + + // 帮我写一个静态函数，接受一个 string 返回一个 std_msgs.msg 类型，例如接受 String 返回 std_msgs.msg.String 类型，而不是具体的实例 + public static Type GetRosMessageType(string typeName) + { + return typeName switch + { + "String" => typeof(std_msgs.msg.String), + "Int32" => typeof(std_msgs.msg.Int32), + "Float32" => typeof(std_msgs.msg.Float32), + "Float64" => typeof(std_msgs.msg.Float64), + "Double" => typeof(std_msgs.msg.Float64), + "Bool" => typeof(std_msgs.msg.Bool), + "Byte" => typeof(std_msgs.msg.Byte), + "Float64MultiArray" => typeof(std_msgs.msg.Float64MultiArray), + _ => throw new ArgumentException($"Unsupported type name: {typeName}", nameof(typeName)) + }; + } + + // 根据名字返回类型，例如Fibonacci 字符串就返回四个类型 Fibonacci, Fibonacci_Goal, Fibonacci_Result, Fibonacci_Feedback + public static List GetActionMessageType(string typeName) + { + switch (typeName) + { + case "Fibonacci": + return new List { typeof(Fibonacci), typeof(Fibonacci_Goal), typeof(Fibonacci_Result), typeof(Fibonacci_Feedback) }; + case "SendCmd": + return new List { typeof(SendCmd), typeof(SendCmd_Goal), typeof(SendCmd_Result), typeof(SendCmd_Feedback) }; + case "Stir": + return new List { typeof(Stir), typeof(Stir_Goal), typeof(Stir_Result), typeof(Stir_Feedback) }; + case "HeatChill": + return new List { typeof(HeatChill), typeof(HeatChill_Goal), typeof(HeatChill_Result), typeof(HeatChill_Feedback) }; + default: + throw new ArgumentException($"Unsupported type name: {typeName}", nameof(typeName)); + } + } + + + // 新增方法: 使用 switch type 识别不同类型的数据并转换为对应的 std_msgs.msg 类型 + public static T ConvertToRosMessage(object data) where T : IRosMessage, new() + { + T rosMessage = new T(); + + switch (rosMessage) + { + case std_msgs.msg.String stringMsg when data is string strData: + stringMsg.Data = strData; + return rosMessage; + + case std_msgs.msg.Int32 int32Msg when data is int intData: + int32Msg.Data = intData; + return rosMessage; + + case std_msgs.msg.Float32 float32Msg when data is float floatData: + float32Msg.Data = floatData; + return rosMessage; + + case std_msgs.msg.Float64 float64Msg when data is double doubleData: + float64Msg.Data = doubleData; + return rosMessage; + + case std_msgs.msg.Bool boolMsg when data is bool boolData: + boolMsg.Data = boolData; + return rosMessage; + + case std_msgs.msg.Byte byteMsg when data is byte byteData: + byteMsg.Data = byteData; + return rosMessage; + + case std_msgs.msg.Char charMsg when data is char charData: + charMsg.Data = (byte)charData; + return rosMessage; + + case std_msgs.msg.ByteMultiArray byteArrayMsg when data is byte[] byteArrayData: + byteArrayMsg.Data = byteArrayData.ToList(); + byteArrayMsg.Layout = new std_msgs.msg.MultiArrayLayout(); + return rosMessage; + + case std_msgs.msg.Float64MultiArray float64ArrayMsg when data is double[] doubleArrayData: + float64ArrayMsg.Data = doubleArrayData.ToList(); + float64ArrayMsg.Layout = new std_msgs.msg.MultiArrayLayout(); + return rosMessage; + + case std_msgs.msg.Float64MultiArray float64ArrayMsg when data is List doubleListData: + float64ArrayMsg.Data = doubleListData; + float64ArrayMsg.Layout = new std_msgs.msg.MultiArrayLayout(); + return rosMessage; + + case builtin_interfaces.msg.Time timeMsg when data is DateTime dateTimeData: + timeMsg.Sec = (int)(dateTimeData.ToUniversalTime().Subtract(new DateTime(1970, 1, 1))).TotalSeconds; + timeMsg.Nanosec = (uint)(dateTimeData.ToUniversalTime().Subtract(new DateTime(1970, 1, 1))).Milliseconds * 1000000; + return rosMessage; + + case builtin_interfaces.msg.Duration durationMsg when data is TimeSpan timeSpanData: + durationMsg.Sec = (int)timeSpanData.TotalSeconds; + durationMsg.Nanosec = (uint)(timeSpanData.Milliseconds * 1000000); + return rosMessage; + + default: + throw new InvalidOperationException( + $"Cannot convert data of type '{data.GetType().Name}' to ROS message type '{typeof(T).Name}'"); + } + } + + // 将ROS消息转换回C#数据类型的方法 + public static object ConvertFromRosMessage(IRosMessage rosMessage) + { + return rosMessage switch + { + std_msgs.msg.String strMsg => strMsg.Data, + std_msgs.msg.Int32 int32Msg => int32Msg.Data, + std_msgs.msg.Float32 float32Msg => float32Msg.Data, + std_msgs.msg.Float64 float64Msg => float64Msg.Data, + std_msgs.msg.Bool boolMsg => boolMsg.Data, + std_msgs.msg.Byte byteMsg => byteMsg.Data, + std_msgs.msg.Char charMsg => charMsg.Data, + std_msgs.msg.ByteMultiArray byteArrayMsg => byteArrayMsg.Data, + std_msgs.msg.Float64MultiArray float64ArrayMsg => float64ArrayMsg.Data, + builtin_interfaces.msg.Time timeMsg => new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc) + .AddSeconds(timeMsg.Sec) + .AddMilliseconds(timeMsg.Nanosec / 1000000.0), + builtin_interfaces.msg.Duration durationMsg => TimeSpan.FromSeconds(durationMsg.Sec) + .Add(TimeSpan.FromMilliseconds(durationMsg.Nanosec / 1000000.0)), + // 可以根据需要添加更多类型的匹配 + _ => throw new ArgumentException($"Unsupported ROS message type: {rosMessage.GetType().Name}", nameof(rosMessage)) + }; + } + + // 泛型版本，当客户端代码知道期望的返回类型时使用 + public static T ConvertFromRosMessage(IRosMessage rosMessage) + { + object result = ConvertFromRosMessage(rosMessage); + if (result is T typedResult) + { + return typedResult; + } + throw new InvalidCastException($"Cannot convert from {rosMessage.GetType().Name} to {typeof(T).Name}"); + } + } +} \ No newline at end of file diff --git a/ros2_csharp/src/ros_driver/utils/Yaml.cs b/ros2_csharp/src/ros_driver/utils/Yaml.cs new file mode 100644 index 00000000..fceb8880 --- /dev/null +++ b/ros2_csharp/src/ros_driver/utils/Yaml.cs @@ -0,0 +1,143 @@ +using System; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using YamlDotNet.Serialization; +using YamlDotNet.Serialization.NamingConventions; +using YamlDotNet.RepresentationModel; + +namespace RosDriver.Utils +{ + public class ActionValueMapping + { + public string Type { get; set; } + public Dictionary Goal { get; set; } + public Dictionary Feedback { get; set; } + public Dictionary Result { get; set; } + + public ActionValueMapping() + { + Goal = new Dictionary(); + Feedback = new Dictionary(); + Result = new Dictionary(); + } + } + + public class DeviceInfo + { + public Dictionary StatusTypes { get; set; } + public Dictionary ActionValueMappings { get; set; } + + public DeviceInfo() + { + StatusTypes = new Dictionary(); + ActionValueMappings = new Dictionary(); + } + } + + public class YamlConfigParser + { + + private static void ExtractMappingSection(YamlMappingNode parentNode, string sectionName, Dictionary targetDict) + { + var sectionKeyNode = new YamlScalarNode(sectionName); + if (parentNode.Children.ContainsKey(sectionKeyNode)) + { + var sectionNode = parentNode.Children[sectionKeyNode] as YamlMappingNode; + if (sectionNode != null) + { + foreach (var entry in sectionNode.Children) + { + if (entry.Key is YamlScalarNode entryKeyNode && + entry.Value is YamlScalarNode entryValueNode) + { + targetDict[entryKeyNode.Value] = entryValueNode.Value; + } + } + } + } + } + + private static Dictionary ParseDeviceInfo(string filePath) + { + var devices = new Dictionary(); + + using (var reader = new StreamReader(filePath)) + { + var yaml = new YamlStream(); + yaml.Load(reader); + + var rootNode = yaml.Documents[0].RootNode as YamlMappingNode; + if (rootNode == null) + { + throw new InvalidDataException("Root node is not a mapping node."); + } + + foreach (var deviceEntry in rootNode.Children) + { + var deviceKey = ((YamlScalarNode)deviceEntry.Key).Value; + var deviceNode = deviceEntry.Value as YamlMappingNode; + if (deviceNode == null) continue; + + var deviceInfo = new DeviceInfo(); + + // Parse status_types + YamlMappingNode classMappingNode = null; + if (deviceNode.Children.TryGetValue(new YamlScalarNode("class"), out var classNode) && + classNode is YamlMappingNode tempClassNode) + { + classMappingNode = tempClassNode; + if (classMappingNode.Children.TryGetValue(new YamlScalarNode("status_types"), out var statusTypesNode) && + statusTypesNode is YamlMappingNode statusTypesMappingNode) + { + foreach (var statusEntry in statusTypesMappingNode.Children) + { + var key = ((YamlScalarNode)statusEntry.Key).Value; + var value = ((YamlScalarNode)statusEntry.Value).Value; + deviceInfo.StatusTypes[key] = value; + } + } + } + + // Parse action_value_mappings + if (classMappingNode != null && + classMappingNode.Children.TryGetValue(new YamlScalarNode("action_value_mappings"), out var actionMappingsNode) && + actionMappingsNode is YamlMappingNode actionMappingsMappingNode) + { + foreach (var actionEntry in actionMappingsMappingNode.Children) + { + var actionKey = ((YamlScalarNode)actionEntry.Key).Value; + var actionNode = actionEntry.Value as YamlMappingNode; + if (actionNode == null) continue; + + var actionValue = new ActionValueMapping(); + + // Extract type + if (actionNode.Children.TryGetValue(new YamlScalarNode("type"), out var typeNode) && + typeNode is YamlScalarNode typeScalarNode) + { + actionValue.Type = typeScalarNode.Value; + } + + // Extract goal, feedback, result + ExtractMappingSection(actionNode, "goal", actionValue.Goal); + ExtractMappingSection(actionNode, "feedback", actionValue.Feedback); + ExtractMappingSection(actionNode, "result", actionValue.Result); + + deviceInfo.ActionValueMappings[actionKey] = actionValue; + } + } + + devices[deviceKey] = deviceInfo; + } + } + + return devices; + } + + public static Dictionary GetDeviceInfoDict(string filePath) + { + return ParseDeviceInfo(filePath); + } + } +}