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

ROS_CRANE+

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

依存関係の追加

今回書くプログラムを、前々回に作ったcrane_urdfパッケージ上で動かすためには、crane_urdfパッケージの依存パッケージが不足していると思います。そこで、依存関係を追加しましょう。端末を開いて、

$ roscd crane_urdf
$ vi CMakeLists.txt

と入力します。前回はこの末尾に3文貼り付けましたが、今回は違う場所を編集します。

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  sensor_msgs
  std_msgs
)

などと書かれているところがあると思います。roscpp,rospy,sensor_msgs,std_msgsの順番は、パッケージを作った際に記述した順番などにもよると思うので、あまり気にしないでください。この依存パッケージ群の中に、dynamixel_msgsを追加します。具体的には、以下のように修正します。

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  sensor_msgs
  dynamixel_msgs
  std_msgs
)

ここを書き直し終わったら、保存してviを終了してください。
次に、

vi package.xml

と入力して、package.xmlを開きましょう。

<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>

などと書かれているところがあると思います。他の依存パッケージの例にならって、dynamixel_msgsも追加します。具体的には、以下のように修正します。

<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>

修正し終わったら、保存してviを終了してください。これで、依存関係の追加は完了しました。

新しいdisplay.launchの作成

前回、

roslaunch urdf_tutorial display.launch model:=crane.urdf gui:=True

なる命令を実行していました。この際、urdf_tutorialというパッケージ内のdisplay.launchを立ち上げていましたが、このdisplay.launchのままだと、今回実行したいプログラムが実行できません。そこで、新たなdisplay.launchをcrane_urdf内に作ることにしました。端末に、

$ roscd crane_urdf/src
$ vi display.launch

と入力します。以下を貼り付けます。

<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>

貼り付けたら、保存してviを終了してください。これで、新しいdisplay.launchが完成しました。完成しましたが、何やってるのかさっぱりな方も多いと思います。基本的には、urdf_tutorialパッケージのdisplay.launchの真似ですが、

        <rosparam param="source_list">["joint_states_source"]</rosparam>

の一文を書き加えています。この後出てくるプログラムでは、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にいる状態で、

vi reflect_crane_in_rviz.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;
}

貼り付けたら、保存してviを終了してください。これがソースコードになります。今まで、ファイルを実行可能にする処理をしてきましたが、あれは別にいらなかったようなので、今回は飛ばします。
次に、

$ roscd crane_urdf
$ vi CMakeLists.txt

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

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)

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

$ cd ~/catkin_ws/
$ catkin_make

と入力します。「「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+の電源を切り、その後再度電源をいれ、操作をやり直してください。
その後、新しい端末を開いて、

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

と入力して、rvizを表示します。新しい端末を開き、

rosrun crane_urdf reflect_crane_in_rviz

と入力すれば、rviz内のモデルが実物のCRANE+の状態を反映するようになります。けがやCRANE+の破壊に注意しながら、CRANE+の各関節を手で動かしてみてください。モデルとCRANE+が同じように動くと思います。なお、モデルに定義された回転角の限界以上まで関節を回すと、変な動きをしてしまいますが、ご容赦ください。

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

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