Full Code of ethz-asl/odom_predictor for AI

Repository: ethz-asl/odom_predictor
Branch: master
Commit: 043d052f2a56
Files: 12
Total size: 13.6 KB

Directory structure:
gitextract_t9ljxdyh/

├── .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

================================================
FILE CONTENTS
================================================

================================================
FILE: .gitignore
================================================
# Prerequisites
*.d

# Compiled Object files
*.slo
*.lo
*.o
*.obj

# Precompiled Headers
*.gch
*.pch

# Compiled Dynamic libraries
*.so
*.dylib
*.dll

# Fortran module files
*.mod
*.smod

# Compiled Static libraries
*.lai
*.la
*.a
*.lib

# Executables
*.exe
*.out
*.app


================================================
FILE: CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(odom_predictor)

find_package(catkin_simple REQUIRED)
catkin_simple(ALL_DEPS_REQUIRED)

add_definitions(-std=gnu++11 -o3)

#############
# LIBRARIES #
#############
cs_add_library(${PROJECT_NAME}
  src/odom_predictor.cpp
)

############
# BINARIES #
############
cs_add_executable(odom_predictor_node
  src/odom_predictor_node.cpp
)
target_link_libraries(odom_predictor_node ${PROJECT_NAME})

##########
# EXPORT #
##########
cs_install()
cs_export()


================================================
FILE: LICENSE
================================================
BSD 3-Clause License

Copyright (c) 2018, ETHZ ASL
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 copyright holder 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 HOLDER 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.


================================================
FILE: README.md
================================================
# odom_predictor
A simple ROS node that integrates an IMU to predict future odometry readings

### Parameters
| Parameter | Description | Default |
| --------------------  |:-----------:| :-------:|
| `max_imu_queue_length` |  Maximum number of IMU measurements to store, this limits how far foward in time the system can predict. | 1000 |


================================================
FILE: include/odom_predictor/odom_predictor.h
================================================
#ifndef ODOM_PREDICTOR_ODOM_PREDICTOR_H_
#define ODOM_PREDICTOR_ODOM_PREDICTOR_H_

#include <geometry_msgs/TransformStamped.h>
#include <kindr/minimal/quat-transformation.h>
#include <minkindr_conversions/kindr_msg.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_broadcaster.h>
#include <list>

typedef kindr::minimal::QuatTransformation Transformation;
typedef kindr::minimal::RotationQuaternion Rotation;
typedef Transformation::Vector3 Vector3;

class OdomPredictor {
 public:
  OdomPredictor(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private);

  void odometryCallback(const nav_msgs::OdometryConstPtr& msg);

  void imuCallback(const sensor_msgs::ImuConstPtr& msg);

  void imuBiasCallback(const sensor_msgs::ImuConstPtr& msg);

 private:
  void integrateIMUData(const sensor_msgs::Imu& msg);

  void publishOdometry();

  void publishTF();

  bool have_odom_;
  bool have_bias_;

  ros::NodeHandle nh_;
  ros::NodeHandle nh_private_;

  ros::Subscriber imu_sub_;
  ros::Subscriber imu_bias_sub_;
  ros::Subscriber odometry_sub_;

  ros::Publisher odom_pub_;
  ros::Publisher transform_pub_;

  tf::TransformBroadcaster br_;

  int max_imu_queue_length_;

  std::list<sensor_msgs::Imu> imu_queue_;

  int seq_;
  std::string frame_id_;
  std::string child_frame_id_;

  ros::Time estimate_timestamp_;
  Transformation transform_;
  Vector3 linear_velocity_;
  Vector3 angular_velocity_;

  Vector3 imu_linear_acceleration_bias_;
  Vector3 imu_angular_velocity_bias_;

  boost::array<double, 36ul> pose_covariance_;
  boost::array<double, 36ul> twist_covariance_;
};

#endif  // ODOM_PREDICTOR_ODOM_PREDICTOR_H_


