ロボットプログラミングⅡ：第13週ロボットアーム

今週はgazeboを使い２自由度のロボットアームを作り、関節を動かします。2自由度ロボットアームのURDFは参考リンクを参照してください。このサンプルでは参考リンクのrrbot.xacroをベースに作っています。

ソース
Turtlebotをテレオペしたソースコードをベースに ~/catkin_ws/src/armbot2/src/teleop.cppを作成しました。ジョイント1とジョイント２の目標角度をパブリッシュし、/armbot2/joint_statesトピックをサブスクライブしジョイントの現在値を取得しています。この例では、目標角度に追従するのに時間がかかるのでループ毎に１秒スリープしてから現在値を取得しています。

 
// ~/catkin_ws/src/armbot2/src/teleop.cpp
#include "ros/ros.h"  // rosで必要はヘッダーファイル
#include "std_msgs/Float64.h"
#include "sensor_msgs/JointState.h"
#include "nav_msgs/Odometry.h"

using namespace std;
std_msgs::Float64 tmp_joint1, tmp_joint2;
double pos_x, pos_y, pos_z;

void monitorJointState(const sensor_msgs::JointState::ConstPtr& jointstate)
{
  tmp_joint1.data = jointstate->position[0];
  tmp_joint2.data = jointstate->position[1]; 
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "teleop");  // ノードの初期化
  ros::NodeHandle nh; // ノードハンドラ  
  ros::Publisher  pub_joint1, pub_joint2;  // パブリッシャの作成
  ros::Subscriber sub_joints, sub_sensor;  // サブスクライバの作成

  std_msgs::Float64 target_joint1, target_joint2;

  pub_joint1 = nh.advertise<std_msgs::Float64>("/armbot2/joint1_position_controller/command", 100);
  pub_joint2 = nh.advertise<std_msgs::Float64>
("/armbot2/joint2_position_controller/command", 100);
  sub_joints = nh.subscribe("/armbot2/joint_states", 100, monitorJointState);

  target_joint1.data = 0;
  target_joint2.data = 0;

  while (ros::ok()) { // このノードが使える間は無限ループ
    char key;  // 入力キーの値

    ROS_INFO("[Input] j: Joint1++, f: Joint1--, k: Joint2++, d:Joint2--");
    cin >> key; 
    cout << key << endl;

    switch (key) {
    case 'j': target_joint1.data  +=  5 * M_PI/180.0; break;
    case 'f': target_joint1.data  -=  5 * M_PI/180.0; break;
    case 'k': target_joint2.data  +=  5 * M_PI/180.0; break;
    case 'd': target_joint2.data  -=  5 * M_PI/180.0; break;
    default: ROS_INFO("Input j,f,k,d");
    }
      
    pub_joint1.publish(target_joint1); // 角度を送信    
    pub_joint2.publish(target_joint2);
    ROS_INFO("Targe: Joint1=%f Joint2=%f", target_joint1.data, target_joint2.data);

    usleep(1000*1000); // 制御に時間がかかるので1秒寝て待つ
    ros::spinOnce();   // コールバック関数を呼ぶ
    ROS_INFO("Tmp:Joint1=%f Joint2=%f", tmp_joint1.data,    tmp_joint2.data);
  }
  
  return 0;
}

なお、ジョイントのPIDゲインやコントローラーは以下のファイルに記述されています。詳細については参考リンクをご覧ください。
~/catkin_ws/src/armbot2/config/armbot2_control.yaml
準　備

$ sudo apt-get update
$ sudo apt-get install ros-indigo-gazebo-ros-pkgs ros-indigo-gazebo-ros-control
$ sudo apt-get install ros-indigo-gazebo-plugins ros-indigo-gazebo-ros
$ sudo apt-get install ros-indigo-moveit-*
$ sudo apt-get install ros-indigo-ros-control ros-indigo-ros-controllers ros-indigo-joint-state-controller ros-indigo-effort-controllers ros-indigo-position-controllers ros-indigo-joint-trajectory-controller
$ sudo apt-get install liburdfdom-tools

プログラムとビルド

以下をホームディレクトリ(/home/ユーザ名)にダウンロードする。
- armbot2-150108.tgz

$ cd
$ cp armbot2-150108.tgz ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ tar xvzf armbot2-150108.tgz
$ cd ~/catkin_ws
$ catkin_make

実　行
端末を２つ開き、次のコマンドを実行する。

$ roslaunch armbot2 armbot2.launch
$ rosrun armbot2 armbot2
- armbot2を実行している端末をクリックし、j, k, f, dコマンドを入力する毎にEnterキーを入力するとロボットアームが動く。各コマンドを入力する度にEnterキーを押さなければ動きません。jは第1関節を正方向、fは負方向、 kは第2関節を正方向、dは負方向に目標角度を5度ずつ変化させる。
成功するとrvizとgazeboの２つのウインドウが開く。そのままではrvizにロボットモデルが表示されないので、次の設定を行う。

rvizの設定