| Note: This tutorial assumes you are familiar with the xml markup language. |
 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: This tutorial gives a full example of a robot model with URDF that uses robot_state_publisher. First, we create the URDF model with all the necessary parts. Then we write a node which publishes the JointState and transforms. Finally, we run all the parts together.Keywords: robot_state_publisher urdf
Tutorial Level: INTERMEDIATE
tf is deprecated in favor of tf2. tf2 provides a superset of the functionality of tf and is actually now the implementation under the hood. If you're just learning now it's strongly recommended to use the tf2/Tutorials instead.
Create the URDF File
Here is the URDF file for a 7-link model roughly approximating R2-D2. Save the following link to your computer: model.xml
Publishing the State
Now we need a method for specifying what state the robot is in. To do this, we must specify all three joints and the overall odometry. Start by creating a package:
cd %TOP_DIR_YOUR_CATKIN_WS%/src catkin_create_pkg r2d2 roscpp rospy tf sensor_msgs std_msgs
Then fire up your favorite editor and paste the following code into the src/state_publisher.cpp file:
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
This launch file assumes you are using the package name "r2d2" and node name "state_publisher" and you have saved this urdf to the "r2d2" package.
1 <launch>
2 <param name="robot_description" command="cat $(find r2d2)/model.xml" />
3 <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
4 <node name="state_publisher" pkg="r2d2" type="state_publisher" />
5 </launch>
Viewing the Results
First we have to edit the CMakeLists.txt in the package where we saved the above source code. Make sure to add the tf dependency in addition to the other dependencies:
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs tf)
Notice that roscpp is used to parse the code that we wrote and generate the state_publisher node. We also have to add the following to the end of the CMakelists.txt in order to generate the state_publisher node:
include_directories(include ${catkin_INCLUDE_DIRS})
add_executable(state_publisher src/state_publisher.cpp)
target_link_libraries(state_publisher ${catkin_LIBRARIES})Now we should go to the directory of the workspace and build it using:
$ catkin_make
Now launch the package (assuming that our launch file is named display.launch):
$ roslaunch r2d2 display.launch
Run rviz in a new terminal using:
$ rosrun rviz rviz
Choose odom as your fixed frame (under Global Options). Then choose "Add Display" and add a Robot Model Display and a TF Display (see http://wiki.ros.org/rviz/UserGuide).
