ros2_javaをビルトして, androidでros2を動かす

はじめに

本記事はROS 2アドベントカレンダー11日目の記事です．

AndroidでROS2対応のコントローラを作成するときUnityを使ってアプリケーションを作るのが一般的？ですが
既に作っていたAndroid Studinoで作ったコントローラを流用したかったため、kotlinでROS2使えたら便利だなとか思っていましてインターネットで探していたところ、ちょうど便利なライブラリを発見しました。

FastDDSを使って直接通信しているのでWebSocketなどを介してROS2と通信するより、リアルタイムタイム性にも優れていそうです。
しかもdockerでビルト用の環境も有志の方が開発されていたりと何かしら便利なので、試しに使ってみましった
ビルト問題とROS_DOMAIN_ID問題でかなり苦戦したのと、あまりこのライブラリを紹介している記事を見ないので布教も兼ねて記事にしてみました

本記事の対象読者

• Android Studinoでの開発経験があること
• rclcppなどを使ってプログラムが組めるレベルの人

注意点

• ros2 humble以外のバージョンは動作未確認（ros2_java_android.reposを編集すれば行けるかも？）
• arm系のCPU以外の（x86_64)などのCPUを搭載したスマートフォンは動作非対応（コンパイルのフラグを変更するれば動作可能？）
• sensor_msgs/Image.msgなどはros2_java自体のバグで動作不良（無理矢理プログラムを改造すれば一応動作可能）
• ros2_javaのドキュメントが全くない
• ros2_java自体３年前から更新が止まっているため、サポートを受けられる可能性はかなり低い（オープンソースだから当然だが）

ライブラリのコンパイル

まずros2javaのdocker build環境をgithubからクローンします。素のものだとライブラリがアップデートされていて依存関係問題でコンパイルが通らないのと。普段使っているrosのバージョンがhumbleなので、少し修正したものを使っています

git clone https://github.com/midarubiccube/ros2-android-build

次にDocker buildしてでDockerimageを作成します。NDKや様々なパッケージのダウンロードがあるため少し待ちます

docker build -t ros2java-android-build ./

次に、ライブラリ作成用のpythonを実行してrosパッケージをコンパイルします

python3 run.py ./out/soOut ./out/jarOut

実行するとros2関連のパッケージがダウンロードされ、Docker内のコンパイラが起動しパッケージがビルトされますが、多分このようなエラーが出ると思います。

/home/user/workspace/build/uncrustify_vendor/uncrustify-0.72.0-prefix/src/uncrustify-0.72.0/src/log_rules.cpp:51:12: error: use of undeclared identifier 'rindex'
   where = rindex(func, ':');

色々調べたところ、uncrustify_vendorのgithubにissuesが立っていて、issuesソースコード自体が間違っていて(多分)、修正しなくてはならないようです。

gitからcloneしたros2-android-buildディレクトリにtmpフォルダとがdockerイメージ内の/home/user/workspace/がリンクしているので、tmp/build/uncrustify_vendor/uncrustify-0.72.0-prefix/src/uncrustify-0.72.0/srcと、たどって行けば問題のファイルにたどりつけるはずです。そしてこのファイルの50行目付近を以下のように編集します。

         break;
      }
   }
#else // not WIN32
-　where = rindex(func, ':');
+　where = strrchr(func, ':'); 
#endif /* ifdef WIN32 */

   if (where == nullptr)

150個ほどのパッケージのビルトが終わればoutフォルダ内のsoOutに1000個ぐらいのsoファイルが、jarOutにはjarファイルが生成されているはずです。

Android Studino Projectにライブラリを組み込む

次に、生成されたjarとsoをandroid studinoのプロジェクトに組み込んで行きます
まず、android studinoでEmpty Views Activiyをベースにプロジェクトを作成します

Minimum SDKはros2ライブラリ側でAPI Level 24に設定されているので　最低でもAPI Level 24に設定します

プロジェクトを作成できたら、前章で作成したライブラリを導入します
まずsoファイルを導入します、<プロジェクト名>\app\src\mainにjniLibsディレクトリを新規作成し直下にarm64-v8aディレクトリを新規作成します。そこにsoOut内のsoファイルをすべてコピーします。

