Go to the documentation of this file.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
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef IMU_FILTER_MESSAGE_HELPER_H
00039 #define IMU_FILTER_MESSAGE_HELPER_H
00040
00041 #include <Eigen/Eigen>
00042 #include <Eigen/Geometry>
00043
00044 #include <boost/array.hpp>
00045
00046 #include <sensor_msgs/Imu.h>
00047
00048 namespace imu_filter
00049 {
00050
00051 namespace message_helper
00052 {
00056 template<typename T, size_t r, size_t c>
00057 static inline void matrixPtrToEigen( const boost::array<T, r*c> & matPtr, Eigen::Matrix<T, r, c> & mat )
00058 {
00059 for( size_t row = 0; row < r; row++ )
00060 for( size_t col = 0; col < c; col++ )
00061 mat( row, col ) = matPtr[ row * c + col ];
00062 }
00063
00067 static inline void quaternionFromImuMsg( const sensor_msgs::Imu::ConstPtr & msg, Eigen::Quaterniond & q )
00068 {
00069 q.x() = msg->orientation.x;
00070 q.y() = msg->orientation.y;
00071 q.z() = msg->orientation.z;
00072 q.w() = msg->orientation.w;
00073 }
00074
00078 static inline void angularRateFromImuMsg( const sensor_msgs::Imu::ConstPtr & msg, Eigen::Vector3d & v )
00079 {
00080 v.x() = msg->angular_velocity.x;
00081 v.y() = msg->angular_velocity.y;
00082 v.z() = msg->angular_velocity.z;
00083 }
00084
00088 static inline void linearAccelerationFromImuMsg( const sensor_msgs::Imu::ConstPtr & msg, Eigen::Vector3d & v )
00089 {
00090 v.x() = msg->linear_acceleration.x;
00091 v.y() = msg->linear_acceleration.y;
00092 v.z() = msg->linear_acceleration.z;
00093 }
00094
00098 static inline void quaternionToMsg( const Eigen::Quaterniond & q, geometry_msgs::Quaternion & msg )
00099 {
00100 msg.x = q.x();
00101 msg.y = q.y();
00102 msg.z = q.z();
00103 msg.w = q.w();
00104 }
00105
00109 static inline void vec3ToMsg( const Eigen::Vector3d & v, geometry_msgs::Vector3 & msg )
00110 {
00111 msg.x = v.x();
00112 msg.y = v.y();
00113 msg.z = v.z();
00114 }
00115
00119 static inline void eigenPose2Msg( const Eigen::Affine3d & pose, geometry_msgs::PoseStamped & msg )
00120 {
00121 Eigen::Quaterniond q;
00122 q = pose.rotation();
00123 msg.pose.orientation.x = q.x();
00124 msg.pose.orientation.y = q.y();
00125 msg.pose.orientation.z = q.z();
00126 msg.pose.orientation.w = q.w();
00127 msg.pose.position.x = pose.translation().x();
00128 msg.pose.position.y = pose.translation().y();
00129 msg.pose.position.z = pose.translation().z();
00130 }
00131 }
00132 }
00133
00134 #endif