================================================
FILE: launch/bluebird.yaml
================================================
T_pose_imu:
- [0.3307485, -0.0010234, 0.9437184, 0.108730087988]
- [-0.0046723, -0.9999889, 0.0005532, -0.00824702192307]
- [0.9437074, -0.0045923, -0.3307496, -0.0714787127425]
- [0.0, 0.0, 0.0, 1.0]

timeshift_cam_imu: 0.0


================================================
FILE: launch/example.yaml
================================================
T_pose_imu:    
- [1.0, 0.0, 0.0, 0.0]    
- [0.0, 1.0, 0.0, 0.0]    
- [0.0, 0.0, 1.0, 0.0]    
- [0.0, 0.0, 0.0, 1.0]

timeshift_cam_imu: 0.0


================================================
FILE: launch/jay.yaml
================================================
T_cam_viimu:
- [0.9999808784790855, -0.0025121287473904726, 0.005650830501216782, -0.042401005842889035]
- [0.002517926766811555, 0.9999963106647353, -0.0010191671676733197, 0.004117869879247734]
- [-0.0056482493742682285, 0.0010333760570207361, 0.999983514570581, -0.02593614217222879]
- [0.0, 0.0, 0.0, 1.0]

T_cam_mavimu:
- [-0.06003693612025088, -0.9981723400647389, 0.006895348502918008, -0.06239961565320997]
- [-0.6113046304631935, 0.03130539682806774, -0.7907759612581153, -0.0011645461428879474]
- [0.7891148300948043, -0.05169092433997463, -0.6120668535914421, -0.09136807240962909]
- [0.0, 0.0, 0.0, 1.0]

timeshift_cam_imu: 0.02437859429848189


================================================
FILE: launch/msf_dataset.launch
================================================
<launch>

<param name="/use_sim_time" value="true"/>

<node pkg="topic_tools" type="drop" name="drop_vicon" args="/vicon/auk/auk 9 10 /vicon/auk/auk/drop" output="screen"/>


<node name="swf" pkg="swf" type="swf_node">

    <rosparam file="$(find swf)/launch/example.yaml"/>
    <param name="sampling_rate_hz" value="10.0"/>
    <param name="optimization_rate_hz" value="10.0"/>
    <param name="max_data_age_secs" value="10.0"/>

    <param name="pose_position_sd" value="0.05"/>
    <param name="pose_orientation_sd" value="0.01"/>

    <param name="model_orientation_sd" value="0.1"/>
    <param name="model_position_sd" value="0.1"/>
    <param name="model_linear_velocity_sd" value="0.1"/>

    <param name="imu_angular_velocity_sd" value="0.01"/>
    <param name="imu_linear_acceleration_sd" value="0.2"/>

    <param name="verbose" value="true"/>
    <remap from="swf/imu" to="/auk/fcu/imu"/>
    <remap from="swf/pose_transform" to="/vicon/auk/auk/drop"/>
</node>

<group ns="mav" >
<param name="msf_core/msf_output_frame" value="odom"/>
<node name="pose_sensor_rovio" pkg="msf_updates" type="pose_sensor" clear_params="true" output="screen">
  <remap from="msf_core/imu_state_input" to="/auk/fcu/imu" />
  <remap from="msf_updates/transform_input" to="/vicon/auk/auk/drop" />
  <rosparam file="$(find mav_startup)/parameters/mavs/bluebird/msf_parameters_vision.yaml"/>
  <param name="capability_group" value="Rovio" />
</node>
</group>

</launch>

================================================
FILE: package.xml
================================================
<?xml version="1.0"?>
<package format="2">
  <name>odom_predictor</name>
  <version>0.0.0</version>
  <description>Predict future odom readings through IMU integration </description>

  <author email="ztaylor@ethz.ch">Zachary Taylor</author>

  <maintainer email="ztaylor@ethz.ch">Zachary Taylor</maintainer>

  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>
  <buildtool_depend>catkin_simple</buildtool_depend>

  <depend>roscpp</depend>
  <depend>roslib</depend>
  <depend>nav_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>minkindr</depend>
  <depend>minkindr_conversions</depend>
  <depend>eigen_catkin</depend>