次にjarファイルを導入します、<プロジェクト名>\appにlibsディレクトリを新規作成し、直下にjarOut内のすべてのファイルをコピーします。

次にbuild.gradle.ktsを編集します。

app/build.gradle.ktsのdefaultConfigに以下の文を追加します

build.gradle.kts

 defaultConfig {
        //省略
        ndk {
            abiFilters += "arm64-v8a"//armしか対応していないのでフィルタしている
        }
    }

またdependenciesに以下の文を追加します

build.gradle.kts

 dependencies {
    //省略
    implementation(fileTree(mapOf("dir" to "libs", "include" to listOf("*.jar", "*.aar"))))//lib内のjarファイルを依存関係に追加
}

エディター上部に表示れているsync nowをクリックすれば下準備は完了です。

以上でライブラリを導入できました

rcljavaを用いてtopicをPublish

まずテストでtutlesimを操作するアプリを作ってみます

まず上の画像２つをダウンロードして名前を左の画像をbase.png、右の画像をstick.pngにファイル名を変更。
次に２つの画像をapp/src/layout/drawableに移します

次にJoyStick.ktを作成します。下のプログラムをコピーて貼り付け

JoyStick.kt

package com.example.ros2test

import android.annotation.SuppressLint
import android.content.Context
import android.content.res.Resources
import android.graphics.Bitmap
import android.graphics.BitmapFactory
import android.graphics.Canvas
import android.graphics.Color
import android.graphics.Paint
import android.graphics.PixelFormat
import android.graphics.PorterDuff
import android.util.AttributeSet
import android.view.MotionEvent
import android.view.SurfaceHolder
import android.view.SurfaceView
import android.view.ViewGroup
import kotlin.math.atan
import kotlin.math.cos
import kotlin.math.pow
import kotlin.math.sin
import kotlin.math.sqrt

