message_to_tf.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <nav_msgs/Odometry.h>
00003 #include <geometry_msgs/PoseStamped.h>
00004 #include <geometry_msgs/Vector3Stamped.h>
00005 #include <geometry_msgs/TransformStamped.h>
00006 #include <sensor_msgs/Imu.h>
00007 #include <tf/transform_broadcaster.h>
00008 #include <tf/transform_listener.h> // for tf::getPrefixParam()
00009 #include <tf/transform_datatypes.h>
00010 
00011 #include <topic_tools/shape_shifter.h>
00012 
00013 std::string g_odometry_topic;
00014 std::string g_pose_topic;
00015 std::string g_imu_topic;
00016 std::string g_topic;
00017 std::string g_frame_id;
00018 std::string g_footprint_frame_id;
00019 std::string g_position_frame_id;
00020 std::string g_stabilized_frame_id;
00021 std::string g_child_frame_id;
00022 
00023 bool g_publish_roll_pitch;
00024 
00025 std::string g_tf_prefix;
00026 
00027 tf::TransformBroadcaster *g_transform_broadcaster;
00028 ros::Publisher g_pose_publisher;
00029 ros::Publisher g_euler_publisher;
00030 
00031 #ifndef TF_MATRIX3x3_H
00032   typedef btScalar tfScalar;
00033   namespace tf { typedef btMatrix3x3 Matrix3x3; }
00034 #endif
00035 
00036 void addTransform(std::vector<geometry_msgs::TransformStamped>& transforms, const tf::StampedTransform& tf)
00037 {
00038   transforms.push_back(geometry_msgs::TransformStamped());
00039   tf::transformStampedTFToMsg(tf, transforms.back());
00040 }
00041 
00042 void sendTransform(geometry_msgs::Pose const &pose, const std_msgs::Header& header, std::string child_frame_id = "")
00043 {
00044   std::vector<geometry_msgs::TransformStamped> transforms;
00045 
00046   tf::StampedTransform tf;
00047   tf.stamp_ = header.stamp;
00048 
00049   tf.frame_id_ = header.frame_id;
00050   if (!g_frame_id.empty()) tf.frame_id_ = g_frame_id;
00051   tf.frame_id_ = tf::resolve(g_tf_prefix, tf.frame_id_);
00052 
00053   if (!g_child_frame_id.empty()) child_frame_id = g_child_frame_id;
00054   if (child_frame_id.empty()) child_frame_id = "base_link";
00055 
00056   tf::Quaternion orientation;
00057   tf::quaternionMsgToTF(pose.orientation, orientation);
00058   tfScalar yaw, pitch, roll;
00059   tf::Matrix3x3(orientation).getEulerYPR(yaw, pitch, roll);
00060   tf::Point position;
00061   tf::pointMsgToTF(pose.position, position);
00062 
00063   // position intermediate transform (x,y,z)
00064   if( !g_position_frame_id.empty() && child_frame_id != g_position_frame_id) {
00065     tf.child_frame_id_ = tf::resolve(g_tf_prefix, g_position_frame_id);
00066     tf.setOrigin(tf::Vector3(position.x(), position.y(), position.z() ));
00067     tf.setRotation(tf::Quaternion(0.0, 0.0, 0.0, 1.0));
00068     addTransform(transforms, tf);
00069   }
00070 
00071   // footprint intermediate transform (x,y,yaw)
00072   if (!g_footprint_frame_id.empty() && child_frame_id != g_footprint_frame_id) {
00073     tf.child_frame_id_ = tf::resolve(g_tf_prefix, g_footprint_frame_id);
00074     tf.setOrigin(tf::Vector3(position.x(), position.y(), 0.0));
00075     tf.setRotation(tf::createQuaternionFromRPY(0.0, 0.0, yaw));
00076     addTransform(transforms, tf);
00077 
00078     yaw = 0.0;
00079     position.setX(0.0);
00080     position.setY(0.0);
00081     tf.frame_id_ = tf::resolve(g_tf_prefix, g_footprint_frame_id);
00082   }
00083 
00084   // stabilized intermediate transform (z)
00085   if (!g_footprint_frame_id.empty() && child_frame_id != g_stabilized_frame_id) {
00086     tf.child_frame_id_ = tf::resolve(g_tf_prefix, g_stabilized_frame_id);
00087     tf.setOrigin(tf::Vector3(0.0, 0.0, position.z()));
00088     tf.setBasis(tf::Matrix3x3::getIdentity());
00089     addTransform(transforms, tf);
00090 
00091     position.setZ(0.0);
00092     tf.frame_id_ = tf::resolve(g_tf_prefix, g_stabilized_frame_id);
00093   }
00094 
00095   // base_link transform (roll, pitch)
00096   if (g_publish_roll_pitch) {
00097     tf.child_frame_id_ = tf::resolve(g_tf_prefix, child_frame_id);
00098     tf.setOrigin(position);
00099     tf.setRotation(tf::createQuaternionFromRPY(roll, pitch, yaw));
00100     addTransform(transforms, tf);
00101   }
00102 
00103   g_transform_broadcaster->sendTransform(transforms);
00104 
00105   // publish pose message
00106   if (g_pose_publisher) {
00107     geometry_msgs::PoseStamped pose_stamped;
00108     pose_stamped.pose = pose;
00109     pose_stamped.header = header;
00110     g_pose_publisher.publish(pose_stamped);
00111   }
00112 
00113   // publish pose message
00114   if (g_euler_publisher) {
00115     geometry_msgs::Vector3Stamped euler_stamped;
00116     euler_stamped.vector.x = roll;
00117     euler_stamped.vector.y = pitch;
00118     euler_stamped.vector.z = yaw;
00119     euler_stamped.header = header;
00120     g_euler_publisher.publish(euler_stamped);
00121   }
00122 }
00123 
00124 void odomCallback(nav_msgs::Odometry const &odometry) {
00125   sendTransform(odometry.pose.pose, odometry.header, odometry.child_frame_id);
00126 }
00127 
00128 void poseCallback(geometry_msgs::PoseStamped const &pose) {
00129   sendTransform(pose.pose, pose.header);
00130 }
00131 
00132 void tfCallback(geometry_msgs::TransformStamped const &tf) {
00133   geometry_msgs::Pose pose;
00134   pose.position.x = tf.transform.translation.x;
00135   pose.position.y = tf.transform.translation.y;
00136   pose.position.z = tf.transform.translation.z;
00137   pose.orientation = tf.transform.rotation;
00138 
00139   sendTransform(pose, tf.header);
00140 }
00141 
00142 void imuCallback(sensor_msgs::Imu const &imu) {
00143   std::vector<geometry_msgs::TransformStamped> transforms;
00144   std::string child_frame_id;
00145 
00146   tf::StampedTransform tf;
00147   tf.stamp_ = imu.header.stamp;
00148 
00149   tf.frame_id_ = tf::resolve(g_tf_prefix, g_stabilized_frame_id);
00150   if (!g_child_frame_id.empty()) child_frame_id = g_child_frame_id;
00151   if (child_frame_id.empty()) child_frame_id = "base_link";
00152 
00153   tf::Quaternion orientation;
00154   tf::quaternionMsgToTF(imu.orientation, orientation);
00155   tfScalar yaw, pitch, roll;
00156   tf::Matrix3x3(orientation).getEulerYPR(yaw, pitch, roll);
00157   tf::Quaternion rollpitch = tf::createQuaternionFromRPY(roll, pitch, 0.0);
00158 
00159   // base_link transform (roll, pitch)
00160   if (g_publish_roll_pitch) {
00161     tf.child_frame_id_ = tf::resolve(g_tf_prefix, child_frame_id);
00162     tf.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00163     tf.setRotation(rollpitch);
00164     addTransform(transforms, tf);
00165   }
00166 
00167   if (!transforms.empty()) g_transform_broadcaster->sendTransform(transforms);
00168 
00169   // publish pose message
00170   if (g_pose_publisher) {
00171     geometry_msgs::PoseStamped pose_stamped;
00172     pose_stamped.header.stamp = imu.header.stamp;
00173     pose_stamped.header.frame_id = g_stabilized_frame_id;
00174     tf::quaternionTFToMsg(rollpitch, pose_stamped.pose.orientation);
00175     g_pose_publisher.publish(pose_stamped);
00176   }
00177 }
00178 
00179 void multiCallback(topic_tools::ShapeShifter const &input) {
00180   if (input.getDataType() == "nav_msgs/Odometry") {
00181     nav_msgs::Odometry::ConstPtr odom = input.instantiate<nav_msgs::Odometry>();
00182     odomCallback(*odom);
00183     return;
00184   }
00185 
00186   if (input.getDataType() == "geometry_msgs/PoseStamped") {
00187     geometry_msgs::PoseStamped::ConstPtr pose = input.instantiate<geometry_msgs::PoseStamped>();
00188     poseCallback(*pose);
00189     return;
00190   }
00191 
00192   if (input.getDataType() == "sensor_msgs/Imu") {
00193     sensor_msgs::Imu::ConstPtr imu = input.instantiate<sensor_msgs::Imu>();
00194     imuCallback(*imu);
00195     return;
00196   }
00197 
00198   if (input.getDataType() == "geometry_msgs/TransformStamped") {
00199     geometry_msgs::TransformStamped::ConstPtr tf = input.instantiate<geometry_msgs::TransformStamped>();
00200     tfCallback(*tf);
00201     return;
00202   }
00203 
00204   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());
00205 }
00206 
00207 int main(int argc, char** argv) {
00208   ros::init(argc, argv, "message_to_tf");
00209 
00210   g_footprint_frame_id = "base_footprint";
00211   g_stabilized_frame_id = "base_stabilized";
00212   // g_position_frame_id = "base_position";
00213   // g_child_frame_id = "base_link";
00214 
00215   ros::NodeHandle priv_nh("~");
00216   priv_nh.getParam("odometry_topic", g_odometry_topic);
00217   priv_nh.getParam("pose_topic", g_pose_topic);
00218   priv_nh.getParam("imu_topic", g_imu_topic);
00219   priv_nh.getParam("topic", g_topic);
00220   priv_nh.getParam("frame_id", g_frame_id);
00221   priv_nh.getParam("footprint_frame_id", g_footprint_frame_id);
00222   priv_nh.getParam("position_frame_id", g_position_frame_id);
00223   priv_nh.getParam("stabilized_frame_id", g_stabilized_frame_id);
00224   priv_nh.getParam("child_frame_id", g_child_frame_id);
00225 
00226   // get topic from the commandline
00227   if (argc > 1) {
00228       g_topic = argv[1];
00229       g_odometry_topic.clear();
00230       g_pose_topic.clear();
00231       g_imu_topic.clear();
00232   }
00233 
00234   g_publish_roll_pitch = true;
00235   priv_nh.getParam("publish_roll_pitch", g_publish_roll_pitch);
00236 
00237   g_tf_prefix = tf::getPrefixParam(priv_nh);
00238   g_transform_broadcaster = new tf::TransformBroadcaster;
00239 
00240   ros::NodeHandle node;
00241   ros::Subscriber sub1, sub2, sub3, sub4;
00242   int subscribers = 0;
00243   if (!g_odometry_topic.empty()) {
00244       sub1 = node.subscribe(g_odometry_topic, 10, &odomCallback);
00245       subscribers++;
00246   }
00247   if (!g_pose_topic.empty()) {
00248       sub2 = node.subscribe(g_pose_topic, 10, &poseCallback);
00249       subscribers++;
00250   }
00251   if (!g_imu_topic.empty()) {
00252       sub3 = node.subscribe(g_imu_topic, 10, &imuCallback);
00253       subscribers++;
00254   }
00255   if (!g_topic.empty()) {
00256       sub4 = node.subscribe(g_topic, 10, &multiCallback);
00257       subscribers++;
00258   }
00259 
00260   if (subscribers == 0) {
00261     ROS_FATAL("Usage: rosrun message_to_tf message_to_tf <topic>");
00262     return 1;
00263   } else if (subscribers > 1) {
00264     ROS_FATAL("More than one of the parameters odometry_topic, pose_topic, imu_topic and topic are set.\n"
00265               "Please specify exactly one of them or simply add the topic name to the command line.");
00266     return 1;
00267   }
00268 
00269   bool publish_pose = true;
00270   priv_nh.getParam("publish_pose", publish_pose);
00271   if (publish_pose) {
00272     std::string publish_pose_topic;
00273     priv_nh.getParam("publish_pose_topic", publish_pose_topic);
00274 
00275     if (!publish_pose_topic.empty())
00276       g_pose_publisher = node.advertise<geometry_msgs::PoseStamped>(publish_pose_topic, 10);
00277     else
00278       g_pose_publisher = priv_nh.advertise<geometry_msgs::PoseStamped>("pose", 10);
00279   }
00280 
00281   bool publish_euler = true;
00282   priv_nh.getParam("publish_euler", publish_euler);
00283   if (publish_euler) {
00284     std::string publish_euler_topic;
00285     priv_nh.getParam("publish_euler_topic", publish_euler_topic);
00286 
00287     if (!publish_euler_topic.empty())
00288       g_euler_publisher = node.advertise<geometry_msgs::Vector3Stamped>(publish_euler_topic, 10);
00289     else
00290       g_euler_publisher = priv_nh.advertise<geometry_msgs::Vector3Stamped>("euler", 10);
00291   }
00292 
00293   ros::spin();
00294   delete g_transform_broadcaster;
00295   return 0;
00296 }


message_to_tf
Author(s): Johannes Meyer
autogenerated on Mon Aug 22 2016 03:53:35