src/tinkerforge_imu2.cpp

   1 #include <stdio.h>
   2 #include <math.h>
   3 #include "ros/ros.h"
   4 #include "sensor_msgs/Imu.h"
   5 #include "ip_connection.h"
   6 #include "brick_imu_v2.h"
   7
   8 ros::Publisher pub_imu;
   9
  10 void cb_all_data(int16_t acceleration[3], int16_t magnetic_field[3], int16_t angular_velocity[3], int16_t euler_angle[3], int16_t quaternion[4], int16_t linear_acceleration[3], int16_t gravity_vector[3], int8_t temperature, uint8_t calibration_status, void *user_data) {
  11         sensor_msgs::Imu msg;
  12         msg.header.stamp = ros::Time::now();
  13         msg.header.frame_id = "base_imu_link";
  14
  15         msg.orientation.x = quaternion[1]/16383.0;
  16         msg.orientation.y = quaternion[2]/16383.0;
  17         msg.orientation.z = quaternion[3]/16383.0;
  18         msg.orientation.w = quaternion[0]/16383.0;
  19         // Observed orientation variance: 0.0 (10k samples)
  20         // Magnometer heading accuracy is +-2.5 deg => 0.088 rad
  21         // With heading accuracy as std dev, variance = 0.088^2 = 0.008
  22         msg.orientation_covariance = {
  23                 0.008, 0     , 0,
  24                 0    , 0.008, 0,
  25                 0    , 0    , 0.008
  26         };
  27
  28         msg.angular_velocity.x = angular_velocity[0]/16.0*M_PI/180;
  29         msg.angular_velocity.y = angular_velocity[1]/16.0*M_PI/180;
  30         msg.angular_velocity.z = angular_velocity[2]/16.0*M_PI/180;
  31         // Observed angular velocity variance: 0.006223 (10k samples), => round up to 0.02
  32         msg.angular_velocity_covariance = {
  33                 0.02, 0   , 0,
  34                 0   , 0.02, 0,
  35                 0   , 0   , 0.02
  36         };
  37
  38         msg.linear_acceleration.x = acceleration[0]/100.0;
  39         msg.linear_acceleration.y = acceleration[1]/100.0;
  40         msg.linear_acceleration.z = acceleration[2]/100.0;
  41         // Observed linear acceleration variance: 0.001532 (10k samples)
  42         // Calculation for variance taken from razor imu:
  43         // nonliniarity spec: 1% of full scale (+-2G) => 0.2m/s^2
  44         // Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
  45         msg.linear_acceleration_covariance = {
  46                 0.04, 0    , 0,
  47                 0    , 0.04, 0,
  48                 0    , 0    , 0.04
  49         };
  50
  51         pub_imu.publish(msg);
  52 }
  53
  54 int main(int argc, char **argv) {
  55         std::string host, uid;
  56         int port;
  57         IPConnection ipcon;
  58         IMUV2 imu;
  59
  60         ros::init(argc, argv, "imu_brick_v2");
  61         ros::NodeHandle nh;
  62         ros::NodeHandle pnh("~");
  63
  64         pub_imu = nh.advertise<sensor_msgs::Imu>("imu", 10);
  65
  66         pnh.param("host", host, std::string("localhost"));
  67         pnh.param("port", port, 4223);
  68         pnh.getParam("uid", uid);
  69
  70         // Create IP connection
  71         ipcon_create(&ipcon);
  72         // Create device object
  73         imu_v2_create(&imu, uid.c_str(), &ipcon);
  74         // Connect to brickd
  75         if(ipcon_connect(&ipcon, host.c_str(), port) < 0) {
  76                 fprintf(stderr, "Could not connect\n");
  77                 return 1;
  78         }
  79         imu_v2_leds_off(&imu);
  80         imu_v2_disable_status_led(&imu);
  81         imu_v2_register_callback(&imu, IMU_V2_CALLBACK_ALL_DATA, (void (*)(void))cb_all_data, NULL);
  82         imu_v2_set_all_data_period(&imu, 20); // ms
  83
  84         ros::spin();
  85
  86         imu_v2_destroy(&imu);
  87         ipcon_destroy(&ipcon); // Calls ipcon_disconnect internally
  88 }