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

こんにちは!長谷川です。
この連載も、ついに10回目に突入しました。ここまで見てくださった皆様、本当にありがとうございます!
前回は、前々回作ったモデルと実物のCRANE+の連動のさせ方について説明しましたが、今回は、CRANE+の状態をモデルに反映させる方法について説明したいと思います。

依存関係の追加

今回書くプログラムを、前々回に作ったcrane_urdfパッケージ上で動かすためには、crane_urdfパッケージの依存パッケージが不足していると思います。そこで、依存関係を追加しましょう。端末を開いて、
[php]
$ roscd crane_urdf
$ vi CMakeLists.txt
[/php]
と入力します。前回はこの末尾に3文貼り付けましたが、今回は違う場所を編集します。
[php firstline=”7″]
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
sensor_msgs
std_msgs
)
[/php]
などと書かれているところがあると思います。roscpp,rospy,sensor_msgs,std_msgsの順番は、パッケージを作った際に記述した順番などにもよると思うので、あまり気にしないでください。この依存パッケージ群の中に、dynamixel_msgsを追加します。具体的には、以下のように修正します。
[php firstline=”7″]
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
sensor_msgs
dynamixel_msgs
std_msgs
)
[/php]
ここを書き直し終わったら、保存してviを終了してください。
次に、[php]vi package.xml[/php]と入力して、package.xmlを開きましょう。
[xml firstline=”42″]
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>std_msgs</run_depend>
[/xml]
などと書かれているところがあると思います。他の依存パッケージの例にならって、dynamixel_msgsも追加します。具体的には、以下のように修正します。
[xml firstline=”42″]
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>dynamixel_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>dynamixel_msgs</run_depend>
<run_depend>std_msgs</run_depend>
[/xml]
修正し終わったら、保存してviを終了してください。これで、依存関係の追加は完了しました。

新しいdisplay.launchの作成

前回、
[php]roslaunch urdf_tutorial display.launch model:=crane.urdf gui:=True[/php]
なる命令を実行していました。この際、urdf_tutorialというパッケージ内のdisplay.launchを立ち上げていましたが、このdisplay.launchのままだと、今回実行したいプログラムが実行できません。そこで、新たなdisplay.launchをcrane_urdf内に作ることにしました。端末に、
[php]
$ roscd crane_urdf/src
$ vi display.launch
[/php]
と入力します。以下を貼り付けます。
[xml]
<launch>
<arg name="model" />
<arg name="gui" default="False" />
<param name="robot_description" textfile="$(arg model)" />
<param name="use_gui" value="$(arg gui)"/>
<rosparam param="source_list">["joint_states_source"]</rosparam>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" required="true" />
</launch>
[/xml]
貼り付けたら、保存してviを終了してください。これで、新しいdisplay.launchが完成しました。完成しましたが、何やってるのかさっぱりな方も多いと思います。基本的には、urdf_tutorialパッケージのdisplay.launchの真似ですが、
[xml firstline=”6″] <rosparam param="source_list">["joint_states_source"]</rosparam>[/xml]
の一文を書き加えています。この後出てくるプログラムでは、joint_state_publisherノード(Joint State Publisherウィンドウを管理しているノード)に各関節の関節角データを送り、それをjoint_state_publisherノードがrvizに送ることにより、モデルを動かしているのですが、ここをよく読むとわかる通り、それをするためには、joint_state_publisherが購読するトピックを指定してあげなければなりません。指定方法は、source_listというパラメータに、トピックの名前を書き込んであげればいいようです。付け加えた文では、このsource_listに/joint_states_sourceというトピック名を書き込んでいます。これにより、display.launchが実行されると、/joint_states_sourceを購読しながら、その値に応じたメッセージを/joint_statesに配信してくれるjoint_state_publisherが起動することになります。なお、この際、/joint_states_sourceトピックが存在していない場合は、自動で生成されるようです。

プログラムの作成

次に、プログラムを作成しましょう。
crane_urdf/srcにいる状態で、[php]vi reflect_crane_in_rviz.cpp[/php]と入力します。以下を貼り付けます。
[cpp]
#include "ros/ros.h"