class JoyStick(context: Context, attrs: AttributeSet?) :
    SurfaceView(context, attrs), SurfaceHolder.Callback {
    private val DENO_RATE_STICK_TALL_TO_SIZE = 25
    private val DENO_RATE_STICK_SIZE_TO_PAD = 2
    private val DENO_RATE_OFFSET_TO_PAD = 3
    private val ALPHA_PAD_DEFAULT = 150
    private val ALPHA_STICK_DEFAULT = 180
    private var alphaStick = 200
    private var alphaLayout = 200
    var offset = 0
    private var surfaceHolder: SurfaceHolder? = null
    private var params: ViewGroup.LayoutParams? = null
    private var stickTall = 0
    private var stickWidth = 0
    private var stickHeight = 0
    private var positionX = 0
    private var positionY = 0
    private var distance = 0f
    private val jsEntity // joy stick entity
            : JoyStickEntity
    private var alphaBacksPaint: Paint? = null
    private var alphaStickPaint: Paint? = null
    private val res: Resources
    private var background: Bitmap? = null
    private var stick: Bitmap? = null


    val getPosX : Float
        get() = if (jsEntity.isTouched()|| jsEntity.islock()) {
            (jsEntity.x - (jsEntity.centerX - stickWidth / 2)) / (params!!.width / 6)

        } else 0f

    val getPosY : Float
        get() = if (jsEntity.isTouched() || jsEntity.islock()) {
            (jsEntity.y - (jsEntity.centerY - stickHeight / 2)) / (params!!.height / 6)
        } else 0f

    var layoutAlpha: Int
        get() = alphaLayout
        set(alpha) {
            alphaLayout = alpha
            alphaBacksPaint!!.alpha = alpha
        }
    var stickAlpha: Int
        get() = alphaStick
        set(alpha) {
            alphaStick = alpha
            alphaStickPaint!!.alpha = alpha
        }

    init {
        if (!isInEditMode) setZOrderOnTop(true)
        initHolder()
        res = context.resources
        loadImages(res)
        initAlphaPaints()
        jsEntity = JoyStickEntity()
        registerOnTouchEvent()
    }

    private fun initHolder() {
        surfaceHolder = holder
        surfaceHolder!!.addCallback(this)
        surfaceHolder!!.setFormat(PixelFormat.TRANSPARENT)
    }

    private fun loadImages(res: Resources) {
        releaseJoyStickImages()
        background = BitmapFactory.decodeResource(res, R.drawable.base)
        stick = BitmapFactory.decodeResource(res, R.drawable.stick)
    }


    private fun initAlphaPaints() {
        alphaBacksPaint = Paint()
        alphaStickPaint = Paint()
    }

    override fun surfaceCreated(holder: SurfaceHolder) {
        init()
        val canvas = surfaceHolder!!.lockCanvas()
        drawBaseCanvas(canvas)
        drawStick(canvas)
        surfaceHolder!!.unlockCanvasAndPost(canvas)
    }

    override fun surfaceChanged(holder: SurfaceHolder, format: Int, width: Int, height: Int) {}

    override fun surfaceDestroyed(holder: SurfaceHolder) {}

    private fun init() {
        registerScreenSize()
        registerLayoutCenter(params!!.width, params!!.height)
        registerStickSize()
        stickTall = stickHeight / DENO_RATE_STICK_TALL_TO_SIZE // make user feel sticky
        setStickSize(params!!.width / DENO_RATE_STICK_SIZE_TO_PAD,
            params!!.height / DENO_RATE_STICK_SIZE_TO_PAD
        )
        layoutAlpha = ALPHA_PAD_DEFAULT
        stickAlpha = ALPHA_STICK_DEFAULT
        offset = params!!.width / DENO_RATE_OFFSET_TO_PAD
        resizeImages()
    }

    private fun registerScreenSize() {
        params = ViewGroup.LayoutParams(width, height)
    }

    private fun registerStickSize() {
        if (stick == null) return
        stickWidth = stick!!.width
        stickHeight = stick!!.height
    }

    @SuppressLint("ClickableViewAccessibility")
    private fun registerOnTouchEvent() {
        setOnTouchListener { _, event ->
            drawJoyStickWith(event)
            true
        }
    }

    private fun registerLayoutCenter(width: Int, height: Int) {
        jsEntity.centerX = (width / 2).toFloat()
        jsEntity.centerY = (height / 2).toFloat()
    }

    private fun releaseJoyStickImages() {
        if (background != null) background!!.recycle()
        if (stick != null) stick!!.recycle()
    }

    private fun drawJoyStickWith(event: MotionEvent) {
        val canvas = surfaceHolder!!.lockCanvas()
        drawBaseCanvas(canvas)
        drawStick(canvas, event)
        surfaceHolder!!.unlockCanvasAndPost(canvas)
    }

    private fun drawBaseCanvas(canvas: Canvas) {
        canvas.drawColor(Color.TRANSPARENT, PorterDuff.Mode.CLEAR)
        drawBackground(canvas)
    }

    private fun drawStick(canvas: Canvas, event: MotionEvent) {
        positionX = (event.x - params!!.width / 2).toInt()
        positionY = (event.y - params!!.height / 2).toInt()
        distance =
            sqrt(positionX.toDouble().pow(2.0) + positionY.toDouble().pow(2.0))
                .toFloat()
        val midDistanceX = (params!!.width / 2 - offset).toFloat()
        val midDistanceY = (params!!.height / 2 - offset).toFloat()
        if (event.action == MotionEvent.ACTION_DOWN) {
            if (distance <= midDistanceX) {
                jsEntity.position(event.x, event.y)
                jsEntity.setlock(false)
                jsEntity.setTouched(true)
            }
        } else if (event.action == MotionEvent.ACTION_MOVE && jsEntity.isTouched()) {
            if (distance <= midDistanceX) {
                jsEntity.position(event.x, event.y)
            } else {
                var x = (cos(
                    Math.toRadians(
                        calAngle(
                            positionX.toFloat(),
                            positionY.toFloat()
                        )
                    )
                ) * midDistanceX).toFloat()
                var y = (sin(
                    Math.toRadians(
                        calAngle(
                            positionX.toFloat(),
                            positionY.toFloat()
                        )
                    )
                ) * midDistanceY).toFloat()
                x += (params!!.width / 2).toFloat()
                y += (params!!.height / 2).toFloat()
                jsEntity.position(x, y)
            }
        } else if (event.action == MotionEvent.ACTION_UP) {
            // reset stick pad
            drawBaseCanvas(canvas)
            jsEntity.position(0f,0f)
            jsEntity.setTouched(false)
        }

        drawStick(canvas)
    }

    private fun drawStick(canvas: Canvas) {
        if (jsEntity.isTouched()) {
            canvas.drawBitmap(stick!!, jsEntity.x, jsEntity.y, alphaStickPaint)
        } else {
            canvas.drawBitmap(
                stick!!,
                jsEntity.centerX - stickWidth / 2,
                jsEntity.centerY - stickHeight / 2, alphaStickPaint
            )
        }
    }

    private fun drawBackground(canvas: Canvas) {
        canvas.drawBitmap(background!!, 0f, 0f, alphaBacksPaint)
    }

    private fun resizeImages() {
        stick = resizeImage(stick, stickWidth, stickHeight)
        background = resizeImage(background, params!!.width, params!!.height)
    }

    private fun resizeImage(original: Bitmap?, targetWidth: Int, targetHeight: Int): Bitmap {
        return Bitmap.createScaledBitmap(original!!, targetWidth, targetHeight, false)
    }

    fun setStickSize(width: Int, height: Int) {
        stickWidth = width
        stickHeight = height
    }

    private fun calAngle(x: Float, y: Float): Double {
        if (x >= 0 && y >= 0) return Math.toDegrees(atan((y / x).toDouble())) else if (x < 0 && y >= 0) return Math.toDegrees(
            atan((y / x).toDouble())
        ) + 180 else if (x < 0 && y < 0) return Math.toDegrees(atan((y / x).toDouble())) + 180 else if (x >= 0 && y < 0) return Math.toDegrees(
            atan((y / x).toDouble())
        ) + 360
        return 0.0
    }

    private inner class JoyStickEntity {
        private var isTouched = false
        private var islock = false
        var x = 0f
        var y = 0f
        var centerX = 0f
        var centerY = 0f // center

        fun position(posx: Float, posy: Float) {
            x = posx - stickWidth / 2
            y = posy - stickHeight / 2
        }

        fun isTouched(): Boolean {
            return isTouched
        }

        fun setTouched(touched: Boolean) {
            isTouched = touched
        }

        fun islock(): Boolean {
            return islock
        }

        fun setlock(lock: Boolean) {
            islock = lock
        }
    }
}

