first initial

.vscode/settings.json

@@ -0,0 +1,61 @@
+{
+    "files.associations": {
+        "dense": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "complex": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "unordered_map": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "fstream": "cpp",
+        "functional": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "numeric": "cpp",
+        "ratio": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "system_error": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "cinttypes": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "typeinfo": "cpp",
+        "algorithm": "cpp",
+        "iterator": "cpp",
+        "map": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "set": "cpp",
+        "string": "cpp",
+        "string_view": "cpp"
+    }
+}

CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(imu_integrator)
+
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+)
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES Imu_Integrator
+#  CATKIN_DEPENDS roscpp std_msgs
+#  DEPENDS system_lib
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(imu_integrator_node src/imu_integrator_node.cpp
+                                   src/imu_integrator.cpp
+                                   )
+target_link_libraries(imu_integrator_node ${catkin_LIBRARIES})

README.md

@@ -0,0 +1 @@
+# imu_integrator

config/config.rviz

@@ -0,0 +1,122 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+      Splitter Ratio: 0.6958039999008179
+    Tree Height: 654
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /imu_odometry
+      Name: imu_odom
+      Namespaces:
+        points_and_lines: true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Angle: 0
+      Class: rviz/TopDownOrtho
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Scale: 10
+      Target Frame: base_link
+      Value: TopDownOrtho (rviz)
+      X: 0
+      Y: 0
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 897
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 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
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1707
+  X: 0
+  Y: 23

include/Imu_Integrator/imu_integrator.h

@@ -0,0 +1,51 @@
+#ifndef IMU_INTEGRATOR_H
+#define IMU_INTEGRATOR_H
+
+#include "ros/ros.h"
+#include <sensor_msgs/Imu.h>
+#include <visualization_msgs/Marker.h>
+#include <Eigen/Dense>
+#include <fstream>
+
+struct Pose
+{
+    Eigen::Vector3d pos;
+    Eigen::Matrix3d orientation;
+};
+
+class ImuIntegrator
+{
+public:
+    ImuIntegrator(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private);
+    ~ImuIntegrator();
+
+    void imu_callback(const sensor_msgs::ImuConstPtr& msg);
+
+private:
+    std::ofstream output_file;
+    bool fisrtTime;
+    double dt;
+    Pose pose;
+    Eigen::Vector3d gravity;
+    Eigen::Vector3d velocity;
+
+    visualization_msgs::Marker path;
+
+    ros::Time time;
+    ros::NodeHandle nh_;
+    ros::NodeHandle nh_private_;
+
+    ros::Subscriber imu_sub;
+    ros::Publisher imu_odometry;
+
+    void setGravity(const geometry_msgs::Vector3& msg);
+
+    void calcOrientation(const geometry_msgs::Vector3& msg); 
+
+    void calcPosition(const geometry_msgs::Vector3& msg);
+
+    void publishMessage();
+};
+
+
+#endif

package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imu_integrator</name>
+  <version>0.0.0</version>
+  <description>The imu_integrator package</description>
+
+  <maintainer email="[email protected]">jeff</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/imu_integrator.cpp

@@ -0,0 +1,99 @@
+#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);
+    imu_odometry = nh_.advertise<visualization_msgs::Marker>("/imu_odometry", 1000);
+
+    ROS_INFO("Starting to subcribe imu data.\n");
+    fisrtTime = true;
+    Eigen::Vector3d zero(0, 0, 0);
+    pose.pos = zero;
+    pose.orientation = Eigen::Matrix3d::Identity();
+    velocity = zero;
+
+    // intial path
+    path.color.r = 1.0;
+    path.color.a = 1.0;
+    path.type = visualization_msgs::Marker::LINE_STRIP;
+    path.header.frame_id = "world";
+    path.ns = "points_and_lines";
+    path.action = visualization_msgs::Marker::ADD;
+    path.pose.orientation.w = 1.0;
+    path.scale.x = 0.4;
+    geometry_msgs::Point p;
+    p.x = 0; p.y = 0; p.z = 0;
+    path.points.push_back(p);
+
+    output_file.open("/home/jeff/imu_integrator.csv");
+    output_file << "x,y,z\n";
+    output_file << p.x << "," << p.y << "," << p.z << '\n';
+}
+
+ImuIntegrator::~ImuIntegrator()
+{
+    output_file.close();
+}
+
+void ImuIntegrator::imu_callback(const sensor_msgs::ImuConstPtr& msg)
+{
+    if(fisrtTime)
+    {
+        time = msg->header.stamp;
+        dt = 0;
+        setGravity(msg->linear_acceleration);
+        fisrtTime = false;
+    }
+    else
+    {
+        dt = (msg->header.stamp - time).toSec();
+        time = msg->header.stamp;
+        calcOrientation(msg->angular_velocity);
+        calcPosition(msg->linear_acceleration);
+        publishMessage();
+    }
+}
+
+void ImuIntegrator::setGravity(const geometry_msgs::Vector3& msg)
+{
+    gravity[0] = msg.x;
+    gravity[1] = msg.y;
+    gravity[2] = msg.z;
+}
+
+void ImuIntegrator::calcOrientation(const geometry_msgs::Vector3& msg)
+{
+    Eigen::Matrix3d B;
+    // std::cout << dt << std::endl;
+    B <<      0     , -msg.z * dt,  msg.y * dt, 
+          msg.z * dt,      0     , -msg.x * dt,
+         -msg.y * dt,  msg.x * dt,      0     ;
+
+    double sigma = std::sqrt(std::pow(msg.x, 2) + std::pow(msg.y, 2) + std::pow(msg.z, 2)) * dt;
+
+    pose.orientation = pose.orientation * (Eigen::Matrix3d::Identity() + 
+                       (std::sin(sigma) / sigma) * B + ((1 - std::cos(sigma)) / std::pow(sigma, 2)) * B * B); 
+
+}
+
+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;
+
+} 
+
+void ImuIntegrator::publishMessage()
+{
+    geometry_msgs::Point p;
+    p.x = pose.pos[0];
+    p.y = pose.pos[1];
+    p.z = pose.pos[2];
+    path.points.push_back(p);
+    imu_odometry.publish(path);
+
+    output_file << p.x << "," << p.y << "," << p.z << '\n';
+}