File content as of revision 0:1ad0b0cdf28f:

#include"mbed.h"
#include <ros.h>
#include "LSM6DSLSensor.h"
#include <std_msgs/String.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Path.h>
#include <ros/time.h>


/* Retrieve the composing elements of the expansion board */

/* Interface definition */
#ifdef TARGET_DISCO_L475VG_IOT01A
static DevI2C devI2c(PB_11,PB_10);
#else // X-Nucleo-IKS01A2 or SensorTile
#ifdef TARGET_SENSOR_TILE
#define SPI_TYPE_LPS22HB   LPS22HBSensor::SPI3W
#define SPI_TYPE_LSM6DSL   LSM6DSLSensor::SPI3W
SPI devSPI(PB_15, NC, PB_13);  // 3-wires SPI on SensorTile  
static Serial ser(PC_12,PD_2); // Serial with SensorTile Cradle Exp. Board + Nucleo   
#define printf(...) ser.printf(__VA_ARGS__)     
#else  // Nucleo-XXX + X-Nucleo-IKS01A2 
static DevI2C devI2c(D14,D15);
#endif
#endif

/* Motion sensors */
#ifdef TARGET_DISCO_L475VG_IOT01A
static LSM6DSLSensor acc_gyro(&devI2c,LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW,PD_11); // low address
#else // X-NUCLEO-IKS01A2 or SensorTile
#if defined (TARGET_SENSOR_TILE)
static LSM6DSLSensor acc_gyro(&devSPI,PB_12, NC, PA_2, SPI_TYPE_LSM6DSL); 
#else
static LSM6DSLSensor acc_gyro(&devI2c,LSM6DSL_ACC_GYRO_I2C_ADDRESS_HIGH,D4,D5); // high address
#endif
#endif

ros::NodeHandle  nh;

sensor_msgs::Imu imu_msg;
ros::Publisher pub_imu("/imu", &imu_msg);

geometry_msgs::PoseStamped pose_msg;
ros::Publisher pub_pose("/pose", &pose_msg);

nav_msgs::Path path_msg;
ros::Publisher pub_path("/path", &path_msg);


char frameid[] = "/imu";
//char frameid_pose[] = "/pose";

DigitalOut led = LED1;

Timer t;

float alpha = 0.1;
float x_acc_old, y_acc_old;

int main() {
    int32_t axes[3];
    int32_t angles[3];
    float dx = 0, dy = 0, x_pos = 0, y_pos= 0, x_acc = 0, y_acc = 0, x_offset = 0, y_offset = 0;
    float x_vel = 0;
    float y_vel = 0;
    
    float dt = 0.04;
    
    //float yaw = ;
    
    /* Init sensors with default params */
    acc_gyro.init(NULL);
    
    /* Enable sensors */
    acc_gyro.enable_x();
    acc_gyro.enable_g();

    /*Offset calibration*/
    for(int i = 0; i<200; i++){
        
        acc_gyro.get_x_axes(axes);
        acc_gyro.get_g_axes(angles);

        x_offset += axes[0]/200;
        y_offset += axes[1]/200;
        wait_ms(10);
    }
    
    nh.initNode();
    nh.advertise(pub_imu);
    imu_msg.header.frame_id = frameid;
    nh.advertise(pub_pose);
    pose_msg.header.frame_id = frameid;
    nh.advertise(pub_path);
    path_msg.header.frame_id = frameid;
    

    
    while (1) {   
        
        for(int i=0; i<20; i++) {   
            
            t.stop();
            dt = t.read();
            t.reset();
            t.start();

            acc_gyro.get_x_axes(axes);
            acc_gyro.get_g_axes(angles);

            
            x_acc = alpha * (axes[0]/104.383 - x_offset/104.383) + (1 - alpha) * x_acc;
            y_vel = 0;
            
            led = !led;

            //Obtain pose from acceleration
            dx = x_vel * dt + (dt*dt)/2.0*x_acc;//* cos(yaw);
            dy = y_vel * dt + (dt*dt)/2.0*y_acc;;//* sin(yaw);

            x_vel = x_vel + x_acc * dt;
            y_vel = y_vel + y_acc * dt;
            x_pos += dx;
            y_pos += dy;
            
       //     nh.spinOnce();
        }
        
        imu_msg.header.stamp = nh.now();
        pose_msg.header.stamp = nh.now();   
        path_msg.header.stamp = nh.now();
        
        pose_msg.pose.position.x = x_pos;
        pose_msg.pose.position.y = y_pos;
        pub_pose.publish( &pose_msg );
        
        imu_msg.linear_acceleration.x = x_acc;
        imu_msg.linear_acceleration.y = y_acc;
        imu_msg.linear_acceleration.z = axes[2]/104.383;
        imu_msg.angular_velocity.x = angles[0];
        imu_msg.angular_velocity.y = angles[1];
        imu_msg.angular_velocity.z = angles[2];
        pub_imu.publish( &imu_msg );
        
        path_msg.poses[1].pose.position.x = x_pos;
        path_msg.poses[1].pose.position.y = y_pos;
        pub_path.publish( &path_msg);
        
        nh.spinOnce();
    }
}