ROS 2でカチャカを動かしてみた Part2　ROS 2サンプルコードとRSJ2023のデモについて

こんにちはアールティ畠山です。

前回に引き続き弊社で取り扱いしている株式会社Preferred Roboticsさんが開発した「カチャカ」やカチャカAPIについて連載していきます。今回は公式のROS 2サンプルコードとRSJ2023の展示用に作成したデモをについて紹介します。この記事から読み初めてた方やカチャカAPIのセットアップができていない方は前回を読んでください。

ROS 2 サンプルコード

公式のGitHubではROS 2のサンプルコードが公開されております。2023年9月時点で公開されているサンプルコードの簡単な説明を以下に記載しました。

• kachaka_follow
  • Object DetectionとLiDARのセンサデータを用いて人を追従
• kachaka_smart_speaker
  • スマートスピーカーと連携してカチャカに指示
• kachaka_speak
  • カチャカ内蔵スピーカーから音声を発声
• kachaka_vision
  • カチャカ内蔵カメラからOpenCVを用いて手のポーズを推定
• kachaka_bringup
  • Navigation2を用いてカチャカを操作(RSJ2023以降に公開)

kachaka_speakerのように、いくつかサンプルではROS 2アクションを使って実装されています。使えるコマンドに関しては KachakaCommand.msg を見ていただくとわかります。RSJ2023の展示したデモも、これらの機能を組み合わせて実装しました。

RSJ2023の展示デモ

RSJ2023の展示のために作成したROS 2を用いた簡単なデモについて紹介します。作成したデモは以下の通りです。

・ジョイスティック操作

・ターミナルから指定した目的地点に移動

ジョイスティック操作

ジョイスティックでカチャカを操作します。

環境構築

sudo apt install joystic
sudo apt install ros-humble-joy*
sudo apt install ros-humble--teleop-tools

実行

ros2_bridgeを起動した状態で以下を実行してください。

ros2 run joy joy-node

別ターミナル

ros2 launch teleop_twist_joy teleop-launch.py joy_vel:=/kachaka/manual_control/cmd_vel

手順通りに実行すると動画のようにカチャカを操作できます。また内部で障害物検知が動いているため壁ぶつけようとしても止まってくれます。

指定した目的地点に移動

ターミナルから任意の座標を指定して目的地点に移動します。

環境構築

前回のROS 2のセットアップが完了してましたら、環境構築はいらないです。

ソースコード

以下のコードを実行すれば、動きます。

import rclpy
from rclpy.node import Node
from kachaka_interfaces.action import ExecKachakaCommand
from kachaka_interfaces.msg import KachakaCommand
from rclpy.action import ActionClient

class WaypointPub(Node):
    def __init__(self):
        super().__init__("waypoint_publisher")
        self._action_client = ActionClient(self, ExecKachakaCommand,
                                           "/kachaka/kachaka_command/execute")
        self._action_client.wait_for_server()
        
    def sent_request(self, pos_x, pos_y, yaw):
        command = KachakaCommand()
        command.command_type = KachakaCommand.MOVE_TO_POSE_COMMAND
        command.move_to_pose_command_x = pos_x
        command.move_to_pose_command_y = pos_y
        command.move_to_pose_command_yaw = yaw
        goal_msg = ExecKachakaCommand.Goal()
        goal_msg.kachaka_command = command
        future = self._action_client.send_goal_async(goal_msg)
        return future

    def return_home(self):
        command = KachakaCommand()
        command.command_type = KachakaCommand.RETURN_HOME_COMMAND
        command.return_home_command_silent = True
        goal_msg = ExecKachakaCommand.Goal()
        goal_msg.kachaka_command = command
        future = self._action_client.send_goal_async(goal_msg)
        return future

    def kacha_speaker(self, text):
        command = KachakaCommand()
        command.command_type = KachakaCommand.SPEAK_COMMAND
        command.speak_command_text = text
        sp_msg = ExecKachakaCommand.Goal()
        sp_msg.kachaka_command = command
        self._action_client.send_goal_async(sp_msg)

        
def main(args=None):
    rclpy.init(args=args)
    waypoint_pub = WaypointPub()
    waypoint_pub.kacha_speaker("アールティーブースへようこそ") 
    while True:
        waypoint_pub.kacha_speaker("もくひょうちてんをせっていしてください")
        try:
            x, y, yaw = map(float, input("目標地点を入力してください x, y, yaw =").split())
        except:
            print("入力がまちがっています")
            continue

        waypoint_pub.kacha_speaker("もくひょうちてんをせっていしました")
        future = waypoint_pub.sent_request(x, y, yaw) 
        rclpy.spin_until_future_complete(waypoint_pub, future)
        waypoint_pub.kacha_speaker("もくひょうちてんにとうちゃくしました")
        
        if input("終了しますか？ [y/n] = ") == "y":
            break
    
    waypoint_pub.kacha_speaker("ホームにもどります")   
    waypoint_pub.return_home()
    
    rclpy.spin_until_future_complete(waypoint_pub, future)
    rclpy.shutdown()

if __name__ == "__main__":
    main()