tf2_sensor_msgs.h
Go to the documentation of this file.
00001 //Remove this header when https://github.com/ros/geometry_experimental/pull/78 is released
00002 
00003 /*
00004  * Copyright (c) 2008, Willow Garage, Inc.
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  *     * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *     * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00016  *       contributors may be used to endorse or promote products derived from
00017  *       this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 
00032 #ifndef TF2_SENSOR_MSGS_H
00033 #define TF2_SENSOR_MSGS_H
00034 
00035 #include <tf2/convert.h>
00036 #include <sensor_msgs/PointCloud2.h>
00037 #include <sensor_msgs/Imu.h>
00038 #include <sensor_msgs/MagneticField.h>
00039 #include <sensor_msgs/point_cloud2_iterator.h>
00040 #include <Eigen/Eigen>
00041 #include <Eigen/Geometry>
00042 
00043 namespace tf2
00044 {
00045 
00046 /**********/
00048 /**********/
00049 
00053   template <>
00054   inline
00055   const ros::Time& getTimestamp(const sensor_msgs::Imu& p) {return p.header.stamp;}
00056 
00060   template <>
00061   inline
00062   const std::string& getFrameId(const sensor_msgs::Imu &p) {return p.header.frame_id;}
00063 
00064 
00068   inline
00069   void transformCovariance(const boost::array<double, 9>& in, boost::array<double, 9>& out, Eigen::Quaternion<double> r){
00070 
00071     Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > cov_in(in.data());
00072     Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > cov_out(out.data());
00073     cov_out = r * cov_in * r.inverse();
00074 
00075   }
00076 
00080   template <>
00081   inline
00082   void doTransform(const sensor_msgs::Imu &imu_in, sensor_msgs::Imu &imu_out, const geometry_msgs::TransformStamped& t_in)
00083   {
00084 
00085     imu_out.header = t_in.header;
00086 
00087     // Discard translation, only use orientation for IMU transform
00088     Eigen::Quaternion<double> r(
00089         t_in.transform.rotation.w, t_in.transform.rotation.x, t_in.transform.rotation.y, t_in.transform.rotation.z);
00090     Eigen::Transform<double,3,Eigen::Affine> t(r);
00091 
00092     Eigen::Vector3d vel = t * Eigen::Vector3d(
00093         imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
00094 
00095     imu_out.angular_velocity.x = vel.x();
00096     imu_out.angular_velocity.y = vel.y();
00097     imu_out.angular_velocity.z = vel.z();
00098 
00099     transformCovariance(imu_in.angular_velocity_covariance, imu_out.angular_velocity_covariance, r);
00100 
00101     Eigen::Vector3d accel = t * Eigen::Vector3d(
00102         imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
00103 
00104 
00105     imu_out.linear_acceleration.x = accel.x();
00106     imu_out.linear_acceleration.y = accel.y();
00107     imu_out.linear_acceleration.z = accel.z();
00108 
00109     transformCovariance(imu_in.linear_acceleration_covariance, imu_out.linear_acceleration_covariance, r);
00110 
00111     Eigen::Quaternion<double> orientation = r * Eigen::Quaternion<double>(
00112         imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z) * r.inverse();
00113 
00114     imu_out.orientation.w = orientation.w();
00115     imu_out.orientation.x = orientation.x();
00116     imu_out.orientation.y = orientation.y();
00117     imu_out.orientation.z = orientation.z();
00118 
00119     transformCovariance(imu_in.orientation_covariance, imu_out.orientation_covariance, r);
00120 
00121   }
00122 
00123   inline
00124   sensor_msgs::Imu toMsg(const sensor_msgs::Imu &in)
00125   {
00126     return in;
00127   }
00128 
00129   inline
00130   void fromMsg(const sensor_msgs::Imu &msg, sensor_msgs::Imu &out)
00131   {
00132     out = msg;
00133   }
00134 
00135 /*********************/
00137 /*********************/
00138 
00142   template <>
00143   inline
00144   const ros::Time& getTimestamp(const sensor_msgs::MagneticField& p) {return p.header.stamp;}
00145 
00149   template <>
00150   inline
00151   const std::string& getFrameId(const sensor_msgs::MagneticField &p) {return p.header.frame_id;}
00152 
00156   template <>
00157   inline
00158   void doTransform(const sensor_msgs::MagneticField &mag_in, sensor_msgs::MagneticField &mag_out, const geometry_msgs::TransformStamped& t_in)
00159   {
00160 
00161     mag_out.header = t_in.header;
00162 
00163     // Discard translation, only use orientation for Magnetic Field transform
00164     Eigen::Quaternion<double> r(
00165         t_in.transform.rotation.w, t_in.transform.rotation.x, t_in.transform.rotation.y, t_in.transform.rotation.z);
00166     Eigen::Transform<double,3,Eigen::Affine> t(r);
00167 
00168     Eigen::Vector3d mag = t * Eigen::Vector3d(
00169         mag_in.magnetic_field.x, mag_in.magnetic_field.y, mag_in.magnetic_field.z);
00170 
00171     mag_out.magnetic_field.x = mag.x();
00172     mag_out.magnetic_field.y = mag.y();
00173     mag_out.magnetic_field.z = mag.z();
00174 
00175     transformCovariance(mag_in.magnetic_field_covariance, mag_out.magnetic_field_covariance, r);
00176 
00177   }
00178 
00179   inline
00180   sensor_msgs::MagneticField toMsg(const sensor_msgs::MagneticField &in)
00181   {
00182     return in;
00183   }
00184 
00185   inline
00186   void fromMsg(const sensor_msgs::MagneticField &msg, sensor_msgs::MagneticField &out)
00187   {
00188     out = msg;
00189   }
00190 
00191 } // namespace
00192 
00193 #endif // TF2_SENSOR_MSGS_IMU_H


imu_transformer
Author(s): Paul Bovbel
autogenerated on Sat Feb 2 2019 03:39:37