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

arm2-gazebo

arm2-rviz

この記事は私が金沢工業大学ロボティクス学科で担当している講義ロボットプログラミングⅡ用です。今週はgazeboを使い2自由度のロボットアームを作り、関節を動かします。2自由度ロボットアームのURDFは参考リンクを参照してください。このサンプルでは参考リンクのrrbot.xacroをベースに作っています。

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

 
// ~/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)にダウンロードする。
  • $ cd  ~/catkin_ws/src
  • $ tar  xvf  armbot2-161208.tar
  • $ cd  ~/catkin_ws
  • $ catkin build

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

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

rvizの設定

rviz-gazebo

  • 次の操作を行う
    1. (1) Global Option -> Fixed Frameをbase_linkに設定
    2. (2) Addボタンをクリックする
    3. (3) RobotModelを追加する

ノードグラフ

ノードグラフは次のとおり。端末を開き、次のコマンドを実行。

  • $ rqt_graph

rqt_graph

参考リンク

 

コメント

タイトルとURLをコピーしました