activity_mainを以下のように編集します。com.example.ros2test.JoyStickはjavaのパッケージ名に合わせて各自で設定してください。

activity_main.xml

<?xml version="1.0" encoding="utf-8"?>
<androidx.constraintlayout.widget.ConstraintLayout xmlns:android="http://schemas.android.com/apk/res/android"
    xmlns:app="http://schemas.android.com/apk/res-auto"
    xmlns:tools="http://schemas.android.com/tools"
    android:id="@+id/main"
    android:layout_width="match_parent"
    android:layout_height="match_parent"
    tools:context=".MainActivity">

    <com.example.ros2test.JoyStick
        android:id="@+id/JoyStick"
        android:layout_width="0dp"
        android:layout_height="200dp"
        android:layout_marginEnd="16dp"
        app:layout_constraintBottom_toBottomOf="parent"
        app:layout_constraintDimensionRatio="1:1"
        app:layout_constraintEnd_toEndOf="parent"
        app:layout_constraintStart_toStartOf="parent"
        app:layout_constraintTop_toTopOf="parent" />

</androidx.constraintlayout.widget.ConstraintLayout>

MainActivity.ktを以下のように変更します

MainActivity.kt

package com.example.ros2test

import android.os.Bundle
import android.os.Handler
import android.widget.Button
import androidx.activity.ComponentActivity
import androidx.activity.enableEdgeToEdge
import geometry_msgs.msg.Vector3
import org.ros2.rcljava.RCLJava
import org.ros2.rcljava.executors.Executor
import org.ros2.rcljava.executors.SingleThreadedExecutor
import org.ros2.rcljava.node.BaseComposableNode
import org.ros2.rcljava.publisher.Publisher
import java.util.Timer
import java.util.TimerTask

