diff --git a/.gitignore b/.gitignore index a5a29994c..9443aeafe 100644 --- a/.gitignore +++ b/.gitignore @@ -23,6 +23,7 @@ dist/ __pycache__/ *_pb2.py *_pb2_grpc.py +*_aimrt_rpc_ros2.py log/ tmp/ diff --git a/document/sphinx-cn/release_notes/v0_9_0.md b/document/sphinx-cn/release_notes/v0_9_0.md index 1423c6a9a..936c34518 100644 --- a/document/sphinx-cn/release_notes/v0_9_0.md +++ b/document/sphinx-cn/release_notes/v0_9_0.md @@ -10,6 +10,7 @@ - 新增 aimrt_cli trans 命令，用于将 使用 aimrt record_playback 插件录制的 bag 文件转换为 ros2 的 bag 文件； - 新增 Echo 插件，用于回显消息； - 新增了基于执行器的定时器，方便执行定时任务； +- aimrt_py channel 和 rpc 支持 ros2 消息类型； **次要修改**： - 缩短了一些 examples 的文件路径长度； diff --git a/document/sphinx-cn/tutorials/examples/examples_py.md b/document/sphinx-cn/tutorials/examples/examples_py.md index e894f7fa9..12a339129 100644 --- a/document/sphinx-cn/tutorials/examples/examples_py.md +++ b/document/sphinx-cn/tutorials/examples/examples_py.md @@ -9,6 +9,8 @@ AimRT 提供了以下 Python 接口使用示例： - {{ '[pb_rpc]({}/src/examples/py/pb_rpc)'.format(code_site_root_path_url) }} - {{ '[pb_chn_bench]({}/src/examples/py/pb_chn_bench)'.format(code_site_root_path_url) }} - {{ '[pb_rpc_bench]({}/src/examples/py/pb_rpc_bench)'.format(code_site_root_path_url) }} + - {{ '[ros2_chn]({}/src/examples/py/ros2_chn)'.format(code_site_root_path_url) }} + - {{ '[ros2_rpc]({}/src/examples/py/ros2_rpc)'.format(code_site_root_path_url) }} 关于这些示例的说明： - 每个示例都有自己独立的 readme 文档，详情请点击示例链接进入后查看； diff --git a/document/sphinx-cn/tutorials/interface_py/channel.md b/document/sphinx-cn/tutorials/interface_py/channel.md index 84efa9284..eeb7a0ce5 100644 --- a/document/sphinx-cn/tutorials/interface_py/channel.md +++ b/document/sphinx-cn/tutorials/interface_py/channel.md @@ -7,11 +7,12 @@ - {{ '[examples_py_pb_chn_publisher_app.py]({}/src/examples/py/pb_chn/examples_py_pb_chn_publisher_app.py)'.format(code_site_root_path_url) }} - {{ '[examples_py_pb_chn_subscriber_app.py]({}/src/examples/py/pb_chn/examples_py_pb_chn_subscriber_app.py)'.format(code_site_root_path_url) }} - -## protobuf 协议 +## 协议 协议用于确定通信各端的消息格式。一般来说，协议都是使用一种与具体的编程语言无关的 IDL ( Interface description language )描述，然后由某种工具转换为各个语言的代码。 +### Protobuf + [Protobuf](https://protobuf.dev/)是一种由 Google 开发的、用于序列化结构化数据的轻量级、高效的数据交换格式，是一种广泛使用的 IDL。 当前版本 AimRT Python 只支持 protobuf 协议。在使用 AimRT Python 发送/订阅消息之前，使用者需要先基于 protobuf 协议生成一些 python 的桩代码。 @@ -36,6 +37,32 @@ protoc --python_out=. example.proto 这将生成`example_pb2.py`文件，包含了根据定义的消息类型生成的 Python 接口，我们的业务代码中需要 import 此文件。 +### ROS2 Message + +ROS2 Message 是一种用于在 ROS2 中进行通信和数据交换的结构化数据格式。在使用时，开发者需要先定义一个 ROS2 Package，在其中定义一个`.msg`文件，比如`example.msg`： + +``` +int32 num +float32 num2 +char data +``` + +然后直接通过 ROS2 提供的 CMake 方法`rosidl_generate_interfaces`，为消息生成 C++ 代码和 CMake Target，例如： +```cmake +rosidl_generate_interfaces( + example_ros2_msg_gencode + "msg/example.msg" +) +``` + +构建之后，还需要设置相应的环境变量，才能在 python 中使用生成的消息类型，在 aimrt 的 build 目录中，执行： + +```bash +source install/share/example_ros2/setup.bash +``` + +关于更多生成自定义 ROS2 Message 的详细信息，请参考[ROS2 官方文档](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Custom-ROS2-Interfaces.html)。 + ## ChannelHandle 模块可以通过调用`CoreRef`句柄的`GetChannelHandle()`接口，获取`ChannelHandleRef`句柄，来使用 Channel 功能。其提供的核心接口如下： diff --git a/document/sphinx-cn/tutorials/interface_py/rpc.md b/document/sphinx-cn/tutorials/interface_py/rpc.md index a09d647a1..b57a93553 100644 --- a/document/sphinx-cn/tutorials/interface_py/rpc.md +++ b/document/sphinx-cn/tutorials/interface_py/rpc.md @@ -6,8 +6,10 @@ 参考示例： - {{ '[examples_py_pb_rpc_client_app.py]({}/src/examples/py/pb_rpc/examples_py_pb_rpc_client_app.py)'.format(code_site_root_path_url) }} - {{ '[examples_py_pb_rpc_server_app.py]({}/src/examples/py/pb_rpc/examples_py_pb_rpc_server_app.py)'.format(code_site_root_path_url) }} +- {{ '[examples_py_ros2_rpc_client_app.py]({}/src/examples/py/ros2_rpc/examples_py_ros2_rpc_client_app.py)'.format(code_site_root_path_url) }} +- {{ '[examples_py_ros2_rpc_server_app.py]({}/src/examples/py/ros2_rpc/examples_py_ros2_rpc_server_app.py)'.format(code_site_root_path_url) }} -## protobuf 协议 +## 协议 协议用于确定 RPC 中客户端和服务端的消息格式。一般来说，协议都是使用一种与具体的编程语言无关的 IDL ( Interface description language )描述，然后由某种工具转换为各个语言的代码。对于 RPC 来说，这里需要两个步骤： @@ -15,11 +17,10 @@ - 开发者需要使用 AimRT 提供的工具，为协议文件中**服务定义**生成指定编程语言中的代码； +### Protobuf + [Protobuf](https://protobuf.dev/)是一种由 Google 开发的、用于序列化结构化数据的轻量级、高效的数据交换格式，是一种广泛使用的 IDL。它不仅能够描述消息结构，还提供了`service`语句来定义 RPC 服务。 -当前版本 AimRT Python 只支持 protobuf 协议。在使用 AimRT Python 发起/处理 RPC 请求之前，使用者需要先基于 protobuf 协议生成一些 python 的桩代码。 - - 在使用时，开发者需要先定义一个`.proto`文件，在其中定义消息结构和 RPC 服务。例如`rpc.proto`： ```protobuf @@ -57,6 +58,29 @@ protoc --aimrt_rpc_out=. --plugin=protoc-gen-aimrt_rpc=./protoc_plugin_py_gen_ai 这将生成`rpc_aimrt_rpc_pb2.py`文件，包含了根据定义的服务生成的 Python 接口，我们的业务代码中需要 import 此文件。 +### ROS2 Srv + +ROS2 Srv 是一种用于在 ROS2 中进行 RPC 定义的格式。在使用时，开发者需要先定义一个 ROS2 Package，在其中定义一个`.srv`文件，比如`example.srv`： + +``` +byte[] data +--- +int64 code +``` + +其中，以`---`来分割 Req 和 Rsp 的定义。 + +aimrt_py 安装时会自动安装一个命令行工具 `aimrt_py-gen-ros2-rpc`，用于根据 ROS2 Srv 文件生成 AimRT Python 的 RPC 桩代码。 + +```shell +aimrt_py-gen-ros2-rpc --pkg_name=example_pkg --srv_file=./example.srv --output_path=./ +``` + +其中 pkg_name 是 ROS2 Package 的名称，srv_file 是 ROS2 Srv 文件的路径，output_path 是生成的桩代码的输出路径。这将生成一个`example_aimrt_rpc_ros2.py`文件，包含了根据定义的服务生成的 Python 接口，我们的业务代码中需要 import 此文件。 + +需要注意的是，`aimrt_py-gen-ros2-rpc` 只会生成 Req 和 Rsp 的定义，不会生成消息结构部分的代码，消息结构部分代码仍然需要使用者自行生成（即构建 ROS2 Package 并生成消息结构代码, 详情可以参考 [ROS2 官方文档](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Custom-ROS2-Interfaces.html)）。 + + ## RpcHandle 模块可以通过调用`CoreRef`句柄的`GetRpcHandle()`接口，获取`RpcHandleRef`句柄。一般情况下，开发者不会直接使用`RpcHandleRef`直接提供的接口，而是根据 RPC IDL 文件生成一些桩代码，对`RpcHandleRef`句柄做一些封装，然后在业务代码中使用这些经过封装的接口。 @@ -126,7 +150,7 @@ RpcContext 是 RPC 调用时上下文信息，开发者可以在 RPC 调用时 -以下是一个使用 AimRT Python 进行 RPC Client 调用的示例，通过 Create Module 方式拿到`CoreRef`句柄。如果是基于`Module`模式在`Initialize`方法中拿到`CoreRef`句柄，使用方式也类似： +以下是一个使用 AimRT Python 基于 protobuf 协议进行 RPC Client 调用的示例，通过 Create Module 方式拿到`CoreRef`句柄。如果是基于`Module`模式在`Initialize`方法中拿到`CoreRef`句柄，使用方式也类似： ```python import aimrt_py @@ -200,7 +224,7 @@ if __name__ == '__main__': - **Step 2**：在`Initialize`阶段调用`RpcHandleRef`的`RegisterService`方法注册 RPC Service； -以下是一个使用 AimRT Python 进行 RPC Service 处理的示例，通过 Create Module 方式拿到`CoreRef`句柄。如果是基于`Module`模式在`Initialize`方法中拿到`CoreRef`句柄，使用方式也类似： +以下是一个使用 AimRT Python 基于 protobuf 协议进行 RPC Service 处理的示例，通过 Create Module 方式拿到`CoreRef`句柄。如果是基于`Module`模式在`Initialize`方法中拿到`CoreRef`句柄，使用方式也类似： ```python import aimrt_py @@ -281,3 +305,9 @@ def main(): if __name__ == '__main__': main() ``` + +基于 ROS2 Srv 协议的 RPC 调用和处理，除数据类型不同外，其他使用方式与基于 protobuf 协议的 RPC 调用和处理基本一致。 + +完整示例请参考： +- {{ '[examples/py/ros2_rpc]({}/src/examples/py/ros2_rpc)'.format(code_site_root_path_url) }} +- {{ '[examples/py/pb_rpc]({}/src/examples/py/pb_rpc)'.format(code_site_root_path_url) }} diff --git a/src/examples/py/pb_chn/README.md b/src/examples/py/pb_chn/README.md index dc9bb903e..5de5bb20b 100644 --- a/src/examples/py/pb_chn/README.md +++ b/src/examples/py/pb_chn/README.md @@ -33,7 +33,7 @@ 说明： - 此示例创建了以下两个模块： - - `NormalPublisherPyModule`：会在启动后向配置的 topic 中发布一条 `ExampleEventMsg` 类型的消息，然后结束进程； + - `NormalPublisherPyModule`：会在启动后向配置的 topic 中发布若干条 `ExampleEventMsg` 类型的消息，然后结束进程； - `NormalSubscriberPyModule`：会订阅配置的 topic 下的 `ExampleEventMsg` 类型的消息； - 此示例使用 http 类型的 channel 后端进行通信，请确保相关端口未被占用； @@ -70,7 +70,7 @@ 说明： - 此示例创建了以下两个模块： - - `NormalPublisherPyModule`：会在启动后向配置的 topic 中发布一条 `ExampleEventMsg` 类型的消息，然后结束进程； + - `NormalPublisherPyModule`：会在启动后向配置的 topic 中发布若干条 `ExampleEventMsg` 类型的消息，然后结束进程； - `NormalSubscriberPyModule`：会订阅配置的 topic 下的 `ExampleEventMsg` 类型的消息； - 此示例使用 ros2 类型的 channel 后端进行通信，请确保本地安装有 ROS2 Humble； diff --git a/src/examples/py/pb_chn/examples_py_pb_chn_publisher_app.py b/src/examples/py/pb_chn/examples_py_pb_chn_publisher_app.py index ebf2e82cf..bd196e725 100644 --- a/src/examples/py/pb_chn/examples_py_pb_chn_publisher_app.py +++ b/src/examples/py/pb_chn/examples_py_pb_chn_publisher_app.py @@ -47,8 +47,9 @@ def main(): # Sleep for seconds time.sleep(1) - # Publish event event_msg = event_pb2.ExampleEventMsg() + + # Publish event event_msg.msg = "Publish without ctx or serialization_type" event_msg.num = 1 aimrt_py.Publish(publisher, event_msg) diff --git a/src/examples/py/ros2_chn/README.md b/src/examples/py/ros2_chn/README.md new file mode 100644 index 000000000..171db1e17 --- /dev/null +++ b/src/examples/py/ros2_chn/README.md @@ -0,0 +1,71 @@ +# ros2 channel examples + + +## ros2 channel http + + +一个基于 ros2 协议与 http 后端的 python channel 示例，演示内容包括： +- 如何在 python 中使用 ros2 协议作为 channel 传输协议； +- 如何基于 aimrt_py 创建模块的方式使用 Channel publish 和 subscribe 功能； +- 如何使用 http 类型的 channel 后端； + + +核心代码： +- [examples_py_ros2_chn_publisher_app.py](./examples_py_ros2_chn_publisher_app.py) +- [examples_py_ros2_chn_subscriber_app.py](./examples_py_ros2_chn_subscriber_app.py) + + +配置文件： +- [examples_py_ros2_chn_http_pub_cfg.yaml](./cfg/examples_py_ros2_chn_http_pub_cfg.yaml) +- [examples_py_ros2_chn_http_sub_cfg.yaml](./cfg/examples_py_ros2_chn_http_sub_cfg.yaml) + + + +运行方式（linux）： +- [安装 `aimrt_py` 包](../../../../document/sphinx-cn/tutorials/quick_start/installation_py.md)； +- 运行本目录下的[start_examples_py_ros2_chn_http_sub.sh](./start_examples_py_ros2_chn_http_sub.sh)脚本，启动 subscriber； +- 在新终端里运行本目录下的[start_examples_py_ros2_chn_http_pub.sh](./start_examples_py_ros2_chn_http_pub.sh)脚本，启动 publisher 发布一条消息； +- 向 subscriber 进程所在终端里键入`ctrl-c`以停止进程； + + +说明： +- 此示例创建了以下两个模块： + - `NormalPublisherPyModule`：会在启动后向配置的 topic 中发布若干条 `ExampleEventMsg` 类型的消息，然后结束进程； + - `NormalSubscriberPyModule`：会订阅配置的 topic 下的 `ExampleEventMsg` 类型的消息； +- 此示例使用 http 类型的 channel 后端进行通信，请确保相关端口未被占用； + + + +## ros2 channel ros2 + +一个基于 ros2 协议与 ros2 后端的 python channel 示例，演示内容包括： +- 如何在 python 中使用 ros2 协议作为 channel 传输协议； +- 如何基于 aimrt_py 创建模块的方式使用 Channel publish 和 subscribe 功能； +- 如何使用 ros2 类型的 channel 后端； + + +核心代码： +- [examples_py_ros2_chn_publisher_app.py](./examples_py_ros2_chn_publisher_app.py) +- [examples_py_ros2_chn_subscriber_app.py](./examples_py_ros2_chn_subscriber_app.py) + + +配置文件： +- [examples_py_ros2_chn_ros2_pub_cfg.yaml](./cfg/examples_py_ros2_chn_ros2_pub_cfg.yaml) +- [examples_py_ros2_chn_ros2_sub_cfg.yaml](./cfg/examples_py_ros2_chn_ros2_sub_cfg.yaml) + + + +运行方式（linux）： +- [安装 `aimrt_py` 包](../../../../document/sphinx-cn/tutorials/quick_start/installation_py.md)； +- 运行本目录下的[start_examples_py_ros2_chn_ros2_sub.sh](./start_examples_py_ros2_chn_ros2_sub.sh)脚本，启动 subscriber； +- 在新终端里运行本目录下的[start_examples_py_pb_chn_ros2_pub.sh](./start_examples_py_pb_chn_ros2_pub.sh)脚本，启动 publisher 发布一条消息； +- 向 subscriber 进程所在终端里键入`ctrl-c`以停止进程； + + +说明： +- 此示例创建了以下两个模块： + - `NormalPublisherPyModule`：会在启动后向配置的 topic 中发布若干条 `ExampleEventMsg` 类型的消息，然后结束进程； + - `NormalSubscriberPyModule`：会订阅配置的 topic 下的 `ExampleEventMsg` 类型的消息； +- 此示例使用 ros2 类型的 channel 后端进行通信，请确保本地安装有 ROS2 Humble； + + diff --git a/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_http_pub_cfg.yaml b/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_http_pub_cfg.yaml new file mode 100644 index 000000000..8580ee684 --- /dev/null +++ b/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_http_pub_cfg.yaml @@ -0,0 +1,35 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +aimrt: + plugin: + plugins: + - name: net_plugin + path: ${AIMRT_PLUGIN_DIR}/libaimrt_net_plugin.so + options: + thread_num: 4 + http_options: + listen_ip: 127.0.0.1 + listen_port: 50081 + log: + core_lvl: INFO # Trace/Debug/Info/Warn/Error/Fatal/Off + default_module_lvl: INFO + backends: + - type: console + channel: + backends: + - type: http + options: + pub_topics_options: + - topic_name: "(.*)" + server_url_list: ["http://127.0.0.1:50080"] + pub_topics_options: + - topic_name: "(.*)" + enable_backends: [http] + module: + modules: + - name: NormalPublisherPyModule + log_lvl: INFO + +NormalPublisherPyModule: + topic_name: test_topic diff --git a/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_http_sub_cfg.yaml b/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_http_sub_cfg.yaml new file mode 100644 index 000000000..7f0505618 --- /dev/null +++ b/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_http_sub_cfg.yaml @@ -0,0 +1,31 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +aimrt: + plugin: + plugins: + - name: net_plugin + path: ${AIMRT_PLUGIN_DIR}/libaimrt_net_plugin.so + options: + thread_num: 4 + http_options: + listen_ip: 127.0.0.1 + listen_port: 50080 + log: + core_lvl: INFO # Trace/Debug/Info/Warn/Error/Fatal/Off + default_module_lvl: INFO + backends: + - type: console + channel: + backends: + - type: http + sub_topics_options: + - topic_name: "(.*)" + enable_backends: [http] + module: + modules: + - name: NormalSubscriberPyModule + log_lvl: INFO + +NormalSubscriberPyModule: + topic_name: test_topic diff --git a/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_ros2_pub_cfg.yaml b/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_ros2_pub_cfg.yaml new file mode 100644 index 000000000..5fa60087b --- /dev/null +++ b/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_ros2_pub_cfg.yaml @@ -0,0 +1,30 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +aimrt: + plugin: + plugins: + - name: ros2_plugin + path: ${AIMRT_PLUGIN_DIR}/libaimrt_ros2_plugin.so + options: + node_name: example_ros2_chn_py_publisher_node + executor_type: MultiThreaded # SingleThreaded/StaticSingleThreaded/MultiThreaded + executor_thread_num: 2 + log: + core_lvl: INFO # Trace/Debug/Info/Warn/Error/Fatal/Off + default_module_lvl: INFO + backends: + - type: console + channel: + backends: + - type: ros2 + pub_topics_options: + - topic_name: "(.*)" + enable_backends: [ros2] + module: + modules: + - name: NormalPublisherPyModule + log_lvl: INFO + +NormalPublisherPyModule: + topic_name: test_topic diff --git a/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_ros2_sub_cfg.yaml b/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_ros2_sub_cfg.yaml new file mode 100644 index 000000000..16d38b927 --- /dev/null +++ b/src/examples/py/ros2_chn/cfg/examples_py_ros2_chn_ros2_sub_cfg.yaml @@ -0,0 +1,30 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +aimrt: + plugin: + plugins: + - name: ros2_plugin + path: ${AIMRT_PLUGIN_DIR}/libaimrt_ros2_plugin.so + options: + node_name: example_ros2_chn_py_subscriber_node + executor_type: MultiThreaded # SingleThreaded/StaticSingleThreaded/MultiThreaded + executor_thread_num: 2 + log: + core_lvl: INFO # Trace/Debug/Info/Warn/Error/Fatal/Off + default_module_lvl: INFO + backends: + - type: console + channel: + backends: + - type: ros2 + sub_topics_options: + - topic_name: "(.*)" + enable_backends: [ros2] + module: + modules: + - name: NormalSubscriberPyModule + log_lvl: INFO + +NormalSubscriberPyModule: + topic_name: test_topic diff --git a/src/examples/py/ros2_chn/examples_py_ros2_chn_publisher_app.py b/src/examples/py/ros2_chn/examples_py_ros2_chn_publisher_app.py new file mode 100644 index 000000000..c4866040b --- /dev/null +++ b/src/examples/py/ros2_chn/examples_py_ros2_chn_publisher_app.py @@ -0,0 +1,94 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +import argparse +import threading +import time + +import aimrt_py +import yaml +from example_ros2.msg import RosTestMsg + + +def main(): + parser = argparse.ArgumentParser(description='Example channel publisher app.') + parser.add_argument('--cfg_file_path', type=str, default="", help='config file path') + args = parser.parse_args() + + print("AimRT start.") + + aimrt_core = aimrt_py.Core() + + # Initialize + core_options = aimrt_py.CoreOptions() + core_options.cfg_file_path = args.cfg_file_path + aimrt_core.Initialize(core_options) + + # Create Module + module_handle = aimrt_core.CreateModule("NormalPublisherPyModule") + + # Read cfg + module_cfg_file_path = module_handle.GetConfigurator().GetConfigFilePath() + with open(module_cfg_file_path, 'r') as file: + data = yaml.safe_load(file) + topic_name = str(data["topic_name"]) + + # Register publish type + publisher = module_handle.GetChannelHandle().GetPublisher(topic_name) + assert publisher, f"Get publisher for topic '{topic_name}' failed." + + aimrt_py.RegisterPublishType(publisher, RosTestMsg) + + # Start + thread = threading.Thread(target=aimrt_core.Start) + thread.start() + + # Sleep for seconds + time.sleep(1) + + msg = RosTestMsg() + msg.num = 1000 + + # Publish event + msg.data = [bytes([x]) for x in [1, 2, 3, 4]] + aimrt_py.Publish(publisher, msg) + aimrt_py.info(module_handle.GetLogger(), + f"Publish new ros2 message, data: {msg.data}") + + # Publish event with ros2 serialization + msg.data = [bytes([x]) for x in [5, 6, 7, 8]] + aimrt_py.Publish(publisher, "ros2", msg) + aimrt_py.info(module_handle.GetLogger(), + f"Publish new ros2 message, data: {msg.data}") + + # Publish event with context + ctx = aimrt_py.Context() + ctx.SetMetaValue("key1", "value1") + msg.data = [bytes([x]) for x in [9, 10, 11, 12]] + aimrt_py.Publish(publisher, ctx, msg) + aimrt_py.info(module_handle.GetLogger(), + f"Publish new ros2 message, data: {msg.data}") + + # Publish event with context ref + ctx.Reset() # Reset context, then it can be used again + ctx_ref = aimrt_py.ContextRef(ctx) + ctx_ref.SetMetaValue("key2", "value2") + ctx_ref.SetSerializationType("ros2") + msg.data = [bytes([x]) for x in [13, 14, 15, 16]] + aimrt_py.Publish(publisher, ctx_ref, msg) + aimrt_py.info(module_handle.GetLogger(), + f"Publish new ros2 message, data: {msg.data}") + + # Sleep for seconds + time.sleep(1) + + # Shutdown + aimrt_core.Shutdown() + + thread.join() + + print("AimRT exit.") + + +if __name__ == '__main__': + main() diff --git a/src/examples/py/ros2_chn/examples_py_ros2_chn_subscriber_app.py b/src/examples/py/ros2_chn/examples_py_ros2_chn_subscriber_app.py new file mode 100644 index 000000000..c4dfb6041 --- /dev/null +++ b/src/examples/py/ros2_chn/examples_py_ros2_chn_subscriber_app.py @@ -0,0 +1,80 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +import argparse +import signal +import sys +import threading + +import aimrt_py +import yaml +from example_ros2.msg import RosTestMsg + +global_aimrt_core = None + + +def signal_handler(sig, frame): + global global_aimrt_core + + if (global_aimrt_core and (sig == signal.SIGINT or sig == signal.SIGTERM)): + global_aimrt_core.Shutdown() + return + + sys.exit(0) + + +def main(): + parser = argparse.ArgumentParser(description='Example channel subscriber app.') + parser.add_argument('--cfg_file_path', type=str, default="", help='config file path') + args = parser.parse_args() + + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + + print("AimRT start.") + + aimrt_core = aimrt_py.Core() + + global global_aimrt_core + global_aimrt_core = aimrt_core + + # Initialize + core_options = aimrt_py.CoreOptions() + core_options.cfg_file_path = args.cfg_file_path + aimrt_core.Initialize(core_options) + + # Create Module + module_handle = aimrt_core.CreateModule("NormalSubscriberPyModule") + + # Read cfg + module_cfg_file_path = module_handle.GetConfigurator().GetConfigFilePath() + with open(module_cfg_file_path, 'r') as file: + data = yaml.safe_load(file) + topic_name = str(data["topic_name"]) + + # Subscribe + subscriber = module_handle.GetChannelHandle().GetSubscriber(topic_name) + assert subscriber, f"Get subscriber for topic '{topic_name}' failed." + + count = 0 + + def EventHandle(ctx_ref: aimrt_py.ContextRef, msg: RosTestMsg): + nonlocal count + count += 1 + aimrt_py.info(module_handle.GetLogger(), + f"Get new ros2 message, ctx: {ctx_ref.ToString()}, data: {msg.data}, count: {count}") + + aimrt_py.Subscribe(subscriber, RosTestMsg, EventHandle) + + # Start + thread = threading.Thread(target=aimrt_core.Start) + thread.start() + + while thread.is_alive(): + thread.join(1.0) + + print("AimRT exit.") + + +if __name__ == '__main__': + main() diff --git a/src/examples/py/ros2_chn/start_examples_py_ros2_chn_http_pub.sh b/src/examples/py/ros2_chn/start_examples_py_ros2_chn_http_pub.sh new file mode 100755 index 000000000..5eccea377 --- /dev/null +++ b/src/examples/py/ros2_chn/start_examples_py_ros2_chn_http_pub.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +export AIMRT_PLUGIN_DIR=$(pip show aimrt_py | grep Location | awk '{print $2}')/aimrt_py +source $AIMRT_PLUGIN_DIR/share/example_ros2/local_setup.bash + +python3 ./examples_py_ros2_chn_publisher_app.py --cfg_file_path=./cfg/examples_py_ros2_chn_http_pub_cfg.yaml diff --git a/src/examples/py/ros2_chn/start_examples_py_ros2_chn_http_sub.sh b/src/examples/py/ros2_chn/start_examples_py_ros2_chn_http_sub.sh new file mode 100755 index 000000000..de8a7ed7d --- /dev/null +++ b/src/examples/py/ros2_chn/start_examples_py_ros2_chn_http_sub.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +export AIMRT_PLUGIN_DIR=$(pip show aimrt_py | grep Location | awk '{print $2}')/aimrt_py +source $AIMRT_PLUGIN_DIR/share/example_ros2/local_setup.bash + +python3 ./examples_py_ros2_chn_subscriber_app.py --cfg_file_path=./cfg/examples_py_ros2_chn_http_sub_cfg.yaml diff --git a/src/examples/py/ros2_chn/start_examples_py_ros2_chn_ros2_pub.sh b/src/examples/py/ros2_chn/start_examples_py_ros2_chn_ros2_pub.sh new file mode 100755 index 000000000..c4619f359 --- /dev/null +++ b/src/examples/py/ros2_chn/start_examples_py_ros2_chn_ros2_pub.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +export AIMRT_PLUGIN_DIR=$(pip show aimrt_py | grep Location | awk '{print $2}')/aimrt_py + +source ${AIMRT_PLUGIN_DIR}/share/ros2_plugin_proto/local_setup.bash + +python3 ./examples_py_ros2_chn_publisher_app.py --cfg_file_path=./cfg/examples_py_ros2_chn_ros2_pub_cfg.yaml diff --git a/src/examples/py/ros2_chn/start_examples_py_ros2_chn_ros2_sub.sh b/src/examples/py/ros2_chn/start_examples_py_ros2_chn_ros2_sub.sh new file mode 100755 index 000000000..897f2ac0d --- /dev/null +++ b/src/examples/py/ros2_chn/start_examples_py_ros2_chn_ros2_sub.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +export AIMRT_PLUGIN_DIR=$(pip show aimrt_py | grep Location | awk '{print $2}')/aimrt_py + +source ${AIMRT_PLUGIN_DIR}/share/ros2_plugin_proto/local_setup.bash + +python3 ./examples_py_ros2_chn_subscriber_app.py --cfg_file_path=./cfg/examples_py_ros2_chn_ros2_sub_cfg.yaml diff --git a/src/examples/py/ros2_rpc/README.md b/src/examples/py/ros2_rpc/README.md new file mode 100644 index 000000000..8c032cad4 --- /dev/null +++ b/src/examples/py/ros2_rpc/README.md @@ -0,0 +1,115 @@ +# ros2 rpc examples + + +## ros2 rpc http + + +一个基于 ros2 协议与 http 后端的 python rpc 示例，演示内容包括： +- 如何在 python 中使用 ros2 协议作为 rpc 传输协议； +- 如何基于 aimrt_py 创建模块的方式使用 rpc client 和 server 功能； +- 如何使用 http 类型的 rpc 后端； + + +核心代码： +- [RosTestRpc.srv](../../../protocols/example_ros2/srv/RosTestRpc.srv) +- [examples_py_ros2_rpc_client_app.py](./examples_py_ros2_rpc_client_app.py) +- [examples_py_ros2_rpc_server_app.py](./examples_py_ros2_rpc_server_app.py) + + +配置文件： +- [examples_py_ros2_rpc_http_client_cfg.yaml](./cfg/examples_py_ros2_rpc_http_client_cfg.yaml) +- [examples_py_ros2_rpc_http_server_cfg.yaml](./cfg/examples_py_ros2_rpc_http_server_cfg.yaml) + + + +运行方式（linux）： +- [安装 `aimrt_py` 包](../../../../document/sphinx-cn/tutorials/quick_start/installation_py.md)； +- 运行本目录下的[build_examples_py_ros2_rpc.sh](./build_examples_py_ros2_rpc.sh)脚本，生成协议桩代码文件； + - 注意生成的 rpc 桩代码文件会引用 `example_ros2` 包，请确保本地安装有 ROS2 Humble，然后打开选项编译本项目生成对应的 `example_ros2` 包，否则无法正常运行本示例； +- 运行本目录下的[start_examples_py_ros2_rpc_http_server.sh](./start_examples_py_ros2_rpc_http_server.sh)脚本，启动 RPC Server； +- 在新终端里运行本目录下的[start_examples_py_ros2_rpc_http_client.sh](./start_examples_py_ros2_rpc_http_client.sh)脚本，启动 Rpc client 发起一次请求； +- 向 RPC Server 进程所在终端里键入`ctrl-c`以停止进程； + + +说明： +- 此示例创建了以下两个模块： + - `NormalRpcClientPyModule`：会在启动后向 `RosTestRpc` 发起一次 `RosTestRpc` 的 RPC 请求，等收到回包并解析后结束进程； + - `NormalRpcServerPymodule`：会注册 `RosTestRpc` 服务端，通过 Server 接口，打印接收到的数据； +- 此示例使用 http 类型的 rpc 后端进行通信，请确保相关端口未被占用； + + + +## ros2 rpc grpc + + +一个基于 ros2 协议与 grpc 后端的 python rpc 示例，演示内容包括： +- 如何在 python 中使用 ros2 协议作为 rpc 传输协议； +- 如何基于 aimrt_py 创建模块的方式使用 rpc client 和 server 功能； +- 如何使用 grpc 类型的 rpc 后端； + + +核心代码： +- [RosTestRpc.srv](../../../protocols/example_ros2/srv/RosTestRpc.srv) +- [examples_py_ros2_rpc_client_app.py](./examples_py_ros2_rpc_client_app.py) +- [examples_py_ros2_rpc_server_app.py](./examples_py_ros2_rpc_server_app.py) + + +配置文件： +- [examples_py_ros2_rpc_grpc_client_cfg.yaml](./cfg/examples_py_ros2_rpc_grpc_client_cfg.yaml) +- [examples_py_ros2_rpc_grpc_server_cfg.yaml](./cfg/examples_py_ros2_rpc_grpc_server_cfg.yaml) + + + +运行方式（linux）： +- [安装 `aimrt_py` 包](../../../../document/sphinx-cn/tutorials/quick_start/installation_py.md)； +- 运行本目录下的[build_examples_py_ros2_rpc.sh](./build_examples_py_ros2_rpc.sh)脚本，生成协议桩代码文件； + - 注意生成的 rpc 桩代码文件会引用 `example_ros2` 包，请确保本地安装有 ROS2 Humble，然后打开选项编译本项目生成对应的 `example_ros2` 包，否则无法正常运行本示例； +- 运行本目录下的[start_examples_py_ros2_rpc_grpc_server.sh](./start_examples_py_ros2_rpc_grpc_server.sh)脚本，启动 RPC Server； +- 在新终端里运行本目录下的[start_examples_py_ros2_rpc_grpc_client.sh](./start_examples_py_ros2_rpc_grpc_client.sh)脚本，启动 Rpc client 发起一次请求； +- 向 RPC Server 进程所在终端里键入`ctrl-c`以停止进程； + + +说明： +- 此示例创建了以下两个模块： + - `NormalRpcClientPyModule`：会在启动后向 `RosTestRpc` 发起一次 `RosTestRpc` 的 RPC 请求，等收到回包并解析后结束进程； + - `NormalRpcServerPymodule`：会注册 `RosTestRpc` 服务端，通过 Server 接口，打印接收到的数据； +- 此示例使用 grpc 类型的 rpc 后端进行通信，请确保相关端口未被占用； + + + +## ros2 rpc ros2 + + + +一个基于 ros2 协议与 ros2 后端的 python rpc 示例，演示内容包括： +- 如何在 python 中使用 ros2 协议作为 rpc 传输协议； +- 如何基于 aimrt_py 创建模块的方式使用 rpc client 和 server 功能； +- 如何使用 ros2 类型的 rpc 后端； + + +核心代码： +- [RosTestRpc.srv](../../../protocols/example_ros2/srv/RosTestRpc.srv) +- [examples_py_ros2_rpc_client_app.py](./examples_py_ros2_rpc_client_app.py) +- [examples_py_ros2_rpc_server_app.py](./examples_py_ros2_rpc_server_app.py) + + +配置文件： +- [examples_py_ros2_rpc_ros2_client_cfg.yaml](./cfg/examples_py_ros2_rpc_ros2_client_cfg.yaml) +- [examples_py_ros2_rpc_ros2_server_cfg.yaml](./cfg/examples_py_ros2_rpc_ros2_server_cfg.yaml) + + + +运行方式（linux）： +- [安装 `aimrt_py` 包](../../../../document/sphinx-cn/tutorials/quick_start/installation_py.md)； +- 运行本目录下的[build_examples_py_ros2_rpc.sh](./build_examples_py_ros2_rpc.sh)脚本，生成协议桩代码文件； + - 注意生成的 rpc 桩代码文件会引用 `example_ros2` 包，请确保本地安装有 ROS2 Humble，然后打开选项编译本项目生成对应的 `example_ros2` 包，否则无法正常运行本示例； +- 运行本目录下的[start_examples_py_ros2_rpc_ros2_server.sh](./start_examples_py_ros2_rpc_ros2_server.sh)脚本，启动 RPC Server； +- 在新终端里运行本目录下的[start_examples_py_ros2_rpc_ros2_client.sh](./start_examples_py_ros2_rpc_ros2_client.sh)脚本，启动 Rpc client 发起一次请求； +- 向 RPC Server 进程所在终端里键入`ctrl-c`以停止进程； + + +说明： +- 此示例创建了以下两个模块： + - `NormalRpcClientPyModule`：会在启动后向 `RosTestRpc` 发起一次 `RosTestRpc` 的 RPC 请求，等收到回包并解析后结束进程； + - `NormalRpcServerPymodule`：会注册 `RosTestRpc` 服务端，通过 Server 接口，打印接收到的数据； +- 此示例使用 ros2 类型的 rpc 后端进行通信，请确保本地安装有 ROS2 Humble； diff --git a/src/examples/py/ros2_rpc/build_examples_py_ros2_rpc.sh b/src/examples/py/ros2_rpc/build_examples_py_ros2_rpc.sh new file mode 100755 index 000000000..6e027ab3e --- /dev/null +++ b/src/examples/py/ros2_rpc/build_examples_py_ros2_rpc.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +protocols_dir=../../../protocols/example_ros2/ + +aimrt_py-gen-ros2-rpc --pkg_name=example_ros2 --srv_file=${protocols_dir}/RosTestRpc.srv --output_path=./ diff --git a/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_grpc_client_cfg.yaml b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_grpc_client_cfg.yaml new file mode 100644 index 000000000..c385c2427 --- /dev/null +++ b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_grpc_client_cfg.yaml @@ -0,0 +1,31 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +aimrt: + plugin: + plugins: + - name: grpc_plugin + path: ${AIMRT_PLUGIN_DIR}/libaimrt_grpc_plugin.so + options: + thread_num: 4 + listen_ip: 127.0.0.1 + listen_port: 50051 + log: + core_lvl: INFO # Trace/Debug/Info/Warn/Error/Fatal/Off + default_module_lvl: INFO + backends: + - type: console + rpc: + backends: + - type: grpc + options: + clients_options: + - func_name: "(.*)" + server_url: http://127.0.0.1:50050 + clients_options: + - func_name: "(.*)" + enable_backends: [grpc] + module: + modules: + - name: NormalRpcClientPyModule + log_lvl: INFO diff --git a/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_grpc_server_cfg.yaml b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_grpc_server_cfg.yaml new file mode 100644 index 000000000..633a91c2f --- /dev/null +++ b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_grpc_server_cfg.yaml @@ -0,0 +1,27 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +aimrt: + plugin: + plugins: + - name: grpc_plugin + path: ${AIMRT_PLUGIN_DIR}/libaimrt_grpc_plugin.so + options: + thread_num: 4 + listen_ip: 127.0.0.1 + listen_port: 50050 + log: + core_lvl: INFO # Trace/Debug/Info/Warn/Error/Fatal/Off + default_module_lvl: INFO + backends: + - type: console + rpc: + backends: + - type: grpc + servers_options: + - func_name: "(.*)" + enable_backends: [grpc] + module: + modules: + - name: NormalRpcServerPyModule + log_lvl: INFO diff --git a/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_http_client_cfg.yaml b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_http_client_cfg.yaml new file mode 100644 index 000000000..585e4f408 --- /dev/null +++ b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_http_client_cfg.yaml @@ -0,0 +1,32 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +aimrt: + plugin: + plugins: + - name: net_plugin + path: ${AIMRT_PLUGIN_DIR}/libaimrt_net_plugin.so + options: + thread_num: 4 + http_options: + listen_ip: 127.0.0.1 + listen_port: 50081 + log: + core_lvl: Trace # Trace/Debug/Info/Warn/Error/Fatal/Off + default_module_lvl: INFO + backends: + - type: console + rpc: + backends: + - type: http + options: + clients_options: + - func_name: "(.*)" + server_url: http://127.0.0.1:50080 + clients_options: + - func_name: "(.*)" + enable_backends: [http] + module: + modules: + - name: NormalRpcClientPyModule + log_lvl: INFO diff --git a/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_http_server_cfg.yaml b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_http_server_cfg.yaml new file mode 100644 index 000000000..a0277f31d --- /dev/null +++ b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_http_server_cfg.yaml @@ -0,0 +1,28 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +aimrt: + plugin: + plugins: + - name: net_plugin + path: ${AIMRT_PLUGIN_DIR}/libaimrt_net_plugin.so + options: + thread_num: 4 + http_options: + listen_ip: 127.0.0.1 + listen_port: 50080 + log: + core_lvl: INFO # Trace/Debug/Info/Warn/Error/Fatal/Off + default_module_lvl: INFO + backends: + - type: console + rpc: + backends: + - type: http + servers_options: + - func_name: "(.*)" + enable_backends: [http] + module: + modules: + - name: NormalRpcServerPyModule + log_lvl: INFO diff --git a/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_ros2_client_cfg.yaml b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_ros2_client_cfg.yaml new file mode 100644 index 000000000..26f66426a --- /dev/null +++ b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_ros2_client_cfg.yaml @@ -0,0 +1,27 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +aimrt: + plugin: + plugins: + - name: ros2_plugin + path: ${AIMRT_PLUGIN_DIR}/libaimrt_ros2_plugin.so + options: + node_name: example_ros2_rpc_client_node + executor_type: MultiThreaded # SingleThreaded/StaticSingleThreaded/MultiThreaded + executor_thread_num: 2 + log: + core_lvl: INFO # Trace/Debug/Info/Warn/Error/Fatal/Off + default_module_lvl: INFO + backends: + - type: console + rpc: + backends: + - type: ros2 + clients_options: + - func_name: "(.*)" + enable_backends: [ros2] + module: + modules: + - name: NormalRpcClientPyModule + log_lvl: INFO diff --git a/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_ros2_server_cfg.yaml b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_ros2_server_cfg.yaml new file mode 100644 index 000000000..23ae2a07c --- /dev/null +++ b/src/examples/py/ros2_rpc/cfg/examples_py_ros2_rpc_ros2_server_cfg.yaml @@ -0,0 +1,27 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +aimrt: + plugin: + plugins: + - name: ros2_plugin + path: ${AIMRT_PLUGIN_DIR}/libaimrt_ros2_plugin.so + options: + node_name: example_ros2_rpc_server_node + executor_type: MultiThreaded # SingleThreaded/StaticSingleThreaded/MultiThreaded + executor_thread_num: 2 + log: + core_lvl: Info # Trace/Debug/Info/Warn/Error/Fatal/Off + default_module_lvl: INFO + backends: + - type: console + rpc: + backends: + - type: ros2 + servers_options: + - func_name: "(.*)" + enable_backends: [ros2] + module: + modules: + - name: NormalRpcServerPyModule + log_lvl: INFO diff --git a/src/examples/py/ros2_rpc/debug_examples_py_ros2_rpc_ros2_client.sh b/src/examples/py/ros2_rpc/debug_examples_py_ros2_rpc_ros2_client.sh new file mode 100755 index 000000000..37e8ad70f --- /dev/null +++ b/src/examples/py/ros2_rpc/debug_examples_py_ros2_rpc_ros2_client.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +export AIMRT_PLUGIN_DIR=$(pip show aimrt_py | grep Location | awk '{print $2}')/aimrt_py + +export AMENT_CURRENT_PREFIX=${AIMRT_PLUGIN_DIR} +source ${AIMRT_PLUGIN_DIR}/share/example_ros2/local_setup.bash +source ${AIMRT_PLUGIN_DIR}/share/ros2_plugin_proto/local_setup.bash + +gdb --args python3 ./examples_py_ros2_rpc_client_app.py --cfg_file_path=./cfg/examples_py_ros2_rpc_ros2_client_cfg.yaml diff --git a/src/examples/py/ros2_rpc/examples_py_ros2_rpc_client_app.py b/src/examples/py/ros2_rpc/examples_py_ros2_rpc_client_app.py new file mode 100644 index 000000000..b1d9730f0 --- /dev/null +++ b/src/examples/py/ros2_rpc/examples_py_ros2_rpc_client_app.py @@ -0,0 +1,71 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +import argparse +import datetime +import threading +import time + +import aimrt_py +import example_ros2.srv +import RosTestRpc_aimrt_rpc_ros2 + + +def main(): + parser = argparse.ArgumentParser(description='Example rpc client app.') + parser.add_argument('--cfg_file_path', type=str, default="", help='config file path') + args = parser.parse_args() + + print("AimRT start.") + + aimrt_core = aimrt_py.Core() + + # Initialize + core_options = aimrt_py.CoreOptions() + core_options.cfg_file_path = args.cfg_file_path + aimrt_core.Initialize(core_options) + + # Create Module + module_handle = aimrt_core.CreateModule("NormalRpcClientPyModule") + + # Register rpc client + rpc_handle = module_handle.GetRpcHandle() + ret = RosTestRpc_aimrt_rpc_ros2.RosTestRpcServiceProxy.RegisterClientFunc(rpc_handle) + assert ret, "Register client failed." + + # Start + thread = threading.Thread(target=aimrt_core.Start) + thread.start() + + # Sleep for seconds + time.sleep(1) + + # Call rpc + proxy = RosTestRpc_aimrt_rpc_ros2.RosTestRpcServiceProxy(rpc_handle) + + req = example_ros2.srv.RosTestRpc.Request() + req.data = [bytes([b]) for b in b"Hello AimRT!"] + + ctx = aimrt_py.RpcContext() + ctx.SetTimeout(datetime.timedelta(seconds=30)) + ctx.SetMetaValue("key1", "value1") + ctx.SetSerializationType("ros2") + ctx_ref = aimrt_py.RpcContextRef(ctx) + + status, rsp = proxy.RosTestRpc(ctx_ref, req) + + aimrt_py.info(module_handle.GetLogger(), + f"Call rpc done, status: {status.ToString()}, " + f"req: {req}, " + f"rsp: {rsp}") + + # Shutdown + aimrt_core.Shutdown() + + thread.join() + + print("AimRT exit.") + + +if __name__ == '__main__': + main() diff --git a/src/examples/py/ros2_rpc/examples_py_ros2_rpc_server_app.py b/src/examples/py/ros2_rpc/examples_py_ros2_rpc_server_app.py new file mode 100644 index 000000000..6f57b888d --- /dev/null +++ b/src/examples/py/ros2_rpc/examples_py_ros2_rpc_server_app.py @@ -0,0 +1,92 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +import argparse +import signal +import sys +import threading + +import aimrt_py +import example_ros2.msg +import example_ros2.srv +import RosTestRpc_aimrt_rpc_ros2 + +global_aimrt_core = None + + +def signal_handler(sig, frame): + global global_aimrt_core + + if (global_aimrt_core and (sig == signal.SIGINT or sig == signal.SIGTERM)): + global_aimrt_core.Shutdown() + return + + sys.exit(0) + + +class RosTestRpcImpl(RosTestRpc_aimrt_rpc_ros2.RosTestRpcService): + def __init__(self, logger): + super().__init__() + self.logger = logger + + @staticmethod + def PrintMetaInfo(logger, ctx_ref): + meta_keys = ctx_ref.GetMetaKeys() + for key in meta_keys: + aimrt_py.info(logger, f"meta key: {key}, value: {ctx_ref.GetMetaValue(key)}") + + def RosTestRpc(self, ctx_ref, req): + rsp = example_ros2.srv.RosTestRpc.Response() + rsp.code = 1000 + + RosTestRpcImpl.PrintMetaInfo(self.logger, ctx_ref) + aimrt_py.info(self.logger, + f"Server handle new rpc call. " + f"context: {ctx_ref.ToString()}, " + f"req: {req}, " + f"return rsp: {rsp}") + + return aimrt_py.RpcStatus(), rsp + + +def main(): + + parser = argparse.ArgumentParser(description='Example rpc server app.') + parser.add_argument('--cfg_file_path', type=str, default="", help='config file path') + args = parser.parse_args() + + signal.signal(signal.SIGINT, signal_handler) + signal.signal(signal.SIGTERM, signal_handler) + + print("AimRT start.") + + aimrt_core = aimrt_py.Core() + + global global_aimrt_core + global_aimrt_core = aimrt_core + + # Initialize + core_options = aimrt_py.CoreOptions() + core_options.cfg_file_path = args.cfg_file_path + aimrt_core.Initialize(core_options) + + # Create Module + module_handle = aimrt_core.CreateModule("NormalRpcServerPyModule") + + # Register rpc service + service = RosTestRpcImpl(module_handle.GetLogger()) + ret = module_handle.GetRpcHandle().RegisterService(service) + assert ret, "Register service failed." + + # Start + thread = threading.Thread(target=aimrt_core.Start) + thread.start() + + while thread.is_alive(): + thread.join(1.0) + + print("AimRT exit.") + + +if __name__ == '__main__': + main() diff --git a/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_grpc_client.sh b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_grpc_client.sh new file mode 100755 index 000000000..55337539b --- /dev/null +++ b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_grpc_client.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +export AIMRT_PLUGIN_DIR=$(pip show aimrt_py | grep Location | awk '{print $2}')/aimrt_py +source $AIMRT_PLUGIN_DIR/share/example_ros2/local_setup.bash + +python3 ./examples_py_ros2_rpc_client_app.py --cfg_file_path=./cfg/examples_py_ros2_rpc_grpc_client_cfg.yaml diff --git a/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_grpc_server.sh b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_grpc_server.sh new file mode 100755 index 000000000..532989d36 --- /dev/null +++ b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_grpc_server.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +export AIMRT_PLUGIN_DIR=$(pip show aimrt_py | grep Location | awk '{print $2}')/aimrt_py +source $AIMRT_PLUGIN_DIR/share/example_ros2/local_setup.bash + +python3 ./examples_py_ros2_rpc_server_app.py --cfg_file_path=./cfg/examples_py_ros2_rpc_grpc_server_cfg.yaml diff --git a/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_http_client.sh b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_http_client.sh new file mode 100755 index 000000000..f774ddafc --- /dev/null +++ b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_http_client.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +export AIMRT_PLUGIN_DIR=$(pip show aimrt_py | grep Location | awk '{print $2}')/aimrt_py +source $AIMRT_PLUGIN_DIR/share/example_ros2/local_setup.bash + +python3 ./examples_py_ros2_rpc_client_app.py --cfg_file_path=./cfg/examples_py_ros2_rpc_http_client_cfg.yaml diff --git a/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_http_server.sh b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_http_server.sh new file mode 100755 index 000000000..6e75d0e36 --- /dev/null +++ b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_http_server.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +export AIMRT_PLUGIN_DIR=$(pip show aimrt_py | grep Location | awk '{print $2}')/aimrt_py +source $AIMRT_PLUGIN_DIR/share/example_ros2/local_setup.bash + +python3 ./examples_py_ros2_rpc_server_app.py --cfg_file_path=./cfg/examples_py_ros2_rpc_http_server_cfg.yaml diff --git a/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_ros2_client.sh b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_ros2_client.sh new file mode 100755 index 000000000..b8eada5a3 --- /dev/null +++ b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_ros2_client.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +export AIMRT_PLUGIN_DIR=$(pip show aimrt_py | grep Location | awk '{print $2}')/aimrt_py + +export AMENT_CURRENT_PREFIX=${AIMRT_PLUGIN_DIR} +source ${AIMRT_PLUGIN_DIR}/share/example_ros2/local_setup.bash +source ${AIMRT_PLUGIN_DIR}/share/ros2_plugin_proto/local_setup.bash + +python3 ./examples_py_ros2_rpc_client_app.py --cfg_file_path=./cfg/examples_py_ros2_rpc_ros2_client_cfg.yaml diff --git a/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_ros2_server.sh b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_ros2_server.sh new file mode 100755 index 000000000..8ad8834e1 --- /dev/null +++ b/src/examples/py/ros2_rpc/start_examples_py_ros2_rpc_ros2_server.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +export AIMRT_PLUGIN_DIR=$(pip show aimrt_py | grep Location | awk '{print $2}')/aimrt_py + +export AMENT_CURRENT_PREFIX=${AIMRT_PLUGIN_DIR} +source ${AIMRT_PLUGIN_DIR}/share/example_ros2/local_setup.bash +source ${AIMRT_PLUGIN_DIR}/share/ros2_plugin_proto/local_setup.bash + +python3 ./examples_py_ros2_rpc_server_app.py --cfg_file_path=./cfg/examples_py_ros2_rpc_ros2_server_cfg.yaml diff --git a/src/interface/aimrt_module_ros2_interface/util/ros2_type_support.h b/src/interface/aimrt_module_ros2_interface/util/ros2_type_support.h index 46f11f730..f19e492fd 100644 --- a/src/interface/aimrt_module_ros2_interface/util/ros2_type_support.h +++ b/src/interface/aimrt_module_ros2_interface/util/ros2_type_support.h @@ -97,7 +97,7 @@ const aimrt_type_support_base_t* GetRos2MessageTypeSupport() { if (aimrt::util::ToStdStringView(serialization_type) == "ros2") { if (buffer_array_view.len == 1) { rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); - serialized_msg.buffer = const_cast(static_cast(buffer_array_view.data[0].data)), + serialized_msg.buffer = const_cast(static_cast(buffer_array_view.data[0].data)); serialized_msg.buffer_length = buffer_array_view.data[0].len; serialized_msg.buffer_capacity = buffer_array_view.data[0].len; rcl_ret_t ret = rmw_deserialize(&serialized_msg, ts_ptr, msg); @@ -120,7 +120,7 @@ const aimrt_type_support_base_t* GetRos2MessageTypeSupport() { } rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); - serialized_msg.buffer = buffer, + serialized_msg.buffer = buffer; serialized_msg.buffer_length = total_size; serialized_msg.buffer_capacity = total_size; rcl_ret_t ret = rmw_deserialize(&serialized_msg, ts_ptr, msg); diff --git a/src/runtime/python_runtime/CMakeLists.txt b/src/runtime/python_runtime/CMakeLists.txt index ef3ae967b..ecfc3de73 100644 --- a/src/runtime/python_runtime/CMakeLists.txt +++ b/src/runtime/python_runtime/CMakeLists.txt @@ -13,8 +13,13 @@ set(CUR_TARGET_NAME ${CUR_SUPERIOR_NAMESPACE_UNDERLINE}_${CUR_DIR}) set(CUR_TARGET_ALIAS_NAME ${CUR_SUPERIOR_NAMESPACE}::${CUR_DIR}) # Set file collection -file(GLOB_RECURSE src ${CMAKE_CURRENT_SOURCE_DIR}/*.cc) -file(GLOB_RECURSE py_files ${CMAKE_CURRENT_SOURCE_DIR}/*.py) +file(GLOB_RECURSE src CONFIGURE_DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/*.cc) +file(GLOB_RECURSE py_files CONFIGURE_DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/*.py) +file(GLOB_RECURSE ros2_files CONFIGURE_DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/ros2_type_support_utils.cc) + +if(NOT AIMRT_BUILD_WITH_ROS2) + list(REMOVE_ITEM src ${ros2_files}) +endif() # Add target pybind11_add_module(${CUR_TARGET_NAME} SHARED) @@ -28,6 +33,12 @@ target_link_libraries( ${CUR_TARGET_NAME} PRIVATE $) +# Link ros2 interface if building with ros2 +if(AIMRT_BUILD_WITH_ROS2) + target_link_libraries(${CUR_TARGET_NAME} PRIVATE aimrt::interface::aimrt_module_ros2_interface) + target_compile_definitions(${CUR_TARGET_NAME} PRIVATE AIMRT_BUILD_WITH_ROS2) +endif() + # Set misc of target set_target_properties(${CUR_TARGET_NAME} PROPERTIES OUTPUT_NAME "aimrt_python_runtime") diff --git a/src/runtime/python_runtime/__init__.py b/src/runtime/python_runtime/__init__.py index 6131254f5..c2dff6229 100644 --- a/src/runtime/python_runtime/__init__.py +++ b/src/runtime/python_runtime/__init__.py @@ -1,6 +1,9 @@ # Copyright (c) 2023, AgiBot Inc. # All rights reserved. +from .aimrt_py_chn import (GetPbMessageTypeName, GetRos2MessageTypeName, + Publish, RegisterPublishType, Subscribe) from .aimrt_py_log import * -from .aimrt_py_pb_chn import Publish, RegisterPublishType, Subscribe from .aimrt_python_runtime import * +from .check_ros2_type import (check_is_valid_ros2_msg_type, + check_is_valid_srv_type) diff --git a/src/runtime/python_runtime/aimrt_py_chn.py b/src/runtime/python_runtime/aimrt_py_chn.py new file mode 100644 index 000000000..3ec002c94 --- /dev/null +++ b/src/runtime/python_runtime/aimrt_py_chn.py @@ -0,0 +1,206 @@ +# Copyright (c) 2023, AgiBot Inc. +# All rights reserved. + +import inspect +import sys +from typing import Callable, TypeVar + +import google._upb._message +import google.protobuf +import google.protobuf.message + +from . import aimrt_python_runtime +from .check_ros2_type import check_is_valid_ros2_msg_type + +Ros2MsgType = TypeVar("Ros2MsgType") + + +def _SerializeProtobufMessage(pb_msg: google.protobuf.message.Message, serialization_type: str) -> bytes: + if serialization_type == "pb": + return pb_msg.SerializeToString() + elif serialization_type == "json": + return google.protobuf.json_format.MessageToJson(pb_msg) + else: + raise ValueError(f"Invalid serialization type: {serialization_type}") + + +def _DeserializeProtobufMessage(msg_buf: bytes, + serialization_type: str, + protobuf_type: google.protobuf.message.Message) -> google.protobuf.message.Message: + msg = protobuf_type() + if serialization_type == "pb": + msg.ParseFromString(msg_buf) + elif serialization_type == "json": + google.protobuf.json_format.Parse(msg_buf, msg) + else: + raise ValueError(f"Invalid serialization type: {serialization_type}") + return msg + + +def _CreateContextRef(ctx_or_type, default_serialization_type: str) -> aimrt_python_runtime.ContextRef: + if isinstance(ctx_or_type, aimrt_python_runtime.Context): + ctx_ref = aimrt_python_runtime.ContextRef(ctx_or_type) + elif isinstance(ctx_or_type, aimrt_python_runtime.ContextRef): + ctx_ref = ctx_or_type + else: + serialization_type = ctx_or_type if isinstance(ctx_or_type, str) else default_serialization_type + ctx = aimrt_python_runtime.Context() + ctx.SetSerializationType(serialization_type) + ctx_ref = aimrt_python_runtime.ContextRef(ctx) + + if ctx_ref.GetSerializationType() == "": + ctx_ref.SetSerializationType(default_serialization_type) + + return ctx_ref + + +def GetRos2MessageTypeName(msg_type: Ros2MsgType) -> str: + module_parts = msg_type.__module__.split('.') + module_name = '/'.join(module_parts[:-1]) + return "ros2:" + "/".join([module_name, msg_type.__name__]) + + +def GetPbMessageTypeName(msg: google._upb._message.MessageMeta) -> str: + return f"pb:{msg.DESCRIPTOR.full_name}" + + +def RegisterPublishType(publisher: aimrt_python_runtime.PublisherRef, + msg_type: google._upb._message.MessageMeta | Ros2MsgType): + """Register a protobuf message type to a publisher. + + Args: + publisher (aimrt_python_runtime.PublisherRef): channel publisher + msg_type (google.protobuf.message.Message | Ros2MsgType): protobuf message type or ROS2 message type + + Returns: + bool: True if success, False otherwise + """ + if isinstance(msg_type, google._upb._message.MessageMeta): + py_pb_ts = aimrt_python_runtime.PyPbTypeSupport() + py_pb_ts.SetTypeName(GetPbMessageTypeName(msg_type)) + py_pb_ts.SetSerializationTypesSupportedList(["pb", "json"]) + return publisher.PbRegisterPublishType(py_pb_ts) + elif check_is_valid_ros2_msg_type(msg_type): + py_ros2_ts = aimrt_python_runtime.PyRos2TypeSupport(msg_type) + py_ros2_ts.SetTypeName(GetRos2MessageTypeName(msg_type)) + py_ros2_ts.SetSerializationTypesSupportedList(["ros2"]) + return publisher.Ros2RegisterPublishType(py_ros2_ts) + else: + raise TypeError(f"Invalid message type: {type(msg_type)}") + + +def Publish(publisher: aimrt_python_runtime.PublisherRef, second, third=None): + """Publish a message to a channel. + + This function can be called in following ways: + - Publish(publisher, msg) + - Publish(publisher, msg, ctx) + - Publish(publisher, msg, serialization_type) + - Publish(publisher, ctx, msg) + - Publish(publisher, serialization_type, msg) + + msg: google.protobuf.message.Message | Ros2MsgType + ctx: aimrt_python_runtime.Context or aimrt_python_runtime.ContextRef + serialization_type: str + + Args: + publisher (aimrt_python_runtime.PublisherRef): channel publisher + second: msg or ctx or serialization_type + third: msg or ctx or serialization_type or None + Raises: + TypeError: if the arguments are invalid + """ + if isinstance(second, google.protobuf.message.Message): + msg = second + ctx_or_type = third + message_type = "pb" + elif isinstance(third, google.protobuf.message.Message): + msg = third + ctx_or_type = second + message_type = "pb" + elif check_is_valid_ros2_msg_type(second): + msg = second + ctx_or_type = third + message_type = "ros2" + elif check_is_valid_ros2_msg_type(third): + msg = third + ctx_or_type = second + message_type = "ros2" + else: + raise TypeError("Invalid arguments, no protobuf message or ROS2 message found") + + if not isinstance(ctx_or_type, (aimrt_python_runtime.Context, aimrt_python_runtime.ContextRef, str)) and \ + ctx_or_type is not None: + raise TypeError( + f"Invalid context type: {type(ctx_or_type)}, " + f"only 'aimrt_python_runtime.Context' or 'aimrt_python_runtime.ContextRef' or 'str' is supported") + + if message_type == "pb": + ctx_ref = _CreateContextRef(ctx_or_type, default_serialization_type="pb") + serialized_msg = _SerializeProtobufMessage(msg, ctx_ref.GetSerializationType()) + publisher.PbPublishWithCtx(GetPbMessageTypeName(msg.__class__), ctx_ref, serialized_msg) + elif message_type == "ros2": + ctx_ref = _CreateContextRef(ctx_or_type, default_serialization_type="ros2") + publisher.Ros2PublishWithCtx(GetRos2MessageTypeName(msg.__class__), ctx_ref, msg) + + +def Subscribe(subscriber: aimrt_python_runtime.SubscriberRef, + msg_type: google._upb._message.MessageMeta | Ros2MsgType, + callback: Callable): + """Subscribe a message from a channel. + + Args: + subscriber (aimrt_python_runtime.SubscriberRef): channel subscriber + msg_type: protobuf message type or ROS2 message type + callback (Callable): callback function + + Raises: + ValueError: if the callback is invalid + TypeError: if the message type is invalid + + Callback function signature (both for Protobuf and ROS2 messages): + - callback(msg) + - callback(ctx, msg) + """ + # Check callback signature + sig = inspect.signature(callback) + required_param_count = sum(1 for param in sig.parameters.values() if param.default == param.empty) + + if not (1 <= required_param_count <= 2): + raise ValueError("Invalid callback: expected 1 or 2 parameters, with at most one optional parameter") + + if isinstance(msg_type, google._upb._message.MessageMeta): + py_pb_ts = aimrt_python_runtime.PyPbTypeSupport() + py_pb_ts.SetTypeName(GetPbMessageTypeName(msg_type)) + py_pb_ts.SetSerializationTypesSupportedList(["pb", "json"]) + + def handle_callback(ctx_ref: aimrt_python_runtime.ContextRef, msg_buf: bytes): + try: + msg = _DeserializeProtobufMessage(msg_buf, ctx_ref.GetSerializationType(), msg_type) + if required_param_count == 1: + callback(msg) + else: + callback(ctx_ref, msg) + except Exception as e: + print(f"AimRT channel handle get exception, {e}", file=sys.stderr) + + subscriber.PbSubscribeWithCtx(py_pb_ts, handle_callback) + + elif check_is_valid_ros2_msg_type(msg_type): + py_ros2_ts = aimrt_python_runtime.PyRos2TypeSupport(msg_type) + py_ros2_ts.SetTypeName(GetRos2MessageTypeName(msg_type)) + py_ros2_ts.SetSerializationTypesSupportedList(["ros2"]) + + def ros2_callback_wrapper(ctx_ref: aimrt_python_runtime.ContextRef, msg): + try: + if required_param_count == 1: + callback(msg) + else: + callback(ctx_ref, msg) + except Exception as e: + print(f"AimRT channel handle get exception, {e}", file=sys.stderr) + + subscriber.Ros2SubscribeWithCtx(py_ros2_ts, msg_type, ros2_callback_wrapper) + + else: + raise TypeError(f"Invalid message type: {type(msg_type)}, expected Protobuf or ROS2 message type") diff --git a/src/runtime/python_runtime/aimrt_py_pb_chn.py b/src/runtime/python_runtime/aimrt_py_pb_chn.py deleted file mode 100644 index d53fbccf8..000000000 --- a/src/runtime/python_runtime/aimrt_py_pb_chn.py +++ /dev/null @@ -1,142 +0,0 @@ -# Copyright (c) 2023, AgiBot Inc. -# All rights reserved. - -import inspect -from typing import Callable - -import google.protobuf -import google.protobuf.message - -from . import aimrt_python_runtime - - -def _SerializeProtobufMessage(pb_msg: google.protobuf.message.Message, serialization_type: str) -> bytes: - if serialization_type == "pb": - return pb_msg.SerializeToString() - elif serialization_type == "json": - return google.protobuf.json_format.MessageToJson(pb_msg) - else: - raise ValueError(f"Invalid serialization type: {serialization_type}") - - -def _DeserializeProtobufMessage(msg_buf: bytes, - serialization_type: str, - protobuf_type: google.protobuf.message.Message) -> google.protobuf.message.Message: - msg = protobuf_type() - if serialization_type == "pb": - msg.ParseFromString(msg_buf) - elif serialization_type == "json": - google.protobuf.json_format.Parse(msg_buf, msg) - else: - raise ValueError(f"Invalid serialization type: {serialization_type}") - return msg - - -def RegisterPublishType(publisher: aimrt_python_runtime.PublisherRef, protobuf_type: google.protobuf.message.Message): - """Register a protobuf message type to a publisher. - - Args: - publisher (aimrt_python_runtime.PublisherRef): channel publisher - protobuf_type (google.protobuf.message.Message): protobuf message type - - Returns: - bool: True if success, False otherwise - """ - aimrt_ts = aimrt_python_runtime.TypeSupport() - aimrt_ts.SetTypeName("pb:" + protobuf_type.DESCRIPTOR.full_name) - aimrt_ts.SetSerializationTypesSupportedList(["pb", "json"]) - - return publisher.RegisterPublishType(aimrt_ts) - - -def Publish(publisher: aimrt_python_runtime.PublisherRef, second, third=None): - """Publish a message to a channel. - - This function can be called in following ways: - - Publish(publisher, pb_msg) - - Publish(publisher, pb_msg, ctx) - - Publish(publisher, pb_msg, serialization_type) - - Publish(publisher, ctx, pb_msg) - - Publish(publisher, serialization_type, pb_msg) - - pb_msg: google.protobuf.message.Message - ctx: aimrt_python_runtime.Context or aimrt_python_runtime.ContextRef - serialization_type: str - - Args: - publisher (aimrt_python_runtime.PublisherRef): channel publisher - second: pb_msg or ctx or serialization_type - third: pb_msg or ctx or serialization_type or None - Raises: - TypeError: if the arguments are invalid - """ - if isinstance(second, google.protobuf.message.Message): - pb_msg = second - ctx = third - elif isinstance(third, google.protobuf.message.Message): - pb_msg = third - ctx = second - else: - raise TypeError("Invalid arguments, no protobuf message found") - - if isinstance(ctx, (aimrt_python_runtime.Context, aimrt_python_runtime.ContextRef)): - if isinstance(ctx, aimrt_python_runtime.Context): - ctx_ref = aimrt_python_runtime.ContextRef(ctx) - else: - ctx_ref = ctx - if ctx_ref.GetSerializationType() == "": - ctx_ref.SetSerializationType("pb") - serialized_msg = _SerializeProtobufMessage(pb_msg, ctx_ref.GetSerializationType()) - publisher.PublishWithCtx(f"pb:{pb_msg.DESCRIPTOR.full_name}", ctx_ref, serialized_msg) - elif isinstance(ctx, str): - serialization_type = ctx - serialized_msg = _SerializeProtobufMessage(pb_msg, serialization_type) - publisher.PublishWithSerializationType(f"pb:{pb_msg.DESCRIPTOR.full_name}", serialization_type, serialized_msg) - elif ctx is None: - # default use pb serialization - serialized_msg = _SerializeProtobufMessage(pb_msg, "pb") - publisher.PublishWithSerializationType(f"pb:{pb_msg.DESCRIPTOR.full_name}", "pb", serialized_msg) - else: - raise TypeError( - f"Invalid context type: {type(ctx)}, " - f"only 'aimrt_python_runtime.Context' or 'aimrt_python_runtime.ContextRef' or 'str' is supported") - - -def Subscribe(subscriber: aimrt_python_runtime.SubscriberRef, - protobuf_type: google.protobuf.message.Message, - callback: Callable): - """Subscribe a message from a channel. - - Args: - subscriber (aimrt_python_runtime.SubscriberRef): channel subscriber - protobuf_type (google.protobuf.message.Message): protobuf message type - callback (Callable): callback function - - Raises: - ValueError: if the callback is invalid - - Callback function signature: - - callback(msg) - - callback(ctx, msg) - """ - aimrt_ts = aimrt_python_runtime.TypeSupport() - aimrt_ts.SetTypeName("pb:" + protobuf_type.DESCRIPTOR.full_name) - aimrt_ts.SetSerializationTypesSupportedList(["pb", "json"]) - - sig = inspect.signature(callback) - required_param_count = sum(1 for param in sig.parameters.values() if param.default == param.empty) - - if not (1 <= required_param_count <= 2): - raise ValueError("Invalid callback: expected 1 or 2 parameters, with at most one optional parameter") - - def handle_callback(ctx_ref: aimrt_python_runtime.ContextRef, msg_buf: bytes): - try: - msg = _DeserializeProtobufMessage(msg_buf, ctx_ref.GetSerializationType(), protobuf_type) - if required_param_count == 1: - callback(msg) - else: - callback(ctx_ref, msg) - except Exception as e: - print(f"AimRT channel handle get exception, {e}") - - subscriber.SubscribeWithCtx(aimrt_ts, handle_callback) diff --git a/src/runtime/python_runtime/check_ros2_type.py b/src/runtime/python_runtime/check_ros2_type.py new file mode 100644 index 000000000..d044fca32 --- /dev/null +++ b/src/runtime/python_runtime/check_ros2_type.py @@ -0,0 +1,39 @@ +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + + +def check_for_ros2_type_support(msg_or_srv_type): + try: + ts = msg_or_srv_type.__class__._TYPE_SUPPORT + except AttributeError: + return False + if ts is None: + msg_or_srv_type.__class__.__import_type_support__() + return msg_or_srv_type.__class__._TYPE_SUPPORT is not None + + +def check_is_valid_ros2_msg_type(msg_type): + if not check_for_ros2_type_support(msg_type): + return False + try: + assert None not in ( + msg_type.__class__._CREATE_ROS_MESSAGE, + msg_type.__class__._CONVERT_FROM_PY, + msg_type.__class__._CONVERT_TO_PY, + msg_type.__class__._DESTROY_ROS_MESSAGE, + ) + except (AssertionError, AttributeError): + return False + return True + + +def check_is_valid_srv_type(srv_type): + check_for_ros2_type_support(srv_type) + try: + assert None not in ( + srv_type.Response, + srv_type.Request, + ) + except (AssertionError, AttributeError): + return False + return True diff --git a/src/runtime/python_runtime/export_channel.h b/src/runtime/python_runtime/export_channel.h index a519b9982..6ab18d80a 100644 --- a/src/runtime/python_runtime/export_channel.h +++ b/src/runtime/python_runtime/export_channel.h @@ -3,13 +3,19 @@ #pragma once +#include "pybind11/pybind11.h" + #include #include "aimrt_module_cpp_interface/channel/channel_context.h" #include "aimrt_module_cpp_interface/channel/channel_handle.h" -#include "python_runtime/export_type_support.h" -#include "pybind11/pybind11.h" +#include "python_runtime/export_pb_type_support.h" + +#ifdef AIMRT_BUILD_WITH_ROS2 + #include "python_runtime/export_ros2_type_support.h" + #include "python_runtime/ros2_type_support_utils.h" +#endif namespace aimrt::runtime::python_runtime { @@ -36,9 +42,9 @@ inline void ExportContext(const pybind11::object& m) { pybind11::class_(m, "ContextRef") .def(pybind11::init<>()) - .def(pybind11::init()) - .def(pybind11::init()) - .def(pybind11::init&>()) + .def(pybind11::init(), pybind11::keep_alive<1, 2>()) + .def(pybind11::init(), pybind11::keep_alive<1, 2>()) + .def(pybind11::init&>(), pybind11::keep_alive<1, 2>()) .def("__bool__", &ContextRef::operator bool) .def("CheckUsed", &ContextRef::CheckUsed) .def("SetUsed", &ContextRef::SetUsed) @@ -51,26 +57,16 @@ inline void ExportContext(const pybind11::object& m) { .def("ToString", &ContextRef::ToString); } -inline bool PyRegisterPublishType( +inline bool PbRegisterPublishType( aimrt::channel::PublisherRef& publisher_ref, - const std::shared_ptr& msg_type_support) { - static std::vector> py_ts_vec; - py_ts_vec.emplace_back(msg_type_support); + const std::shared_ptr& msg_type_support) { + static std::vector> py_pb_ts_vec; + py_pb_ts_vec.emplace_back(msg_type_support); return publisher_ref.RegisterPublishType(msg_type_support->NativeHandle()); } -inline void PyPublishWithSerializationType( - aimrt::channel::PublisherRef& publisher_ref, - std::string_view msg_type, - std::string_view serialization_type, - const std::string& msg_buf) { - aimrt::channel::Context ctx; - ctx.SetSerializationType(serialization_type); - publisher_ref.Publish(msg_type, ctx, static_cast(&msg_buf)); -} - -inline void PyPublishWithCtx( +inline void PbPublishWithCtx( aimrt::channel::PublisherRef& publisher_ref, std::string_view msg_type, const aimrt::channel::ContextRef& ctx_ref, @@ -78,24 +74,11 @@ inline void PyPublishWithCtx( publisher_ref.Publish(msg_type, ctx_ref, static_cast(&msg_buf)); } -inline void ExportPublisherRef(pybind11::object m) { - using aimrt::channel::PublisherRef; - - pybind11::class_(std::move(m), "PublisherRef") - .def(pybind11::init<>()) - .def("__bool__", &PublisherRef::operator bool) - .def("RegisterPublishType", &PyRegisterPublishType) - .def("PublishWithSerializationType", &PyPublishWithSerializationType) - .def("PublishWithCtx", &PyPublishWithCtx) - .def("GetTopic", &PublisherRef::GetTopic) - .def("MergeSubscribeContextToPublishContext", &PublisherRef::MergeSubscribeContextToPublishContext); -} - -inline bool PySubscribeWithCtx( +inline bool PbSubscribeWithCtx( aimrt::channel::SubscriberRef& subscriber_ref, - const std::shared_ptr& msg_type_support, + const std::shared_ptr& msg_type_support, std::function&& callback) { - static std::vector> py_ts_vec; + static std::vector> py_ts_vec; py_ts_vec.emplace_back(msg_type_support); return subscriber_ref.Subscribe( @@ -121,14 +104,105 @@ inline bool PySubscribeWithCtx( }); } +#ifdef AIMRT_BUILD_WITH_ROS2 + +inline bool Ros2RegisterPublishType( + aimrt::channel::PublisherRef& publisher_ref, + const std::shared_ptr& py_ros2_type_support) { + static std::vector> py_ros2_ts_vec; + py_ros2_ts_vec.emplace_back(py_ros2_type_support); + + return publisher_ref.RegisterPublishType(py_ros2_type_support->NativeHandle()); +} + +inline void Ros2PublishWithCtx( + aimrt::channel::PublisherRef& publisher_ref, + std::string_view msg_type, + const aimrt::channel::ContextRef& ctx_ref, + pybind11::object msg_obj) { + auto msg_ptr = convert_from_py(msg_obj); + if (!msg_ptr) { + throw py::error_already_set(); + } + + publisher_ref.Publish(msg_type, ctx_ref, static_cast(msg_ptr.get())); +} + +inline bool Ros2SubscribeWithCtx( + aimrt::channel::SubscriberRef& subscriber_ref, + const std::shared_ptr& py_ros2_type_support, + pybind11::object pyclass, + std::function&& callback) { + static std::vector> py_ros2_ts_vec; + py_ros2_ts_vec.emplace_back(py_ros2_type_support); + + pybind11::gil_scoped_acquire acquire; + + pybind11::object pymetaclass = pyclass.attr("__class__"); + auto* capsule_ptr = static_cast(pymetaclass.attr("_CONVERT_TO_PY").cast()); + typedef PyObject* convert_to_py_function(void*); + auto convert = reinterpret_cast(capsule_ptr); + if (!convert) { + throw py::error_already_set(); + } + + pybind11::gil_scoped_release release; + + return subscriber_ref.Subscribe( + py_ros2_type_support->NativeHandle(), + [callback = std::move(callback), convert]( + const aimrt_channel_context_base_t* ctx_ptr, + const void* msg_ptr, + aimrt_function_base_t* release_callback_base) { + aimrt::channel::SubscriberReleaseCallback release_callback(release_callback_base); + + pybind11::gil_scoped_acquire acquire; + + auto msg_obj = pybind11::reinterpret_steal(convert(const_cast(msg_ptr))); + if (!msg_obj) { + throw py::error_already_set(); + } + + auto ctx_ref = aimrt::channel::ContextRef(ctx_ptr); + callback(ctx_ref, msg_obj); + + pybind11::gil_scoped_release release; + + release_callback(); + }); +} + +#endif + +inline void ExportPublisherRef(pybind11::object m) { + using aimrt::channel::PublisherRef; + + pybind11::class_(std::move(m), "PublisherRef") + .def(pybind11::init<>()) + .def("__bool__", &PublisherRef::operator bool) + .def("GetTopic", &PublisherRef::GetTopic) + .def("MergeSubscribeContextToPublishContext", &PublisherRef::MergeSubscribeContextToPublishContext) + .def("PbPublishWithCtx", &PbPublishWithCtx) + .def("PbRegisterPublishType", &PbRegisterPublishType) +#ifdef AIMRT_BUILD_WITH_ROS2 + .def("Ros2PublishWithCtx", &Ros2PublishWithCtx) + .def("Ros2RegisterPublishType", &Ros2RegisterPublishType) +#endif + ; +} + inline void ExportSubscriberRef(pybind11::object m) { using aimrt::channel::SubscriberRef; pybind11::class_(std::move(m), "SubscriberRef") .def(pybind11::init<>()) .def("__bool__", &SubscriberRef::operator bool) - .def("SubscribeWithCtx", &PySubscribeWithCtx) - .def("GetTopic", &SubscriberRef::GetTopic); + .def("GetTopic", &SubscriberRef::GetTopic) + .def("PbSubscribeWithCtx", &PbSubscribeWithCtx) +#ifdef AIMRT_BUILD_WITH_ROS2 + .def("Ros2SubscribeWithCtx", &Ros2SubscribeWithCtx) +#endif + ; } inline void ExportChannelHandleRef(pybind11::object m) { diff --git a/src/runtime/python_runtime/export_configurator.h b/src/runtime/python_runtime/export_configurator.h index f3e4a590c..fb777fb2c 100644 --- a/src/runtime/python_runtime/export_configurator.h +++ b/src/runtime/python_runtime/export_configurator.h @@ -3,12 +3,12 @@ #pragma once +#include "pybind11/pybind11.h" + #include #include "aimrt_module_cpp_interface/configurator/configurator.h" -#include "pybind11/pybind11.h" - namespace aimrt::runtime::python_runtime { inline void ExportConfiguratorRef(pybind11::object m) { diff --git a/src/runtime/python_runtime/export_core.h b/src/runtime/python_runtime/export_core.h index 87b6e73b7..e2c60b836 100644 --- a/src/runtime/python_runtime/export_core.h +++ b/src/runtime/python_runtime/export_core.h @@ -3,12 +3,12 @@ #pragma once +#include "pybind11/pybind11.h" + #include #include "aimrt_module_cpp_interface/core.h" -#include "pybind11/pybind11.h" - namespace aimrt::runtime::python_runtime { inline void ExportModuleInfo(pybind11::object m) { diff --git a/src/runtime/python_runtime/export_core_runtime.h b/src/runtime/python_runtime/export_core_runtime.h index 38245af57..153fe94cc 100644 --- a/src/runtime/python_runtime/export_core_runtime.h +++ b/src/runtime/python_runtime/export_core_runtime.h @@ -3,13 +3,13 @@ #pragma once +#include "pybind11/pybind11.h" + #include #include "aimrt_module_cpp_interface/module_base.h" #include "core/aimrt_core.h" -#include "pybind11/pybind11.h" - namespace aimrt::runtime::python_runtime { inline void ExportCoreOptions(pybind11::object m) { diff --git a/src/runtime/python_runtime/export_executor.h b/src/runtime/python_runtime/export_executor.h index bf5ec8aef..85af044c4 100644 --- a/src/runtime/python_runtime/export_executor.h +++ b/src/runtime/python_runtime/export_executor.h @@ -3,13 +3,14 @@ #pragma once +#include "pybind11/pybind11.h" + #include #include "aimrt_module_cpp_interface/executor/executor_manager.h" #include "pybind11/chrono.h" #include "pybind11/functional.h" -#include "pybind11/pybind11.h" #include "pybind11/stl.h" namespace aimrt::runtime::python_runtime { diff --git a/src/runtime/python_runtime/export_logger.h b/src/runtime/python_runtime/export_logger.h index a86ae2b2c..2c1fc041a 100644 --- a/src/runtime/python_runtime/export_logger.h +++ b/src/runtime/python_runtime/export_logger.h @@ -3,12 +3,12 @@ #pragma once +#include "pybind11/pybind11.h" + #include #include "aimrt_module_cpp_interface/logger/logger.h" -#include "pybind11/pybind11.h" - namespace aimrt::runtime::python_runtime { inline void ExportLoggerRef(pybind11::object m) { diff --git a/src/runtime/python_runtime/export_module_base.h b/src/runtime/python_runtime/export_module_base.h index edeefbd0b..53f388c59 100644 --- a/src/runtime/python_runtime/export_module_base.h +++ b/src/runtime/python_runtime/export_module_base.h @@ -3,12 +3,12 @@ #pragma once +#include "pybind11/pybind11.h" + #include #include "aimrt_module_cpp_interface/module_base.h" -#include "pybind11/pybind11.h" - namespace aimrt::runtime::python_runtime { class PyModuleBaseAdapter : public ModuleBase { diff --git a/src/runtime/python_runtime/export_parameter.h b/src/runtime/python_runtime/export_parameter.h index 74a67eb2e..378b488ae 100644 --- a/src/runtime/python_runtime/export_parameter.h +++ b/src/runtime/python_runtime/export_parameter.h @@ -3,10 +3,10 @@ #pragma once -#include "aimrt_module_cpp_interface/parameter/parameter_handle.h" - #include "pybind11/pybind11.h" +#include "aimrt_module_cpp_interface/parameter/parameter_handle.h" + namespace aimrt::runtime::python_runtime { inline void ExportParameter(pybind11::object m) { @@ -19,4 +19,4 @@ inline void ExportParameter(pybind11::object m) { .def("SetParameter", &ParameterHandleRef::SetParameter); } -} // namespace aimrt::runtime::python_runtime \ No newline at end of file +} // namespace aimrt::runtime::python_runtime diff --git a/src/runtime/python_runtime/export_type_support.h b/src/runtime/python_runtime/export_pb_type_support.h similarity index 76% rename from src/runtime/python_runtime/export_type_support.h rename to src/runtime/python_runtime/export_pb_type_support.h index da81bfdb8..3006657ef 100644 --- a/src/runtime/python_runtime/export_type_support.h +++ b/src/runtime/python_runtime/export_pb_type_support.h @@ -3,6 +3,8 @@ #pragma once +#include "pybind11/pybind11.h" + #include #include @@ -10,16 +12,15 @@ #include "aimrt_module_cpp_interface/util/string.h" #include "pybind11/functional.h" -#include "pybind11/pybind11.h" #include "pybind11/stl.h" namespace aimrt::runtime::python_runtime { -class PyTypeSupport { +class PyPbTypeSupport { public: using BufType = std::string; - PyTypeSupport() : base_(GenBase(this)) {} + PyPbTypeSupport() : base_(GenBase(this)) {} void SetTypeName(const std::string& s) { type_name_ = s; @@ -97,31 +98,31 @@ class PyTypeSupport { static aimrt_type_support_base_t GenBase(void* impl) { return aimrt_type_support_base_t{ .type_name = [](void* impl) -> aimrt_string_view_t { - return aimrt::util::ToAimRTStringView(static_cast(impl)->type_name_); + return aimrt::util::ToAimRTStringView(static_cast(impl)->type_name_); }, .create = [](void* impl) -> void* { - return static_cast(impl)->Create(); + return static_cast(impl)->Create(); }, .destroy = [](void* impl, void* msg) { - static_cast(impl)->Destroy(msg); // + static_cast(impl)->Destroy(msg); // }, .copy = [](void* impl, const void* from, void* to) { - static_cast(impl)->Copy(from, to); // + static_cast(impl)->Copy(from, to); // }, .move = [](void* impl, void* from, void* to) { - static_cast(impl)->Move(from, to); // + static_cast(impl)->Move(from, to); // }, .serialize = [](void* impl, aimrt_string_view_t serialization_type, const void* msg, const aimrt_buffer_array_allocator_t* allocator, aimrt_buffer_array_t* buffer_array) -> bool { - return static_cast(impl)->Serialize(serialization_type, msg, allocator, buffer_array); + return static_cast(impl)->Serialize(serialization_type, msg, allocator, buffer_array); }, .deserialize = [](void* impl, aimrt_string_view_t serialization_type, aimrt_buffer_array_view_t buffer_array_view, void* msg) -> bool { - return static_cast(impl)->Deserialize(serialization_type, buffer_array_view, msg); + return static_cast(impl)->Deserialize(serialization_type, buffer_array_view, msg); }, .serialization_types_supported_num = [](void* impl) -> size_t { - return static_cast(impl)->serialization_types_supported_list_inner_.size(); + return static_cast(impl)->serialization_types_supported_list_inner_.size(); }, .serialization_types_supported_list = [](void* impl) -> const aimrt_string_view_t* { - return static_cast(impl)->serialization_types_supported_list_inner_.data(); + return static_cast(impl)->serialization_types_supported_list_inner_.data(); }, .custom_type_support_ptr = [](void* impl) -> const void* { return nullptr; @@ -136,11 +137,11 @@ class PyTypeSupport { std::vector serialization_types_supported_list_inner_; }; -inline void ExportTypeSupport(pybind11::object m) { - pybind11::class_>(std::move(m), "TypeSupport") +inline void ExportPbTypeSupport(pybind11::object m) { + pybind11::class_>(std::move(m), "PyPbTypeSupport") .def(pybind11::init<>()) - .def("SetTypeName", &PyTypeSupport::SetTypeName) - .def("SetSerializationTypesSupportedList", &PyTypeSupport::SetSerializationTypesSupportedList); + .def("SetTypeName", &PyPbTypeSupport::SetTypeName) + .def("SetSerializationTypesSupportedList", &PyPbTypeSupport::SetSerializationTypesSupportedList); } -} // namespace aimrt::runtime::python_runtime \ No newline at end of file +} // namespace aimrt::runtime::python_runtime diff --git a/src/runtime/python_runtime/export_ros2_type_support.h b/src/runtime/python_runtime/export_ros2_type_support.h new file mode 100644 index 000000000..df4cc9abe --- /dev/null +++ b/src/runtime/python_runtime/export_ros2_type_support.h @@ -0,0 +1,191 @@ +// Copyright(c) 2024 The AimRT Authors. +// AimRT is licensed under Mulan PSL v2. + +#pragma once + +#include "pybind11/pybind11.h" + +#include + +#include "aimrt_module_c_interface/util/type_support_base.h" +#include "aimrt_module_cpp_interface/util/string.h" +#include "aimrt_module_ros2_interface/util/ros2_rcl_serialized_message_adapter.h" + +#include "pybind11/functional.h" +#include "pybind11/stl.h" +#include "rosidl_runtime_cpp/message_type_support_decl.hpp" + +#include "python_runtime/ros2_type_support_utils.h" + +namespace aimrt::runtime::python_runtime { + +namespace py = pybind11; + +class PyRos2TypeSupport { + public: + explicit PyRos2TypeSupport(py::object pymsg_type) : base_(GenBase(this, pymsg_type)) {} + + void SetTypeName(const std::string& s) { + type_name_ = s; + } + + void SetSerializationTypesSupportedList(const std::vector& v) { + serialization_types_supported_list_ = v; + serialization_types_supported_list_inner_.clear(); + for (auto& item : serialization_types_supported_list_) { + serialization_types_supported_list_inner_.emplace_back(aimrt::util::ToAimRTStringView(item)); + } + } + + const aimrt_type_support_base_t* NativeHandle() const { return &base_; } + + private: + void* Create() const { return create_ros_message_(); } + + void Destroy(void* msg) const { destroy_ros_message_(msg); } + + void Copy(const void* from, void* to) const { + const auto* members = GetRosMembersInfo(msg_type_support_); + CopyRosMessage(members, from, to); + } + + void Move(void* from, void* to) const { + const auto* members = GetRosMembersInfo(msg_type_support_); + MoveRosMessage(members, from, to); + } + + bool Serialize( + aimrt_string_view_t serialization_type, + const void* msg, + const aimrt_buffer_array_allocator_t* allocator, + aimrt_buffer_array_t* buffer_array) const { + try { + if (aimrt::util::ToStdStringView(serialization_type) == "ros2") { + aimrt_buffer_array_with_allocator_t bawa{.buffer_array = buffer_array, .allocator = allocator}; + Ros2RclSerializedMessageAdapter serialized_msg_adapter(&bawa); + rcl_ret_t ret = rmw_serialize(msg, msg_type_support_, serialized_msg_adapter.GetRclSerializedMessage()); + return (ret == RMW_RET_OK); + } + return false; + } catch (const std::exception& e) { + return false; + } + } + + bool Deserialize( + aimrt_string_view_t serialization_type, + aimrt_buffer_array_view_t buffer_array_view, + void* msg) const { + try { + if (aimrt::util::ToStdStringView(serialization_type) == "ros2") { + if (buffer_array_view.len == 1) { + rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); + serialized_msg.buffer = const_cast(static_cast(buffer_array_view.data[0].data)); + serialized_msg.buffer_length = buffer_array_view.data[0].len; + serialized_msg.buffer_capacity = buffer_array_view.data[0].len; + rcl_ret_t ret = rmw_deserialize(&serialized_msg, msg_type_support_, msg); + return (ret == RMW_RET_OK); + } + + if (buffer_array_view.len > 1) { + size_t total_size = 0; + for (size_t ii = 0; ii < buffer_array_view.len; ++ii) { + total_size += buffer_array_view.data[ii].len; + } + std::vector buffer_vec(total_size); + uint8_t* buffer = buffer_vec.data(); + size_t cur_size = 0; + for (size_t ii = 0; ii < buffer_array_view.len; ++ii) { + memcpy(buffer + cur_size, + buffer_array_view.data[ii].data, + buffer_array_view.data[ii].len); + cur_size += buffer_array_view.data[ii].len; + } + + rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); + serialized_msg.buffer = buffer; + serialized_msg.buffer_length = total_size; + serialized_msg.buffer_capacity = total_size; + rcl_ret_t ret = rmw_deserialize(&serialized_msg, msg_type_support_, msg); + return (ret == RMW_RET_OK); + } + } + return false; + } catch (const std::exception& e) { + return false; + } + } + + aimrt_type_support_base_t GenBase(PyRos2TypeSupport* impl, py::object pymsg_type) { + msg_type_support_ = static_cast(common_get_type_support(pymsg_type)); + + py::object pymetaclass = pymsg_type.attr("__class__"); + py::object value = pymetaclass.attr("_CREATE_ROS_MESSAGE"); + auto* capsule_ptr = static_cast(value.cast()); + create_ros_message_ = reinterpret_cast(capsule_ptr); + // TODO(zhangyi): throw exception in the constructor might not be a good idea. + if (!create_ros_message_) { + throw std::runtime_error("create_ros_message_ is nullptr"); + } + + value = pymetaclass.attr("_DESTROY_ROS_MESSAGE"); + capsule_ptr = static_cast(value.cast()); + destroy_ros_message_ = reinterpret_cast(capsule_ptr); + // TODO(zhangyi): throw exception in the constructor might not be a good idea. + if (!destroy_ros_message_) { + throw std::runtime_error("destroy_ros_message_ is nullptr"); + } + + return aimrt_type_support_base_t{ + .type_name = [](void* impl) -> aimrt_string_view_t { + return aimrt::util::ToAimRTStringView(static_cast(impl)->type_name_); + }, + .create = [](void* impl) -> void* { + return static_cast(impl)->Create(); + }, + .destroy = [](void* impl, void* msg) { // + static_cast(impl)->Destroy(msg); + }, + .copy = [](void* impl, const void* from, void* to) { // + static_cast(impl)->Copy(from, to); + }, + .move = [](void* impl, void* from, void* to) { // + static_cast(impl)->Move(from, to); + }, + .serialize = [](void* impl, aimrt_string_view_t serialization_type, const void* msg, const aimrt_buffer_array_allocator_t* allocator, aimrt_buffer_array_t* buffer_array) -> bool { + return static_cast(impl)->Serialize(serialization_type, msg, allocator, buffer_array); + }, + .deserialize = [](void* impl, aimrt_string_view_t serialization_type, aimrt_buffer_array_view_t buffer_array_view, void* msg) -> bool { + return static_cast(impl)->Deserialize(serialization_type, buffer_array_view, msg); + }, + .serialization_types_supported_num = [](void* impl) -> size_t { + return static_cast(impl)->serialization_types_supported_list_inner_.size(); + }, + .serialization_types_supported_list = [](void* impl) -> const aimrt_string_view_t* { + return static_cast(impl)->serialization_types_supported_list_inner_.data(); + }, + .custom_type_support_ptr = [](void* impl) -> const void* { + return static_cast(impl)->msg_type_support_; + }, + .impl = impl}; + } + + private: + rosidl_message_type_support_t* msg_type_support_; + create_ros_message_function* create_ros_message_; + destroy_ros_message_function* destroy_ros_message_; + + aimrt_type_support_base_t base_; + std::string type_name_; + std::vector serialization_types_supported_list_; + std::vector serialization_types_supported_list_inner_; +}; + +inline void ExportRos2TypeSupport(py::object m) { + py::class_>(std::move(m), "PyRos2TypeSupport") + .def(py::init()) + .def("SetTypeName", &PyRos2TypeSupport::SetTypeName) + .def("SetSerializationTypesSupportedList", &PyRos2TypeSupport::SetSerializationTypesSupportedList); +} + +} // namespace aimrt::runtime::python_runtime diff --git a/src/runtime/python_runtime/export_rpc.h b/src/runtime/python_runtime/export_rpc.h index c06618fb0..f55a843eb 100644 --- a/src/runtime/python_runtime/export_rpc.h +++ b/src/runtime/python_runtime/export_rpc.h @@ -3,16 +3,21 @@ #pragma once +#include "pybind11/pybind11.h" + #include #include #include #include "aimrt_module_cpp_interface/rpc/rpc_handle.h" -#include "python_runtime/export_type_support.h" - -#include "pybind11/pybind11.h" +#include "python_runtime/export_pb_type_support.h" #include "rpc/rpc_context_base.h" +#ifdef AIMRT_BUILD_WITH_ROS2 + #include "python_runtime/export_ros2_type_support.h" + #include "python_runtime/ros2_type_support_utils.h" +#endif + namespace aimrt::runtime::python_runtime { inline void ExportRpcStatus(const pybind11::object& m) { @@ -84,9 +89,9 @@ inline void ExportRpcContext(const pybind11::object& m) { pybind11::class_(m, "RpcContextRef") .def(pybind11::init<>()) - .def(pybind11::init()) - .def(pybind11::init()) - .def(pybind11::init&>()) + .def(pybind11::init(), pybind11::keep_alive<1, 2>()) + .def(pybind11::init(), pybind11::keep_alive<1, 2>()) + .def(pybind11::init&>(), pybind11::keep_alive<1, 2>()) .def("__bool__", &ContextRef::operator bool) .def("CheckUsed", &ContextRef::CheckUsed) .def("SetUsed", &ContextRef::SetUsed) @@ -108,13 +113,13 @@ inline void ExportRpcContext(const pybind11::object& m) { using ServiceFuncReturnType = std::tuple; using ServiceFuncType = std::function; -inline void PyRpcServiceBaseRegisterServiceFunc( +inline void PbRpcServiceBaseRegisterServiceFunc( aimrt::rpc::ServiceBase& service, std::string_view func_name, - const std::shared_ptr& req_type_support, - const std::shared_ptr& rsp_type_support, + const std::shared_ptr& req_type_support, + const std::shared_ptr& rsp_type_support, ServiceFuncType&& service_func) { - static std::vector> py_ts_vec; + static std::vector> py_ts_vec; py_ts_vec.emplace_back(req_type_support); py_ts_vec.emplace_back(rsp_type_support); @@ -154,23 +159,12 @@ inline void PyRpcServiceBaseRegisterServiceFunc( std::move(aimrt_service_func)); } -inline void ExportRpcServiceBase(pybind11::object m) { - using aimrt::rpc::ServiceBase; - - pybind11::class_(std::move(m), "ServiceBase") - .def(pybind11::init()) - .def("RpcType", &ServiceBase::RpcType) - .def("ServiceName", &ServiceBase::ServiceName) - .def("SetServiceName", &ServiceBase::SetServiceName) - .def("RegisterServiceFunc", &PyRpcServiceBaseRegisterServiceFunc); -} - -inline bool PyRpcHandleRefRegisterClientFunc( +inline bool PbRpcHandleRefRegisterClientFunc( aimrt::rpc::RpcHandleRef& rpc_handle_ref, std::string_view func_name, - const std::shared_ptr& req_type_support, - const std::shared_ptr& rsp_type_support) { - static std::vector> py_ts_vec; + const std::shared_ptr& req_type_support, + const std::shared_ptr& rsp_type_support) { + static std::vector> py_ts_vec; py_ts_vec.emplace_back(req_type_support); py_ts_vec.emplace_back(rsp_type_support); @@ -181,7 +175,7 @@ inline bool PyRpcHandleRefRegisterClientFunc( rsp_type_support->NativeHandle()); } -inline std::tuple PyRpcHandleRefInvoke( +inline std::tuple PbRpcHandleRefInvoke( aimrt::rpc::RpcHandleRef& rpc_handle_ref, std::string_view func_name, aimrt::rpc::ContextRef ctx_ref, @@ -191,10 +185,9 @@ inline std::tuple PyRpcHandleRefInvoke( std::string rsp_buf; std::promise status_promise; - aimrt::rpc::ClientCallback callback( - [&status_promise](uint32_t status) { - status_promise.set_value(status); - }); + aimrt::rpc::ClientCallback callback([&status_promise](uint32_t status) { + status_promise.set_value(status); + }); rpc_handle_ref.Invoke( func_name, @@ -211,6 +204,150 @@ inline std::tuple PyRpcHandleRefInvoke( return {aimrt::rpc::Status(fu.get()), pybind11::bytes(rsp_buf)}; } +#ifdef AIMRT_BUILD_WITH_ROS2 + +using Ros2ServiceFuncReturnType = std::tuple; +using Ros2ServiceFuncType = std::function; + +inline void Ros2RpcServiceBaseRegisterServiceFunc( + aimrt::rpc::ServiceBase& service, + std::string_view func_name, + pybind11::object srv_pyclass, + const std::shared_ptr& req_type_support, + pybind11::object req_pyclass, + const std::shared_ptr& rsp_type_support, + pybind11::object rsp_pyclass, + Ros2ServiceFuncType&& service_func) { + static std::vector> py_ts_vec; + py_ts_vec.emplace_back(req_type_support); + py_ts_vec.emplace_back(rsp_type_support); + + pybind11::gil_scoped_acquire acquire; + auto req_convert_to_py = get_convert_to_py_function(req_pyclass); + auto rsp_convert_from_py = get_convert_from_py_function(rsp_pyclass); + void* srv_type_support = common_get_type_support(srv_pyclass); + pybind11::gil_scoped_release release; + + aimrt::rpc::ServiceFunc aimrt_service_func( + [service_func{std::move(service_func)}, req_convert_to_py, rsp_convert_from_py]( + const aimrt_rpc_context_base_t* ctx, const void* req_ptr, void* rsp_ptr, aimrt_function_base_t* callback) { + aimrt::rpc::ServiceCallback callback_f(callback); + + aimrt::rpc::ContextRef ctx_ref(ctx); + + try { + pybind11::gil_scoped_acquire acquire; + + auto req_obj = pybind11::reinterpret_steal(req_convert_to_py(const_cast(req_ptr))); + if (!req_obj) { + throw py::error_already_set(); + } + + auto [status, rsp_obj] = service_func(ctx_ref, req_obj); + + if (!rsp_convert_from_py(rsp_obj.ptr(), rsp_ptr)) { + throw py::error_already_set(); + } + + pybind11::gil_scoped_release release; + + callback_f(status.Code()); + return; + } catch (const std::exception& e) { + callback_f(AIMRT_RPC_STATUS_SVR_HANDLE_FAILED); + return; + } + }); + + service.RegisterServiceFunc( + func_name, + srv_type_support, + req_type_support->NativeHandle(), + rsp_type_support->NativeHandle(), + std::move(aimrt_service_func)); +} + +inline bool PyRos2RpcHandleRefRegisterClientFunc( + aimrt::rpc::RpcHandleRef& rpc_handle_ref, + std::string_view func_name, + pybind11::object srv_pyclass, + const std::shared_ptr& req_type_support, + const std::shared_ptr& rsp_type_support) { + static std::vector> py_ts_vec; + py_ts_vec.emplace_back(req_type_support); + py_ts_vec.emplace_back(rsp_type_support); + + pybind11::gil_scoped_acquire acquire; + void* srv_type_support = common_get_type_support(srv_pyclass); + pybind11::gil_scoped_release release; + + return rpc_handle_ref.RegisterClientFunc( + func_name, + srv_type_support, + req_type_support->NativeHandle(), + rsp_type_support->NativeHandle()); +} + +inline std::tuple Ros2RpcHandleRefInvoke( + aimrt::rpc::RpcHandleRef& rpc_handle_ref, + std::string_view func_name, + aimrt::rpc::ContextRef ctx_ref, + pybind11::object req, + pybind11::object rsp_type) { + auto req_ptr = convert_from_py(req); + if (!req_ptr) { + throw py::error_already_set(); + } + + static auto rsp_create = get_create_ros_message_function(rsp_type); + auto* rsp_ptr = rsp_create(); + + pybind11::gil_scoped_release release; + + std::promise status_promise; + aimrt::rpc::ClientCallback callback([&status_promise](uint32_t status) { + status_promise.set_value(status); + }); + + rpc_handle_ref.Invoke( + func_name, + ctx_ref, + static_cast(req_ptr.get()), + rsp_ptr, + std::move(callback)); + + auto fu = status_promise.get_future(); + fu.wait(); + + pybind11::gil_scoped_acquire acquire; + + static auto rsp_convert_to_py = get_convert_to_py_function(rsp_type); + + auto rsp_obj = pybind11::reinterpret_steal(rsp_convert_to_py(rsp_ptr)); + if (!rsp_obj) { + throw py::error_already_set(); + } + + return {aimrt::rpc::Status(fu.get()), rsp_obj}; +} + +#endif + +inline void ExportRpcServiceBase(pybind11::object m) { + using aimrt::rpc::ServiceBase; + + pybind11::class_(std::move(m), "ServiceBase") + .def(pybind11::init()) + .def("RpcType", &ServiceBase::RpcType) + .def("ServiceName", &ServiceBase::ServiceName) + .def("SetServiceName", &ServiceBase::SetServiceName) + .def("PbRegisterServiceFunc", &PbRpcServiceBaseRegisterServiceFunc) +#ifdef AIMRT_BUILD_WITH_ROS2 + .def("Ros2RegisterServiceFunc", &Ros2RpcServiceBaseRegisterServiceFunc) +#endif + ; +} + inline void ExportRpcHandleRef(pybind11::object m) { using aimrt::rpc::RpcHandleRef; @@ -221,8 +358,13 @@ inline void ExportRpcHandleRef(pybind11::object m) { pybind11::overload_cast(&RpcHandleRef::RegisterService)) .def("RegisterService", pybind11::overload_cast(&RpcHandleRef::RegisterService)) - .def("RegisterClientFunc", &PyRpcHandleRefRegisterClientFunc) - .def("Invoke", &PyRpcHandleRefInvoke); + .def("PbRegisterClientFunc", &PbRpcHandleRefRegisterClientFunc) + .def("PbInvoke", &PbRpcHandleRefInvoke) +#ifdef AIMRT_BUILD_WITH_ROS2 + .def("Ros2RegisterClientFunc", &PyRos2RpcHandleRefRegisterClientFunc) + .def("Ros2Invoke", &Ros2RpcHandleRefInvoke) +#endif + ; } inline void ExportRpcProxyBase(pybind11::object m) { diff --git a/src/runtime/python_runtime/python_runtime_main.cc b/src/runtime/python_runtime/python_runtime_main.cc index aae413348..abdc09830 100644 --- a/src/runtime/python_runtime/python_runtime_main.cc +++ b/src/runtime/python_runtime/python_runtime_main.cc @@ -1,6 +1,8 @@ // Copyright (c) 2023, AgiBot Inc. // All rights reserved. +#include "pybind11/pybind11.h" + #include "python_runtime/export_channel.h" #include "python_runtime/export_configurator.h" #include "python_runtime/export_core.h" @@ -9,10 +11,12 @@ #include "python_runtime/export_logger.h" #include "python_runtime/export_module_base.h" #include "python_runtime/export_parameter.h" +#include "python_runtime/export_pb_type_support.h" #include "python_runtime/export_rpc.h" -#include "python_runtime/export_type_support.h" -#include "pybind11/pybind11.h" +#ifdef AIMRT_BUILD_WITH_ROS2 + #include "python_runtime/export_ros2_type_support.h" +#endif using namespace aimrt::runtime::python_runtime; @@ -24,7 +28,10 @@ PYBIND11_MODULE(aimrt_python_runtime, m) { ExportCore(m); // type support - ExportTypeSupport(m); + ExportPbTypeSupport(m); +#ifdef AIMRT_BUILD_WITH_ROS2 + ExportRos2TypeSupport(m); +#endif // core handle ExportModuleInfo(m); diff --git a/src/runtime/python_runtime/ros2_type_support_utils.cc b/src/runtime/python_runtime/ros2_type_support_utils.cc new file mode 100644 index 000000000..fce8d515e --- /dev/null +++ b/src/runtime/python_runtime/ros2_type_support_utils.cc @@ -0,0 +1,557 @@ +// Copyright(c) 2024 The AimRT Authors. +// AimRT is licensed under Mulan PSL v2. + +#include "python_runtime/ros2_type_support_utils.h" + +#include + +#include "rcutils/allocator.h" +#include "rosidl_runtime_c/message_type_support_struct.h" +#include "rosidl_runtime_c/primitives_sequence.h" // IWYU pragma: keep +#include "rosidl_runtime_c/primitives_sequence_functions.h" // IWYU pragma: keep +#include "rosidl_runtime_c/string.h" +#include "rosidl_runtime_c/string_functions.h" // IWYU pragma: keep +#include "rosidl_runtime_c/u16string.h" +#include "rosidl_runtime_c/u16string_functions.h" // IWYU pragma: keep +#include "rosidl_typesupport_introspection_c/field_types.h" +#include "rosidl_typesupport_introspection_c/message_introspection.h" + +namespace aimrt::runtime::python_runtime { + +// NOLINTBEGIN(performance-unnecessary-value-param) + +// The following code is adapted from ros2 rclpy. +// see: https://github.com/ros2/rclpy/blob/humble/rclpy/src/rclpy/utils.cpp + +typedef void destroy_ros_message_function(void*); +typedef void* create_ros_message_function(); +typedef bool convert_from_py_function(PyObject*, void*); +typedef PyObject* convert_to_py_function(void*); + +void* common_get_type_support(py::object pymessage) { + py::object pymetaclass = pymessage.attr("__class__"); + + py::object value = pymetaclass.attr("_TYPE_SUPPORT"); + auto* capsule_ptr = static_cast(value.cast()); + + return capsule_ptr; +} + +std::unique_ptr +create_from_py(py::object pymessage) { + py::object pymetaclass = pymessage.attr("__class__"); + + py::object value = pymetaclass.attr("_CREATE_ROS_MESSAGE"); + auto* capsule_ptr = static_cast(value.cast()); + auto create_ros_message = + reinterpret_cast(capsule_ptr); + if (!create_ros_message) { + throw py::error_already_set(); + } + + value = pymetaclass.attr("_DESTROY_ROS_MESSAGE"); + capsule_ptr = static_cast(value.cast()); + auto destroy_ros_message = + reinterpret_cast(capsule_ptr); + if (!destroy_ros_message) { + throw py::error_already_set(); + } + + void* message = create_ros_message(); + if (!message) { + throw std::bad_alloc(); + } + return std::unique_ptr< + void, destroy_ros_message_function*>(message, destroy_ros_message); +} + +std::unique_ptr +convert_from_py(py::object pymessage) { + std::unique_ptr message = + create_from_py(pymessage); + + py::object pymetaclass = pymessage.attr("__class__"); + + auto* capsule_ptr = static_cast( + pymetaclass.attr("_CONVERT_FROM_PY").cast()); + auto convert = + reinterpret_cast(capsule_ptr); + if (!convert) { + throw py::error_already_set(); + } + + if (!convert(pymessage.ptr(), message.get())) { + throw py::error_already_set(); + } + + return message; +} + +create_ros_message_function* get_create_ros_message_function(py::object pyclass) { + py::object pymetaclass = pyclass.attr("__class__"); + auto* capsule_ptr = static_cast(pymetaclass.attr("_CREATE_ROS_MESSAGE").cast()); + auto create = reinterpret_cast(capsule_ptr); + if (!create) { + throw py::error_already_set(); + } + return create; +} + +destroy_ros_message_function* get_destroy_ros_message_function(py::object pyclass) { + py::object pymetaclass = pyclass.attr("__class__"); + auto* capsule_ptr = static_cast(pymetaclass.attr("_DESTROY_ROS_MESSAGE").cast()); + auto destroy = reinterpret_cast(capsule_ptr); + if (!destroy) { + throw py::error_already_set(); + } + return destroy; +} + +convert_to_py_function* get_convert_to_py_function(py::object pyclass) { + py::object pymetaclass = pyclass.attr("__class__"); + auto* capsule_ptr = static_cast(pymetaclass.attr("_CONVERT_TO_PY").cast()); + auto convert = reinterpret_cast(capsule_ptr); + if (!convert) { + throw py::error_already_set(); + } + return convert; +} + +convert_from_py_function* get_convert_from_py_function(py::object pyclass) { + py::object pymetaclass = pyclass.attr("__class__"); + auto* capsule_ptr = static_cast(pymetaclass.attr("_CONVERT_FROM_PY").cast()); + auto convert = reinterpret_cast(capsule_ptr); + if (!convert) { + throw py::error_already_set(); + } + return convert; +} + +// End of adapted code from ros2 rclpy. + +// NOLINTEND(performance-unnecessary-value-param) + +namespace { + +template +struct RosTypeMapping; + +#define ROS_TYPE_ID(ros_type) \ + rosidl_typesupport_introspection_c__ROS_TYPE_##ros_type + +#define ROS_SEQUENCE_TYPE(name_in_sequence) \ + rosidl_runtime_c__##name_in_sequence##__Sequence + +#define ROS_SEQUENCE_COPY_FUNCTION(name_in_sequence) \ + rosidl_runtime_c__##name_in_sequence##__Sequence__copy + +// There is no move function for sequence in ROS2. +// So we implement these functions in RosTypeMapping. +#define ROS_SEQUENCE_MOVE_FUNCTION(name_in_sequence) \ + rosidl_runtime_c__##name_in_sequence##__Sequence__move + +#define DEFINE_ROS_BASIC_TYPE_MAPPING(ros_type, name_in_sequence, c_type) \ + template <> \ + struct RosTypeMapping { \ + using CType = c_type; \ + using SequenceType = ROS_SEQUENCE_TYPE(name_in_sequence); \ + static constexpr auto SequenceCopyFunction = ROS_SEQUENCE_COPY_FUNCTION(name_in_sequence); \ + static void SequenceMoveFunction(SequenceType* from_seq, SequenceType* to_seq) { \ + auto allocator = rcutils_get_default_allocator(); \ + allocator.deallocate(to_seq->data, allocator.state); \ + *to_seq = *from_seq; \ + memset(from_seq, 0, sizeof(SequenceType)); \ + } \ + }; + +#define ROS_STRING_COPY_FUNCTION(name_in_sequence) \ + rosidl_runtime_c__##name_in_sequence##__copy + +#define DEFINE_ROS_STRING_TYPE_MAPPING(ros_type, name_in_sequence, c_type) \ + template <> \ + struct RosTypeMapping { \ + using CType = c_type; \ + using SequenceType = ROS_SEQUENCE_TYPE(name_in_sequence); \ + static constexpr auto CopyFunction = ROS_STRING_COPY_FUNCTION(name_in_sequence); \ + static constexpr auto SequenceCopyFunction = ROS_SEQUENCE_COPY_FUNCTION(name_in_sequence); \ + static void MoveFunction(CType* from_str, CType* to_str) { \ + auto allocator = rcutils_get_default_allocator(); \ + allocator.deallocate(to_str->data, allocator.state); \ + *to_str = *from_str; \ + memset(from_str, 0, sizeof(CType)); \ + } \ + static void SequenceMoveFunction(SequenceType* from_seq, SequenceType* to_seq) { \ + auto allocator = rcutils_get_default_allocator(); \ + for (size_t ii = 0; ii < to_seq->capacity; ++ii) { \ + allocator.deallocate(to_seq->data[ii].data, allocator.state); \ + } \ + allocator.deallocate(to_seq->data, allocator.state); \ + *to_seq = *from_seq; \ + memset(from_seq, 0, sizeof(SequenceType)); \ + } \ + }; + +DEFINE_ROS_BASIC_TYPE_MAPPING(FLOAT, float, float) +DEFINE_ROS_BASIC_TYPE_MAPPING(DOUBLE, double, double) +DEFINE_ROS_BASIC_TYPE_MAPPING(LONG_DOUBLE, long_double, long double) +DEFINE_ROS_BASIC_TYPE_MAPPING(CHAR, char, unsigned char) +DEFINE_ROS_BASIC_TYPE_MAPPING(WCHAR, wchar, char16_t) +DEFINE_ROS_BASIC_TYPE_MAPPING(BOOLEAN, boolean, bool) +DEFINE_ROS_BASIC_TYPE_MAPPING(OCTET, octet, std::byte) +DEFINE_ROS_BASIC_TYPE_MAPPING(UINT8, uint8, uint8_t) +DEFINE_ROS_BASIC_TYPE_MAPPING(INT8, int8, int8_t) +DEFINE_ROS_BASIC_TYPE_MAPPING(UINT16, uint16, uint16_t) +DEFINE_ROS_BASIC_TYPE_MAPPING(INT16, int16, int16_t) +DEFINE_ROS_BASIC_TYPE_MAPPING(UINT32, uint32, uint32_t) +DEFINE_ROS_BASIC_TYPE_MAPPING(INT32, int32, int32_t) +DEFINE_ROS_BASIC_TYPE_MAPPING(UINT64, uint64, uint64_t) +DEFINE_ROS_BASIC_TYPE_MAPPING(INT64, int64, int64_t) +DEFINE_ROS_STRING_TYPE_MAPPING(STRING, String, rosidl_runtime_c__String) +DEFINE_ROS_STRING_TYPE_MAPPING(WSTRING, U16String, rosidl_runtime_c__U16String) + +#define COPY_ROS_BASIC_TYPE(ros_type) \ + case ROS_TYPE_ID(ros_type): \ + *reinterpret_cast::CType*>(to_ptr) = \ + *reinterpret_cast::CType*>(from_ptr); \ + break; + +#define COPY_ROS_STRING_TYPE(ros_type) \ + case ROS_TYPE_ID(ros_type): \ + RosTypeMapping::CopyFunction( \ + reinterpret_cast::CType*>(from_ptr), \ + reinterpret_cast::CType*>(to_ptr)); \ + break; + +template +concept introspection_c_copy_function = std::is_invocable_v< + CopyFunction, + const rosidl_typesupport_introspection_c__MessageMembers*, const void*, void*>; + +template +concept introspection_c_move_function = std::is_invocable_v< + MoveFunction, + const rosidl_typesupport_introspection_c__MessageMembers*, void*, void*>; + +template +concept introspection_c_copy_or_move_function = introspection_c_copy_function || + introspection_c_move_function; + +template +void HandleBasicMemberEmbeddedType(const rosidl_typesupport_introspection_c__MessageMember& member, + void* from_ptr, void* to_ptr, + CopyOrMoveFunction copy_or_move) { + const auto* sub_members = + reinterpret_cast(member.members_->data); + copy_or_move(sub_members, from_ptr, to_ptr); +} + +void CopyBasicMember( + const rosidl_typesupport_introspection_c__MessageMember& member, + const void* from_ptr, void* to_ptr) { + switch (member.type_id_) { + COPY_ROS_BASIC_TYPE(FLOAT) + COPY_ROS_BASIC_TYPE(DOUBLE) + COPY_ROS_BASIC_TYPE(LONG_DOUBLE) + COPY_ROS_BASIC_TYPE(CHAR) + COPY_ROS_BASIC_TYPE(WCHAR) + COPY_ROS_BASIC_TYPE(BOOLEAN) + COPY_ROS_BASIC_TYPE(OCTET) + COPY_ROS_BASIC_TYPE(UINT8) + COPY_ROS_BASIC_TYPE(INT8) + COPY_ROS_BASIC_TYPE(UINT16) + COPY_ROS_BASIC_TYPE(INT16) + COPY_ROS_BASIC_TYPE(UINT32) + COPY_ROS_BASIC_TYPE(INT32) + COPY_ROS_BASIC_TYPE(UINT64) + COPY_ROS_BASIC_TYPE(INT64) + COPY_ROS_STRING_TYPE(STRING) + COPY_ROS_STRING_TYPE(WSTRING) + case rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE: + HandleBasicMemberEmbeddedType(member, const_cast(from_ptr), to_ptr, CopyRosMessage); + break; + default: + throw std::runtime_error("Unknown basic type: " + std::string(member.name_) + + " with type id: " + std::to_string(member.type_id_)); + } +} + +#define COPY_ROS_BASIC_TYPE_ARRAY(ros_type) \ + case ROS_TYPE_ID(ros_type): \ + std::memcpy(to_ptr, from_ptr, member.array_size_ * sizeof(RosTypeMapping::CType)); \ + break; + +#define COPY_ROS_STRING_TYPE_ARRAY(ros_type) \ + case ROS_TYPE_ID(ros_type): { \ + auto* to_str = reinterpret_cast::CType*>(to_ptr); \ + const auto* from_str = reinterpret_cast::CType*>(from_ptr); \ + for (size_t i = 0; i < member.array_size_; ++i) { \ + RosTypeMapping::CopyFunction(&from_str[i], &to_str[i]); \ + } \ + break; \ + } + +template +void HandleStaticArrayEmbeddedType(const rosidl_typesupport_introspection_c__MessageMember& member, + void* from_ptr, void* to_ptr, + CopyOrMoveFunction copy_or_move) { + const auto* sub_members = + reinterpret_cast(member.members_->data); + if (!member.get_function) [[unlikely]] { + throw std::runtime_error("Failed to get get function for message: " + std::string(member.name_)); + } + for (size_t ii = 0; ii < member.array_size_; ++ii) { + copy_or_move(sub_members, member.get_function(from_ptr, ii), member.get_function(to_ptr, ii)); + } +} + +void CopyStaticSizeArray( + const rosidl_typesupport_introspection_c__MessageMember& member, + const void* from_ptr, void* to_ptr) { + switch (member.type_id_) { + COPY_ROS_BASIC_TYPE_ARRAY(FLOAT) + COPY_ROS_BASIC_TYPE_ARRAY(DOUBLE) + COPY_ROS_BASIC_TYPE_ARRAY(LONG_DOUBLE) + COPY_ROS_BASIC_TYPE_ARRAY(CHAR) + COPY_ROS_BASIC_TYPE_ARRAY(WCHAR) + COPY_ROS_BASIC_TYPE_ARRAY(BOOLEAN) + COPY_ROS_BASIC_TYPE_ARRAY(OCTET) + COPY_ROS_BASIC_TYPE_ARRAY(UINT8) + COPY_ROS_BASIC_TYPE_ARRAY(INT8) + COPY_ROS_BASIC_TYPE_ARRAY(UINT16) + COPY_ROS_BASIC_TYPE_ARRAY(INT16) + COPY_ROS_BASIC_TYPE_ARRAY(UINT32) + COPY_ROS_BASIC_TYPE_ARRAY(INT32) + COPY_ROS_BASIC_TYPE_ARRAY(UINT64) + COPY_ROS_BASIC_TYPE_ARRAY(INT64) + COPY_ROS_STRING_TYPE_ARRAY(STRING) + COPY_ROS_STRING_TYPE_ARRAY(WSTRING) + case rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE: + HandleStaticArrayEmbeddedType(member, const_cast(from_ptr), to_ptr, CopyRosMessage); + break; + default: + throw std::runtime_error("Unknown array type: " + std::string(member.name_) + + " with type id: " + std::to_string(member.type_id_)); + } +} + +#define COPY_ROS_SEQUENCE(ros_type) \ + case ROS_TYPE_ID(ros_type): { \ + auto* to_seq = reinterpret_cast::SequenceType*>(to_ptr); \ + const auto* from_seq = reinterpret_cast::SequenceType*>(from_ptr); \ + RosTypeMapping::SequenceCopyFunction(from_seq, to_seq); \ + break; \ + } + +template +void HandleDynamicArrayEmbeddedType(const rosidl_typesupport_introspection_c__MessageMember& member, + void* from_ptr, void* to_ptr, + CopyOrMoveFunction copy_or_move) { + const auto* sub_members = + reinterpret_cast(member.members_->data); + if (!member.get_function) [[unlikely]] { + throw std::runtime_error("Failed to get get function for message: " + std::string(member.name_)); + } + if (!member.resize_function) [[unlikely]] { + throw std::runtime_error("Failed to get resize function for message: " + std::string(member.name_)); + } + if (!member.size_function) [[unlikely]] { + throw std::runtime_error("Failed to get size function for message: " + std::string(member.name_)); + } + auto from_size = member.size_function(from_ptr); + member.resize_function(to_ptr, from_size); + for (size_t ii = 0; ii < from_size; ++ii) { + copy_or_move(sub_members, + member.get_function(from_ptr, ii), + member.get_function(to_ptr, ii)); + } +} + +void CopyDynamicSizeArray( + const rosidl_typesupport_introspection_c__MessageMember& member, + const void* from_ptr, void* to_ptr) { + switch (member.type_id_) { + COPY_ROS_SEQUENCE(FLOAT) + COPY_ROS_SEQUENCE(DOUBLE) + COPY_ROS_SEQUENCE(LONG_DOUBLE) + COPY_ROS_SEQUENCE(CHAR) + COPY_ROS_SEQUENCE(WCHAR) + COPY_ROS_SEQUENCE(BOOLEAN) + COPY_ROS_SEQUENCE(OCTET) + COPY_ROS_SEQUENCE(UINT8) + COPY_ROS_SEQUENCE(INT8) + COPY_ROS_SEQUENCE(UINT16) + COPY_ROS_SEQUENCE(INT16) + COPY_ROS_SEQUENCE(UINT32) + COPY_ROS_SEQUENCE(INT32) + COPY_ROS_SEQUENCE(UINT64) + COPY_ROS_SEQUENCE(INT64) + COPY_ROS_SEQUENCE(STRING) + COPY_ROS_SEQUENCE(WSTRING) + case rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE: + HandleDynamicArrayEmbeddedType(member, const_cast(from_ptr), to_ptr, CopyRosMessage); + break; + default: + throw std::runtime_error("Unknown array type: " + std::string(member.name_) + + " with type id: " + std::to_string(member.type_id_)); + } +} + +#define MOVE_ROS_STRING_TYPE(ros_type) \ + case ROS_TYPE_ID(ros_type): \ + RosTypeMapping::MoveFunction( \ + reinterpret_cast::CType*>(from_ptr), \ + reinterpret_cast::CType*>(to_ptr)); \ + break; + +void MoveBasicMember(const rosidl_typesupport_introspection_c__MessageMember& member, + void* from_ptr, void* to_ptr) { + switch (member.type_id_) { + COPY_ROS_BASIC_TYPE(FLOAT) + COPY_ROS_BASIC_TYPE(DOUBLE) + COPY_ROS_BASIC_TYPE(LONG_DOUBLE) + COPY_ROS_BASIC_TYPE(CHAR) + COPY_ROS_BASIC_TYPE(WCHAR) + COPY_ROS_BASIC_TYPE(BOOLEAN) + COPY_ROS_BASIC_TYPE(OCTET) + COPY_ROS_BASIC_TYPE(UINT8) + COPY_ROS_BASIC_TYPE(INT8) + COPY_ROS_BASIC_TYPE(UINT16) + COPY_ROS_BASIC_TYPE(INT16) + COPY_ROS_BASIC_TYPE(UINT32) + COPY_ROS_BASIC_TYPE(INT32) + COPY_ROS_BASIC_TYPE(UINT64) + COPY_ROS_BASIC_TYPE(INT64) + MOVE_ROS_STRING_TYPE(STRING) + MOVE_ROS_STRING_TYPE(WSTRING) + case rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE: { + HandleBasicMemberEmbeddedType(member, from_ptr, to_ptr, MoveRosMessage); + break; + } + default: + throw std::runtime_error("Unknown basic type: " + std::string(member.name_) + + " with type id: " + std::to_string(member.type_id_)); + } +} + +#define MOVE_ROS_STRING_TYPE_ARRAY(ros_type) \ + case ROS_TYPE_ID(ros_type): { \ + auto* to_str = reinterpret_cast::CType*>(to_ptr); \ + auto* from_str = reinterpret_cast::CType*>(from_ptr); \ + for (size_t i = 0; i < member.array_size_; ++i) { \ + RosTypeMapping::MoveFunction(&from_str[i], &to_str[i]); \ + } \ + break; \ + } + +void MoveStaticSizeArray(const rosidl_typesupport_introspection_c__MessageMember& member, + void* from_ptr, void* to_ptr) { + switch (member.type_id_) { + COPY_ROS_BASIC_TYPE_ARRAY(FLOAT) + COPY_ROS_BASIC_TYPE_ARRAY(DOUBLE) + COPY_ROS_BASIC_TYPE_ARRAY(LONG_DOUBLE) + COPY_ROS_BASIC_TYPE_ARRAY(CHAR) + COPY_ROS_BASIC_TYPE_ARRAY(WCHAR) + COPY_ROS_BASIC_TYPE_ARRAY(BOOLEAN) + COPY_ROS_BASIC_TYPE_ARRAY(OCTET) + COPY_ROS_BASIC_TYPE_ARRAY(UINT8) + COPY_ROS_BASIC_TYPE_ARRAY(INT8) + COPY_ROS_BASIC_TYPE_ARRAY(UINT16) + COPY_ROS_BASIC_TYPE_ARRAY(INT16) + COPY_ROS_BASIC_TYPE_ARRAY(UINT32) + COPY_ROS_BASIC_TYPE_ARRAY(INT32) + COPY_ROS_BASIC_TYPE_ARRAY(UINT64) + COPY_ROS_BASIC_TYPE_ARRAY(INT64) + MOVE_ROS_STRING_TYPE_ARRAY(STRING) + MOVE_ROS_STRING_TYPE_ARRAY(WSTRING) + case rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE: { + HandleStaticArrayEmbeddedType(member, from_ptr, to_ptr, MoveRosMessage); + break; + } + default: + throw std::runtime_error("Unknown array type: " + std::string(member.name_) + + " with type id: " + std::to_string(member.type_id_)); + } +} + +#define MOVE_ROS_SEQUENCE(ros_type) \ + case ROS_TYPE_ID(ros_type): { \ + auto* to_seq = reinterpret_cast::SequenceType*>(to_ptr); \ + auto* from_seq = reinterpret_cast::SequenceType*>(from_ptr); \ + RosTypeMapping::SequenceMoveFunction(from_seq, to_seq); \ + break; \ + } + +void MoveDynamicSizeArray(const rosidl_typesupport_introspection_c__MessageMember& member, + void* from_ptr, void* to_ptr) { + switch (member.type_id_) { + MOVE_ROS_SEQUENCE(FLOAT) + MOVE_ROS_SEQUENCE(DOUBLE) + MOVE_ROS_SEQUENCE(LONG_DOUBLE) + MOVE_ROS_SEQUENCE(CHAR) + MOVE_ROS_SEQUENCE(WCHAR) + MOVE_ROS_SEQUENCE(BOOLEAN) + MOVE_ROS_SEQUENCE(OCTET) + MOVE_ROS_SEQUENCE(UINT8) + MOVE_ROS_SEQUENCE(INT8) + MOVE_ROS_SEQUENCE(UINT16) + MOVE_ROS_SEQUENCE(INT16) + MOVE_ROS_SEQUENCE(UINT32) + MOVE_ROS_SEQUENCE(INT32) + MOVE_ROS_SEQUENCE(UINT64) + MOVE_ROS_SEQUENCE(INT64) + MOVE_ROS_SEQUENCE(STRING) + MOVE_ROS_SEQUENCE(WSTRING) + case rosidl_typesupport_introspection_c__ROS_TYPE_MESSAGE: { + HandleDynamicArrayEmbeddedType(member, from_ptr, to_ptr, MoveRosMessage); + break; + } + default: + throw std::runtime_error("Unknown array type: " + std::string(member.name_) + + " with type id: " + std::to_string(member.type_id_)); + } +} + +} // namespace + +const rosidl_typesupport_introspection_c__MessageMembers* GetRosMembersInfo( + const rosidl_message_type_support_t* type_support) { + const auto* ts = get_message_typesupport_handle(type_support, rosidl_typesupport_introspection_c__identifier); + if (!ts) [[unlikely]] { + throw std::runtime_error("Failed to get introspection type support."); + } + return reinterpret_cast(ts->data); +} + +void CopyRosMessage(const rosidl_typesupport_introspection_c__MessageMembers* members, + const void* from, void* to) { + for (size_t ii = 0; ii < members->member_count_; ++ii) { + const auto& member = members->members_[ii]; + const void* from_ptr = static_cast(static_cast(from) + member.offset_); + void* to_ptr = static_cast(static_cast(to) + member.offset_); + if (!member.is_array_) { + CopyBasicMember(member, from_ptr, to_ptr); + } else if (member.array_size_ > 0 && !member.is_upper_bound_) { + CopyStaticSizeArray(member, from_ptr, to_ptr); + } else { + CopyDynamicSizeArray(member, from_ptr, to_ptr); + } + } +} + +void MoveRosMessage(const rosidl_typesupport_introspection_c__MessageMembers* members, + void* from, void* to) { + for (size_t ii = 0; ii < members->member_count_; ++ii) { + const auto& member = members->members_[ii]; + void* from_ptr = static_cast(static_cast(from) + member.offset_); + void* to_ptr = static_cast(static_cast(to) + member.offset_); + if (!member.is_array_) { + MoveBasicMember(member, from_ptr, to_ptr); + } else if (member.array_size_ > 0 && !member.is_upper_bound_) { + MoveStaticSizeArray(member, from_ptr, to_ptr); + } else { + MoveDynamicSizeArray(member, from_ptr, to_ptr); + } + } +} + +} // namespace aimrt::runtime::python_runtime diff --git a/src/runtime/python_runtime/ros2_type_support_utils.h b/src/runtime/python_runtime/ros2_type_support_utils.h new file mode 100644 index 000000000..b92883636 --- /dev/null +++ b/src/runtime/python_runtime/ros2_type_support_utils.h @@ -0,0 +1,46 @@ +// Copyright(c) 2024 The AimRT Authors. +// AimRT is licensed under Mulan PSL v2. + +#pragma once + +#include "pybind11/pybind11.h" // IWYU pragma: keep + +#include "rosidl_typesupport_introspection_c/identifier.h" +#include "rosidl_typesupport_introspection_c/message_introspection.h" + +namespace aimrt::runtime::python_runtime { + +namespace py = pybind11; + +// The following code is adapted from ros2 rclpy. +// see: https://github.com/ros2/rclpy/blob/humble/rclpy/src/rclpy/utils.cpp + +typedef void destroy_ros_message_function(void*); +typedef void* create_ros_message_function(); +typedef bool convert_from_py_function(PyObject*, void*); +typedef PyObject* convert_to_py_function(void*); + +void* common_get_type_support(py::object pymessage); + +std::unique_ptr create_from_py(py::object pymessage); + +std::unique_ptr convert_from_py(py::object pymessage); + +create_ros_message_function* get_create_ros_message_function(py::object pyclass); + +destroy_ros_message_function* get_destroy_ros_message_function(py::object pyclass); + +convert_to_py_function* get_convert_to_py_function(py::object pyclass); + +convert_from_py_function* get_convert_from_py_function(py::object pyclass); + +// End of adapted code from ros2 rclpy. + +const rosidl_typesupport_introspection_c__MessageMembers* GetRosMembersInfo( + const rosidl_message_type_support_t* type_support); + +void CopyRosMessage(const rosidl_typesupport_introspection_c__MessageMembers* members, const void* from, void* to); + +void MoveRosMessage(const rosidl_typesupport_introspection_c__MessageMembers* members, void* from, void* to); + +} // namespace aimrt::runtime::python_runtime diff --git a/src/tools/package_aimrt_py/CMakeLists.txt b/src/tools/package_aimrt_py/CMakeLists.txt index e740fec75..b06fb2e33 100644 --- a/src/tools/package_aimrt_py/CMakeLists.txt +++ b/src/tools/package_aimrt_py/CMakeLists.txt @@ -2,8 +2,9 @@ # All rights reserved. get_target_property(py_files aimrt::runtime::python_runtime PY_FILES) -file(GLOB_RECURSE gencode_file ${PROJECT_SOURCE_DIR}/src/tools/protoc_plugin_py_gen_aimrt_py_rpc/*.py) -list(APPEND py_files ${gencode_file}) +file(GLOB_RECURSE gencode_file CONFIGURE_DEPENDS ${PROJECT_SOURCE_DIR}/src/tools/protoc_plugin_py_gen_aimrt_py_rpc/*.py) +file(GLOB_RECURSE ros2_gencode_file CONFIGURE_DEPENDS ${PROJECT_SOURCE_DIR}/src/tools/ros2_py_gen_aimrt_py_rpc/*.py) +list(APPEND py_files ${gencode_file} ${ros2_gencode_file}) set(AIMRT_PY_PKG_DIR ${CMAKE_BINARY_DIR}/aimrt_py_pkg) diff --git a/src/tools/package_aimrt_py/pyproject.toml b/src/tools/package_aimrt_py/pyproject.toml index bb18a4e36..159fc5317 100644 --- a/src/tools/package_aimrt_py/pyproject.toml +++ b/src/tools/package_aimrt_py/pyproject.toml @@ -22,3 +22,4 @@ dynamic = ["version"] [project.scripts] protoc_plugin_py_gen_aimrt_py_rpc = "aimrt_py.protoc_plugin_py_gen_aimrt_py_rpc:generate" +aimrt_py-gen-ros2-rpc = "aimrt_py.ros2_py_gen_aimrt_py_rpc:generate" diff --git a/src/tools/protoc_plugin_py_gen_aimrt_py_rpc/protoc_plugin_py_gen_aimrt_py_rpc.py b/src/tools/protoc_plugin_py_gen_aimrt_py_rpc/protoc_plugin_py_gen_aimrt_py_rpc.py index 096928bd5..c34714ed8 100755 --- a/src/tools/protoc_plugin_py_gen_aimrt_py_rpc/protoc_plugin_py_gen_aimrt_py_rpc.py +++ b/src/tools/protoc_plugin_py_gen_aimrt_py_rpc/protoc_plugin_py_gen_aimrt_py_rpc.py @@ -32,11 +32,11 @@ class {{service_name}}(aimrt_py.ServiceBase): {{for method begin}} # {{rpc_func_name}} - {{simple_rpc_req_name}}_aimrt_ts = aimrt_py.TypeSupport() + {{simple_rpc_req_name}}_aimrt_ts = aimrt_py.PyPbTypeSupport() {{simple_rpc_req_name}}_aimrt_ts.SetTypeName("pb:" + {{full_rpc_req_py_name}}.DESCRIPTOR.full_name) {{simple_rpc_req_name}}_aimrt_ts.SetSerializationTypesSupportedList(["pb", "json"]) - {{simple_rpc_rsp_name}}_aimrt_ts = aimrt_py.TypeSupport() + {{simple_rpc_rsp_name}}_aimrt_ts = aimrt_py.PyPbTypeSupport() {{simple_rpc_rsp_name}}_aimrt_ts.SetTypeName("pb:" + {{full_rpc_rsp_py_name}}.DESCRIPTOR.full_name) {{simple_rpc_rsp_name}}_aimrt_ts.SetSerializationTypesSupportedList(["pb", "json"]) @@ -72,7 +72,7 @@ class {{service_name}}(aimrt_py.ServiceBase): return (st, rsp_str) - self.RegisterServiceFunc("{{rpc_func_name}}", + self.PbRegisterServiceFunc("{{rpc_func_name}}", {{simple_rpc_req_name}}_aimrt_ts, {{simple_rpc_rsp_name}}_aimrt_ts, {{rpc_func_name}}AdapterFunc) {{method end}} @@ -97,7 +97,7 @@ class {{service_name}}Proxy(aimrt_py.ProxyBase): @overload def {{rpc_func_name}}( - self, ctx_ref: aimrt_py.RpcContext, req: {{full_rpc_req_py_name}} + self, ctx: aimrt_py.RpcContext, req: {{full_rpc_req_py_name}} ) -> tuple[aimrt_py.RpcStatus, {{full_rpc_rsp_py_name}}]: ... @overload @@ -145,7 +145,7 @@ class {{service_name}}Proxy(aimrt_py.ProxyBase): except Exception as e: return (aimrt_py.RpcStatus(aimrt_py.RpcStatusRetCode.CLI_SERIALIZATION_FAILED), rsp) - status, rsp_str = self.rpc_handle_ref.Invoke("pb:/{{package_name}}.{{service_name}}/{{rpc_func_name}}", + status, rsp_str = self.rpc_handle_ref.PbInvoke("pb:/{{package_name}}.{{service_name}}/{{rpc_func_name}}", ctx_ref, req_str) try: @@ -166,15 +166,15 @@ class {{service_name}}Proxy(aimrt_py.ProxyBase): def RegisterClientFunc(rpc_handle): {{for method begin}} # {{rpc_func_name}} - {{simple_rpc_req_name}}_aimrt_ts = aimrt_py.TypeSupport() + {{simple_rpc_req_name}}_aimrt_ts = aimrt_py.PyPbTypeSupport() {{simple_rpc_req_name}}_aimrt_ts.SetTypeName("pb:" + {{full_rpc_req_py_name}}.DESCRIPTOR.full_name) {{simple_rpc_req_name}}_aimrt_ts.SetSerializationTypesSupportedList(["pb", "json"]) - {{simple_rpc_rsp_name}}_aimrt_ts = aimrt_py.TypeSupport() + {{simple_rpc_rsp_name}}_aimrt_ts = aimrt_py.PyPbTypeSupport() {{simple_rpc_rsp_name}}_aimrt_ts.SetTypeName("pb:" + {{full_rpc_rsp_py_name}}.DESCRIPTOR.full_name) {{simple_rpc_rsp_name}}_aimrt_ts.SetSerializationTypesSupportedList(["pb", "json"]) - ret = rpc_handle.RegisterClientFunc("pb:/{{package_name}}.{{service_name}}/{{rpc_func_name}}", + ret = rpc_handle.PbRegisterClientFunc("pb:/{{package_name}}.{{service_name}}/{{rpc_func_name}}", {{simple_rpc_req_name}}_aimrt_ts, {{simple_rpc_rsp_name}}_aimrt_ts) if(not ret): return False diff --git a/src/tools/ros2_py_gen_aimrt_py_rpc/ros2_py_gen_aimrt_py_rpc.py b/src/tools/ros2_py_gen_aimrt_py_rpc/ros2_py_gen_aimrt_py_rpc.py new file mode 100644 index 000000000..d9aef7723 --- /dev/null +++ b/src/tools/ros2_py_gen_aimrt_py_rpc/ros2_py_gen_aimrt_py_rpc.py @@ -0,0 +1,151 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +# Copyright (c) 2024 The AimRT Authors. +# AimRT is licensed under Mulan PSL v2. + +import os +import sys + +t_pyfile: str = r"""# This file was generated by ros2-aimrt_py_rpc-gen-code-tool, do not edit it!!! + +from typing import overload + +import aimrt_py +import {{package_name}}.srv + +if {{package_name}}.srv.{{service_name}}._TYPE_SUPPORT is None: + {{package_name}}.srv.{{service_name}}.__import_type_support__() + + +class {{service_name}}Service(aimrt_py.ServiceBase): + def __init__(self): + super().__init__("ros2", "{{package_name}}/srv") + + # {{service_name}} + {{service_name}}_req_aimrt_ts = aimrt_py.PyRos2TypeSupport({{package_name}}.srv.{{service_name}}.Request) + {{service_name}}_req_aimrt_ts.SetTypeName(aimrt_py.GetRos2MessageTypeName({{package_name}}.srv.{{service_name}}.Request)) + {{service_name}}_req_aimrt_ts.SetSerializationTypesSupportedList(["ros2"]) + + {{service_name}}_rsp_aimrt_ts = aimrt_py.PyRos2TypeSupport({{package_name}}.srv.{{service_name}}.Response) + {{service_name}}_rsp_aimrt_ts.SetTypeName(aimrt_py.GetRos2MessageTypeName({{package_name}}.srv.{{service_name}}.Response)) + {{service_name}}_rsp_aimrt_ts.SetSerializationTypesSupportedList(["ros2"]) + + if not aimrt_py.check_is_valid_srv_type({{package_name}}.srv.{{service_name}}): + raise RuntimeError("The service type provided is not valid") + + self.Ros2RegisterServiceFunc("{{service_name}}", + {{package_name}}.srv.{{service_name}}, + {{service_name}}_req_aimrt_ts, + {{package_name}}.srv.{{service_name}}.Request, + {{service_name}}_rsp_aimrt_ts, + {{package_name}}.srv.{{service_name}}.Response, + self.{{service_name}}) + + def {{service_name}}(self, ctx_ref, req) -> tuple[aimrt_py.RpcStatus, {{package_name}}.srv.{{service_name}}.Response]: + return (aimrt_py.RpcStatus(aimrt_py.RpcStatusRetCode.SVR_NOT_IMPLEMENTED), {{package_name}}.srv.{{service_name}}.Response()) + +class {{service_name}}ServiceProxy(aimrt_py.ProxyBase): + def __init__(self, rpc_handle_ref=aimrt_py.RpcHandleRef()): + super().__init__(rpc_handle_ref, "ros2", "{{package_name}}") + self.rpc_handle_ref = rpc_handle_ref + + @overload + def {{service_name}}( + self, req: {{package_name}}.srv.{{service_name}}.Request + ) -> tuple[aimrt_py.RpcStatus, {{package_name}}.srv.{{service_name}}.Response]: ... + + @overload + def {{service_name}}( + self, ctx: aimrt_py.RpcContext, req: {{package_name}}.srv.{{service_name}}.Request + ) -> tuple[aimrt_py.RpcStatus, {{package_name}}.srv.{{service_name}}.Response]: ... + + @overload + def {{service_name}}( + self, ctx_ref: aimrt_py.RpcContextRef, req: {{package_name}}.srv.{{service_name}}.Request + ) -> tuple[aimrt_py.RpcStatus, {{package_name}}.srv.{{service_name}}.Response]: ... + + def {{service_name}}(self, *args) -> tuple[aimrt_py.RpcStatus, {{package_name}}.srv.{{service_name}}.Response]: + if len(args) == 1: + ctx = super().NewContextSharedPtr() + req = args[0] + elif len(args) == 2: + ctx = args[0] + req = args[1] + else: + raise TypeError(f"{{service_name}} expects 1 or 2 arguments, got {len(args)}") + + if isinstance(ctx, aimrt_py.RpcContext): + ctx_ref = aimrt_py.RpcContextRef(ctx) + elif isinstance(ctx, aimrt_py.RpcContextRef): + ctx_ref = ctx + else: + raise TypeError(f"ctx must be 'aimrt_py.RpcContext' or 'aimrt_py.RpcContextRef', got {type(ctx)}") + + if ctx_ref: + if ctx_ref.GetSerializationType() == "": + ctx_ref.SetSerializationType("ros2") + else: + real_ctx = aimrt_py.RpcContext() + ctx_ref = aimrt_py.RpcContextRef(real_ctx) + ctx_ref.SetSerializationType("ros2") + + status, rsp = self.rpc_handle_ref.Ros2Invoke( + "ros2:/{{package_name}}/srv/{{service_name}}", ctx_ref, req, {{package_name}}.srv.{{service_name}}.Response) + + return status, rsp + + + @staticmethod + def RegisterClientFunc(rpc_handle): + # {{service_name}} + {{service_name}}_req_aimrt_ts = aimrt_py.PyRos2TypeSupport({{package_name}}.srv.{{service_name}}.Request) + {{service_name}}_req_aimrt_ts.SetTypeName(aimrt_py.GetRos2MessageTypeName({{package_name}}.srv.{{service_name}}.Request)) + {{service_name}}_req_aimrt_ts.SetSerializationTypesSupportedList(["ros2"]) + + {{service_name}}_rsp_aimrt_ts = aimrt_py.PyRos2TypeSupport({{package_name}}.srv.{{service_name}}.Response) + {{service_name}}_rsp_aimrt_ts.SetTypeName(aimrt_py.GetRos2MessageTypeName({{package_name}}.srv.{{service_name}}.Response)) + {{service_name}}_rsp_aimrt_ts.SetSerializationTypesSupportedList(["ros2"]) + + return rpc_handle.Ros2RegisterClientFunc("ros2:/{{package_name}}/srv/{{service_name}}", + {{package_name}}.srv.{{service_name}}, + {{service_name}}_req_aimrt_ts, + {{service_name}}_rsp_aimrt_ts) + +""" + + +def generate(): + package_name = "" + service_file = "" + output_path = "" + for arg in sys.argv: + kv = arg.split('=') + if (kv[0] == '--pkg_name'): + package_name = kv[1] + elif (kv[0] == '--srv_file'): + service_file = kv[1] + elif (kv[0] == '--output_path'): + output_path = kv[1] + + if package_name == "" or service_file == "" or output_path == "": + print(f"Usage: python3 ros2_py_gen_aimrt_py_rpc.py " + "--pkg_name= " + "--srv_file= " + "--output_path=") + exit(1) + + path, file = os.path.split(service_file) + filename, ext = os.path.splitext(file) + py_file_path = os.path.join(output_path, filename + "_aimrt_rpc_ros2.py") + + # py file + f_py_file = open(py_file_path, 'w') + f_py_file.write(t_pyfile + .replace("{{package_name}}", package_name) + .replace("{{service_name}}", filename)) + f_py_file.close() + + +if __name__ == '__main__': + generate()