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

ROS_CRANE+
2015.04.16

こんにちは！長谷川です。
前回はCRANE+のURDFモデルの作り方について説明しましたが、今回は、前回作ったモデルと実物のCRANE+の連動のさせ方について説明したいと思います。

プログラムの作成

まずは、モデルとCRANE+を連動させるプログラムを作成しましょう。端末を起動し、

$ roscd crane_urdf/src
$ vi reflect_rviz_in_crane.cpp

と入力してviを起動します。以下を貼りつけます。

#include "ros/ros.h"
 
#include "std_msgs/Float64.h"
#include "sensor_msgs/JointState.h"
 
#include <sstream>

std_msgs::Float64 pos1;
std_msgs::Float64 pos2;
std_msgs::Float64 pos3;
std_msgs::Float64 pos4;
std_msgs::Float64 pos5;
char renew = 0;

void reflect_jointstate(const sensor_msgs::JointState::ConstPtr& jointstate);

int main(int argc, char **argv)
{
 ros::init(argc, argv, "reflect_rviz_in_crane");
 
 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);

 ros::Subscriber sub = n.subscribe("/joint_states", 10, reflect_jointstate); 
 
 ros::Rate loop_rate(100); 
 while (ros::ok())
 {
  while(ros::ok() && renew==0){
 	ros::spinOnce();
	loop_rate.sleep();
  }
  joint1_pub.publish(pos1);
  joint2_pub.publish(pos2);
  joint3_pub.publish(pos3);
  joint4_pub.publish(pos4);
  joint5_pub.publish(pos5);
  renew = 0;

  ros::spinOnce();
  loop_rate.sleep();
 } 
 return 0;
}

void reflect_jointstate(const sensor_msgs::JointState::ConstPtr& jointstate)
{
 pos1.data = jointstate->position[0];
 pos2.data = jointstate->position[1];
 pos3.data = jointstate->position[2]; 
 pos4.data = jointstate->position[3];
 pos5.data = jointstate->position[4];

 renew = 1;
}

これが、C++言語で書かれたソースコードになります。貼りつけたら、保存してviを終了してください。その後、

chmod +x reflect_rviz_in_crane.cpp

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

$ roscd crane_urdf
$ vi CMakeLists.txt

と入力します。末尾に以下の3文を貼りつけます。

add_executable(reflect_rviz_in_crane src/reflect_rviz_in_crane.cpp)
target_link_libraries(reflect_rviz_in_crane ${catkin_LIBRARIES})
add_dependencies(reflect_rviz_in_crane crane_urdf_generate_messages_cpp)

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

$ cd ~/catkin_ws/
$ catkin_make

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

CRANE+とモデルの連動

これで準備は整ったので、CRANE+を実際に動かしてみましょう。第5回目の記事にしたがって、CRANE+とPCを接続し、「roslaunch my_dynamixel_tutorial start_tilt_controller.launch 」まで入力してください。
その後、

$ roscd crane_urdf/src
$ roslaunch urdf_tutorial display.launch model:=crane.urdf gui:=True

と入力して、rvizとJoint State Publisherウィンドウを表示します。新しい端末を開き、

rosrun crane_urdf reflect_rviz_in_crane

と入力すれば、実物のCRANE+がモデルの状態を反映するようになります。けがやCRANE+の破壊に注意しながら、Joint State Publisherウィンドウのバーを動かしてみてください。モデルとCRANE+が同じように動くと思います。
※天板上で動くので、天板にPC等を置くのは危険です。
※関節の可動範囲は最低限しか制限していないので、アームの先端が天板に当たったり、リンク同士がぶつかる可能性があります。CRANE+を破壊したり、けがをすることがないよう、無理のない速度と範囲で動かしてください。
※関節に指を挟まないよう注意してください。

プログラムの解説

今回のプログラムには、第7回目の記事で解説していない部分があるので、そこを説明したいと思います。

ros::Subscriber sub = n.subscribe("/joint_states", 10, reflect_jointstate);

ここで、/joint_statesというトピックを購読することを宣言しています。このトピックに新しいメッセージが来ると、reflect_jointstate()が呼び出されます。10というのは蓄えることのできるデータの最大数で、第7回で説明したものと同じ意味です。
なお、この/joint_statesには、Joint State Publisherが関節角を配信しており、そのトピックをrvizが購読して、取得した関節角をモデルに反映させることで、GUIでモデルを動かせるようになっています。このプログラムは、その仕組みを利用し、関節角をrvizとは独立して取得してCRANE+に反映させることで、CRANE+をrviz内のモデルと同じように動かしています。

void reflect_jointstate(const sensor_msgs::JointState::ConstPtr& jointstate)

この関数が呼び出された際、自動的に引数に新しいメッセージの内容が引き渡されます。つまり、sensor_msgs::JointState型のポインタjointstateの指し示すインスタンスには、新しいメッセージの内容が格納されています。

pos1.data = jointstate->position[0];

jointstate->position[0]には、モデルのjoint1の関節角が格納されています。それをpos1.dataに格納し、メイン文でpos1を/tilt1_controller/commandに配信することで、CRANE+のjoint1の関節角に反映します。

以上、前回作ったモデルと実物のCRANE+の連動のさせ方について説明しました。
次回は、CRANE+の状態をモデルに反映させる方法について説明したいと思います。

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