specific_transform_subscriber.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <tf/tf.h>
00003 #include <tf/transform_listener.h>
00004 #include <tf/transform_broadcaster.h>
00005 #include <iostream>
00006 #include <jsk_topic_tools/specific_transform_subscriber.h>
00007 #include <dynamic_tf_publisher/SetDynamicTF.h>
00008 
00009 void CallSetDynamicTf(std::string parent_frame_id, std::string frame_id, geometry_msgs::Transform transform){
00010   dynamic_tf_publisher::SetDynamicTF SetTf;
00011   SetTf.request.freq = 10;
00012   SetTf.request.cur_tf.header.stamp = ros::Time::now();
00013   SetTf.request.cur_tf.header.frame_id = parent_frame_id;
00014   SetTf.request.cur_tf.child_frame_id = frame_id;
00015   SetTf.request.cur_tf.transform = transform;
00016   dynamic_tf_publisher_client.call(SetTf);
00017 }
00018 
00019 
00020 void transformCallback(const tf::tfMessage::ConstPtr& msg){
00021   for(int i=0; i<msg->transforms.size(); i++){
00022     CallSetDynamicTf(msg->transforms[i].header.frame_id, msg->transforms[i].child_frame_id, msg->transforms[i].transform);
00023   }
00024 }
00025 
00026 int main(int argc, char** argv)
00027 {
00028   ros::init(argc, argv, "specific_transform_subscriber");
00029 
00030   ros::NodeHandle nh_;
00031   ros::NodeHandle pnh_("~");
00032 
00033   ros::Subscriber sub_ =  pnh_.subscribe<tf::tfMessage>
00034     ("/specific_transform", 100, transformCallback);
00035 
00036   dynamic_tf_publisher_client = nh_.serviceClient<dynamic_tf_publisher::SetDynamicTF>("set_dynamic_tf");
00037   ros::service::waitForService("set_dynamic_tf", -1);
00038   
00039   ros::spin();
00040 }


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Sun Jan 25 2015 12:38:34