specific_transform_publisher.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 
00007 int main(int argc, char** argv)
00008 {
00009   ros::init(argc, argv, "jsk_model_marker_interface");
00010   ros::NodeHandle n;
00011   ros::NodeHandle pnh_("~");
00012   tf::TransformListener tfl_;
00013 
00014   ros::Publisher pub_ =  pnh_.advertise<tf::tfMessage> ("/specific_transform", 1);
00015 
00016   std::vector<std::string> parent_frame;
00017   std::vector<std::string> child_frame;
00018 
00019   {
00020     XmlRpc::XmlRpcValue param_val;
00021     pnh_.getParam("parent_frame", param_val);
00022     if (param_val.getType() ==  XmlRpc::XmlRpcValue::TypeArray) {
00023       for(int i = 0; i < param_val.size(); i++) {
00024         std::string st = param_val[i];
00025         parent_frame.push_back(st);
00026       }
00027     } else if (param_val.getType() ==  XmlRpc::XmlRpcValue::TypeString) {
00028       std::string st = param_val;
00029       parent_frame.push_back(st);
00030     }
00031   }
00032   {
00033     XmlRpc::XmlRpcValue param_val;
00034     pnh_.getParam("child_frame", param_val);
00035     if (param_val.getType() ==  XmlRpc::XmlRpcValue::TypeArray) {
00036       for(int i = 0; i < param_val.size(); i++) {
00037         std::string st = param_val[i];
00038         child_frame.push_back(st);
00039       }
00040     } else if (param_val.getType() ==  XmlRpc::XmlRpcValue::TypeString) {
00041       std::string st = param_val;
00042       child_frame.push_back(st);
00043     }
00044   }
00045 
00046   if (parent_frame.size() != child_frame.size()) {
00047     ROS_FATAL("size difference between parent frames and child frames");
00048   }
00049 
00050   double loop_hz;
00051   pnh_.param("loop_hz", loop_hz, 1.0 );
00052 
00053   for(int i = 0; i < parent_frame.size(); i++) {
00054     ROS_INFO_STREAM("parent->child: " << parent_frame[i] << " -> " << child_frame[i]);
00055   }
00056   ROS_INFO_STREAM("loop_hz:" << loop_hz);
00057 
00058   ros::Rate rate(loop_hz);
00059 
00060   while (ros::ok())
00061     {
00062       tf::tfMessage tf_msg;
00063       for(int i = 0; i < parent_frame.size(); i++) {
00064         geometry_msgs::TransformStamped tfs_msg;
00065         tf::StampedTransform stf;
00066         try {
00067           tfl_.lookupTransform(parent_frame[i], child_frame[i], ros::Time(0), stf);
00068           tf::transformStampedTFToMsg(stf, tfs_msg);
00069           tf_msg.transforms.push_back(tfs_msg);
00070         } catch(tf::TransformException ex) {
00071           ROS_INFO_STREAM("missing transform: " << parent_frame[i] << " to " << child_frame[i]);
00072         }
00073       }
00074       pub_.publish(tf_msg);
00075 
00076       ros::spinOnce();
00077       rate.sleep();
00078     }
00079 }


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Sun Jan 25 2015 00:30:41