Turtlebot3(ROS1)をAR Markerでナビゲーションしてみた

はじめに

株式会社Ristのロボットチームの原山です。

Ristは画像認識AIの会社ですが、その他にも色々とやっております。
その中にロボット事業もあり、私はその一員です。

この記事について

会社では検証機として、Turtlebot3 BurgerとWaffle Piを買ってもらいました。
今回はWaffle Piの方を使用したデモ開発について書いていこうと思います。

私について

2019年10月にRistに入社。

元々はWebプログラマーを長くやっていました。
LAMPで仕事をすることが多かったLaravelとかVue.jsが好きなプログラマでした。
Ristでは、Flaskを使用したアプリに携わったりしていて、本当にWebの人間がROSに挑戦してみた話でもあります。

どんなデモ？

製造業の工場内の検査を想定したデモを開発します。
走行ロボットが工場内を想定し社内を巡回します。
アナログメーターの前で停止し、メーターの表示に異常が無いかをチェックします。

このデモでやりたいこと

ARMarkerを利用した自律制御
Manipulatorの制御

環境

OS:Ubuntu 16.04
ROS:ROS1 Kinetic

実機

Turtlebot 3 Waffle Pi
Open Manipulator

カスタマイズしました！

Turtlebot3 FriendsのCarrierを参考にカスタマイズを行いました。

違いはマニュピレーターの先端にグリッパーを付けず、変わりにメーターを撮影するためにWebカメラを取り付けています。

ツール

ArduinoIDE
Dynamixel Wizard 2.0

セットアップ

セットアップ等もここにそって、catkin_makeではなくcatkin buildを使います。

Remote PCセットアップ

1. スクリプト落としてきてシェル変更して実行します。

aptパッケージのインストールやワークスペースを作ってくれます。

• catkin buildをするので、install_ros_kinetic.shの67行目〜76行目いらないですね。
  使わなかったので、コメントか削除してしまって問題ないと思います。

$ sudo apt-get update
$ sudo apt-get upgrade
$ wget https://raw.githubusercontent.com/ROBOTIS-GIT/robotis_tools/master/install_ros_kinetic.sh && chmod 755 ./install_ros_kinetic.sh && bash ./install_ros_kinetic.sh

シェルの中身はこちら

2. Turtlebot3を動かすためのパッケージをインストールします。

$ sudo apt-get install ros-kinetic-joy ros-kinetic-teleop-twist-joy ros-kinetic-teleop-twist-keyboard ros-kinetic-laser-proc ros-kinetic-rgbd-launch ros-kinetic-depthimage-to-laserscan ros-kinetic-rosserial-arduino ros-kinetic-rosserial-python ros-kinetic-rosserial-server ros-kinetic-rosserial-client ros-kinetic-rosserial-msgs ros-kinetic-amcl ros-kinetic-map-server ros-kinetic-move-base ros-kinetic-urdf ros-kinetic-xacro ros-kinetic-compressed-image-transport ros-kinetic-rqt-image-view ros-kinetic-gmapping ros-kinetic-navigation ros-kinetic-interactive-markers

• Turtlebot3リポジトリとTurtlebot3 Manipulatorリポジトリから

$ cd ~/catkin_ws/src/
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
$ git clone -b kinetic-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_manipulation.git
$ git clone https://github.com/ROBOTIS-GIT/turtlebot3_manipulation_simulations.git
$ cd ~/catkin_ws && catkin build

Turtlebot3のセットアップ

Turtlebot3はRaspberry pi3 + OpenCR + Open Manipulator

1.Raspberry pi 3 B+

• こちらにそってOS imageをSDカードに焼きます。

  • Raspian用のOS imageをDownloadして、Etcher.ioソフトで焼いて設定を少ししているだけです。

• 起動後にIPの確認をしてください。

$ ifconfig

• IPの設定

$ vi ~/.bashrc

• 追加

  # REMOTE_PC_IP　RASPBERRY_PI_3_IPをそれそれ設定する
  
  export ROS_MASTER_URI=http://REMOTE_PC_IP:11311
  export ROS_HOSTNAME=RASPBERRY_PI_3_IP

• 反映実行する

source ~/.bashrc

ラズパイカメラとUSBカメラをTurtlebot3上で実行するためにクローンとビルド

• ラズパイカメラ

$ git clone https://github.com/UbiquityRobotics/raspicam_node
$ catkin build

• USBカメラ

$ git clone  https://github.com/ros-drivers/usb_cam.git
$ catkin build

2. OpenCRへのプログラムアップロード

ArduinoIDEを使用します。ない人はインストールをします。

• PCとOpenCRを繋いでください。

• ArduinoIDEを起動します。

• ファイル->環境設定->追加ボードマネージャーのURL

• 下記を追加

https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json

• ツール->ボードｰ>ボードマネージャーでOpenCRを選択で OpenCR by ROBOTISパッケージをインストール*

• ツールｰ>シリアルポートｰ>/dev/ttyACM0を選択

3. モーターにIDをふる

ID振らなきゃいけないの知らなくて、長い間、マニュピレーターを動かせずにいましたね。反省。

• マニュアルからインストールしてください。
• ファイルｰ>スッケッチ例ｰ>OpenCR->10.Etc->usb_to_dxlを選択
• 矢印ボタン押下でアップロードします。
• コンソールに"jump_to_fw"と出れば成功です。
• Dynamixel Wizard 2.0を起動すると左上のスキャンボタンを押下するとスキャン開始。
• wheel motorsにID:1とID:２を振ります。
• マニュピレーターは下からID:11、ID:12、ID:13、ID:14と振ります。
• Baud Rate 100000000(桁あってるかな。。。)に設定

