ROSを使用したCRANE+の動かし方　その6

こんにちは！長谷川です。
前回はCRANE+を動かすための環境の構築について説明しましたが、今回はCRANE+の全ての関節を同時に、かつ自動で動かす方法について説明したいと思います。

ROSのノードとトピックについて

CRANE+を動かす前に、ROSのシステムの一つ、ノードとトピックについて勉強しなければなりません。
ノードとは、ROSパッケージ内の実行ファイルです。ROSでは多くの場合、複数のノードが起動していて、それぞれの処理のために情報をやりとりしています。例えば、あるノードが常にロボットの位置を計測していて、そのノードからロボットの位置情報を受け取った別のノードが、目的の位置にロボットを動かす、といった具合です。このノード間の情報のやりとりに使われるのがトピックです。各ノードは、やりとりする情報をメッセージとしてトピックに配信し、配信されたメッセージをトピックを購読することで受け取ることができます。上の例で言えば、ロボットの位置情報についてのトピックがあって、位置計測を行うノードはそのトピックに位置情報を配信し、ロボットを動かすノードはそのトピックを購読して位置情報を受け取る、というわけです。

前回の最後に、

rostopic pub -1 /tilt_controller/command std_msgs/Float64 -- 1.5

というコマンドをご紹介しましたが、このコマンドは、「/tilt_controller/commandというトピックに、std_msgs/Float64というデータ型で1.5という値を配信する」という意味です。/tilt_controller/commandというトピックは、モーターを動かすノードに購読されていまして、そのノードがモーターに設定されている中心位置からこのトピックに入った値（単位はラジアン）だけずれた位置を目標にしてサーボを動かします。3.1415…ラジアン=180度です。
つまり、上のコマンドで、tilt.yamlに記述されたIDのサーボモーターが基準位置から1.5ラジアンだけずれた位置まで動くように指令を送ったというわけです。なので、最後の数字を-1.5とした命令を入力すると、モーターは左右対称な位置まで動きます。

環境の構築

前回は、tilt.yamlなるファイルを作りましたが、今回はこのようなyamlファイルを各モーターに1個ずつ、計5個作ることになります。ID1~5のモーターに、tilt1.yaml~tilt5.yamlをそれぞれ対応させます。まずは、tilt1.yamlを作りましょう。端末に

$ roscd my_dynamixel_tutorial
$ vi tilt1.yaml

と入力してviを起動します。編集モードに入り、以下の内容を貼り付けます。

tilt1_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: tilt_joint
    joint_speed: 1.17
    motor:
        id: 1
        init: 512
        min: 0
        max: 1023

貼り付けたらEscapeを押してコマンドモードに戻り、「:wq」と打ち込んで、tilt1.yamlを保存してviを終了します。同様に、tilt2.yaml~tilt5.yamlを作ります。一行目のtilt1_controllerの1の代わりに各モーターのIDを書き、9行目の「id:」の右側に各モーターのIDを書いてください。

次に、チュートリアルで作ったstart_tilt_controller.launchを、5個のモーターを動かせるように書き換えます。新しいファイルを作ってもいいですが、その場合は、以降の「start_tilt_controller.launch」を自分の設定したファイル名に読み替えてください。端末に

vi start_tilt_controller.launch

と入力し、viを起動します。前回書いたものは邪魔になるので、コマンドモードのままddを押していき、何もなくなった所でiを押して編集モードに入ります。以下を貼り付けます。

<launch>
    <!-- Start tilt joint controller -->
    <rosparam file="$(find my_dynamixel_tutorial)/tilt1.yaml" command="load"/>
    <node name="tilt1_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --port pan_tilt_port
                tilt1_controller"
          output="screen"/>

    <!-- Start tilt joint controller -->
    <rosparam file="$(find my_dynamixel_tutorial)/tilt2.yaml" command="load"/>
    <node name="tilt2_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --port pan_tilt_port
                tilt2_controller"
          output="screen"/>

    <!-- Start tilt joint controller -->
    <rosparam file="$(find my_dynamixel_tutorial)/tilt3.yaml" command="load"/>
    <node name="tilt3_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --port pan_tilt_port
                tilt3_controller"
          output="screen"/>

    <!-- Start tilt joint controller -->
    <rosparam file="$(find my_dynamixel_tutorial)/tilt4.yaml" command="load"/>
    <node name="tilt4_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --port pan_tilt_port
                tilt4_controller"
          output="screen"/>

    <!-- Start tilt joint controller -->
    <rosparam file="$(find my_dynamixel_tutorial)/tilt5.yaml" command="load"/>
    <node name="tilt5_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --port pan_tilt_port
                tilt5_controller"
          output="screen"/>