</package>


================================================
FILE: src/odom_predictor.cpp
================================================
#include "odom_predictor/odom_predictor.h"

OdomPredictor::OdomPredictor(const ros::NodeHandle& nh,
                             const ros::NodeHandle& nh_private)
    : nh_(nh),
      nh_private_(nh_private),
      seq_(0),
      have_odom_(false),
      have_bias_(false) {
  nh_private.param("max_imu_queue_length", max_imu_queue_length_, 1000);

  constexpr size_t kROSQueueLength = 100;
  imu_sub_ =
      nh_.subscribe("imu", kROSQueueLength, &OdomPredictor::imuCallback, this);
  imu_bias_sub_ = nh_.subscribe("imu_bias", kROSQueueLength,
                                &OdomPredictor::imuBiasCallback, this);
  odometry_sub_ = nh_.subscribe("odometry", kROSQueueLength,
                                &OdomPredictor::odometryCallback, this);

  odom_pub_ = nh_private_.advertise<nav_msgs::Odometry>("predicted_odometry",
                                                        kROSQueueLength);
  transform_pub_ = nh_private_.advertise<geometry_msgs::TransformStamped>(
      "predicted_transform", kROSQueueLength);
}

void OdomPredictor::odometryCallback(const nav_msgs::OdometryConstPtr& msg) {
  if (!have_bias_) {
    return;
  }

  // clear old IMU measurements
  while (!imu_queue_.empty() &&
         imu_queue_.front().header.stamp < msg->header.stamp) {
    imu_queue_.pop_front();
  }

  // extract useful information from message
  tf::poseMsgToKindr(msg->pose.pose, &transform_);
  pose_covariance_ = msg->pose.covariance;
  tf::vectorMsgToKindr(msg->twist.twist.linear, &linear_velocity_);
  tf::vectorMsgToKindr(msg->twist.twist.angular, &angular_velocity_);
  twist_covariance_ = msg->twist.covariance;
  frame_id_ = msg->header.frame_id;
  child_frame_id_ = msg->child_frame_id;

  // reintegrate IMU messages
  estimate_timestamp_ = msg->header.stamp;

  try {
    for (const sensor_msgs::Imu& imu_msg : imu_queue_) {
      integrateIMUData(imu_msg);
    }
  } catch (std::exception& e) {
    ROS_ERROR_STREAM(
        "IMU INTEGRATION FAILED, RESETING EVERYTHING: " << e.what());
    have_bias_ = false;
    have_odom_ = false;
    imu_queue_.clear();
    return;
  }

  have_odom_ = true;
}

void OdomPredictor::imuCallback(const sensor_msgs::ImuConstPtr& msg) {
  if (msg->header.stamp < imu_queue_.back().header.stamp) {
    ROS_ERROR_STREAM("Latest IMU message occured at time: "
                     << msg->header.stamp
                     << ". This is before the previously received IMU "
                        "message that ouccured at: "
                     << imu_queue_.back().header.stamp
                     << ". The current imu queue will be reset.");
    imu_queue_.clear();
  }
  if (imu_queue_.size() > max_imu_queue_length_) {
    ROS_WARN_STREAM_THROTTLE(
        10, "There has been over "
                << max_imu_queue_length_
                << " IMU messages since the last odometry update. The oldest "
                   "measurement will be forgotten. This message is printed "
                   "once every 10 seconds");
    imu_queue_.pop_front();
  }

  imu_queue_.push_back(*msg);

  if (!have_bias_ || !have_odom_) {
    return;
  }

  try {
    integrateIMUData(*msg);
  } catch (std::exception& e) {
    ROS_ERROR_STREAM(
        "IMU INTEGRATION FAILED, RESETING EVERYTHING: " << e.what());
    have_bias_ = false;
    have_odom_ = false;
    imu_queue_.clear();
    return;
  }

  publishOdometry();
  publishTF();
  ++seq_;
}

