ROS演習11-2019：ロボットアーム

この記事は私が金沢工業大学ロボティクス学科で担当している講義ロボットプログラミングⅡ用です。今週は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 update
$ sudo apt upgrade
$ sudo apt install ros-melodic-gazebo-ros-pkgs ros-melodic-gazebo-ros-control
$ sudo apt install ros-melodic-gazebo-plugins ros-melodic-gazebo-ros
$ sudo apt install ros-melodic-moveit-*
$ sudo apt install ros-melodic-ros-control ros-melodic-ros-controllers ros-melodic-joint-state-controller ros-melodic-effort-controllers ros-melodic-position-controllers ros-melodic-joint-trajectory-controller
$ sudo apt install liburdfdom-tools

プログラムとビルド

以下を~/catkin_ws/src(/home/ユーザ名/catkin_ws/src)にダウンロードする。
- armbot2-161208.tar

$ cd ~/catkin_ws/src
$ tar xvf armbot2-161208.tar
$ cd ~/catkin_ws
$ catkin build

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

$ 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の設定