#include "std_msgs/Float64.h"
#include "sensor_msgs/JointState.h"
#include "dynamixel_msgs/MotorStateList.h"

#include <sstream>

#define PI 3.14159265
#define MIDDLE_VAL 512
#define CHANGE_TO_DEG (300.0/1024.0)
#define CHANGE_TO_RAD (PI/180.0)

sensor_msgs::JointState jointstate;
char renew = 0;

void reflect_jointstate(const dynamixel_msgs::MotorStateList::ConstPtr& motorstates);

int main(int argc, char **argv)
{
ros::init(argc, argv, "reflect_crane_in_rviz");

ros::NodeHandle n;

ros::Publisher joint_states_pub = n.advertise<sensor_msgs::JointState>("/joint_states_source", 10);

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

ros::Rate loop_rate(100);

jointstate.name.push_back("joint1");
jointstate.name.push_back("joint2");
jointstate.name.push_back("joint3");
jointstate.name.push_back("joint4");
jointstate.name.push_back("joint5");
jointstate.position.push_back(0.0);
jointstate.position.push_back(0.0);
jointstate.position.push_back(0.0);
jointstate.position.push_back(0.0);
jointstate.position.push_back(0.0);

while (ros::ok())
{
while(ros::ok() && renew==0){
ros::spinOnce();
loop_rate.sleep();
}
jointstate.header.stamp = ros::Time::now();
joint_states_pub.publish(jointstate);
renew = 0;

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

void reflect_jointstate(const dynamixel_msgs::MotorStateList::ConstPtr& motorstates)
{
int id;
for(id=1;id<=5;id++){
jointstate.position[id-1] = ((double)(motorstates->motor_states[id-1].position)-MIDDLE_VAL)*CHANGE_TO_DEG*CHANGE_TO_RAD;
}
renew = 1;
}
[/cpp]
貼り付けたら、保存してviを終了してください。これがソースコードになります。今まで、ファイルを実行可能にする処理をしてきましたが、あれは別にいらなかったようなので、今回は飛ばします。
次に、
[php]
$ roscd crane_urdf
$ vi CMakeLists.txt
[/php]
と入力します。末尾に以下の3文を貼り付けます。
[php]
add_executable(reflect_crane_in_rviz src/reflect_crane_in_rviz.cpp)
target_link_libraries(reflect_crane_in_rviz ${catkin_LIBRARIES})
add_dependencies(reflect_crane_in_rviz crane_urdf_generate_messages_cpp)
[/php]
これで保存してviを終了してください。
次に、crane_urdfパッケージをビルドしましょう。
[php]
$ cd ~/catkin_ws/
$ catkin_make
[/php]
と入力します。「「100%」 Built target reflect_crane_in_rviz」と出てきていたら、ビルドが成功していると思います。

CRANE+とモデルの連動

以上で準備は整いました。第5回目の記事にしたがって、CRANE+とPCを接続し、今回は「roslaunch my_dynamixel_tutorial controller_manager.launch」
まで入力してください。「roslaunch my_dynamixel_tutorial start_tilt_controller.launch」まで入力してしまうと、CRANE+の関節角が固定されてしまい、手で動かせなくなるのでご注意ください。もしそうなった場合は、一回Ctrl-cを押してROSを終了してから、CRANE+の電源を切り、その後再度電源をいれ、操作をやり直してください。
その後、新しい端末を開いて、
[php]
$ roscd crane_urdf/src
$ roslaunch crane_urdf display.launch model:=crane.urdf
[/php]
と入力して、rvizを表示します。新しい端末を開き、
[php]rosrun crane_urdf reflect_crane_in_rviz[/php]
と入力すれば、rviz内のモデルが実物のCRANE+の状態を反映するようになります。けがやCRANE+の破壊に注意しながら、CRANE+の各関節を手で動かしてみてください。モデルとCRANE+が同じように動くと思います。なお、モデルに定義された回転角の限界以上まで関節を回すと、変な動きをしてしまいますが、ご容赦ください。

以上、CRANE+の状態をモデルに反映させる方法について説明しました。
次回は、CRANE+の状態をrviz上でモニタリングしつつ、CRANE+を自動で動かすプログラムを作ってみたいと思います。