00001 #include <ros/ros.h>
00002 #include <tf/transform_broadcaster.h>
00003 #include <tf/tf.h>
00004
00005 using namespace std;
00006
00007
00008
00009
00010 int main(int argc, char** argv)
00011 {
00012 ros::init(argc, argv, "example_tf_tree");
00013
00014 ros::NodeHandle node;
00015
00016 tf::TransformBroadcaster tb;
00017
00018 ros::Rate rate(30.0);
00019
00020
00021 double angle = 0;
00022 ros::Time start_time = ros::Time::now();
00023
00024
00025 while (node.ok())
00026 {
00027
00028 tf::Transform trans;
00029 trans.setOrigin(tf::Vector3(0.7,0,0));
00030 trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), -M_PI / 5));
00031 tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/map","/base_link"));
00032 trans.setOrigin(tf::Vector3(0,0.2,0));
00033 trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), M_PI / 4));
00034 tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/base_link","/eye"));
00035 trans.setOrigin(tf::Vector3(0,0.5,0));
00036 trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), M_PI / 3));
00037 tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/eye","object"));
00038 trans.setOrigin(tf::Vector3(0.2,0,0));
00039 trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), M_PI / 2));
00040 tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/base_link","/gripper"));
00041
00042 angle = (ros::Time::now() - start_time).toSec() * 10 / 180 * M_PI;
00043
00044 trans.setOrigin(tf::Vector3(-1.5,0,0));
00045 trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), 0));
00046 tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/map","/table"));
00047
00048 trans.setOrigin(tf::Vector3(.5,.5,.5));
00049 trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), angle));
00050 tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/table","/turntable"));
00051
00052 trans.setOrigin(tf::Vector3(0.4,0.4,0.1));
00053 trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), 0));
00054 tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/turntable","/object_on_table"));
00055
00056
00057 rate.sleep();
00058
00059 ros::spinOnce();
00060
00061 }
00062 return 0;
00063 };
00064