void OdomPredictor::imuBiasCallback(const sensor_msgs::ImuConstPtr& msg) {
  tf::vectorMsgToKindr(msg->linear_acceleration,
                       &imu_linear_acceleration_bias_);
  tf::vectorMsgToKindr(msg->angular_velocity, &imu_angular_velocity_bias_);

  have_bias_ = true;
}

void OdomPredictor::integrateIMUData(const sensor_msgs::Imu& msg) {
  const double delta_time = (msg.header.stamp - estimate_timestamp_).toSec();

  const Vector3 kGravity(0.0, 0.0, -9.81);

  Vector3 imu_linear_acceleration, imu_angular_velocity;
  tf::vectorMsgToKindr(msg.linear_acceleration, &imu_linear_acceleration);
  tf::vectorMsgToKindr(msg.angular_velocity, &imu_angular_velocity);

  // find changes in angular velocity and rotation delta
  const Vector3 final_angular_velocity =
      (imu_angular_velocity - imu_angular_velocity_bias_);
  const Vector3 delta_angle =
      delta_time * (final_angular_velocity + angular_velocity_) / 2.0;
  angular_velocity_ = final_angular_velocity;

  // apply half of the rotation delta
  const Rotation half_delta_rotation = Rotation::exp(delta_angle / 2.0);
  transform_.getRotation() = transform_.getRotation() * half_delta_rotation;

  // find changes in linear velocity and position
  const Vector3 delta_linear_velocity =
      delta_time * (imu_linear_acceleration +
                    transform_.getRotation().inverse().rotate(kGravity) -
                    imu_linear_acceleration_bias_);
  transform_.getPosition() =
      transform_.getPosition() +
      transform_.getRotation().rotate(
          delta_time * (linear_velocity_ + delta_linear_velocity / 2.0));
  linear_velocity_ += delta_linear_velocity;

  // apply the other half of the rotation delta
  transform_.getRotation() = transform_.getRotation() * half_delta_rotation;

  estimate_timestamp_ = msg.header.stamp;
}

void OdomPredictor::publishOdometry() {
  if (!have_odom_) {
    return;
  }

  nav_msgs::Odometry msg;

  msg.header.frame_id = frame_id_;
  msg.header.seq = seq_;
  msg.header.stamp = estimate_timestamp_;
  msg.child_frame_id = child_frame_id_;

  tf::poseKindrToMsg(transform_, &msg.pose.pose);
  msg.pose.covariance = pose_covariance_;

  tf::vectorKindrToMsg(linear_velocity_, &msg.twist.twist.linear);
  tf::vectorKindrToMsg(angular_velocity_, &msg.twist.twist.angular);
  msg.twist.covariance = twist_covariance_;

  odom_pub_.publish(msg);
}

void OdomPredictor::publishTF() {
  if (!have_odom_) {
    return;
  }

  geometry_msgs::TransformStamped msg;

  msg.header.frame_id = frame_id_;
  msg.header.seq = seq_;
  msg.header.stamp = estimate_timestamp_;
  msg.child_frame_id = child_frame_id_;

  tf::transformKindrToMsg(transform_, &msg.transform);

  transform_pub_.publish(msg);
  br_.sendTransform(msg);
}


================================================
FILE: src/odom_predictor_node.cpp
================================================
#include "odom_predictor/odom_predictor.h"

int main(int argc, char** argv) {
  ros::init(argc, argv, "odom_predictor_node");

  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");

  OdomPredictor odom_predictor(nh, nh_private);

  ros::spin();

  return 0;
}