00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "ros/ros.h"
00031 #include "tf/transform_broadcaster.h"
00032 #include <sensor_msgs/Imu.h>
00033 #include <geometry_msgs/PoseStamped.h>
00034 #include <nav_msgs/Odometry.h>
00035
00036 std::string p_map_frame_;
00037 std::string p_base_footprint_frame_;
00038 std::string p_base_stabilized_frame_;
00039 std::string p_base_frame_;
00040 tf::TransformBroadcaster* tfB_;
00041 tf::StampedTransform transform_;
00042
00043 tf::Quaternion robot_pose_quaternion_;
00044 tf::Point robot_pose_position_;
00045 tf::Transform robot_pose_transform_;
00046
00047 tf::Quaternion tmp_;
00048 tf::Quaternion orientation_quaternion_;
00049
00050 sensor_msgs::ImuConstPtr last_imu_msg_;
00051 sensor_msgs::Imu fused_imu_msg_;
00052 nav_msgs::Odometry odom_msg_;
00053 geometry_msgs::PoseStampedConstPtr last_pose_msg_;
00054
00055 ros::Publisher fused_imu_publisher_;
00056 ros::Publisher odometry_publisher_;
00057
00058 size_t callback_count_;
00059
00060 #ifndef TF_MATRIX3x3_H
00061 typedef btScalar tfScalar;
00062 namespace tf { typedef btMatrix3x3 Matrix3x3; }
00063 #endif
00064
00065 void imuMsgCallback(const sensor_msgs::Imu::ConstPtr& imu_msg)
00066 {
00067 callback_count_++;
00068
00069 tf::quaternionMsgToTF(imu_msg->orientation, tmp_);
00070
00071 tfScalar imu_yaw, imu_pitch, imu_roll;
00072 tf::Matrix3x3(tmp_).getRPY(imu_roll, imu_pitch, imu_yaw);
00073
00074 tf::Transform transform;
00075 transform.setIdentity();
00076 tf::Quaternion quat;
00077
00078 quat.setRPY(imu_roll, imu_pitch, 0.0);
00079
00080 if (true){
00081 transform.setRotation(quat);
00082 tfB_->sendTransform(tf::StampedTransform(transform, imu_msg->header.stamp, p_base_stabilized_frame_, p_base_frame_));
00083 }
00084
00085 tfScalar pose_yaw, pose_pitch, pose_roll;
00086
00087 if (last_pose_msg_ != 0){
00088 tf::quaternionMsgToTF(last_pose_msg_->pose.orientation, tmp_);
00089
00090 tf::Matrix3x3(tmp_).getRPY(pose_roll, pose_pitch, pose_yaw);
00091 }else{
00092 pose_yaw = 0.0;
00093 }
00094
00095 orientation_quaternion_.setRPY(imu_roll, imu_pitch, pose_yaw);
00096
00097 fused_imu_msg_.header.stamp = imu_msg->header.stamp;
00098
00099 fused_imu_msg_.orientation.x = orientation_quaternion_.getX();
00100 fused_imu_msg_.orientation.y = orientation_quaternion_.getY();
00101 fused_imu_msg_.orientation.z = orientation_quaternion_.getZ();
00102 fused_imu_msg_.orientation.w = orientation_quaternion_.getW();
00103
00104 fused_imu_publisher_.publish(fused_imu_msg_);
00105
00106
00107
00108 if (last_pose_msg_ != 0){
00109 if ( (callback_count_ % 5) == 0){
00110 odom_msg_.header.stamp = imu_msg->header.stamp;
00111 odom_msg_.pose.pose.orientation = fused_imu_msg_.orientation;
00112 odom_msg_.pose.pose.position = last_pose_msg_->pose.position;
00113
00114 odometry_publisher_.publish(odom_msg_);
00115 }
00116 }
00117
00118 }
00119
00120 void poseMsgCallback(const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
00121 {
00122
00123 std::vector<tf::StampedTransform> transforms;
00124 transforms.resize(2);
00125
00126 tf::quaternionMsgToTF(pose_msg->pose.orientation, robot_pose_quaternion_);
00127 tf::pointMsgToTF(pose_msg->pose.position, robot_pose_position_);
00128
00129 robot_pose_transform_.setRotation(robot_pose_quaternion_);
00130 robot_pose_transform_.setOrigin(robot_pose_position_);
00131
00132 tf::Transform height_transform;
00133 height_transform.setIdentity();
00134 height_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
00135
00136 transforms[0] = tf::StampedTransform(robot_pose_transform_, pose_msg->header.stamp, p_map_frame_, p_base_footprint_frame_);
00137 transforms[1] = tf::StampedTransform(height_transform, pose_msg->header.stamp, p_base_footprint_frame_, p_base_stabilized_frame_);
00138
00139 tfB_->sendTransform(transforms);
00140
00141
00142 if (last_pose_msg_){
00143 tf::Vector3 plane_normal = tf::Matrix3x3(orientation_quaternion_) * tf::Vector3(0.0, 0.0, 1.0);
00144
00145 tf::Vector3 last_position;
00146 tf::pointMsgToTF(last_pose_msg_->pose.position, last_position);
00147
00148 double height_difference =
00149 (-plane_normal.getX() * (robot_pose_position_.getX() - last_position.getX())
00150 -plane_normal.getY() * (robot_pose_position_.getY() - last_position.getY())
00151 +plane_normal.getZ() * last_position.getZ()) / last_position.getZ();
00152
00153 }
00154
00155
00156
00157 last_pose_msg_ = pose_msg;
00158
00159 }
00160
00161 int main(int argc, char **argv) {
00162 ros::init(argc, argv, ROS_PACKAGE_NAME);
00163
00164 ros::NodeHandle n;
00165 ros::NodeHandle pn("~");
00166
00167 pn.param("map_frame", p_map_frame_, std::string("map"));
00168 pn.param("base_footprint_frame", p_base_footprint_frame_, std::string("base_footprint"));
00169 pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized"));
00170 pn.param("base_frame", p_base_frame_, std::string("base_link"));
00171
00172 fused_imu_msg_.header.frame_id = p_base_stabilized_frame_;
00173 odom_msg_.header.frame_id = "map";
00174
00175 tfB_ = new tf::TransformBroadcaster();
00176
00177 fused_imu_publisher_ = n.advertise<sensor_msgs::Imu>("/fused_imu",1,false);
00178 odometry_publisher_ = n.advertise<nav_msgs::Odometry>("/state", 1, false);
00179
00180 ros::Subscriber imu_subscriber = n.subscribe("/imu", 10, imuMsgCallback);
00181 ros::Subscriber pose_subscriber = n.subscribe("/pose", 10, poseMsgCallback);
00182
00183 callback_count_ = 0;
00184
00185 ros::spin();
00186
00187 delete tfB_;
00188
00189 return 0;
00190 }