tf_remapper.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
3 #include <set>
4 
6 
8 
9 tf_remapper_cpp::TfRemapper::TfRemapper(const std::map<std::string, std::string> mappings, const bool reverse) {
10  for (TfRemapper::MappingsType::const_iterator it = mappings.begin(); it != mappings.end(); ++it) {
11  if (it->first.empty())
12  throw ros::InvalidParameterException("Key 'old' in each mapping has to be set to a nonempty string.");
13  }
14 
15  if (!reverse) {
16  this->mappings = mappings;
17  } else {
18  for (std::map<std::string, std::string>::const_iterator it = mappings.begin(); it != mappings.end(); ++it) {
19  if (!it->second.empty())
20  this->mappings[it->second] = it->first;
21  }
22  }
23 }
24 
25 tf_remapper_cpp::TfRemapper::TfRemapper(const XmlRpc::XmlRpcValue& mappingsParam, const bool reverse) {
26  if(mappingsParam.getType() != XmlRpc::XmlRpcValue::TypeArray)
27  throw ros::InvalidParameterException("The 'mappings' parameter must be an array of dictionaries.");
28 
29  for (size_t i = 0; i < mappingsParam.size(); ++i) {
30  XmlRpc::XmlRpcValue mapping = mappingsParam[i];
32  throw ros::InvalidParameterException("The 'mappings' parameter must be an array of dictionaries.");
33 
34  std::string oldTopic;
35  std::string newTopic;
36  const std::string oldKey = (reverse ? "new" : "old");
37  const std::string newKey = (reverse ? "old" : "new");
38  for (XmlRpc::XmlRpcValue::iterator it = mapping.begin(); it != mapping.end(); ++it)
39  {
40  if (it->second.getType() != XmlRpc::XmlRpcValue::TypeString)
41  throw ros::InvalidParameterException("Dict values must be strings");
42  if (it->first == oldKey)
43  oldTopic = (const std::string&)it->second;
44  else if (it->first == newKey)
45  newTopic = (std::string&)it->second;
46  else
47  throw ros::InvalidParameterException("Each dict in the mapping has to have only keys 'old' and 'new'.");
48  }
49 
50  if (oldTopic.empty() && newTopic.empty())
51  throw ros::InvalidParameterException("Either 'old' or 'new' key in each mapping has to be set to a "
52  "nonempty string.");
53 
54  if (oldTopic == newTopic)
55  ROS_WARN_STREAM("Remapped topic '" << oldTopic << "' is the same as the old topic.");
56 
57  if (!oldTopic.empty())
58  this->mappings[oldTopic] = newTopic;
59  }
60 }
61 
62 tf2_msgs::TFMessage tf_remapper_cpp::TfRemapper::doRemapping(const tf2_msgs::TFMessage& inMessage) const {
63  tf2_msgs::TFMessage message = inMessage;
64  this->doRemapping(message);
65  return message;
66 }
67 
68 void tf_remapper_cpp::TfRemapper::doRemapping(tf2_msgs::TFMessage& message) const {
69  for (std::vector<geometry_msgs::TransformStamped>::iterator it = message.transforms.begin();
70  it != message.transforms.end(); /* ++it is called at the end of the loop to allow erasing */) {
71  geometry_msgs::TransformStamped& transform = *it;
72  if (this->mappings.find(transform.header.frame_id) != this->mappings.end()) {
73  if (this->mappings.at(transform.header.frame_id).empty()) { // delete the transform
74  it = message.transforms.erase(it);
75  continue; // it already points to the next element or end()
76  } else {
77  transform.header.frame_id = this->mappings.at(transform.header.frame_id);
78  }
79  }
80  if (this->mappings.find(transform.child_frame_id) != this->mappings.end()) {
81  if (this->mappings.at(transform.child_frame_id).empty()) { // delete the transform
82  it = message.transforms.erase(it);
83  continue; // it already points to the next element or end()
84  } else {
85  transform.child_frame_id = this->mappings.at(transform.child_frame_id);
86  }
87  }
88  ++it; // we did not delete the transform, so go to the next one
89  }
90 }
91 
93  return this->mappings;
94 }
ValueStruct::iterator iterator
int size() const
void doRemapping(tf2_msgs::TFMessage &message) const
Rewrite TF frame names according to the rules given in constructor.
Definition: tf_remapper.cpp:68
MappingsType mappings
The mappings (keys are old frame names, values are remapped names).
Definition: tf_remapper.h:49
TfRemapper()
Empty constructor with no remappings.
Definition: tf_remapper.cpp:5
Type const & getType() const
std::map< std::string, std::string > MappingsType
Definition: tf_remapper.h:12
const MappingsType & getMappings() const
Get the mappings this remapper uses.
Definition: tf_remapper.cpp:92
#define ROS_WARN_STREAM(args)


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