GoでROS2による通信を実装する

ROS2について

OMGが策定したDDSという通信プロトコルをベースにロボット関連の汎用部品とサードパーティによる部品のエコシステムを構築するロボット開発向けフレームワークです。

DDSプロトコルの特徴

• LAN内でのノードを自動発見する
• PubSub機能やサービス利用するのにトピック名やサービス名だけで通信が確立できる
• ネットワークエンドポイントがどこにあるのかを意識する必要がない
• LAN外（例えばクラウド上）のノードと疎通を取るにはブリッジか直接接続が必要
• UDPベースでQoS機能を持つがTCPを利用することもできる
• サーバースタブやクライアントスタブをIDLから生成するツールがある

インストール

公式のdockerイメージ上で作業することをお勧めします。

docker pull ros:foxy

Dockerfile

FROM ros:foxy AS build

RUN apt update && apt install curl
RUN curl -L https://golang.org/dl/go1.17.2.linux-amd64.tar.gz | tar -C /usr/local -xzf -
ENV PATH=/usr/local/go/bin:/root/go/bin:$PATH
RUN go install github.com/tiiuae/rclgo/cmd/...@latest

シンプルノード

mkdir node1
cd node1
go mod init node1
go get github.com/tiiuae/rclgo/pkg/rclgo

以下のコマンドで指定したフォルダ以下の*.msgや*.srvからコード生成される。

rclgo-gen generate -r ./node1

rclgo-gen generate-rclgoの方はエラーがでる。解消法は不明。

root@0d8c8d7d5838:/app# rclgo-gen generate-rclgo -r ./node1
Generating primitive types: /root/go/pkg/mod/github.com/tiiuae/rclgo@v0.0.0-20210714082909-df8f75e57842/pkg/rclgo/primitives/primitives.gen.go
Looking for rcl C include files to parse error definitions from './node1'
Looking for rcl C include files to parse error definitions from '.'
Looking for rcl C include files to parse error definitions from '..'
Analyzing: ../opt/ros/foxy/include/rcl/types.h
Analyzing: ../opt/ros/foxy/include/rmw/ret_types.h
Generating ROS2 Error definitions: /root/go/pkg/mod/github.com/tiiuae/rclgo@v0.0.0-20210714082909-df8f75e57842/pkg/rclgo/errortypes.gen.go

サンプル

msg/sample.msg

int64 num

生成されるコード

sample.gen.go

/*
This file is part of rclgo

Copyright © 2021 Technology Innovation Institute, United Arab Emirates

Licensed under the Apache License, Version 2.0 (the "License");
	http://www.apache.org/licenses/LICENSE-2.0
*/

/*
THIS FILE IS AUTOGENERATED BY 'rclgo-gen generate'
*/

package sample_msg

import (
	"unsafe"

	"github.com/tiiuae/rclgo/pkg/rclgo/typemap"
	"github.com/tiiuae/rclgo/pkg/rclgo/types"
)

/*
#cgo LDFLAGS: -L/opt/ros/foxy/lib -Wl,-rpath=/opt/ros/foxy/lib -lrcl -lrosidl_runtime_c -lrosidl_typesupport_c -lrcutils -lrmw_implementation
#cgo LDFLAGS: -lsample__rosidl_typesupport_c -lsample__rosidl_generator_c

#cgo CFLAGS: -I/opt/ros/foxy/include

#include <rosidl_runtime_c/message_type_support_struct.h>

//#include <sample/msg/sample.h>

*/
import "C"

func init() {
	typemap.RegisterMessage("sample/sample", sampleTypeSupport)
}

// Do not create instances of this type directly. Always use Newsample
// function instead.
type sample struct {
	Num int64 `yaml:"num"`
}

// Newsample creates a new sample with default values.
func Newsample() *sample {
	self := sample{}
	self.SetDefaults()
	return &self
}

func (t *sample) Clone() *sample {
	c := &sample{}
	c.Num = t.Num
	return c
}

func (t *sample) CloneMsg() types.Message {
	return t.Clone()
}

func (t *sample) SetDefaults() {
	t.Num = 0
}

// ClonesampleSlice clones src to dst by calling Clone for each element in
// src. Panics if len(dst) < len(src).
func ClonesampleSlice(dst, src []sample) {
	for i := range src {
		dst[i] = *src[i].Clone()
	}
}

// Modifying this variable is undefined behavior.
var sampleTypeSupport types.MessageTypeSupport = _sampleTypeSupport{}

type _sampleTypeSupport struct{}

func (t _sampleTypeSupport) New() types.Message {
	return Newsample()
}

func (t _sampleTypeSupport) PrepareMemory() unsafe.Pointer { //returns *C.sample__msg__sample
	return (unsafe.Pointer)(C.sample__msg__sample__create())
}

