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 }