turtle_tf2_broadcaster.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <tf2/LinearMath/Quaternion.h>
00003 #include <tf2_ros/transform_broadcaster.h>
00004 #include <geometry_msgs/TransformStamped.h>
00005 #include <turtlesim/Pose.h>
00006 
00007 std::string turtle_name;
00008 
00009 
00010 
00011 void poseCallback(const turtlesim::PoseConstPtr& msg){
00012   static tf2_ros::TransformBroadcaster br;
00013         geometry_msgs::TransformStamped transformStamped;
00014   
00015         transformStamped.header.stamp = ros::Time::now();
00016         transformStamped.header.frame_id = "world";
00017         transformStamped.child_frame_id = turtle_name;
00018         transformStamped.transform.translation.x = msg->x;
00019         transformStamped.transform.translation.y = msg->y;
00020         transformStamped.transform.translation.z = 0.0;
00021         tf2::Quaternion q;
00022         q.setRPY(0, 0, msg->theta);
00023         transformStamped.transform.rotation.x = q.x();
00024         transformStamped.transform.rotation.y = q.y();
00025         transformStamped.transform.rotation.z = q.z();
00026         transformStamped.transform.rotation.w = q.w();
00027 
00028         br.sendTransform(transformStamped);
00029 }
00030 
00031 int main(int argc, char** argv){
00032   ros::init(argc, argv, "my_tf2_broadcaster");
00033 
00034   ros::NodeHandle private_node("~");
00035   if (! private_node.hasParam("turtle"))
00036   {
00037     if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
00038     turtle_name = argv[1];
00039   }
00040   else
00041   {
00042     private_node.getParam("turtle", turtle_name);
00043   }
00044     
00045   ros::NodeHandle node;
00046   ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
00047 
00048   ros::spin();
00049   return 0;
00050 };
00051 


turtle_tf2
Author(s): Denis Štogl
autogenerated on Thu Aug 10 2017 02:47:28