rclpyでttyUSB0を経由してM5stackにテキストを送り込むserviceを書く

段々内容が入り組んできましたが……
rclpyでttyUSB0を経由してM5stackにテキストを送り込むserviceを書いてみます
これが出来たら、もうあとはカメラやサウンドデバイスみたいな事例しかないんでは

実際のところ

サービス定義の作成

まずはサービスの定義ファイルを用意

$ cd dev_ws/src
$ ros2 pkg create m5stack_service_msg --build-type ament_cmake 
$ mkdir m5stack_service_msg/srv
$ touch m5stack_service_msg/srv/DisplayText.srv

DisplayText.srvというサービスファイルを作成し、以下の内容を追記

string text
---
bool success
string message

CMakeLists.txtに追加

##　これを追加
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/DisplayText.srv"
)
##　↓より上に
ament_package()

package.xmlにも同様に

<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

サービスサーバの作成

$ cd dev_ws/src
$ ros2 pkg create m5stack_service_ctrl --build-type ament_python  --dependencies rclpy m5stack_service_msg
$ touch m5stack_service_ctrl/m5stack_service_ctrl/service_server.py

シリアル通信を

$ pip install pyserial

service_server.pyに以下のように追記

import rclpy
from rclpy.node import Node
from m5stack_service_msg.srv import DisplayText

import serial

class M5StackServiceServer(Node):
    def __init__(self):
        super().__init__('m5stack_service_server')
        self.srv = self.create_service(DisplayText, 'display_text', self.display_text_callback)
        self.serial_port = serial.Serial('/dev/ttyUSB0', 9600)

    def display_text_callback(self, request, response):
        try:
            self.serial_port.write(request.text.encode())
            response.success = True
            response.message = "Text sent successfully!"
        except Exception as e:
            response.success = False
            response.message = str(e)
        return response

def main(args=None):
    rclpy.init(args=args)
    service_server = M5StackServiceServer()
    rclpy.spin(service_server)

if __name__ == "__main__":
    main()

setup.pyに追加もお忘れなく

#...
entry_points={
    'console_scripts': [
        'service_server = m5stack_service_ctrl.service_server:main'
    ],
},
#...

M5Stackのコード

M5Stack側もシリアル通信でデータを受け取る以下のようなプログラムを作成しM5Stackにアップロードします

#include <M5Stack.h>

void setup() {
  M5.begin();
  Serial.begin(9600);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.setTextSize(6);
}

void loop() {
  if (Serial.available()) {
    String text = Serial.readString();
    M5.Lcd.fillScreen(BLACK);
    M5.Lcd.setCursor(0, 0);
    M5.Lcd.println(text);
  }
}

ビルド

$ cd ~/dev_ws
$ colcon build
$ source install/setup.bash

実行

ターミナルを開き、以下のコマンドでサービスサーバを実行します：

$ ros2 run m5stack_service_ctrl service_server.py

別のターミナルを開き、以下のコマンドでサービスを呼び出してM5Stackにテキストを送信します：

$ ros2 service call /display_text m5stack_service_msg/srv/DisplayText "{text: 'Hello M5Stack!'}"