4. OpenCRへのTurtlbot Manipulatorプログラムをアップロード

• ファイルｰ>スッケッチ例ｰ>Turtlebot3->turtlebot3_with_open_manipulator->turtlebot3_with_manipulator_coreを選択

• 矢印ボタン押下でアップロードします。

• このやり方が走行用のモーターがおかしくなった時に役立ちます。

5. 組み立てる

Raspberry pi3 + OpenCR + Manipulatorの完成形
組み立て方は、Robotis e-ManualからTurtlebot3とOpen Manipulatorがダウンロードできます。部品が細かくて大変、極力落とさないように、Lidar乗っていますが使ってないです。笑

ARMarkerを使えるようにする

ar_track_usb_cam.launchをもとにar_track_rpicam_9.0cm.launchを作成"9.0"はマーカーの大きさです。

• ar_tag_toolboxをクローンして、ビルドしてください。

$ cd ~/catkin_ws/src
$ git clone https://github.com/atomoclast/ar_tag_toolbox.git
$ catkin build

~/catkin_ws/src内にオリジナルのパッケージを作成する。依存関係は良しなにrospyだけでいいと思います。

• ar_controllerって名前にします。

$ cd ~/catkin_ws/src
# 例 : catkin_create_pkg 任意のパッケージ名 std_msgs rospy
$ catkin_create_pkg ar_controller std_msgs rospy

• ar_tag_toolboxパッケージのar_track_usb_cam.launchをコピーする。
  
  • roscp コピー元パッケージ名 コピー元ファイル名 コピー先ファイルパスですね。
  • ar_track_rpicam_9.0cm.launchを作る

$ roscp ar_tag_toolbox ar_track_usb_cam.launch ar_track_rpicam_9.0cm.launch

• ar_track_rpicam_9.0cm.launchの中

<launch>

    <!-- <arg name="marker_size" default="5.0" /> -->
    <arg name="marker_size" default="9.0" />
    <arg name="max_new_marker_error" default="0.08" />
    <arg name="max_track_error" default="0.2" />

    <!-- <arg name="cam_image_topic"      default="/usb_cam/image_raw" /> -->
    <!-- <arg name="cam_info_topic"       default="/usb_cam/camera_info" /> -->
    <arg name="cam_image_topic" default="/raspicam_node/image_raw" />
    <arg name="cam_info_topic" default="/raspicam_node/camera_info" />


    <!-- <arg name="output_frame" default="usb_cam" /> -->
    <arg name="output_frame" default="odom" />


    <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
        <param name="marker_size"           type="double" value="$(arg marker_size)" />
        <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
        <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
        <param name="output_frame"          type="string" value="$(arg output_frame)" />

        <remap from="camera_image"  to="$(arg cam_image_topic)" />
        <remap from="camera_info"   to="$(arg cam_info_topic)" />
    </node>
    <!-- 固定のTF -->
    <node name="static_tf_pub_raspicam_frame" type="static_transform_publisher" pkg="tf2_ros" output="screen" args="0 0 0 0 0 0 1 /raspicam /odom" />
</launch>

• ar_controller.launch作成、ar_track_rpicam_9.0cm.launchをインクルードして、ar_controller.pyを実行しています。

<launch>
    <!-- 先程作成したランチファイルを実行する -->
    <include file="$(find rist_ar_controller)/launch/ar_track_rpicam_9.0cm.launch" />
    
    <!-- /raspicam_node/imageから/raspicam_node/image_rawにリパブリッシュ -->
    <node name="republish" type="republish" pkg="image_transport" output="screen" args="compressed in:=/raspicam_node/image raw out:=/raspicam_node/image_raw" />
    
    <!-- ar_controller.pyを実行 -->
    <node pkg="rist_ar_controller" type="ar_controller.py" name="ar_controller" output="screen" />
</launch>

• ar_controller.pyの中身
  
  カメラ内にマーカーが存在する場合に、下記のvisualization_markerトピックにPublishされるのでvisualization_markerトピックをSubscribeする。コールバック内でIDデータを取得して、指定マーカーIDを突き合わせることができます。

  • 指定マーカーID : [0,1,2,3] - 四ヶ所に移動するように指定できる

# Subscribe
self.controller_sub = rospy.Subscriber(
                        'visualization_marker',
                        Marker,
                        self.callback_controller)

# callbackでマーカーIDだけ取得
def callback_controller(self, data):
    rospy.loginfo('(callback_controller)')
    self.tag = data
    self.marker_id = data.id
    rospy.loginfo('tag in viewport: ' + str(self.marker_id))

上記でマーカーのIDが取れれば、指定マーカーへ動くようの/cmd_velトピックをいじって上げればマーカーに向うようなプログラム書けばいいです。

Remote PCとTurtlebot3 PC上でランチファイルを実行し、ナビゲーションする

[Remote PC]

• ROSマスタ起動

$ roscore

[Turtlebot3 PC] - sshでログインしてランチファイル実行

$ ssh [e-mail]

• Turtlebot3 起動

$ roslaunch turtlebot3_bringup turtlebot3_robot.launch

• ラズパイカメラ起動

$ roslaunch raspicam_node camerav2_1280x720.launch

• USBカメラ起動

$ roslaunch usb_cam usb-test.launch

[Remote PC]に戻る

• Turtlebot Manipulaterの起動

$ roslaunch turtlebot3_manipulation_bringup turtlebot3_manipulation_bringup.launch

• Turtlebot3 Manipulatorのmoveit_configパッケージのmove_group実行

roslaunch turtlebot3_manipulation_moveit_config move_group.launch

• ar_controller.pyを実行

roslaunch ar_controller ar_controller.launch

長い。。。続く。。。