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

2020.01.09

arm_sensor
この記事は私が金沢工業大学ロボティクス学科で担当している講義ロボットプログラミングⅡ用です。今回は先回作った２自由度のロボットアームの先端に位置センサを取り付け、先端位置を取得するプログラムを作ります。

位置センサ用プラグインの追加
gazeboのロボットモデルに位置センサを取り付けるために、~/catkin_ws/src/armbot2_sensor/urdf/armbot2_sensor.gazeboに以下のプラグインを追加します。

 
 <gazebo>
    <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <bodyName>link_sensor</bodyName>
      <topicName>/armbot2_sensor/pose_ground_truth</topicName>
      <gaussianNoise>0</gaussianNoise>
      <frameName>base_link</frameName>
      <xyzOffsets>0 0 0</xyzOffsets> 
      <rpyOffsets>0 0 0</rpyOffsets>
    </plugin>
  </gazebo>

ソース
前週のソースコードにgroundTruthCallback関数を追加し、トピック/armbot2_sensor/pose_ground_truthTurtlebtをサブスクライブするだけで、位置センサからのデータを取得できます。

 
#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 groundTruthCallback(const nav_msgs::Odometry::ConstPtr& msg)
{ 
  //msg->pose.pose.position, msg->pose.pose.orientation, 
  pos_x = msg->pose.pose.position.x;
  pos_y = msg->pose.pose.position.y;
  pos_z = msg->pose.pose.position.z;
  ROS_INFO("Pose: x=%f y=%f \n",pos_x,pos_y);
}

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_sensor"); 
  // initでROSを初期化して、my_teleopという名前をノードにつける                        
  // 同じ名前のノードが複数あってはいけないので、ユニークな名前をつける

  ros::NodeHandle nh;
  // ノードハンドラの作成。ハンドラは必要になったら起動される。
  ros::Publisher  pub_joint1, pub_joint2;
  // パブリッシャの作成。トピックに対してデータを送信。

  ros::Subscriber sub_joints, sub_sensor;
  // サブスクライバの作成

  ros::Rate rate(10);
  // ループの頻度を設定。この場合は10Hz、1秒間に10回数、1ループ100ms。

  std_msgs::Float64 target_joint1, target_joint2;

  pub_joint1 = nh.advertise<std_msgs::Float64>("/armbot2_sensor/joint1_position_controller/command", 100);
  pub_joint2 = nh.advertise<std_msgs::Float64>("/armbot2_sensor/joint2_position_controller/command", 100);
  sub_sensor = nh.subscribe<nav_msgs::Odometry>("/armbot2_sensor/pose_ground_truth", 100, groundTruthCallback);
  sub_joints = nh.subscribe("/armbot2_sensor/joint_states", 100, monitorJointState);

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

  int loop = 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);
    ros::spinOnce(); // コールバック関数を呼ぶ
    ROS_INFO("Tmp:   Joint1=%f Joint2=%f", tmp_joint1.data,    tmp_joint2.data);
    //rate.sleep();     // 指定した周期でループするよう寝て待つ
  }
 
  return 0;
}

launchファイル
この演習で使うlaunchファイルarmbot2_sensor.launchを以下示す。説明はコメントを読んで頂きたい。このlaunchファイルでgazeboやロボットコントローラなどの必要なノードを起動している。

 
<launch>
  <!-- armbot2_sensor.launch -->
  <!-- GAZEBO用のパラメータ -->
  <arg name="paused" default="false"/>       <!-- true：起動時は一時停止 -->
  <arg name="use_sim_time" default="true"/>  <!-- true：システムクロックではなくROSの/clockトピックを使う -->
  <arg name="gui" default="true"/>           <!-- true：GAZEBOのグラフィクスユーザインタフェースを起動 -->
  <arg name="headless" default="false"/>     <!-- true：GAZEBOのログを取る -->
  <arg name="debug" default="false"/>        <!-- true：デバッグモードでGAZEBOサーバーを起動 -->

  <!-- ワールドの起動 -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch"> 
   <arg name="world_name" value="$(find armbot2_sensor)/launch/armbot2.world"/>
  </include>

  <!-- ROSパラメータサーバーへURDFをロード -->
  <param name="robot_description"
  command="$(find xacro)/xacro '$(find armbot2_sensor)/urdf/armbot2_sensor.xacro'" />

  <!-- URDFで記述されたロボットを生成するためgazebo_rosへサービスコールを送付 -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
	args="-urdf -model armbot2_sensor -param robot_description"/>

  <!-- ROSパラメータサーバーへのジョイントコントローラ設定ファイルのロード -->
  <rosparam file="$(find armbot2_sensor)/config/armbot2_control.yaml" command="load"/>

  <!-- コントローラのロード -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
	output="screen" ns="/armbot2_sensor" args="joint_state_controller
					  joint1_position_controller
					  joint2_position_controller"/>

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
      <remap from="/joint_states" to="/armbot2_sensor/joint_states" /> 
  </node> 
</launch>

準　備 ([ROS演習11-2019：ロボットアーム]で実施した場合は必要ありません)

  • $ sudo apt-get update
  • $ sudo apt-get install ros-melodic-gazebo-ros-pkgs ros-melodic-gazebo-ros-control
  • $ sudo apt-get install ros-melodic-gazebo-plugins  ros-melodic-gazebo-ros
  • $ sudo apt-get install ros-melodic-moveit-*
  • $ sudo apt-get 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-get install liburdfdom-tools

プログラムとビルド

  • 以下を~/catkin_ws/srcディレクトリ(/home/ユーザ名/catkin_ws/src)にダウンロードする。
  • $ cd  ~/catkin_ws/src
  • $ tar  xvf  armbot2_sensor-171208.tar
  • $ cd  ~/catkin_ws
  • $ catkin build

実　行

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

  • $ roslaunch  armbot2_sensor  armbot2_sensor.launch
  • $ rosrun armbot2_sensor armbot2_sensor
    • armbot2を実行している端末をクリックし、j, k, f, dコマンドを入力する毎にEnterキーを入力するとロボットアームが動く。各コマンドを入力する度にEnterキーを押さなければ動きません。jは第1関節を正方向、fは負方向、 kは第2関節を正方向、dは負方向に目標角度を5度ずつ変化させる。

演　習

  •  運動学
    運動学を実装しよう。その計算結果と位置センサの出力を比較し、プログラムが正しいか確認する。
  •  逆運動学
    逆運動学を実装しよう。２自由度の場合は解が２つあるので、数字キー１を押すと解１、数字キー２を押すと解２を選択するにしなさい。

参考リンク

 

コメント

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