message_to_tf.cc
Go to the documentation of this file.
1 // Copyright (c) 2016 The UUV Simulator Authors.
2 // All rights reserved.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 //
16 // This source code is derived from hector_localization
17 // (https://github.com/tu-darmstadt-ros-pkg/hector_localization)
18 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt,
19 // licensed under the BSD 3-Clause license,
20 // cf. 3rd-party-licenses.txt file in the root directory of this source tree.
21 //
22 // The original code was modified to:
23 // - be more consistent with other sensor plugins within uuv_simulator,
24 // - adhere to Gazebo's coding standards.
25 
26 #include <ros/ros.h>
27 #include <nav_msgs/Odometry.h>
28 #include <geometry_msgs/PoseStamped.h>
29 #include <geometry_msgs/Vector3Stamped.h>
30 #include <geometry_msgs/TransformStamped.h>
31 #include <sensor_msgs/Imu.h>
33 #include <tf/transform_listener.h> // for tf::getPrefixParam()
34 #include <tf/transform_datatypes.h>
35 
37 
38 std::string g_odometry_topic;
39 std::string g_pose_topic;
40 std::string g_imu_topic;
41 std::string g_topic;
42 std::string g_frame_id;
44 std::string g_position_frame_id;
46 std::string g_child_frame_id;
47 
49 
50 std::string g_tf_prefix;
51 
55 
56 #ifndef TF_MATRIX3x3_H
57  typedef btScalar tfScalar;
58  namespace tf { typedef btMatrix3x3 Matrix3x3; }
59 #endif
60 
61 void addTransform(std::vector<geometry_msgs::TransformStamped>& transforms, const tf::StampedTransform& tf)
62 {
63  transforms.push_back(geometry_msgs::TransformStamped());
64  tf::transformStampedTFToMsg(tf, transforms.back());
65 }
66 
67 void sendTransform(geometry_msgs::Pose const &pose, const std_msgs::Header& header, std::string child_frame_id = "")
68 {
69  std::vector<geometry_msgs::TransformStamped> transforms;
70 
72  tf.stamp_ = header.stamp;
73 
74  tf.frame_id_ = header.frame_id;
75  if (!g_frame_id.empty()) tf.frame_id_ = g_frame_id;
77 
78  if (!g_child_frame_id.empty()) child_frame_id = g_child_frame_id;
79  if (child_frame_id.empty()) child_frame_id = "base_link";
80 
81  tf::Quaternion orientation;
82  tf::quaternionMsgToTF(pose.orientation, orientation);
83  tfScalar yaw, pitch, roll;
84  tf::Matrix3x3(orientation).getEulerYPR(yaw, pitch, roll);
85  tf::Point position;
86  tf::pointMsgToTF(pose.position, position);
87 
88  // position intermediate transform (x,y,z)
89  if( !g_position_frame_id.empty() && child_frame_id != g_position_frame_id) {
91  tf.setOrigin(tf::Vector3(position.x(), position.y(), position.z() ));
92  tf.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
93  addTransform(transforms, tf);
94  }
95 
96  // footprint intermediate transform (x,y,yaw)
97  if (!g_footprint_frame_id.empty() && child_frame_id != g_footprint_frame_id) {
99  tf.setOrigin(tf::Vector3(position.x(), position.y(), 0.0));
100  tf.setRotation(tf::createQuaternionFromRPY(0.0, 0.0, yaw));
101  addTransform(transforms, tf);
102 
103  yaw = 0.0;
104  position.setX(0.0);
105  position.setY(0.0);
107  }
108 
109  // stabilized intermediate transform (z)
110  if (!g_footprint_frame_id.empty() && child_frame_id != g_stabilized_frame_id) {
112  tf.setOrigin(tf::Vector3(0.0, 0.0, position.z()));
114  addTransform(transforms, tf);
115 
116  position.setZ(0.0);
118  }
119 
120  // base_link transform (roll, pitch)
121  if (g_publish_roll_pitch) {
122  tf.child_frame_id_ = tf::resolve(g_tf_prefix, child_frame_id);
123  tf.setOrigin(position);
124  tf.setRotation(tf::createQuaternionFromRPY(roll, pitch, yaw));
125  addTransform(transforms, tf);
126  }
127 
128  g_transform_broadcaster->sendTransform(transforms);
129 
130  // publish pose message
131  if (g_pose_publisher) {
132  geometry_msgs::PoseStamped pose_stamped;
133  pose_stamped.pose = pose;
134  pose_stamped.header = header;
135  g_pose_publisher.publish(pose_stamped);
136  }
137 
138  // publish pose message
139  if (g_euler_publisher) {
140  geometry_msgs::Vector3Stamped euler_stamped;
141  euler_stamped.vector.x = roll;
142  euler_stamped.vector.y = pitch;
143  euler_stamped.vector.z = yaw;
144  euler_stamped.header = header;
145  g_euler_publisher.publish(euler_stamped);
146  }
147 }
148 
149 void odomCallback(nav_msgs::Odometry const &odometry) {
150  sendTransform(odometry.pose.pose, odometry.header, odometry.child_frame_id);
151 }
152 
153 void poseCallback(geometry_msgs::PoseStamped const &pose) {
154  sendTransform(pose.pose, pose.header);
155 }
156 
157 void tfCallback(geometry_msgs::TransformStamped const &tf) {
158  geometry_msgs::Pose pose;
159  pose.position.x = tf.transform.translation.x;
160  pose.position.y = tf.transform.translation.y;
161  pose.position.z = tf.transform.translation.z;
162  pose.orientation = tf.transform.rotation;
163 
164  sendTransform(pose, tf.header);
165 }
166 
167 void imuCallback(sensor_msgs::Imu const &imu) {
168  std::vector<geometry_msgs::TransformStamped> transforms;
169  std::string child_frame_id;
170 
172  tf.stamp_ = imu.header.stamp;
173 
175  if (!g_child_frame_id.empty()) child_frame_id = g_child_frame_id;
176  if (child_frame_id.empty()) child_frame_id = "base_link";
177 
178  tf::Quaternion orientation;
179  tf::quaternionMsgToTF(imu.orientation, orientation);
180  tfScalar yaw, pitch, roll;
181  tf::Matrix3x3(orientation).getEulerYPR(yaw, pitch, roll);
182  tf::Quaternion rollpitch = tf::createQuaternionFromRPY(roll, pitch, 0.0);
183 
184  // base_link transform (roll, pitch)
185  if (g_publish_roll_pitch) {
186  tf.child_frame_id_ = tf::resolve(g_tf_prefix, child_frame_id);
187  tf.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
188  tf.setRotation(rollpitch);
189  addTransform(transforms, tf);
190  }
191 
192  if (!transforms.empty()) g_transform_broadcaster->sendTransform(transforms);
193 
194  // publish pose message
195  if (g_pose_publisher) {
196  geometry_msgs::PoseStamped pose_stamped;
197  pose_stamped.header.stamp = imu.header.stamp;
198  pose_stamped.header.frame_id = g_stabilized_frame_id;
199  tf::quaternionTFToMsg(rollpitch, pose_stamped.pose.orientation);
200  g_pose_publisher.publish(pose_stamped);
201  }
202 }
203 
205  if (input.getDataType() == "nav_msgs/Odometry") {
206  nav_msgs::Odometry::ConstPtr odom = input.instantiate<nav_msgs::Odometry>();
207  odomCallback(*odom);
208  return;
209  }
210 
211  if (input.getDataType() == "geometry_msgs/PoseStamped") {
212  geometry_msgs::PoseStamped::ConstPtr pose = input.instantiate<geometry_msgs::PoseStamped>();
213  poseCallback(*pose);
214  return;
215  }
216 
217  if (input.getDataType() == "sensor_msgs/Imu") {
218  sensor_msgs::Imu::ConstPtr imu = input.instantiate<sensor_msgs::Imu>();
219  imuCallback(*imu);
220  return;
221  }
222 
223  if (input.getDataType() == "geometry_msgs/TransformStamped") {
224  geometry_msgs::TransformStamped::ConstPtr tf = input.instantiate<geometry_msgs::TransformStamped>();
225  tfCallback(*tf);
226  return;
227  }
228 
229  ROS_ERROR_THROTTLE(1.0, "message_to_tf received a %s message. Supported message types: nav_msgs/Odometry geometry_msgs/PoseStamped geometry_msgs/TransformStamped sensor_msgs/Imu", input.getDataType().c_str());
230 }
231 
232 int main(int argc, char** argv) {
233  ros::init(argc, argv, "message_to_tf");
234 
235  g_footprint_frame_id = "base_footprint";
236  g_stabilized_frame_id = "base_stabilized";
237  // g_position_frame_id = "base_position";
238  // g_child_frame_id = "base_link";
239 
240  ros::NodeHandle priv_nh("~");
241  priv_nh.getParam("odometry_topic", g_odometry_topic);
242  priv_nh.getParam("pose_topic", g_pose_topic);
243  priv_nh.getParam("imu_topic", g_imu_topic);
244  priv_nh.getParam("topic", g_topic);
245  priv_nh.getParam("frame_id", g_frame_id);
246  priv_nh.getParam("footprint_frame_id", g_footprint_frame_id);
247  priv_nh.getParam("position_frame_id", g_position_frame_id);
248  priv_nh.getParam("stabilized_frame_id", g_stabilized_frame_id);
249  priv_nh.getParam("child_frame_id", g_child_frame_id);
250 
251  // get topic from the commandline
252  if (argc > 1) {
253  g_topic = argv[1];
254  g_odometry_topic.clear();
255  g_pose_topic.clear();
256  g_imu_topic.clear();
257  }
258 
259  g_publish_roll_pitch = true;
260  priv_nh.getParam("publish_roll_pitch", g_publish_roll_pitch);
261 
262  g_tf_prefix = tf::getPrefixParam(priv_nh);
263  g_transform_broadcaster = new tf::TransformBroadcaster;
264 
266  ros::Subscriber sub1, sub2, sub3, sub4;
267  int subscribers = 0;
268  if (!g_odometry_topic.empty()) {
269  sub1 = node.subscribe(g_odometry_topic, 10, &odomCallback);
270  subscribers++;
271  }
272  if (!g_pose_topic.empty()) {
273  sub2 = node.subscribe(g_pose_topic, 10, &poseCallback);
274  subscribers++;
275  }
276  if (!g_imu_topic.empty()) {
277  sub3 = node.subscribe(g_imu_topic, 10, &imuCallback);
278  subscribers++;
279  }
280  if (!g_topic.empty()) {
281  sub4 = node.subscribe(g_topic, 10, &multiCallback);
282  subscribers++;
283  }
284 
285  if (subscribers == 0) {
286  ROS_FATAL("Usage: rosrun message_to_tf message_to_tf <topic>");
287  return 1;
288  } else if (subscribers > 1) {
289  ROS_FATAL("More than one of the parameters odometry_topic, pose_topic, imu_topic and topic are set.\n"
290  "Please specify exactly one of them or simply add the topic name to the command line.");
291  return 1;
292  }
293 
294  bool publish_pose = true;
295  priv_nh.getParam("publish_pose", publish_pose);
296  if (publish_pose) {
297  std::string publish_pose_topic;
298  priv_nh.getParam("publish_pose_topic", publish_pose_topic);
299 
300  if (!publish_pose_topic.empty())
301  g_pose_publisher = node.advertise<geometry_msgs::PoseStamped>(publish_pose_topic, 10);
302  else
303  g_pose_publisher = priv_nh.advertise<geometry_msgs::PoseStamped>("pose", 10);
304  }
305 
306  bool publish_euler = true;
307  priv_nh.getParam("publish_euler", publish_euler);
308  if (publish_euler) {
309  std::string publish_euler_topic;
310  priv_nh.getParam("publish_euler_topic", publish_euler_topic);
311 
312  if (!publish_euler_topic.empty())
313  g_euler_publisher = node.advertise<geometry_msgs::Vector3Stamped>(publish_euler_topic, 10);
314  else
315  g_euler_publisher = priv_nh.advertise<geometry_msgs::Vector3Stamped>("euler", 10);
316  }
317 
318  ros::spin();
320  return 0;
321 }
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
bool g_publish_roll_pitch
#define ROS_FATAL(...)
ros::Publisher g_euler_publisher
std::string g_position_frame_id
std::string const & getDataType() const
std::string g_odometry_topic
static void pointMsgToTF(const geometry_msgs::Point &msg_v, Point &bt_v)
std::string g_child_frame_id
void publish(const boost::shared_ptr< M > &message) const
std::string getPrefixParam(ros::NodeHandle &nh)
TFSIMD_FORCE_INLINE void setX(tfScalar x)
TFSIMD_FORCE_INLINE void setY(tfScalar y)
double tfScalar
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::string g_topic
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
btMatrix3x3 Matrix3x3
void tfCallback(geometry_msgs::TransformStamped const &tf)
std::string g_imu_topic
void odomCallback(nav_msgs::Odometry const &odometry)
ROSCPP_DECL void spin(Spinner &spinner)
void imuCallback(sensor_msgs::Imu const &imu)
std::string resolve(const std::string &prefix, const std::string &frame_name)
TFSIMD_FORCE_INLINE const tfScalar & x() const
#define ROS_ERROR_THROTTLE(rate,...)
std::string child_frame_id_
btScalar tfScalar
std::string g_frame_id
TFSIMD_FORCE_INLINE const tfScalar & z() const
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
std::string g_pose_topic
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
TFSIMD_FORCE_INLINE const tfScalar & y() const
void sendTransform(const StampedTransform &transform)
void sendTransform(geometry_msgs::Pose const &pose, const std_msgs::Header &header, std::string child_frame_id="")
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void poseCallback(geometry_msgs::PoseStamped const &pose)
TFSIMD_FORCE_INLINE void setBasis(const Matrix3x3 &basis)
boost::shared_ptr< M > instantiate() const
std::string g_stabilized_frame_id
void addTransform(std::vector< geometry_msgs::TransformStamped > &transforms, const tf::StampedTransform &tf)
tf::TransformBroadcaster * g_transform_broadcaster
void multiCallback(topic_tools::ShapeShifter const &input)
int main(int argc, char **argv)
ros::Publisher g_pose_publisher
bool getParam(const std::string &key, std::string &s) const
std::string g_footprint_frame_id
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
static const Matrix3x3 & getIdentity()
std::string frame_id_
std::string g_tf_prefix


uuv_assistants
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Thu Jun 18 2020 03:28:20