using vehicle speed to update imu odometry instead of imu integration

README.md

@@ -1 +1,5 @@
 # imu_integrator
+
+Given a rosbag with imu and vechie_state from LGSVL Simulator, caculate imu odometry
+
+

include/Imu_Integrator/imu_integrator.h → include/imu_integrator/imu_integrator.h

@@ -3,8 +3,11 @@
 
 #include "ros/ros.h"
 #include <sensor_msgs/Imu.h>
+#include <lgsvl_msgs/VehicleOdometry.h>
 #include <visualization_msgs/Marker.h>
+
 #include <Eigen/Dense>
+
 #include <fstream>
 
 struct Pose
@@ -21,10 +24,13 @@ class ImuIntegrator
 
     void imu_callback(const sensor_msgs::ImuConstPtr& msg);
 
+    void vehicle_odom_callback(const lgsvl_msgs::VehicleOdometryPtr& msg);
+
 private:
     std::ofstream output_file;
     bool fisrtTime;
     double dt;
+    double v_speed;
     Pose pose;
     Eigen::Vector3d gravity;
     Eigen::Vector3d velocity;
@@ -36,6 +42,7 @@ class ImuIntegrator
     ros::NodeHandle nh_private_;
 
     ros::Subscriber imu_sub;
+    ros::Subscriber vehicle_odom_sub;
     ros::Publisher imu_odometry;
 
     void setGravity(const geometry_msgs::Vector3& msg);

package.xml

@@ -4,7 +4,7 @@
   <version>0.0.0</version>
   <description>The imu_integrator package</description>
 
-  <maintainer email="jeff.pc.chang@foxconn.com">jeff</maintainer>
+  <maintainer email="jaychang0501@gmail.com">jeff</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->

src/imu_integrator.cpp

@@ -1,12 +1,16 @@
-#include "Imu_Integrator/imu_integrator.h"
+#include "imu_integrator/imu_integrator.h"
 
 ImuIntegrator::ImuIntegrator(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private)
     : nh_(nh), nh_private_(nh_private)
 {
     imu_sub = nh_.subscribe("/imu_raw", 1000, &ImuIntegrator::imu_callback, this);
+    vehicle_odom_sub = nh_.subscribe("/lgsvl/vehicle_odom", 100, &ImuIntegrator::vehicle_odom_callback, this);
+
     imu_odometry = nh_.advertise<visualization_msgs::Marker>("/imu_odometry", 1000);
 
-    ROS_INFO("Starting to subcribe imu data.\n");
+    ROS_INFO("Starting to subscribe imu data.\n");
+    ROS_INFO("Starting to subscribe lgsvl vehicle state.\n");
+
     fisrtTime = true;
     Eigen::Vector3d zero(0, 0, 0);
     pose.pos = zero;
@@ -21,7 +25,7 @@ ImuIntegrator::ImuIntegrator(const ros::NodeHandle& nh, const ros::NodeHandle& n
     path.ns = "points_and_lines";
     path.action = visualization_msgs::Marker::ADD;
     path.pose.orientation.w = 1.0;
-    path.scale.x = 0.4;
+    path.scale.x = 0.3;
     geometry_msgs::Point p;
     p.x = 0; p.y = 0; p.z = 0;
     path.points.push_back(p);
@@ -55,6 +59,13 @@ void ImuIntegrator::imu_callback(const sensor_msgs::ImuConstPtr& msg)
     }
 }
 
+void ImuIntegrator::vehicle_odom_callback(const lgsvl_msgs::VehicleOdometryPtr& msg)
+{
+    v_speed = msg->velocity;
+    // Eigen::Vector3d vv(v_speed, 0, 0);
+    // std::cout << vv << std::endl;
+}
+
 void ImuIntegrator::setGravity(const geometry_msgs::Vector3& msg)
 {
     gravity[0] = msg.x;
@@ -81,9 +92,14 @@ void ImuIntegrator::calcPosition(const geometry_msgs::Vector3& msg)
 {
     Eigen::Vector3d acc_1(msg.x, msg.y, msg.z);
     Eigen::Vector3d acc_g = pose.orientation * acc_1;
-    velocity = velocity + dt * (acc_g - gravity);
-    pose.pos = pose.pos + dt * velocity;
+
+    // calculate velocity from imu linear acceleration, then get imu position
+    // velocity = velocity + dt * (acc_g - gravity);
+    // pose.pos = pose.pos + dt * velocity;
 
+    // get speed from /lgsvl/vehicle_odom, then calculate imu position
+    Eigen::Vector3d speed(v_speed, 0, 0);
+    pose.pos = pose.pos + dt * pose.orientation * speed;
 } 
 
 void ImuIntegrator::publishMessage()

src/imu_integrator_node.cpp

@@ -1,4 +1,4 @@
-#include "Imu_Integrator/imu_integrator.h"
+#include "imu_integrator/imu_integrator.h"
 
 int main(int argc, char** argv)
 {