## For instruction on writing tutorials
## http://wiki.ros.org/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note = This tutorial assumes you are familiar with the xml markup language
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links
## note.0=
## descriptive title for the tutorial
## title = Using urdf with robot_state_publisher
## multi-line description to be displayed in search
## 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.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## what level user is this tutorial for
## level= IntermediateCategory
## keywords = robot_state_publisher urdf
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

{{{#!wiki caution
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.
}}}

<<TableOfContents(4)>>

## AUTOGENERATED DO NOT DELETE
## StepByStepURDFCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE

== 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: [[http://wiki.ros.org/urdf/Tutorials/Using%20urdf%20with%20robot_state_publisher?action=AttachFile&do=get&target=model.xml|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:

{{{#!cplusplus
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n;
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(30);

    const double degree = M_PI/180;

    // robot state
    double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;

    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "axis";

    while (ros::ok()) {
        //update joint_state
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(3);
        joint_state.position.resize(3);
        joint_state.name[0] ="swivel";
        joint_state.position[0] = swivel;
        joint_state.name[1] ="tilt";
        joint_state.position[1] = tilt;
        joint_state.name[2] ="periscope";
        joint_state.position[2] = height;


        // update transform
        // (moving in a circle with radius=2)
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = cos(angle)*2;
        odom_trans.transform.translation.y = sin(angle)*2;
        odom_trans.transform.translation.z = .7;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);

        //send the joint state and transform
        joint_pub.publish(joint_state);
        broadcaster.sendTransform(odom_trans);

        // Create new robot state
        tilt += tinc;
        if (tilt<-.5 || tilt>0) tinc *= -1;
        height += hinc;
        if (height>.2 || height<0) hinc *= -1;
        swivel += degree;
        angle += degree/4;

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }


    return 0;
}
}}}
== Launch File ==
This launch file assumes you are using the package name "r2d2" and node name "state_publisher" and you have saved [[http://wiki.ros.org/urdf/Tutorials/Using%20urdf%20with%20robot_state_publisher?action=AttachFile&do=get&target=model.xml|this urdf]] to the "r2d2" package.

{{{#!xml
<launch>
        <param name="robot_description" command="cat $(find r2d2)/model.xml" />
        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
        <node name="state_publisher" pkg="r2d2" type="state_publisher" />
</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).