| Note: This tutorial assumes you are familiar with the xml markup language. |
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
Contents
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
Some Key Features of the URDF
Error: No code_block found
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.
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, "intial_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.
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
Run rviz, with odom as your fixed frame. Add a Robot Model and TF.