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

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

プログラムの作成

まずは、モデルとCRANE+を連動させるプログラムを作成しましょう。端末を起動し、
[php]
$ roscd crane_urdf/src
$ vi reflect_rviz_in_crane.cpp
[/php]
と入力してviを起動します。以下を貼りつけます。
[cpp]
#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;
}
[/cpp]
これが、C++言語で書かれたソースコードになります。貼りつけたら、保存してviを終了してください。その後、
[php]chmod +x reflect_rviz_in_crane.cpp[/php]
と入力して、ファイルを実行可能にしておきましょう。
次に、
[php]
$ roscd crane_urdf
$ vi CMakeLists.txt
[/php]
と入力します。末尾に以下の3文を貼りつけます。
[php]
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)
[/php]
これで保存してviを終了してください。
次に、crane_urdfパッケージをビルドしましょう。
[php]
$ cd ~/catkin_ws/
$ catkin_make
[/php]
と入力します。「Linking ~」と書かれた一文の後に、「「100%」 Built target reflect_rviz_in_crane」と出てきていたら、ビルドが成功していると思います。

CRANE+とモデルの連動

これで準備は整ったので、CRANE+を実際に動かしてみましょう。第5回目の記事にしたがって、CRANE+とPCを接続し、「roslaunch my_dynamixel_tutorial start_tilt_controller.launch 」まで入力してください。
その後、
[php]
$ roscd crane_urdf/src
$ roslaunch urdf_tutorial display.launch model:=crane.urdf gui:=True
[/php]
と入力して、rvizとJoint State Publisherウィンドウを表示します。新しい端末を開き、
[php]rosrun crane_urdf reflect_rviz_in_crane[/php]
と入力すれば、実物のCRANE+がモデルの状態を反映するようになります。けがやCRANE+の破壊に注意しながら、Joint State Publisherウィンドウのバーを動かしてみてください。モデルとCRANE+が同じように動くと思います。
※天板上で動くので、天板にPC等を置くのは危険です。
※関節の可動範囲は最低限しか制限していないので、アームの先端が天板に当たったり、リンク同士がぶつかる可能性があります。CRANE+を破壊したり、けがをすることがないよう、無理のない速度と範囲で動かしてください。
※関節に指を挟まないよう注意してください。

プログラムの解説

今回のプログラムには、第7回目の記事で解説していない部分があるので、そこを説明したいと思います。
[cpp firstline=”32″]ros::Subscriber sub = n.subscribe("/joint_states", 10, reflect_jointstate);[/cpp]
ここで、/joint_statesというトピックを購読することを宣言しています。このトピックに新しいメッセージが来ると、reflect_jointstate()が呼び出されます。10というのは蓄えることのできるデータの最大数で、第7回で説明したものと同じ意味です。
なお、この/joint_statesには、Joint State Publisherが関節角を配信しており、そのトピックをrvizが購読して、取得した関節角をモデルに反映させることで、GUIでモデルを動かせるようになっています。このプログラムは、その仕組みを利用し、関節角をrvizとは独立して取得してCRANE+に反映させることで、CRANE+をrviz内のモデルと同じように動かしています。
[cpp firstline=”54″]void reflect_jointstate(const sensor_msgs::JointState::ConstPtr& jointstate)[/cpp]
この関数が呼び出された際、自動的に引数に新しいメッセージの内容が引き渡されます。つまり、sensor_msgs::JointState型のポインタjointstateの指し示すインスタンスには、新しいメッセージの内容が格納されています。
[cpp firstline=”56″]pos1.data = jointstate->position[0];[/cpp]
jointstate->position[0]には、モデルのjoint1の関節角が格納されています。それをpos1.dataに格納し、メイン文でpos1を/tilt1_controller/commandに配信することで、CRANE+のjoint1の関節角に反映します。

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