Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <geometry_msgs/PoseStamped.h>
00003 #include <sensor_msgs/Imu.h>
00004 #include <tf/transform_broadcaster.h>
00005 #include <tf/transform_datatypes.h>
00006 #include "mav_msgs/Height.h"
00007
00008 std::string global_frame_id;
00009 std::string footprint_frame_id;
00010 std::string stabilized_frame_id;
00011 std::string base_frame_id;
00012 std::string odom_frame_id;
00013
00014 std::string imu_topic;
00015 std::string height_topic;
00016
00017 tf::TransformBroadcaster *g_transform_broadcaster;
00018
00019
00020 #ifndef TF_MATRIX3x3_H
00021 typedef btScalar tfScalar;
00022 namespace tf { typedef btMatrix3x3 Matrix3x3; }
00023 #endif
00024
00025 void addTransform(std::vector<geometry_msgs::TransformStamped>& transforms, const tf::StampedTransform& tf)
00026 {
00027 transforms.resize(transforms.size()+1);
00028 tf::transformStampedTFToMsg(tf, transforms.back());
00029 }
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091 void heightCallback(const mav_msgs::Height& msg){
00092 std::vector<geometry_msgs::TransformStamped> transforms;
00093
00094
00095 tf::StampedTransform tf;
00096 tf.stamp_ = msg.header.stamp;
00097 tf.frame_id_ = footprint_frame_id;
00098 tf.child_frame_id_ = stabilized_frame_id;
00099 tf.setOrigin(tf::Vector3(0.0, 0.0, msg.height));
00100 tf.setBasis(tf::Matrix3x3::getIdentity());
00101
00102 g_transform_broadcaster->sendTransform(tf);
00103 }
00104
00105 void imuCallback(sensor_msgs::Imu const &imu) {
00106 std::vector<geometry_msgs::TransformStamped> transforms;
00107
00108 tf::Quaternion orientation;
00109 tf::quaternionMsgToTF(imu.orientation, orientation);
00110 tfScalar yaw, pitch, roll;
00111 tf::Matrix3x3(orientation).getEulerYPR(yaw, pitch, roll);
00112
00113 tf::StampedTransform tf;
00114 tf.stamp_ = imu.header.stamp;
00115
00116
00117 tf.frame_id_ = global_frame_id;
00118 tf.child_frame_id_ = footprint_frame_id;
00119 tf.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00120 tf.setRotation(tf::createQuaternionFromRPY(0.0, 0.0, yaw));
00121 addTransform(transforms, tf);
00122
00123
00124 tf.frame_id_ = stabilized_frame_id;
00125 tf.child_frame_id_ = base_frame_id;
00126 tf.setRotation(tf::createQuaternionFromRPY(roll, pitch, 0.0));
00127 addTransform(transforms, tf);
00128
00129 g_transform_broadcaster->sendTransform(transforms);
00130
00131 }
00132
00133 int main(int argc, char** argv) {
00134 ros::init(argc, argv, "message_pub_tf");
00135
00136 ros::NodeHandle nh("~");
00137 nh.param<std::string>("global_frame", global_frame_id, "map");
00138 nh.param<std::string>("footprint_frame", footprint_frame_id, "base_footprint");
00139 nh.param<std::string>("stabilized_frame", stabilized_frame_id, "base_stabilized");
00140 nh.param<std::string>("base_frame", base_frame_id, "base_link");
00141 nh.param<std::string>("odom_frame", odom_frame_id, "odom");
00142
00143 nh.getParam("imu_topic", imu_topic);
00144 nh.getParam("height_topic", height_topic);
00145
00146 g_transform_broadcaster = new tf::TransformBroadcaster;
00147
00148 ros::Subscriber sub1, sub2;
00149 if (!imu_topic.empty()) sub1 = nh.subscribe(imu_topic, 10, &imuCallback);
00150 if (!height_topic.empty()) sub2 = nh.subscribe(height_topic, 10, &heightCallback);
00151
00152 if (!sub1 || !sub2) {
00153 ROS_FATAL("Params imu_topic, height_topic are empty... nothing to do for me!");
00154 return 1;
00155 }
00156
00157 ros::spin();
00158 delete g_transform_broadcaster;
00159 return 0;
00160 }
00161