Note: このチュートリアルはあなたが xml マークアップ言語になじみがあると想定します.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Using urdf with robot_state_publisher

Description: このチュートリアルは robot_state_publisher を使う URDF でロボットモデルの完全な例を与えます。 最初に、すべての必要なパーツで URDF モデルを作ります。 それから私たちは JointState を配信して、そして変わるノードを書きます。 最終的に、私たちは一緒にすべてのパーツを動かします。

Keywords: robot_state_publisher urdf

Tutorial Level: INTERMEDIATE

Create the URDF File

ここに荒く R2-D2 に似ている7リンクのモデルのに関する URDF ファイルがあります。 あなたのコンピュータへ次のリンクをセーブしてください: model.xml

Publishing the State

今私たちはロボットがどんな状態にあるか明示するための方法を必要とします。これをするために、私たちはすべての3つの Joint と全体的な odometry を指定しなくてはなりません。下のソースコード(この例を通じて、私たちはパッケージネーム"r2d2"とノードネーム"state_publisher"を仮定します)を含むためにパッケージを作成してください:

   1 #include <string>
   2 #include <ros/ros.h>
   3 #include <sensor_msgs/JointState.h>
   4 #include <tf/transform_broadcaster.h>
   5 
   6 int main(int argc, char** argv) {
   7     ros::init(argc, argv, "state_publisher");
   8     ros::NodeHandle n;
   9     ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
  10     tf::TransformBroadcaster broadcaster;
  11     ros::Rate loop_rate(30);
  12 
  13     const double degree = M_PI/180;
  14 
  15     // robot state
  16     double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;
  17 
  18     // message declarations
  19     geometry_msgs::TransformStamped odom_trans;
  20     sensor_msgs::JointState joint_state;
  21     odom_trans.header.frame_id = "odom";
  22     odom_trans.child_frame_id = "axis";
  23 
  24     while (ros::ok()) {
  25         //update joint_state
  26         joint_state.header.stamp = ros::Time::now();
  27         joint_state.name.resize(3);
  28         joint_state.position.resize(3);
  29         joint_state.name[0] ="swivel";
  30         joint_state.position[0] = swivel;
  31         joint_state.name[1] ="tilt";
  32         joint_state.position[1] = tilt;
  33         joint_state.name[2] ="periscope";
  34         joint_state.position[2] = height;
  35 
  36 
  37         // update transform
  38         // (moving in a circle with radius=2)
  39         odom_trans.header.stamp = ros::Time::now();
  40         odom_trans.transform.translation.x = cos(angle)*2;
  41         odom_trans.transform.translation.y = sin(angle)*2;
  42         odom_trans.transform.translation.z = .7;
  43         odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);
  44 
  45         //send the joint state and transform
  46         joint_pub.publish(joint_state);
  47         broadcaster.sendTransform(odom_trans);
  48 
  49         // Create new robot state
  50         tilt += tinc;
  51         if (tilt<-.5 || tilt>0) tinc *= -1;
  52         height += hinc;
  53         if (height>.2 || height<0) hinc *= -1;
  54         swivel += degree;
  55         angle += degree/4;
  56 
  57         // This will adjust as needed per iteration
  58         loop_rate.sleep();
  59     }
  60 
  61 
  62     return 0;
  63 }

Launch File

この launch ファイルはあなたがパッケージネーム"r2d2"とノードネーム"state_publisher"を使っていると想定します。

   1 <launch>
   2         <param name="robot_description" command="cat $(find r2d2)/model.xml" />
   3         <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
   4         <node name="state_publisher" pkg="r2d2" type="state_publisher" />
   5 </launch>

Viewing the Results

ターミナルを新たに開き、rvizを起動します。

$ rosrun rviz rviz

(Global Optionsの)Fixed Frameとして odom を選択してください。 それから"Add Display"を選択して、そしてRobot Model DisplayとTF Displayを加えてください(http://www.ros.org/wiki/rviz/UserGuide 参照) 。

Wiki: ja/urdf/Tutorials/Using urdf with robot_state_publisher (last edited 2016-11-16 15:27:48 by ronekko)