[
  {
    "path": ".gitignore",
    "content": "# Prerequisites\n*.d\n\n# Compiled Object files\n*.slo\n*.lo\n*.o\n*.obj\n\n# Precompiled Headers\n*.gch\n*.pch\n\n# Compiled Dynamic libraries\n*.so\n*.dylib\n*.dll\n\n# Fortran module files\n*.mod\n*.smod\n\n# Compiled Static libraries\n*.lai\n*.la\n*.a\n*.lib\n\n# Executables\n*.exe\n*.out\n*.app\n"
  },
  {
    "path": "CMakeLists.txt",
    "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(odom_predictor)\n\nfind_package(catkin_simple REQUIRED)\ncatkin_simple(ALL_DEPS_REQUIRED)\n\nadd_definitions(-std=gnu++11 -o3)\n\n#############\n# LIBRARIES #\n#############\ncs_add_library(${PROJECT_NAME}\n  src/odom_predictor.cpp\n)\n\n############\n# BINARIES #\n############\ncs_add_executable(odom_predictor_node\n  src/odom_predictor_node.cpp\n)\ntarget_link_libraries(odom_predictor_node ${PROJECT_NAME})\n\n##########\n# EXPORT #\n##########\ncs_install()\ncs_export()\n"
  },
  {
    "path": "LICENSE",
    "content": "BSD 3-Clause License\n\nCopyright (c) 2018, ETHZ ASL\nAll rights reserved.\n\nRedistribution and use in source and binary forms, with or without\nmodification, are permitted provided that the following conditions are met:\n\n* Redistributions of source code must retain the above copyright notice, this\n  list of conditions and the following disclaimer.\n\n* Redistributions in binary form must reproduce the above copyright notice,\n  this list of conditions and the following disclaimer in the documentation\n  and/or other materials provided with the distribution.\n\n* Neither the name of the copyright holder nor the names of its\n  contributors may be used to endorse or promote products derived from\n  this software without specific prior written permission.\n\nTHIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS \"AS IS\"\nAND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\nIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\nDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE\nFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL\nDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR\nSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER\nCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,\nOR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE\nOF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\n"
  },
  {
    "path": "README.md",
    "content": "# odom_predictor\nA simple ROS node that integrates an IMU to predict future odometry readings\n\n### Parameters\n| Parameter | Description | Default |\n| --------------------  |:-----------:| :-------:|\n| `max_imu_queue_length` |  Maximum number of IMU measurements to store, this limits how far foward in time the system can predict. | 1000 |\n"
  },
  {
    "path": "include/odom_predictor/odom_predictor.h",
    "content": "#ifndef ODOM_PREDICTOR_ODOM_PREDICTOR_H_\n#define ODOM_PREDICTOR_ODOM_PREDICTOR_H_\n\n#include <geometry_msgs/TransformStamped.h>\n#include <kindr/minimal/quat-transformation.h>\n#include <minkindr_conversions/kindr_msg.h>\n#include <nav_msgs/Odometry.h>\n#include <ros/ros.h>\n#include <sensor_msgs/Imu.h>\n#include <tf/transform_broadcaster.h>\n#include <list>\n\ntypedef kindr::minimal::QuatTransformation Transformation;\ntypedef kindr::minimal::RotationQuaternion Rotation;\ntypedef Transformation::Vector3 Vector3;\n\nclass OdomPredictor {\n public:\n  OdomPredictor(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private);\n\n  void odometryCallback(const nav_msgs::OdometryConstPtr& msg);\n\n  void imuCallback(const sensor_msgs::ImuConstPtr& msg);\n\n  void imuBiasCallback(const sensor_msgs::ImuConstPtr& msg);\n\n private:\n  void integrateIMUData(const sensor_msgs::Imu& msg);\n\n  void publishOdometry();\n\n  void publishTF();\n\n  bool have_odom_;\n  bool have_bias_;\n\n  ros::NodeHandle nh_;\n  ros::NodeHandle nh_private_;\n\n  ros::Subscriber imu_sub_;\n  ros::Subscriber imu_bias_sub_;\n  ros::Subscriber odometry_sub_;\n\n  ros::Publisher odom_pub_;\n  ros::Publisher transform_pub_;\n\n  tf::TransformBroadcaster br_;\n\n  int max_imu_queue_length_;\n\n  std::list<sensor_msgs::Imu> imu_queue_;\n\n  int seq_;\n  std::string frame_id_;\n  std::string child_frame_id_;\n\n  ros::Time estimate_timestamp_;\n  Transformation transform_;\n  Vector3 linear_velocity_;\n  Vector3 angular_velocity_;\n\n  Vector3 imu_linear_acceleration_bias_;\n  Vector3 imu_angular_velocity_bias_;\n\n  boost::array<double, 36ul> pose_covariance_;\n  boost::array<double, 36ul> twist_covariance_;\n};\n\n#endif  // ODOM_PREDICTOR_ODOM_PREDICTOR_H_\n"
  },
  {
    "path": "launch/bluebird.yaml",
    "content": "T_pose_imu:\n- [0.3307485, -0.0010234, 0.9437184, 0.108730087988]\n- [-0.0046723, -0.9999889, 0.0005532, -0.00824702192307]\n- [0.9437074, -0.0045923, -0.3307496, -0.0714787127425]\n- [0.0, 0.0, 0.0, 1.0]\n\ntimeshift_cam_imu: 0.0\n"
  },
  {
    "path": "launch/example.yaml",
    "content": "T_pose_imu:    \n- [1.0, 0.0, 0.0, 0.0]    \n- [0.0, 1.0, 0.0, 0.0]    \n- [0.0, 0.0, 1.0, 0.0]    \n- [0.0, 0.0, 0.0, 1.0]\n\ntimeshift_cam_imu: 0.0\n"
  },
  {
    "path": "launch/jay.yaml",
    "content": "T_cam_viimu:\n- [0.9999808784790855, -0.0025121287473904726, 0.005650830501216782, -0.042401005842889035]\n- [0.002517926766811555, 0.9999963106647353, -0.0010191671676733197, 0.004117869879247734]\n- [-0.0056482493742682285, 0.0010333760570207361, 0.999983514570581, -0.02593614217222879]\n- [0.0, 0.0, 0.0, 1.0]\n\nT_cam_mavimu:\n- [-0.06003693612025088, -0.9981723400647389, 0.006895348502918008, -0.06239961565320997]\n- [-0.6113046304631935, 0.03130539682806774, -0.7907759612581153, -0.0011645461428879474]\n- [0.7891148300948043, -0.05169092433997463, -0.6120668535914421, -0.09136807240962909]\n- [0.0, 0.0, 0.0, 1.0]\n\ntimeshift_cam_imu: 0.02437859429848189\n"
  },
  {
    "path": "launch/msf_dataset.launch",
    "content": "<launch>\n\n<param name=\"/use_sim_time\" value=\"true\"/>\n\n<node pkg=\"topic_tools\" type=\"drop\" name=\"drop_vicon\" args=\"/vicon/auk/auk 9 10 /vicon/auk/auk/drop\" output=\"screen\"/>\n\n\n<node name=\"swf\" pkg=\"swf\" type=\"swf_node\">\n\n    <rosparam file=\"$(find swf)/launch/example.yaml\"/>\n    <param name=\"sampling_rate_hz\" value=\"10.0\"/>\n    <param name=\"optimization_rate_hz\" value=\"10.0\"/>\n    <param name=\"max_data_age_secs\" value=\"10.0\"/>\n\n    <param name=\"pose_position_sd\" value=\"0.05\"/>\n    <param name=\"pose_orientation_sd\" value=\"0.01\"/>\n\n    <param name=\"model_orientation_sd\" value=\"0.1\"/>\n    <param name=\"model_position_sd\" value=\"0.1\"/>\n    <param name=\"model_linear_velocity_sd\" value=\"0.1\"/>\n\n    <param name=\"imu_angular_velocity_sd\" value=\"0.01\"/>\n    <param name=\"imu_linear_acceleration_sd\" value=\"0.2\"/>\n\n    <param name=\"verbose\" value=\"true\"/>\n    <remap from=\"swf/imu\" to=\"/auk/fcu/imu\"/>\n    <remap from=\"swf/pose_transform\" to=\"/vicon/auk/auk/drop\"/>\n</node>\n\n<group ns=\"mav\" >\n<param name=\"msf_core/msf_output_frame\" value=\"odom\"/>\n<node name=\"pose_sensor_rovio\" pkg=\"msf_updates\" type=\"pose_sensor\" clear_params=\"true\" output=\"screen\">\n  <remap from=\"msf_core/imu_state_input\" to=\"/auk/fcu/imu\" />\n  <remap from=\"msf_updates/transform_input\" to=\"/vicon/auk/auk/drop\" />\n  <rosparam file=\"$(find mav_startup)/parameters/mavs/bluebird/msf_parameters_vision.yaml\"/>\n  <param name=\"capability_group\" value=\"Rovio\" />\n</node>\n</group>\n\n</launch>"
  },
  {
    "path": "package.xml",
    "content": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n  <name>odom_predictor</name>\n  <version>0.0.0</version>\n  <description>Predict future odom readings through IMU integration </description>\n\n  <author email=\"ztaylor@ethz.ch\">Zachary Taylor</author>\n\n  <maintainer email=\"ztaylor@ethz.ch\">Zachary Taylor</maintainer>\n\n  <license>BSD</license>\n\n  <buildtool_depend>catkin</buildtool_depend>\n  <buildtool_depend>catkin_simple</buildtool_depend>\n\n  <depend>roscpp</depend>\n  <depend>roslib</depend>\n  <depend>nav_msgs</depend>\n  <depend>sensor_msgs</depend>\n  <depend>geometry_msgs</depend>\n  <depend>minkindr</depend>\n  <depend>minkindr_conversions</depend>\n  <depend>eigen_catkin</depend>\n</package>\n"
  },
  {
    "path": "src/odom_predictor.cpp",
    "content": "#include \"odom_predictor/odom_predictor.h\"\n\nOdomPredictor::OdomPredictor(const ros::NodeHandle& nh,\n                             const ros::NodeHandle& nh_private)\n    : nh_(nh),\n      nh_private_(nh_private),\n      seq_(0),\n      have_odom_(false),\n      have_bias_(false) {\n  nh_private.param(\"max_imu_queue_length\", max_imu_queue_length_, 1000);\n\n  constexpr size_t kROSQueueLength = 100;\n  imu_sub_ =\n      nh_.subscribe(\"imu\", kROSQueueLength, &OdomPredictor::imuCallback, this);\n  imu_bias_sub_ = nh_.subscribe(\"imu_bias\", kROSQueueLength,\n                                &OdomPredictor::imuBiasCallback, this);\n  odometry_sub_ = nh_.subscribe(\"odometry\", kROSQueueLength,\n                                &OdomPredictor::odometryCallback, this);\n\n  odom_pub_ = nh_private_.advertise<nav_msgs::Odometry>(\"predicted_odometry\",\n                                                        kROSQueueLength);\n  transform_pub_ = nh_private_.advertise<geometry_msgs::TransformStamped>(\n      \"predicted_transform\", kROSQueueLength);\n}\n\nvoid OdomPredictor::odometryCallback(const nav_msgs::OdometryConstPtr& msg) {\n  if (!have_bias_) {\n    return;\n  }\n\n  // clear old IMU measurements\n  while (!imu_queue_.empty() &&\n         imu_queue_.front().header.stamp < msg->header.stamp) {\n    imu_queue_.pop_front();\n  }\n\n  // extract useful information from message\n  tf::poseMsgToKindr(msg->pose.pose, &transform_);\n  pose_covariance_ = msg->pose.covariance;\n  tf::vectorMsgToKindr(msg->twist.twist.linear, &linear_velocity_);\n  tf::vectorMsgToKindr(msg->twist.twist.angular, &angular_velocity_);\n  twist_covariance_ = msg->twist.covariance;\n  frame_id_ = msg->header.frame_id;\n  child_frame_id_ = msg->child_frame_id;\n\n  // reintegrate IMU messages\n  estimate_timestamp_ = msg->header.stamp;\n\n  try {\n    for (const sensor_msgs::Imu& imu_msg : imu_queue_) {\n      integrateIMUData(imu_msg);\n    }\n  } catch (std::exception& e) {\n    ROS_ERROR_STREAM(\n        \"IMU INTEGRATION FAILED, RESETING EVERYTHING: \" << e.what());\n    have_bias_ = false;\n    have_odom_ = false;\n    imu_queue_.clear();\n    return;\n  }\n\n  have_odom_ = true;\n}\n\nvoid OdomPredictor::imuCallback(const sensor_msgs::ImuConstPtr& msg) {\n  if (msg->header.stamp < imu_queue_.back().header.stamp) {\n    ROS_ERROR_STREAM(\"Latest IMU message occured at time: \"\n                     << msg->header.stamp\n                     << \". This is before the previously received IMU \"\n                        \"message that ouccured at: \"\n                     << imu_queue_.back().header.stamp\n                     << \". The current imu queue will be reset.\");\n    imu_queue_.clear();\n  }\n  if (imu_queue_.size() > max_imu_queue_length_) {\n    ROS_WARN_STREAM_THROTTLE(\n        10, \"There has been over \"\n                << max_imu_queue_length_\n                << \" IMU messages since the last odometry update. The oldest \"\n                   \"measurement will be forgotten. This message is printed \"\n                   \"once every 10 seconds\");\n    imu_queue_.pop_front();\n  }\n\n  imu_queue_.push_back(*msg);\n\n  if (!have_bias_ || !have_odom_) {\n    return;\n  }\n\n  try {\n    integrateIMUData(*msg);\n  } catch (std::exception& e) {\n    ROS_ERROR_STREAM(\n        \"IMU INTEGRATION FAILED, RESETING EVERYTHING: \" << e.what());\n    have_bias_ = false;\n    have_odom_ = false;\n    imu_queue_.clear();\n    return;\n  }\n\n  publishOdometry();\n  publishTF();\n  ++seq_;\n}\n\nvoid OdomPredictor::imuBiasCallback(const sensor_msgs::ImuConstPtr& msg) {\n  tf::vectorMsgToKindr(msg->linear_acceleration,\n                       &imu_linear_acceleration_bias_);\n  tf::vectorMsgToKindr(msg->angular_velocity, &imu_angular_velocity_bias_);\n\n  have_bias_ = true;\n}\n\nvoid OdomPredictor::integrateIMUData(const sensor_msgs::Imu& msg) {\n  const double delta_time = (msg.header.stamp - estimate_timestamp_).toSec();\n\n  const Vector3 kGravity(0.0, 0.0, -9.81);\n\n  Vector3 imu_linear_acceleration, imu_angular_velocity;\n  tf::vectorMsgToKindr(msg.linear_acceleration, &imu_linear_acceleration);\n  tf::vectorMsgToKindr(msg.angular_velocity, &imu_angular_velocity);\n\n  // find changes in angular velocity and rotation delta\n  const Vector3 final_angular_velocity =\n      (imu_angular_velocity - imu_angular_velocity_bias_);\n  const Vector3 delta_angle =\n      delta_time * (final_angular_velocity + angular_velocity_) / 2.0;\n  angular_velocity_ = final_angular_velocity;\n\n  // apply half of the rotation delta\n  const Rotation half_delta_rotation = Rotation::exp(delta_angle / 2.0);\n  transform_.getRotation() = transform_.getRotation() * half_delta_rotation;\n\n  // find changes in linear velocity and position\n  const Vector3 delta_linear_velocity =\n      delta_time * (imu_linear_acceleration +\n                    transform_.getRotation().inverse().rotate(kGravity) -\n                    imu_linear_acceleration_bias_);\n  transform_.getPosition() =\n      transform_.getPosition() +\n      transform_.getRotation().rotate(\n          delta_time * (linear_velocity_ + delta_linear_velocity / 2.0));\n  linear_velocity_ += delta_linear_velocity;\n\n  // apply the other half of the rotation delta\n  transform_.getRotation() = transform_.getRotation() * half_delta_rotation;\n\n  estimate_timestamp_ = msg.header.stamp;\n}\n\nvoid OdomPredictor::publishOdometry() {\n  if (!have_odom_) {\n    return;\n  }\n\n  nav_msgs::Odometry msg;\n\n  msg.header.frame_id = frame_id_;\n  msg.header.seq = seq_;\n  msg.header.stamp = estimate_timestamp_;\n  msg.child_frame_id = child_frame_id_;\n\n  tf::poseKindrToMsg(transform_, &msg.pose.pose);\n  msg.pose.covariance = pose_covariance_;\n\n  tf::vectorKindrToMsg(linear_velocity_, &msg.twist.twist.linear);\n  tf::vectorKindrToMsg(angular_velocity_, &msg.twist.twist.angular);\n  msg.twist.covariance = twist_covariance_;\n\n  odom_pub_.publish(msg);\n}\n\nvoid OdomPredictor::publishTF() {\n  if (!have_odom_) {\n    return;\n  }\n\n  geometry_msgs::TransformStamped msg;\n\n  msg.header.frame_id = frame_id_;\n  msg.header.seq = seq_;\n  msg.header.stamp = estimate_timestamp_;\n  msg.child_frame_id = child_frame_id_;\n\n  tf::transformKindrToMsg(transform_, &msg.transform);\n\n  transform_pub_.publish(msg);\n  br_.sendTransform(msg);\n}\n"
  },
  {
    "path": "src/odom_predictor_node.cpp",
    "content": "#include \"odom_predictor/odom_predictor.h\"\n\nint main(int argc, char** argv) {\n  ros::init(argc, argv, \"odom_predictor_node\");\n\n  ros::NodeHandle nh;\n  ros::NodeHandle nh_private(\"~\");\n\n  OdomPredictor odom_predictor(nh, nh_private);\n\n  ros::spin();\n\n  return 0;\n}\n"
  }
]