.. _program_listing_file__tmp_ws_src_imu_pipeline_imu_transformer_include_imu_transformer_tf2_sensor_msgs.h: Program Listing for File tf2_sensor_msgs.h ========================================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/imu_pipeline/imu_transformer/include/imu_transformer/tf2_sensor_msgs.h``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp //Remove this header when https://github.com/ros/geometry_experimental/pull/78 is released /* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #ifndef TF2_SENSOR_MSGS_H #define TF2_SENSOR_MSGS_H #include #include #include #include #include namespace tf2 { /**********/ /**********/ template <> inline tf2::TimePoint getTimestamp(const sensor_msgs::msg::Imu& p) {return tf2_ros::fromMsg(p.header.stamp);} template <> inline std::string getFrameId(const sensor_msgs::msg::Imu &p) {return p.header.frame_id;} inline void transformCovariance(const std::array& in, std::array& out, Eigen::Quaternion r){ Eigen::Map > cov_in(in.data()); Eigen::Map > cov_out(out.data()); cov_out = r * cov_in * r.inverse(); } template <> inline void doTransform(const sensor_msgs::msg::Imu &imu_in, sensor_msgs::msg::Imu &imu_out, const geometry_msgs::msg::TransformStamped& t_in) { imu_out.header = t_in.header; // Discard translation, only use orientation for IMU transform Eigen::Quaternion r( t_in.transform.rotation.w, t_in.transform.rotation.x, t_in.transform.rotation.y, t_in.transform.rotation.z); Eigen::Transform t(r); Eigen::Vector3d vel = t * Eigen::Vector3d( imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z); imu_out.angular_velocity.x = vel.x(); imu_out.angular_velocity.y = vel.y(); imu_out.angular_velocity.z = vel.z(); transformCovariance(imu_in.angular_velocity_covariance, imu_out.angular_velocity_covariance, r); Eigen::Vector3d accel = t * Eigen::Vector3d( imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z); imu_out.linear_acceleration.x = accel.x(); imu_out.linear_acceleration.y = accel.y(); imu_out.linear_acceleration.z = accel.z(); transformCovariance(imu_in.linear_acceleration_covariance, imu_out.linear_acceleration_covariance, r); // Orientation expresses attitude of the new frame_id in a fixed world frame. This is why the transform here applies // in the opposite direction. Eigen::Quaternion orientation = Eigen::Quaternion( imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z) * r.inverse(); imu_out.orientation.w = orientation.w(); imu_out.orientation.x = orientation.x(); imu_out.orientation.y = orientation.y(); imu_out.orientation.z = orientation.z(); // Orientation is measured relative to the fixed world frame, so it doesn't change when applying a static // transform to the sensor frame. imu_out.orientation_covariance = imu_in.orientation_covariance; } inline sensor_msgs::msg::Imu toMsg(const sensor_msgs::msg::Imu &in) { return in; } inline void fromMsg(const sensor_msgs::msg::Imu &msg, sensor_msgs::msg::Imu &out) { out = msg; } /*********************/ /*********************/ template <> inline tf2::TimePoint getTimestamp(const sensor_msgs::msg::MagneticField& p) {return tf2_ros::fromMsg(p.header.stamp);} template <> inline std::string getFrameId(const sensor_msgs::msg::MagneticField &p) {return p.header.frame_id;} template <> inline void doTransform(const sensor_msgs::msg::MagneticField &mag_in, sensor_msgs::msg::MagneticField &mag_out, const geometry_msgs::msg::TransformStamped& t_in) { mag_out.header = t_in.header; // Discard translation, only use orientation for Magnetic Field transform Eigen::Quaternion r( t_in.transform.rotation.w, t_in.transform.rotation.x, t_in.transform.rotation.y, t_in.transform.rotation.z); Eigen::Transform t(r); Eigen::Vector3d mag = t * Eigen::Vector3d( mag_in.magnetic_field.x, mag_in.magnetic_field.y, mag_in.magnetic_field.z); mag_out.magnetic_field.x = mag.x(); mag_out.magnetic_field.y = mag.y(); mag_out.magnetic_field.z = mag.z(); transformCovariance(mag_in.magnetic_field_covariance, mag_out.magnetic_field_covariance, r); } inline sensor_msgs::msg::MagneticField toMsg(const sensor_msgs::msg::MagneticField &in) { return in; } inline void fromMsg(const sensor_msgs::msg::MagneticField &msg, sensor_msgs::msg::MagneticField &out) { out = msg; } } // namespace #endif // TF2_SENSOR_MSGS_IMU_H