Jaime Lozano / rosserial_mbed_poseAndImu_filter

Retrieves data from LSM6DSL sensors, filters and send to ROS

Dependencies:   LSM6DSL kalman ros_lib_kinetic

main.cpp@0:1ad0b0cdf28f, 2018-03-28 (annotated)

Committer:
Jaimelt
Date:
Wed Mar 28 19:42:58 2018 +0000
Revision:
0:1ad0b0cdf28f
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jaimelt 0:1ad0b0cdf28f 1 #include"mbed.h"
Jaimelt 0:1ad0b0cdf28f 2 #include <ros.h>
Jaimelt 0:1ad0b0cdf28f 3 #include "LSM6DSLSensor.h"
Jaimelt 0:1ad0b0cdf28f 4 #include <std_msgs/String.h>
Jaimelt 0:1ad0b0cdf28f 5 #include <sensor_msgs/Imu.h>
Jaimelt 0:1ad0b0cdf28f 6 #include <geometry_msgs/PoseStamped.h>
Jaimelt 0:1ad0b0cdf28f 7 #include <geometry_msgs/Pose.h>
Jaimelt 0:1ad0b0cdf28f 8 #include <nav_msgs/Path.h>
Jaimelt 0:1ad0b0cdf28f 9 #include <ros/time.h>
Jaimelt 0:1ad0b0cdf28f 10
Jaimelt 0:1ad0b0cdf28f 11
Jaimelt 0:1ad0b0cdf28f 12 /* Retrieve the composing elements of the expansion board */
Jaimelt 0:1ad0b0cdf28f 13
Jaimelt 0:1ad0b0cdf28f 14 /* Interface definition */
Jaimelt 0:1ad0b0cdf28f 15 #ifdef TARGET_DISCO_L475VG_IOT01A
Jaimelt 0:1ad0b0cdf28f 16 static DevI2C devI2c(PB_11,PB_10);
Jaimelt 0:1ad0b0cdf28f 17 #else // X-Nucleo-IKS01A2 or SensorTile
Jaimelt 0:1ad0b0cdf28f 18 #ifdef TARGET_SENSOR_TILE
Jaimelt 0:1ad0b0cdf28f 19 #define SPI_TYPE_LPS22HB LPS22HBSensor::SPI3W
Jaimelt 0:1ad0b0cdf28f 20 #define SPI_TYPE_LSM6DSL LSM6DSLSensor::SPI3W
Jaimelt 0:1ad0b0cdf28f 21 SPI devSPI(PB_15, NC, PB_13); // 3-wires SPI on SensorTile
Jaimelt 0:1ad0b0cdf28f 22 static Serial ser(PC_12,PD_2); // Serial with SensorTile Cradle Exp. Board + Nucleo
Jaimelt 0:1ad0b0cdf28f 23 #define printf(...) ser.printf(__VA_ARGS__)
Jaimelt 0:1ad0b0cdf28f 24 #else // Nucleo-XXX + X-Nucleo-IKS01A2
Jaimelt 0:1ad0b0cdf28f 25 static DevI2C devI2c(D14,D15);
Jaimelt 0:1ad0b0cdf28f 26 #endif
Jaimelt 0:1ad0b0cdf28f 27 #endif
Jaimelt 0:1ad0b0cdf28f 28
Jaimelt 0:1ad0b0cdf28f 29 /* Motion sensors */
Jaimelt 0:1ad0b0cdf28f 30 #ifdef TARGET_DISCO_L475VG_IOT01A
Jaimelt 0:1ad0b0cdf28f 31 static LSM6DSLSensor acc_gyro(&devI2c,LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW,PD_11); // low address
Jaimelt 0:1ad0b0cdf28f 32 #else // X-NUCLEO-IKS01A2 or SensorTile
Jaimelt 0:1ad0b0cdf28f 33 #if defined (TARGET_SENSOR_TILE)
Jaimelt 0:1ad0b0cdf28f 34 static LSM6DSLSensor acc_gyro(&devSPI,PB_12, NC, PA_2, SPI_TYPE_LSM6DSL);
Jaimelt 0:1ad0b0cdf28f 35 #else
Jaimelt 0:1ad0b0cdf28f 36 static LSM6DSLSensor acc_gyro(&devI2c,LSM6DSL_ACC_GYRO_I2C_ADDRESS_HIGH,D4,D5); // high address
Jaimelt 0:1ad0b0cdf28f 37 #endif
Jaimelt 0:1ad0b0cdf28f 38 #endif
Jaimelt 0:1ad0b0cdf28f 39
Jaimelt 0:1ad0b0cdf28f 40 ros::NodeHandle nh;
Jaimelt 0:1ad0b0cdf28f 41
Jaimelt 0:1ad0b0cdf28f 42 sensor_msgs::Imu imu_msg;
Jaimelt 0:1ad0b0cdf28f 43 ros::Publisher pub_imu("/imu", &imu_msg);
Jaimelt 0:1ad0b0cdf28f 44
Jaimelt 0:1ad0b0cdf28f 45 geometry_msgs::PoseStamped pose_msg;
Jaimelt 0:1ad0b0cdf28f 46 ros::Publisher pub_pose("/pose", &pose_msg);
Jaimelt 0:1ad0b0cdf28f 47
Jaimelt 0:1ad0b0cdf28f 48 nav_msgs::Path path_msg;
Jaimelt 0:1ad0b0cdf28f 49 ros::Publisher pub_path("/path", &path_msg);
Jaimelt 0:1ad0b0cdf28f 50
Jaimelt 0:1ad0b0cdf28f 51
Jaimelt 0:1ad0b0cdf28f 52 char frameid[] = "/imu";
Jaimelt 0:1ad0b0cdf28f 53 //char frameid_pose[] = "/pose";
Jaimelt 0:1ad0b0cdf28f 54
Jaimelt 0:1ad0b0cdf28f 55 DigitalOut led = LED1;
Jaimelt 0:1ad0b0cdf28f 56
Jaimelt 0:1ad0b0cdf28f 57 Timer t;
Jaimelt 0:1ad0b0cdf28f 58
Jaimelt 0:1ad0b0cdf28f 59 float alpha = 0.1;
Jaimelt 0:1ad0b0cdf28f 60 float x_acc_old, y_acc_old;
Jaimelt 0:1ad0b0cdf28f 61
Jaimelt 0:1ad0b0cdf28f 62 int main() {
Jaimelt 0:1ad0b0cdf28f 63 int32_t axes[3];
Jaimelt 0:1ad0b0cdf28f 64 int32_t angles[3];
Jaimelt 0:1ad0b0cdf28f 65 float dx = 0, dy = 0, x_pos = 0, y_pos= 0, x_acc = 0, y_acc = 0, x_offset = 0, y_offset = 0;
Jaimelt 0:1ad0b0cdf28f 66 float x_vel = 0;
Jaimelt 0:1ad0b0cdf28f 67 float y_vel = 0;
Jaimelt 0:1ad0b0cdf28f 68
Jaimelt 0:1ad0b0cdf28f 69 float dt = 0.04;
Jaimelt 0:1ad0b0cdf28f 70
Jaimelt 0:1ad0b0cdf28f 71 //float yaw = ;
Jaimelt 0:1ad0b0cdf28f 72
Jaimelt 0:1ad0b0cdf28f 73 /* Init sensors with default params */
Jaimelt 0:1ad0b0cdf28f 74 acc_gyro.init(NULL);
Jaimelt 0:1ad0b0cdf28f 75
Jaimelt 0:1ad0b0cdf28f 76 /* Enable sensors */
Jaimelt 0:1ad0b0cdf28f 77 acc_gyro.enable_x();
Jaimelt 0:1ad0b0cdf28f 78 acc_gyro.enable_g();
Jaimelt 0:1ad0b0cdf28f 79
Jaimelt 0:1ad0b0cdf28f 80 /*Offset calibration*/
Jaimelt 0:1ad0b0cdf28f 81 for(int i = 0; i<200; i++){
Jaimelt 0:1ad0b0cdf28f 82
Jaimelt 0:1ad0b0cdf28f 83 acc_gyro.get_x_axes(axes);
Jaimelt 0:1ad0b0cdf28f 84 acc_gyro.get_g_axes(angles);
Jaimelt 0:1ad0b0cdf28f 85
Jaimelt 0:1ad0b0cdf28f 86 x_offset += axes[0]/200;
Jaimelt 0:1ad0b0cdf28f 87 y_offset += axes[1]/200;
Jaimelt 0:1ad0b0cdf28f 88 wait_ms(10);
Jaimelt 0:1ad0b0cdf28f 89 }
Jaimelt 0:1ad0b0cdf28f 90
Jaimelt 0:1ad0b0cdf28f 91 nh.initNode();
Jaimelt 0:1ad0b0cdf28f 92 nh.advertise(pub_imu);
Jaimelt 0:1ad0b0cdf28f 93 imu_msg.header.frame_id = frameid;
Jaimelt 0:1ad0b0cdf28f 94 nh.advertise(pub_pose);
Jaimelt 0:1ad0b0cdf28f 95 pose_msg.header.frame_id = frameid;
Jaimelt 0:1ad0b0cdf28f 96 nh.advertise(pub_path);
Jaimelt 0:1ad0b0cdf28f 97 path_msg.header.frame_id = frameid;
Jaimelt 0:1ad0b0cdf28f 98
Jaimelt 0:1ad0b0cdf28f 99
Jaimelt 0:1ad0b0cdf28f 100
Jaimelt 0:1ad0b0cdf28f 101 while (1) {
Jaimelt 0:1ad0b0cdf28f 102
Jaimelt 0:1ad0b0cdf28f 103 for(int i=0; i<20; i++) {
Jaimelt 0:1ad0b0cdf28f 104
Jaimelt 0:1ad0b0cdf28f 105 t.stop();
Jaimelt 0:1ad0b0cdf28f 106 dt = t.read();
Jaimelt 0:1ad0b0cdf28f 107 t.reset();
Jaimelt 0:1ad0b0cdf28f 108 t.start();
Jaimelt 0:1ad0b0cdf28f 109
Jaimelt 0:1ad0b0cdf28f 110 acc_gyro.get_x_axes(axes);
Jaimelt 0:1ad0b0cdf28f 111 acc_gyro.get_g_axes(angles);
Jaimelt 0:1ad0b0cdf28f 112
Jaimelt 0:1ad0b0cdf28f 113
Jaimelt 0:1ad0b0cdf28f 114 x_acc = alpha * (axes[0]/104.383 - x_offset/104.383) + (1 - alpha) * x_acc;
Jaimelt 0:1ad0b0cdf28f 115 y_vel = 0;
Jaimelt 0:1ad0b0cdf28f 116
Jaimelt 0:1ad0b0cdf28f 117 led = !led;
Jaimelt 0:1ad0b0cdf28f 118
Jaimelt 0:1ad0b0cdf28f 119 //Obtain pose from acceleration
Jaimelt 0:1ad0b0cdf28f 120 dx = x_vel * dt + (dt*dt)/2.0*x_acc;//* cos(yaw);
Jaimelt 0:1ad0b0cdf28f 121 dy = y_vel * dt + (dt*dt)/2.0*y_acc;;//* sin(yaw);
Jaimelt 0:1ad0b0cdf28f 122
Jaimelt 0:1ad0b0cdf28f 123 x_vel = x_vel + x_acc * dt;
Jaimelt 0:1ad0b0cdf28f 124 y_vel = y_vel + y_acc * dt;
Jaimelt 0:1ad0b0cdf28f 125 x_pos += dx;
Jaimelt 0:1ad0b0cdf28f 126 y_pos += dy;
Jaimelt 0:1ad0b0cdf28f 127
Jaimelt 0:1ad0b0cdf28f 128 // nh.spinOnce();
Jaimelt 0:1ad0b0cdf28f 129 }
Jaimelt 0:1ad0b0cdf28f 130
Jaimelt 0:1ad0b0cdf28f 131 imu_msg.header.stamp = nh.now();
Jaimelt 0:1ad0b0cdf28f 132 pose_msg.header.stamp = nh.now();
Jaimelt 0:1ad0b0cdf28f 133 path_msg.header.stamp = nh.now();
Jaimelt 0:1ad0b0cdf28f 134
Jaimelt 0:1ad0b0cdf28f 135 pose_msg.pose.position.x = x_pos;
Jaimelt 0:1ad0b0cdf28f 136 pose_msg.pose.position.y = y_pos;
Jaimelt 0:1ad0b0cdf28f 137 pub_pose.publish( &pose_msg );
Jaimelt 0:1ad0b0cdf28f 138
Jaimelt 0:1ad0b0cdf28f 139 imu_msg.linear_acceleration.x = x_acc;
Jaimelt 0:1ad0b0cdf28f 140 imu_msg.linear_acceleration.y = y_acc;
Jaimelt 0:1ad0b0cdf28f 141 imu_msg.linear_acceleration.z = axes[2]/104.383;
Jaimelt 0:1ad0b0cdf28f 142 imu_msg.angular_velocity.x = angles[0];
Jaimelt 0:1ad0b0cdf28f 143 imu_msg.angular_velocity.y = angles[1];
Jaimelt 0:1ad0b0cdf28f 144 imu_msg.angular_velocity.z = angles[2];
Jaimelt 0:1ad0b0cdf28f 145 pub_imu.publish( &imu_msg );
Jaimelt 0:1ad0b0cdf28f 146
Jaimelt 0:1ad0b0cdf28f 147 path_msg.poses[1].pose.position.x = x_pos;
Jaimelt 0:1ad0b0cdf28f 148 path_msg.poses[1].pose.position.y = y_pos;
Jaimelt 0:1ad0b0cdf28f 149 pub_path.publish( &path_msg);
Jaimelt 0:1ad0b0cdf28f 150
Jaimelt 0:1ad0b0cdf28f 151 nh.spinOnce();
Jaimelt 0:1ad0b0cdf28f 152 }
Jaimelt 0:1ad0b0cdf28f 153 }