class MainActivity : ComponentActivity() {
    lateinit var Node : BaseComposableNode
    lateinit var publisher: Publisher<geometry_msgs.msg.Twist>
    lateinit var executor: Executor
    lateinit var timer: Timer
    lateinit var handler: Handler

    private val SPINNER_PERIOD_MS : Long = 200//実行周期を200ミリ秒に設定
    private val SPINNER_DELAY : Long  = 0

    lateinit var joyStick: JoyStick
    private var count = 0

    override fun onCreate(savedInstanceState: Bundle?) {
        super.onCreate(savedInstanceState)
        enableEdgeToEdge()
        setContentView(R.layout.activity_main)

        this.handler = Handler(mainLooper)
        RCLJava.rclJavaInit()
        this.executor = SingleThreadedExecutor()

        Node = BaseComposableNode("android_controller")//ノード名を設定

        publisher = Node.node.createPublisher(
            geometry_msgs.msg.Twist::class.java, "/turtle1/cmd_vel" //Publisherを作成
        )

        joyStick = findViewById(R.id.JoyStick)

        timer = Timer()
        timer.schedule(
            object : TimerTask() {
                override fun run() {
                    val msg = geometry_msgs.msg.Twist()
                    val linear = Vector3()
                    val angular = Vector3()
                    linear.x = joyStick.getPosX.toDouble()
                    linear.y = joyStick.getPosY.toDouble() * -1
                    msg.linear = linear
                    msg.angular = angular
                    publisher.publish(msg);
                }
            }, 100, 10
        )
    }

    override fun onResume() {
        super.onResume()
        timer = Timer()
        timer.schedule(
            object: TimerTask() {
                override fun run() {
                    val runnable = Runnable { executor.spinSome() }//スピンを実行
                    handler.post(runnable)
                }
            }, SPINNER_DELAY, SPINNER_PERIOD_MS)
    }

    override fun onPause() {
        super.onPause()
        timer.cancel()//ポーズ時にspinの実行をとめる
    }
}

インターネットを利用するためAndroidManifest.xmlにパーミッションを追加します。

AndroidManifest.xml

<?xml version="1.0" encoding="utf-8"?>
<manifest xmlns:android="http://schemas.android.com/apk/res/android"
    xmlns:tools="http://schemas.android.com/tools">

    <uses-permission android:name="android.permission.INTERNET" />

以上を編集してビルトします。

ビルトしたアプリを起動し、ros2が動かせるubuntuなどを用意し，スマホと同じWiFiに接続して，ターミナルに以下のコマンドを入力します

$ ros2 topic list
$ ros2 node list

うまく実装できていれば
以下のとおりになるです

うまく認識されない場合、スマホのモバイルデータをオフにしてみてください。

次に以下のコマンドを実行してください

$ ros2 run turtlesim turtlesim_node

画面中央のJoystickを操作すると亀が動きます

ROS_DOMAIN_IDを変更したい場合

色々調べたところROS_DOMAIN_IDを変更するにはrcljavaのプログラムの変更が必要らしいです。さらに調べてみたところ

ros2forunityでは環境変数いじって設定しているので、もしかしたらros2javaも環境変数いじればどうにかなるかもと思って実装してみたら何故か？うまく変更できました。

Os.setenv("ROS_DOMAIN_ID", "123", true)

123を設定したいID変更して、この一文をRCLJava.rclJavaInit()の前に追加してください。

終わり

ライブラリのビルトやROS_DOMAIN_IDの変更にかなり手こずりましたが、androidでなんとかros2動かすことができました、標準メッセージやサービスに対応しているので、これを応用すればかなりいいロボットコントロールアプリができるのではないでしょうか。

参考サイト

ros2_javaを使って、Android用ROS2ライブラリを作成する方法
How to use ROS2 on Android
Compile error：use of undeclared identifier 'rindex
empy version incompatibility #602
Androidでジョイスティックを作ってみた