Dora-rs 机器人框架学习教程（3）——C++节点发送JSON字符串到python节点再发送到ros2

前言

目标：linux系统中在Dora框架下建立C++节点，并以JSON格式发送字符串Python节点进行ROS2的话题发布。
背景：由于Dora框架处于开发阶段，我们在调试过程中，需要将传感器等数据发送到rviz中显示，但时经常出现各种BUG，因此使用字符串的形式发送到ROS2中，ROS2再写节点进行解析。
完成程度：可以从C++发送JSON字符串到Pyhon节点并发布到ROS2中。
Dora版本：0.3.1

1 编写C++节点

首先，安装JSON库：

sudo apt update
sudo apt install nlohmann-json3-dev

其次，创建C++节点，在node-c-api目录下，创建文件main.cc，写入以下测试代码：

extern "C"
{
#include "../../../apis/c/node/node_api.h"//如果找不到node_api.h文件换成绝对路径。
}

#include <iostream>
#include <vector>

#include <nlohmann/json.hpp>

using json = nlohmann::json;
int run(void *dora_context)
{
    unsigned char counter = 0;

    for (int i = 0; ; i++)
    {
        void *event = dora_next_event(dora_context);
        if (event == NULL)
        {
            printf("[c node] ERROR: unexpected end of event\n");
            return -1;
        }

        enum DoraEventType ty = read_dora_event_type(event);

        if (ty == DoraEventType_Input)
        {
            counter += 1;

            char *id_ptr;
            size_t id_len;
            read_dora_input_id(event, &id_ptr, &id_len);
            std::string id(id_ptr, id_len);

            char *data_ptr;
            size_t data_len;
            read_dora_input_data(event, &data_ptr, &data_len);
            std::vector<unsigned char> data;
            for (size_t i = 0; i < data_len; i++)
            {
                data.push_back(*(data_ptr + i));
            }

            std::cout
                << "Received input "
                << " (counter: " << (unsigned int)counter << ") data: [";
            for (unsigned char &v : data)
            {
                std::cout << (unsigned int)v << ", ";
            }
            std::cout << "]" << std::endl;

            std::vector<unsigned char> out_vec{counter};
            std::string out = "counter";
            std::string out_id = "counter";
            // 创建一个空的 JSON 对象
            json j;

            // 添加数据到 JSON 对象
            j["name"] = "John Doe";
            j["age"] = 30;
            j["is_student"] = false;
            j["courses"] = {"math", "english", "history"};

            // 将 JSON 对象序列化为字符串
            std::string json_string = j.dump(4); // 参数 4 表示缩进宽度
            // 将字符串转换为 char* 类型
            char *c_json_string = new char[json_string.length() + 1];
            strcpy(c_json_string, json_string.c_str());

            std::cout<<json_string;
            int result = dora_send_output(dora_context, &out_id[0], out_id.length(), c_json_string, std::strlen(c_json_string));
            if (result != 0)
            {
                std::cerr << "failed to send output" << std::endl;
                return 1;
            }
        }
        else if (ty == DoraEventType_Stop)
        {
            printf("[c node] received stop event\n");
        }
        else
        {
            printf("[c node] received unexpected event: %d\n", ty);
        }

        free_dora_event(event);
    }
    return 0;
}

int main()
{
    std::cout << "HELLO FROM C++ (using C API)" << std::endl;

    auto dora_context = init_dora_context_from_env();
    auto ret = run(dora_context);
    free_dora_context(dora_context);

    std::cout << "GOODBYE FROM C++ node (using C API)" << std::endl;

    return ret;
}

编译C++库：

clang++ node-c-api/main.cc -lm -lrt -ldl -lpcap -pthread -std=c++14 -ldora_node_api_c -L ../../target/release --output build/node_c_api

2 创建python的节点示例

创建脚本test_operator.py，写入

from typing import Callable, Optional
from dora import DoraStatus
import json
from json import *
import dora
import pyarrow as pa
class Operator:
    def __init__(self) -> None:
        print("Python Operator Init")
        """Called on initialisation"""
         # Create a ROS2 Context
        self.ros2_context = dora.experimental.ros2_bridge.Ros2Context()
        self.ros2_node = self.ros2_context.new_node(
            "turtle_teleop",
            "/ros2_demo_imu",
            dora.experimental.ros2_bridge.Ros2NodeOptions(rosout=True),
        )

        # Define a ROS2 QOS
        self.topic_qos = dora.experimental.ros2_bridge.Ros2QosPolicies(
            reliable=True, max_blocking_time=0.1
        )

        # Create a publisher to imu topic
        self.turtle_imu_topic = self.ros2_node.create_topic(
            "/turtle1/string", "std_msgs::String", self.topic_qos
        )
        self.imu_writer = self.ros2_node.create_publisher(self.turtle_imu_topic)
 
    def on_event(
        self,
        dora_event,
        send_output,
    ) -> DoraStatus:
        print("Python Operator working")

        if dora_event["type"] == "INPUT":
            data = dora_event["value"].to_pylist()
            json_string = ''.join(chr(int(num)) for num in data)
            print(json_string)
            # 假设 json_string 是收到的 JSON 数据字符串
            json_dict = json.loads(json_string)

            # 从 JSON 数据中提取关键字
            age = json_dict["age"]
            name = json_dict["name"]
            is_student = json_dict["is_student"]
            courses = json_dict["courses"]

            # 输出提取的关键字
            print("Age:", age)
            print("Name:", name)
            print("Is Student:", is_student)
            print("Courses:", courses)
            
            imu_dict = {"data":json_string}
            self.imu_writer.publish(pa.array([imu_dict]))
            # std_msgs/String
        return DoraStatus.CONTINUE
        

3 编写test.yml文件

创建test.yml并运行

touch test.yml

并将以下代码写入其中：

nodes:
  - id: cxx-node-c-api
    custom:
      source: build/node_c_api
      inputs:
        tick: dora/timer/millis/10
      outputs:
        - counter

  - id: test
    operator:
      python: test_operator.py
      inputs: 
        counter: cxx-node-c-api/counter

运行数据流test.yml

dora up
dora start test.yml --name test

Dora查看日志

dora logs test test

效果：

ROS2查看话题

ros2 topic echo /turtle1/string

效果：