func (t _sampleTypeSupport) ReleaseMemory(pointer_to_free unsafe.Pointer) {
	C.sample__msg__sample__destroy((*C.sample__msg__sample)(pointer_to_free))
}

func (t _sampleTypeSupport) AsCStruct(dst unsafe.Pointer, msg types.Message) {
	m := msg.(*sample)
	mem := (*C.sample__msg__sample)(dst)
	mem.num = C.int64_t(m.Num)
}

func (t _sampleTypeSupport) AsGoStruct(msg types.Message, ros2_message_buffer unsafe.Pointer) {
	m := msg.(*sample)
	mem := (*C.sample__msg__sample)(ros2_message_buffer)
	m.Num = int64(mem.num)
}

func (t _sampleTypeSupport) TypeSupport() unsafe.Pointer {
	return unsafe.Pointer(C.rosidl_typesupport_c__get_message_type_support_handle__sample__msg__sample())
}

type Csample = C.sample__msg__sample
type Csample__Sequence = C.sample__msg__sample__Sequence

func sample__Sequence_to_Go(goSlice *[]sample, cSlice Csample__Sequence) {
	if cSlice.size == 0 {
		return
	}
	*goSlice = make([]sample, int64(cSlice.size))
	for i := 0; i < int(cSlice.size); i++ {
		cIdx := (*C.sample__msg__sample__Sequence)(unsafe.Pointer(
			uintptr(unsafe.Pointer(cSlice.data)) + (C.sizeof_struct_sample__msg__sample * uintptr(i)),
		))
		sampleTypeSupport.AsGoStruct(&(*goSlice)[i], unsafe.Pointer(cIdx))
	}
}
func sample__Sequence_to_C(cSlice *Csample__Sequence, goSlice []sample) {
	if len(goSlice) == 0 {
		return
	}
	cSlice.data = (*C.sample__msg__sample)(C.malloc((C.size_t)(C.sizeof_struct_sample__msg__sample * uintptr(len(goSlice)))))
	cSlice.capacity = C.size_t(len(goSlice))
	cSlice.size = cSlice.capacity

	for i, v := range goSlice {
		cIdx := (*C.sample__msg__sample)(unsafe.Pointer(
			uintptr(unsafe.Pointer(cSlice.data)) + (C.sizeof_struct_sample__msg__sample * uintptr(i)),
		))
		sampleTypeSupport.AsCStruct(unsafe.Pointer(cIdx), &v)
	}
}
func sample__Array_to_Go(goSlice []sample, cSlice []Csample) {
	for i := 0; i < len(cSlice); i++ {
		sampleTypeSupport.AsGoStruct(&goSlice[i], unsafe.Pointer(&cSlice[i]))
	}
}
func sample__Array_to_C(cSlice []Csample, goSlice []sample) {
	for i := 0; i < len(goSlice); i++ {
		sampleTypeSupport.AsCStruct(unsafe.Pointer(&cSlice[i]), &goSlice[i])
	}
}

main.go

package main

import (
	"context"
	"log"
	"time"

	std_srvs_srv "github.com/tiiuae/rclgo-msgs/std_srvs/srv"
	"github.com/tiiuae/rclgo/pkg/rclgo"
	"github.com/tiiuae/rclgo/pkg/rclgo/types"

	_ "sample_node/msgs"
)

//go:generate go run github.com/tiiuae/rclgo/cmd/rclgo-gen generate -r ./

func main() {
	ctx, cancel := context.WithCancel(context.Background())
	defer cancel()
	rclCtx, err := rclgo.NewContext(nil, 0, nil)
	if err != nil {
		log.Fatal(err)
	}
	node, err := rclCtx.NewNode("service", "/health")
	if err != nil {
		log.Fatal(err)
	}
	service, err := node.NewService(
		"check",
		std_srvs_srv.EmptyTypeSupport,
		nil,
		func(r *rclgo.RmwServiceInfo, t types.Message, rs rclgo.ServiceResponseSender) {
			log.Println(r, t, rs)
		},
	)
	if err != nil {
		log.Fatal(err)
	}
	log.Println(node)
	waitSet, err := rclCtx.NewWaitSet(time.Second)
	waitSet.AddServices(service)
	waitSet.RunGoroutine(ctx)
	rclCtx.WG.Wait()
}

まとめ

この記事は書きかけのまま長く放置してたんだけど、ROSに関連するイベントに展示することになったので話のネタに公開しておく。
ただROSもROS2も依存解決方法がOSのバージョンに深く依存しすぎているように感じる。
利用者は常にOSのバージョンやROSのバージョンの組み合わせに苦労している。

ここでGoのこのツールチェインならほぼOSのバージョンどころかmacOSでもWindowsでも問題なくROS2のノードを構築できる。これは大きなメリットかなぁと思う。
本家の定義を参照するためにdockerなどの環境は必要だけど、アプリケーションを動かすのにはdockerは不要。