tf_remapper_node.cpp
Go to the documentation of this file.
2 #include <set>
3 
5 {
6  this->oldTfTopic = this->privateNodeHandle.param<std::string>("old_tf_topic_name", "/tf_old");
7  this->remappedTfTopic = this->privateNodeHandle.param<std::string>("new_tf_topic_name", "/tf");
8 
9  if (this->privateNodeHandle.hasParam("static_tf"))
10  this->staticTf = this->privateNodeHandle.param<bool>("static_tf", false);
11  else
12  // Autodetect if the parameter is not set
13  this->staticTf = this->remappedTfTopic == "tf_static" || this->remappedTfTopic == "/tf_static";
14 
15  // Parse the 'mappings' parameter, which should be an array of dicts, e.g. [{"old": "b", "new": "d"}]
16  XmlRpc::XmlRpcValue mappingsParam;
17  const bool hasMappingsParam = this->privateNodeHandle.getParam("mappings", mappingsParam);
18  if (!hasMappingsParam) {
19  mappingsParam.setSize(0); // makes it an empty array
20  ROS_WARN("The 'mappings' parameter to tf_remap is missing");
21  }
22 
23  const bool bidirectional = this->privateNodeHandle.param<bool>("is_bidirectional", false);
24 
25  this->tfRemapper = TfRemapper(mappingsParam);
26  if (bidirectional)
27  this->reverseTfRemapper = TfRemapper(mappingsParam, true);
28 
29  if (!tfRemapper.getMappings().empty()) {
30  ROS_INFO_STREAM("Applying the following mappings" << (bidirectional ? " bidirectionally" : "") <<
31  " to incoming tf frame ids:");
32  for (TfRemapper::MappingsType::const_iterator it = tfRemapper.getMappings().begin();
33  it != tfRemapper.getMappings().end(); ++it) {
34  ROS_INFO_STREAM("* " << it->first << " " << (bidirectional && !it->second.empty() ? "<" : "") << "-> " <<
35  (it->second.empty() ? "DELETE" : it->second));
36  }
37  if (bidirectional) {
38  for (TfRemapper::MappingsType::const_iterator it = reverseTfRemapper.getMappings().begin();
39  it != reverseTfRemapper.getMappings().end(); ++it) {
40  if (it->second.empty())
41  ROS_INFO_STREAM("* DELETE <- " << it->first);
42  }
43  }
44  } else {
45  ROS_WARN("No mappings defined.");
46  }
47 
48  ROS_INFO_STREAM("Old TF topic: " << this->oldTfTopic);
49  ROS_INFO_STREAM("Remapped TF topic: " << this->remappedTfTopic);
50  if (this->staticTf)
51  ROS_INFO("Running in static TF mode (caching all TFs, latched publisher)");
52 
54  this->oldTfTopic, 100, &TfRemapperNode::oldTfCallback, this);
55  this->remappedTfPublisher = this->publicNodeHandle.advertise<tf2_msgs::TFMessage>(
56  this->remappedTfTopic, 100, this->staticTf);
57 
58  if (bidirectional) {
60  this->remappedTfTopic, 100, &TfRemapperNode::remappedTfCallback, this);
61  this->oldTfPublisher = this->publicNodeHandle.advertise<tf2_msgs::TFMessage>(
62  this->oldTfTopic, 100, this->staticTf);
63  }
64 }
65 
67  // Prevent reacting to own messages from bidirectional mode
68  const std::string& callerid = event.getPublisherName();
69  if (callerid == ros::this_node::getName())
70  return;
71 
72  tf2_msgs::TFMessage message = this->tfRemapper.doRemapping(*event.getConstMessage());
73 
74  // Since static TF can come from many latched publishers, and we are only a single publisher, we must gather all
75  // the static TF messages in a cache and every time publish all of them.
76  if (this->staticTf) {
77  this->addToStaticTfCache(message, this->staticTfCache);
79  } else {
80  this->remappedTfPublisher.publish(message);
81  }
82 }
83 
85  // Prevent reacting to own messages from bidirectional mode
86  const std::string& callerid = event.getPublisherName();
87  if (callerid == ros::this_node::getName())
88  return;
89 
90  tf2_msgs::TFMessage message = this->reverseTfRemapper.doRemapping(*event.getConstMessage());
91 
92  // Since static TF can come from many latched publishers, and we are only a single publisher, we must gather all
93  // the static TF messages in a cache and every time publish all of them.
94  if (this->staticTf) {
95  this->addToStaticTfCache(message, this->reverseStaticTfCache);
97  } else {
98  this->oldTfPublisher.publish(message);
99  }
100 }
101 
103  const tf2_msgs::TFMessage& message, tf2_msgs::TFMessage& cache) const {
104  // We do an inefficient O(N^2) search here, but there should not be that many static TFs that it would matter
105  for (std::vector<geometry_msgs::TransformStamped>::const_iterator it = message.transforms.begin();
106  it != message.transforms.end(); ++it) {
107  bool found = false;
108  for (std::vector<geometry_msgs::TransformStamped>::iterator cacheIt = cache.transforms.begin();
109  cacheIt != cache.transforms.end(); ++cacheIt) {
110  if (it->child_frame_id == cacheIt->child_frame_id) {
111  found = true;
112  *cacheIt = *it;
113  break;
114  }
115  }
116  if (!found)
117  cache.transforms.push_back(*it);
118  }
119 }
120 
121 int main(int argc, char * argv[])
122 {
123  ros::init(argc, argv, "tf_remapper");
125  ros::spin();
126  return 0;
127 }
TfRemapper reverseTfRemapper
The remapper that actually changes the TF messages.
TfRemapper tfRemapper
Name of the remapped topic ("/tf").
int main(int argc, char *argv[])
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())
void doRemapping(tf2_msgs::TFMessage &message) const
Rewrite TF frame names according to the rules given in constructor.
Definition: tf_remapper.cpp:68
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
Remap frames in TF messages according to given rules.
Definition: tf_remapper.h:10
tf2_msgs::TFMessage staticTfCache
If true, this node works with static TF, which need special care.
#define ROS_WARN(...)
const boost::shared_ptr< ConstMessage > & getConstMessage() const
ros::Publisher oldTfPublisher
Publisher of /tf.
A node that remaps TF frame names according to the given set of rules.
void setSize(int size)
ros::Subscriber remappedTfSubscriber
Subscriber to /tf_old.
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::NodeHandle privateNodeHandle
ROS node handle.
const MappingsType & getMappings() const
Get the mappings this remapper uses.
Definition: tf_remapper.cpp:92
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
tf2_msgs::TFMessage reverseStaticTfCache
Cache of static TF messages.
std::string remappedTfTopic
Name of the old topic ("/tf_old").
ros::Publisher remappedTfPublisher
Subscriber to /tf (only in bidirectional mode).
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
void oldTfCallback(const ros::MessageEvent< tf2_msgs::TFMessage const > &event)
The remapper that actually changes the TF messages in reverse direction.
ros::Subscriber oldTfSubscriber
Private ROS node handle.
void remappedTfCallback(const ros::MessageEvent< tf2_msgs::TFMessage const > &event)
Callback when a TF message arrives on the remapped TF topic.
bool staticTf
Publisher of /tf_old (only in bidirectional mode).
bool hasParam(const std::string &key) const
void addToStaticTfCache(const tf2_msgs::TFMessage &message, tf2_msgs::TFMessage &cache) const
Add the given TF message to the static TF cache. Newer TFs overwrite the older ones.
std::string oldTfTopic
Cache of static TF messages in the reverse direction.


tf_remapper_cpp
Author(s): Martin Pecka
autogenerated on Mon Jun 10 2019 15:29:46