C++ (ROS node) for Euler Integration to estimate Velocity knowing the acceleration

Question

Hi I have IMU reading and would like to estimate the linear Velocity knowing the linear acceleration from the IMU using the Euler method. Im aware of the accumulated error. So need a C++(python) code ( possible a ROS node) that takes the IMU data reading from the IMU sensor and gives the estimated Linear velocity using the Euler Integration.

I know that acceleration a is

a=dv/dt

and that will be the function.

dv/dt = f(t) . i.e. v(n+1) = v(n) + f(n) * dt

So how are the steps calculated? I have the time stamps from the ros topic.. So any help about the C++ (ROS node) code?

Thanks

Answer 1

I find it easiest to make a class for stuff like this, where you have data you want to use between callbacks, or your want to publish a message inside of another callback. For example, your class might look something like (in the header:)

class IMUIntegrator
{
    public:
        void start();
    private:
        void imu_callback(const your_imu::Message::ConstPtr &msg);
        void send_velocity();
        ros::Publisher  m_velocity_pub;///< Publisher for the stopping estimate marker
        ros::Subscriber m_imu_sub; ///< Velocity subscriber
        float m_velocity = 0.0f;
        float m_prev_timestamp = 0.0f;
}

then your cpp file does something like:

void IMUIntegrator::start()
{
    ROS_INFO("Starting...");
    // Setup the publisher first so it's ready to go before we trigger the first sub callback. 
    ros::NodeHandle velocity_node;
    ros::NodeHandle imu_node;
    m_velocity_pub = velocity_node.advertise<your_velocity::Message>("/integrated_velocity", 1);
    m_imu_sub = imu_node.subscribe("/imu", 10, &IMUIntegrator::imu_callback, this);
    ros::spin();
}

void IMUIntegrator::imu_callback(const your_imu::Message::ConstPtr &msg)
{
    float timestamp = msg.header.timestamp;
    if(m_prev_timestamp==0.0f)
    {
        m_prev_timestamp = timestamp;
        return;
    }
    float dt = timestamp - m_prev_timestamp;
    m_prev_timestamp = timestamp;
    m_velocity += msg.acceleration[0]*dt;
    send_velocity();
}
void IMUIntegrator::send_velocity()
{
    your_velocity::Message message;
    message.header.frame_id = "your_frame";
    message.header.stamp = ros::Time();
    message.velocity = m_velocity;
    m_velocity_pub.publish(message);
}

and finally you start it from your main cpp file:

int main(int argc, char **argv)
{
  ros::init(argc, argv, "velocity_integrator");
  IMUIntegrator integrator;
  integrator.start();
  return 0;
}

I used placeholder names in there for your messages, etc., but that's generally the way it goes. This is all generally taken from the ros tutorials, which are actually pretty excellent, with the only exception that they're mashed together into a class and then used as a class. The class is nice because it lets you use member variables to pass information from the callback to the publisher. Member variables are kind of like global variables, but they're exclusive to the particular class, so you're not polluting your whole project with the information they contain (and other things aren't allowed to touch your class' data without permission!)