transform_merger.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <tf/tf.h>
5 #include <iostream>
6 #include <map>
7 #include <vector>
8 
9 std::map<std::string, geometry_msgs::TransformStamped> tf_map;
10 
11 void transformCallback(const tf::tfMessage::ConstPtr& msg){
12  std::pair<std::map<std::string, geometry_msgs::TransformStamped>::iterator, bool> ret;
13  for(int i=0; i<msg->transforms.size(); i++){
14  geometry_msgs::TransformStamped tfs = msg->transforms[i];
15  ret = tf_map.insert( std::map<std::string, geometry_msgs::TransformStamped>::value_type(tfs.child_frame_id, tfs) );
16 
17  //update value
18  if(!ret.second){
19  tf_map[tfs.child_frame_id] = tfs;
20  }
21  }
22 }
23 
24 int main(int argc, char** argv)
25 {
26  ros::init(argc, argv, "transform_merger");
28  ros::NodeHandle pnh_("~");
30 
31  ros::Publisher pub_ = pnh_.advertise<tf::tfMessage> ("/tf_merged", 1);
32  ros::Subscriber sub_ = pnh_.subscribe<tf::tfMessage>
33  ("/tf", 100, transformCallback);
34 
35  double loop_hz;
36  pnh_.param("loop_hz", loop_hz, 1.0 );
37  ROS_INFO_STREAM("loop_hz:" << loop_hz);
38 
39  ros::Rate rate(loop_hz);
40 
41  // set ignore tf
42  std::vector< std::string > ignore_tf_vec;
43 
44 
46  pnh_.param("transform_config", v, v);
47  if(v.hasMember("ignore_tf")){
48  XmlRpc::XmlRpcValue ignore_v = v["ignore_tf"];
49  ROS_INFO_STREAM("ignore following transform");
50  for(int i=0; i< ignore_v.size(); i++){
51  ignore_tf_vec.push_back(ignore_v[i]);
52  ROS_INFO_STREAM("ignore: " << ignore_v[i]);
53  }
54  }
55 
56 
57  while (ros::ok())
58  {
59  tf::tfMessage tf_msg;
60  std::map<std::string, geometry_msgs::TransformStamped>::iterator it = tf_map.begin();
61  while( it != tf_map.end() )
62  {
63 
64  std::vector< std::string >::iterator ignore_it = find( ignore_tf_vec.begin(), ignore_tf_vec.end(), it->second.child_frame_id );
65  if(ignore_it == ignore_tf_vec.end()){
66  tf_msg.transforms.push_back( it->second );
67  }
68  ++it;
69  }
70  pub_.publish(tf_msg);
71  tf_map.clear();
72  rate.sleep();
73  ros::spinOnce();
74  }
75 }
int main(int argc, char **argv)
void transformCallback(const tf::tfMessage::ConstPtr &msg)
std::map< std::string, geometry_msgs::TransformStamped > tf_map
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int size() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
bool hasMember(const std::string &name) const
#define ROS_INFO_STREAM(args)
ROSCPP_DECL void spinOnce()


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Tue Feb 6 2018 03:45:19