├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include └── odom_predictor │ └── odom_predictor.h ├── launch ├── bluebird.yaml ├── example.yaml ├── jay.yaml └── msf_dataset.launch ├── package.xml └── src ├── odom_predictor.cpp └── odom_predictor_node.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(odom_predictor) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | add_definitions(-std=gnu++11 -o3) 8 | 9 | ############# 10 | # LIBRARIES # 11 | ############# 12 | cs_add_library(${PROJECT_NAME} 13 | src/odom_predictor.cpp 14 | ) 15 | 16 | ############ 17 | # BINARIES # 18 | ############ 19 | cs_add_executable(odom_predictor_node 20 | src/odom_predictor_node.cpp 21 | ) 22 | target_link_libraries(odom_predictor_node ${PROJECT_NAME}) 23 | 24 | ########## 25 | # EXPORT # 26 | ########## 27 | cs_install() 28 | cs_export() 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, ETHZ ASL 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # odom_predictor 2 | A simple ROS node that integrates an IMU to predict future odometry readings 3 | 4 | ### Parameters 5 | | Parameter | Description | Default | 6 | | -------------------- |:-----------:| :-------:| 7 | | `max_imu_queue_length` | Maximum number of IMU measurements to store, this limits how far foward in time the system can predict. | 1000 | 8 | -------------------------------------------------------------------------------- /include/odom_predictor/odom_predictor.h: -------------------------------------------------------------------------------- 1 | #ifndef ODOM_PREDICTOR_ODOM_PREDICTOR_H_ 2 | #define ODOM_PREDICTOR_ODOM_PREDICTOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | typedef kindr::minimal::QuatTransformation Transformation; 14 | typedef kindr::minimal::RotationQuaternion Rotation; 15 | typedef Transformation::Vector3 Vector3; 16 | 17 | class OdomPredictor { 18 | public: 19 | OdomPredictor(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); 20 | 21 | void odometryCallback(const nav_msgs::OdometryConstPtr& msg); 22 | 23 | void imuCallback(const sensor_msgs::ImuConstPtr& msg); 24 | 25 | void imuBiasCallback(const sensor_msgs::ImuConstPtr& msg); 26 | 27 | private: 28 | void integrateIMUData(const sensor_msgs::Imu& msg); 29 | 30 | void publishOdometry(); 31 | 32 | void publishTF(); 33 | 34 | bool have_odom_; 35 | bool have_bias_; 36 | 37 | ros::NodeHandle nh_; 38 | ros::NodeHandle nh_private_; 39 | 40 | ros::Subscriber imu_sub_; 41 | ros::Subscriber imu_bias_sub_; 42 | ros::Subscriber odometry_sub_; 43 | 44 | ros::Publisher odom_pub_; 45 | ros::Publisher transform_pub_; 46 | 47 | tf::TransformBroadcaster br_; 48 | 49 | int max_imu_queue_length_; 50 | 51 | std::list imu_queue_; 52 | 53 | int seq_; 54 | std::string frame_id_; 55 | std::string child_frame_id_; 56 | 57 | ros::Time estimate_timestamp_; 58 | Transformation transform_; 59 | Vector3 linear_velocity_; 60 | Vector3 angular_velocity_; 61 | 62 | Vector3 imu_linear_acceleration_bias_; 63 | Vector3 imu_angular_velocity_bias_; 64 | 65 | boost::array pose_covariance_; 66 | boost::array twist_covariance_; 67 | }; 68 | 69 | #endif // ODOM_PREDICTOR_ODOM_PREDICTOR_H_ 70 | -------------------------------------------------------------------------------- /launch/bluebird.yaml: -------------------------------------------------------------------------------- 1 | T_pose_imu: 2 | - [0.3307485, -0.0010234, 0.9437184, 0.108730087988] 3 | - [-0.0046723, -0.9999889, 0.0005532, -0.00824702192307] 4 | - [0.9437074, -0.0045923, -0.3307496, -0.0714787127425] 5 | - [0.0, 0.0, 0.0, 1.0] 6 | 7 | timeshift_cam_imu: 0.0 8 | -------------------------------------------------------------------------------- /launch/example.yaml: -------------------------------------------------------------------------------- 1 | T_pose_imu: 2 | - [1.0, 0.0, 0.0, 0.0] 3 | - [0.0, 1.0, 0.0, 0.0] 4 | - [0.0, 0.0, 1.0, 0.0] 5 | - [0.0, 0.0, 0.0, 1.0] 6 | 7 | timeshift_cam_imu: 0.0 8 | -------------------------------------------------------------------------------- /launch/jay.yaml: -------------------------------------------------------------------------------- 1 | T_cam_viimu: 2 | - [0.9999808784790855, -0.0025121287473904726, 0.005650830501216782, -0.042401005842889035] 3 | - [0.002517926766811555, 0.9999963106647353, -0.0010191671676733197, 0.004117869879247734] 4 | - [-0.0056482493742682285, 0.0010333760570207361, 0.999983514570581, -0.02593614217222879] 5 | - [0.0, 0.0, 0.0, 1.0] 6 | 7 | T_cam_mavimu: 8 | - [-0.06003693612025088, -0.9981723400647389, 0.006895348502918008, -0.06239961565320997] 9 | - [-0.6113046304631935, 0.03130539682806774, -0.7907759612581153, -0.0011645461428879474] 10 | - [0.7891148300948043, -0.05169092433997463, -0.6120668535914421, -0.09136807240962909] 11 | - [0.0, 0.0, 0.0, 1.0] 12 | 13 | timeshift_cam_imu: 0.02437859429848189 14 | -------------------------------------------------------------------------------- /launch/msf_dataset.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | odom_predictor 4 | 0.0.0 5 | Predict future odom readings through IMU integration 6 | 7 | Zachary Taylor 8 | 9 | Zachary Taylor 10 | 11 | BSD 12 | 13 | catkin 14 | catkin_simple 15 | 16 | roscpp 17 | roslib 18 | nav_msgs 19 | sensor_msgs 20 | geometry_msgs 21 | minkindr 22 | minkindr_conversions 23 | eigen_catkin 24 | 25 | -------------------------------------------------------------------------------- /src/odom_predictor.cpp: -------------------------------------------------------------------------------- 1 | #include "odom_predictor/odom_predictor.h" 2 | 3 | OdomPredictor::OdomPredictor(const ros::NodeHandle& nh, 4 | const ros::NodeHandle& nh_private) 5 | : nh_(nh), 6 | nh_private_(nh_private), 7 | seq_(0), 8 | have_odom_(false), 9 | have_bias_(false) { 10 | nh_private.param("max_imu_queue_length", max_imu_queue_length_, 1000); 11 | 12 | constexpr size_t kROSQueueLength = 100; 13 | imu_sub_ = 14 | nh_.subscribe("imu", kROSQueueLength, &OdomPredictor::imuCallback, this); 15 | imu_bias_sub_ = nh_.subscribe("imu_bias", kROSQueueLength, 16 | &OdomPredictor::imuBiasCallback, this); 17 | odometry_sub_ = nh_.subscribe("odometry", kROSQueueLength, 18 | &OdomPredictor::odometryCallback, this); 19 | 20 | odom_pub_ = nh_private_.advertise("predicted_odometry", 21 | kROSQueueLength); 22 | transform_pub_ = nh_private_.advertise( 23 | "predicted_transform", kROSQueueLength); 24 | } 25 | 26 | void OdomPredictor::odometryCallback(const nav_msgs::OdometryConstPtr& msg) { 27 | if (!have_bias_) { 28 | return; 29 | } 30 | 31 | // clear old IMU measurements 32 | while (!imu_queue_.empty() && 33 | imu_queue_.front().header.stamp < msg->header.stamp) { 34 | imu_queue_.pop_front(); 35 | } 36 | 37 | // extract useful information from message 38 | tf::poseMsgToKindr(msg->pose.pose, &transform_); 39 | pose_covariance_ = msg->pose.covariance; 40 | tf::vectorMsgToKindr(msg->twist.twist.linear, &linear_velocity_); 41 | tf::vectorMsgToKindr(msg->twist.twist.angular, &angular_velocity_); 42 | twist_covariance_ = msg->twist.covariance; 43 | frame_id_ = msg->header.frame_id; 44 | child_frame_id_ = msg->child_frame_id; 45 | 46 | // reintegrate IMU messages 47 | estimate_timestamp_ = msg->header.stamp; 48 | 49 | try { 50 | for (const sensor_msgs::Imu& imu_msg : imu_queue_) { 51 | integrateIMUData(imu_msg); 52 | } 53 | } catch (std::exception& e) { 54 | ROS_ERROR_STREAM( 55 | "IMU INTEGRATION FAILED, RESETING EVERYTHING: " << e.what()); 56 | have_bias_ = false; 57 | have_odom_ = false; 58 | imu_queue_.clear(); 59 | return; 60 | } 61 | 62 | have_odom_ = true; 63 | } 64 | 65 | void OdomPredictor::imuCallback(const sensor_msgs::ImuConstPtr& msg) { 66 | if (msg->header.stamp < imu_queue_.back().header.stamp) { 67 | ROS_ERROR_STREAM("Latest IMU message occured at time: " 68 | << msg->header.stamp 69 | << ". This is before the previously received IMU " 70 | "message that ouccured at: " 71 | << imu_queue_.back().header.stamp 72 | << ". The current imu queue will be reset."); 73 | imu_queue_.clear(); 74 | } 75 | if (imu_queue_.size() > max_imu_queue_length_) { 76 | ROS_WARN_STREAM_THROTTLE( 77 | 10, "There has been over " 78 | << max_imu_queue_length_ 79 | << " IMU messages since the last odometry update. The oldest " 80 | "measurement will be forgotten. This message is printed " 81 | "once every 10 seconds"); 82 | imu_queue_.pop_front(); 83 | } 84 | 85 | imu_queue_.push_back(*msg); 86 | 87 | if (!have_bias_ || !have_odom_) { 88 | return; 89 | } 90 | 91 | try { 92 | integrateIMUData(*msg); 93 | } catch (std::exception& e) { 94 | ROS_ERROR_STREAM( 95 | "IMU INTEGRATION FAILED, RESETING EVERYTHING: " << e.what()); 96 | have_bias_ = false; 97 | have_odom_ = false; 98 | imu_queue_.clear(); 99 | return; 100 | } 101 | 102 | publishOdometry(); 103 | publishTF(); 104 | ++seq_; 105 | } 106 | 107 | void OdomPredictor::imuBiasCallback(const sensor_msgs::ImuConstPtr& msg) { 108 | tf::vectorMsgToKindr(msg->linear_acceleration, 109 | &imu_linear_acceleration_bias_); 110 | tf::vectorMsgToKindr(msg->angular_velocity, &imu_angular_velocity_bias_); 111 | 112 | have_bias_ = true; 113 | } 114 | 115 | void OdomPredictor::integrateIMUData(const sensor_msgs::Imu& msg) { 116 | const double delta_time = (msg.header.stamp - estimate_timestamp_).toSec(); 117 | 118 | const Vector3 kGravity(0.0, 0.0, -9.81); 119 | 120 | Vector3 imu_linear_acceleration, imu_angular_velocity; 121 | tf::vectorMsgToKindr(msg.linear_acceleration, &imu_linear_acceleration); 122 | tf::vectorMsgToKindr(msg.angular_velocity, &imu_angular_velocity); 123 | 124 | // find changes in angular velocity and rotation delta 125 | const Vector3 final_angular_velocity = 126 | (imu_angular_velocity - imu_angular_velocity_bias_); 127 | const Vector3 delta_angle = 128 | delta_time * (final_angular_velocity + angular_velocity_) / 2.0; 129 | angular_velocity_ = final_angular_velocity; 130 | 131 | // apply half of the rotation delta 132 | const Rotation half_delta_rotation = Rotation::exp(delta_angle / 2.0); 133 | transform_.getRotation() = transform_.getRotation() * half_delta_rotation; 134 | 135 | // find changes in linear velocity and position 136 | const Vector3 delta_linear_velocity = 137 | delta_time * (imu_linear_acceleration + 138 | transform_.getRotation().inverse().rotate(kGravity) - 139 | imu_linear_acceleration_bias_); 140 | transform_.getPosition() = 141 | transform_.getPosition() + 142 | transform_.getRotation().rotate( 143 | delta_time * (linear_velocity_ + delta_linear_velocity / 2.0)); 144 | linear_velocity_ += delta_linear_velocity; 145 | 146 | // apply the other half of the rotation delta 147 | transform_.getRotation() = transform_.getRotation() * half_delta_rotation; 148 | 149 | estimate_timestamp_ = msg.header.stamp; 150 | } 151 | 152 | void OdomPredictor::publishOdometry() { 153 | if (!have_odom_) { 154 | return; 155 | } 156 | 157 | nav_msgs::Odometry msg; 158 | 159 | msg.header.frame_id = frame_id_; 160 | msg.header.seq = seq_; 161 | msg.header.stamp = estimate_timestamp_; 162 | msg.child_frame_id = child_frame_id_; 163 | 164 | tf::poseKindrToMsg(transform_, &msg.pose.pose); 165 | msg.pose.covariance = pose_covariance_; 166 | 167 | tf::vectorKindrToMsg(linear_velocity_, &msg.twist.twist.linear); 168 | tf::vectorKindrToMsg(angular_velocity_, &msg.twist.twist.angular); 169 | msg.twist.covariance = twist_covariance_; 170 | 171 | odom_pub_.publish(msg); 172 | } 173 | 174 | void OdomPredictor::publishTF() { 175 | if (!have_odom_) { 176 | return; 177 | } 178 | 179 | geometry_msgs::TransformStamped msg; 180 | 181 | msg.header.frame_id = frame_id_; 182 | msg.header.seq = seq_; 183 | msg.header.stamp = estimate_timestamp_; 184 | msg.child_frame_id = child_frame_id_; 185 | 186 | tf::transformKindrToMsg(transform_, &msg.transform); 187 | 188 | transform_pub_.publish(msg); 189 | br_.sendTransform(msg); 190 | } 191 | -------------------------------------------------------------------------------- /src/odom_predictor_node.cpp: -------------------------------------------------------------------------------- 1 | #include "odom_predictor/odom_predictor.h" 2 | 3 | int main(int argc, char** argv) { 4 | ros::init(argc, argv, "odom_predictor_node"); 5 | 6 | ros::NodeHandle nh; 7 | ros::NodeHandle nh_private("~"); 8 | 9 | OdomPredictor odom_predictor(nh, nh_private); 10 | 11 | ros::spin(); 12 | 13 | return 0; 14 | } 15 | --------------------------------------------------------------------------------