</launch>

貼り付けたらEscapeを押してコマンドモードに戻り、「:wq」と打ち込んで、start_tilt_controller.launchを保存してviを終了します。
これで、チュートリアルと同じように「roslaunch my_dynamixel_tutorial start_tilt_controller.launch」まで入力し、「rostopic pub -1 /tilt(動かしたいモーターのID)_controller/command std_msgs/Float64 — 1.5」と入力すると、該当するモーターが動きます。例えば、

rostopic pub -1 /tilt1_controller/command std_msgs/Float64 -- 1.5

と入力すると、根元のモーターが動きます。これで、コマンドを手打ちすることによりCRANE+を動かすことができるようになりました。

プログラムによる自動駆動

次は、プログラムにより、あらかじめ決めておいた動きを自動でCRANE+にさせてみましょう。少し長くなるので、CRANE+との接続は一旦切っておいた方がいいと思います。まず、端末に

$ cd ~/catkin_ws/src
$ catkin_create_pkg turtlebot_arm std_msgs rospy roscpp

と入力して、catkin_wsのソースディレクトリに移動してからturtlebot_armパッケージを作成します。(この作業は、ここの「4. catkin形式のパッケージを作る」に基づいています。)そして、

$ cd turtlebot_arm/src
$ vi turtlebot_arm.cpp

と入力してviを起動します。編集モードに入り、以下を貼り付けます。

#include "ros/ros.h"
/**
 * turtlebot_arm.cpp - move turtlebot arm according to predefined gestures
 */

#include "std_msgs/Float64.h"

#include <sstream>

int main(int argc, char **argv)
{

 ros::init(argc, argv, "turtlebot_arm");

 ros::NodeHandle n;

 //publish command message to joints/servos of arm
 ros::Publisher joint1_pub = n.advertise<std_msgs::Float64>("/tilt1_controller/command", 1000);
 ros::Publisher joint2_pub = n.advertise<std_msgs::Float64>("/tilt2_controller/command", 1000);
 ros::Publisher joint3_pub = n.advertise<std_msgs::Float64>("/tilt3_controller/command", 1000);
 ros::Publisher joint4_pub = n.advertise<std_msgs::Float64>("/tilt4_controller/command", 1000);
 ros::Publisher joint5_pub = n.advertise<std_msgs::Float64>("/tilt5_controller/command", 1000);
 std_msgs::Float64 pos1;
 std_msgs::Float64 pos2;
 std_msgs::Float64 pos3;
 std_msgs::Float64 pos4;
 std_msgs::Float64 pos5;
 //Initial gesture of robot arm
 pos1.data = 0.0; 
 pos2.data = 0.0; 
 pos3.data = 0.0; 
 pos4.data = 0.0; 
 pos5.data = -0.3; 
 joint1_pub.publish(pos1);
 joint2_pub.publish(pos2);
 joint3_pub.publish(pos3);
 joint4_pub.publish(pos4);
 joint5_pub.publish(pos5);

 ros::Rate loop_rate(0.5);

 while (ros::ok())
 {
  //gesture 1
  pos1.data = -0.9; 
  pos2.data = 0.0;
  pos3.data = 0.0;
  pos4.data = 0.0;
  pos5.data = -0.3;
  joint1_pub.publish(pos1);
  joint2_pub.publish(pos2);
  joint3_pub.publish(pos3);
  joint4_pub.publish(pos4);
  joint5_pub.publish(pos5);

  loop_rate.sleep();

  //gesture 2 
  pos1.data = -0.9;
  pos2.data = 0.9;
  pos3.data = 1.8;
  pos4.data = 1.0;
  pos5.data = -0.3;
  joint1_pub.publish(pos1);
  joint2_pub.publish(pos2);
  joint3_pub.publish(pos3);
  joint4_pub.publish(pos4);
  joint5_pub.publish(pos5);

  loop_rate.sleep();

  //gesture 3 
  pos1.data = -0.9;
  pos2.data = 0.9;
  pos3.data = 1.8; 
  pos4.data = 1.0;
  pos5.data = 0.3;
  joint1_pub.publish(pos1);
  joint2_pub.publish(pos2);
  joint3_pub.publish(pos3);
  joint4_pub.publish(pos4);
  joint5_pub.publish(pos5);

  loop_rate.sleep();

  //gesture 4 
  pos1.data = -0.9;
  pos2.data = 0.0;
  pos3.data = 0.0;
  pos4.data = 0.0;
  pos5.data = 0.3;
  joint1_pub.publish(pos1);
  joint2_pub.publish(pos2);
  joint3_pub.publish(pos3);
  joint4_pub.publish(pos4);
  joint5_pub.publish(pos5);

  loop_rate.sleep();

  //gesture 5 
  pos1.data = 0.95;
  pos2.data = 0.0;
  pos3.data = 0.0;
  pos4.data = 0.0;
  pos5.data = 0.3;
  joint1_pub.publish(pos1);
  joint2_pub.publish(pos2);
  joint3_pub.publish(pos3);
  joint4_pub.publish(pos4);
  joint5_pub.publish(pos5);

  loop_rate.sleep();

  //gesture 6 
  pos1.data = 0.95;
  pos2.data = 0.3;
  pos3.data = 2.0;
  pos4.data = -0.2;
  pos5.data = 0.3;
  joint1_pub.publish(pos1);
  joint2_pub.publish(pos2);
  joint3_pub.publish(pos3);
  joint4_pub.publish(pos4);
  joint5_pub.publish(pos5);

  loop_rate.sleep();

  //gesture 7 
  pos1.data = 0.95;
  pos2.data = 0.3;
  pos3.data = 2.0;
  pos4.data = -0.2;
  pos5.data = -0.3;
  joint1_pub.publish(pos1);
  joint2_pub.publish(pos2);
  joint3_pub.publish(pos3);
  joint4_pub.publish(pos4);
  joint5_pub.publish(pos5);

  loop_rate.sleep();

  //gesture 8 
  pos1.data = 0.95;
  pos2.data = 0.4;
  pos3.data = 0.0;
  pos4.data = -0.2;
  pos5.data = -0.3;
  joint1_pub.publish(pos1);
  joint2_pub.publish(pos2);
  joint3_pub.publish(pos3);
  joint4_pub.publish(pos4);
  joint5_pub.publish(pos5);

  loop_rate.sleep();

  //gesture 9 
  pos1.data = 0.95;
  pos2.data = 0.0;
  pos3.data = 0.0;
  pos4.data = 0.0;
  pos5.data = -0.3;
  joint1_pub.publish(pos1);
  joint2_pub.publish(pos2);
  joint3_pub.publish(pos3);
  joint4_pub.publish(pos4);
  joint5_pub.publish(pos5);

  loop_rate.sleep();

  //Initial gesture of robot arm
  pos1.data = 0.0;
  pos2.data = 0.0;
  pos3.data = 0.0;
  pos4.data = 0.0;
  pos5.data = -0.3;
  joint1_pub.publish(pos1);
  joint2_pub.publish(pos2);
  joint3_pub.publish(pos3);
  joint4_pub.publish(pos4);
  joint5_pub.publish(pos5);

  loop_rate.sleep();

  ros::spinOnce();
 }

 return 0;
}

これがC++言語で書かれたソースコードになります。このソースコードについての解説は、次回行いたいと思います。貼り付けたらEscapeを押してコマンドモードに戻り、「:wq」と打ち込んで、turtlebot_arm.cppを保存してviを終了します。その後、

chmod +x turtlebot_arm.cpp

と入力して、ファイルを実行可能にしておきましょう。

次に、

$ roscd turtlebot_arm
$ vi CMakeLists.txt

と入力します。色々出てきますが、末尾に以下の3文を貼り付けます。

add_executable(turtlebot_arm src/turtlebot_arm.cpp)
target_link_libraries(turtlebot_arm ${catkin_LIBRARIES})
add_dependencies(turtlebot_arm turtlebot_arm_generate_messages_cpp)

これで保存してviを終了してください。
次に、このturtlebot_armパッケージをビルドしましょう。

$ cd ~/catkin_ws/
$ catkin_make

と入力します。「Linking ~」と書かれた一文の後に、「「100%」 Built target turtlebot_arm」と出てきていたら、ビルドが成功していると思います。

ここまで実行したら、CRANE+とPCを接続し、チュートリアルと同じ手順で「roslaunch my_dynamixel_tutorial start_tilt_controller.launch」まで入力してください。端末に

rosrun turtlebot_arm turtlebot_arm

と入力すると、CRANE+が自動で動きだすと思います。天板上で動くので、天板にPC等を置くのは危険です。物をつかんで運ぶ動きをすると思うので、ペットボトルをつかませて遊んでみましょう。

以上、CRANE+の全ての関節を同時に、かつ自動で動かす方法について説明しました。
次回は、今回出てきたソースコードの解説を通して、ROSのプログラムの書き方について説明したいと思います。