Created
January 22, 2017 04:41
-
-
Save mitsuhito/ade304e7f2b955090039f66f712f8a99 to your computer and use it in GitHub Desktop.
lsm9ds0 ros node
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| // | |
| // lsm9ds0 ros node | |
| // rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 | |
| // | |
| #include <Wire.h> | |
| //#include <SPI.h> | |
| #include <Adafruit_LSM9DS0.h> | |
| #include <Adafruit_Sensor.h> | |
| #include <math.h> | |
| #include <ros.h> | |
| //#include <ros/time.h> | |
| #include <sensor_msgs/Imu.h> | |
| //#include <geometry_msgs/Quaternion.h> | |
| //#include <geometry_msgs/Vector3.h> | |
| //#include <std_msgs/String.h> | |
| //#include <std_msgs/Empty.h> | |
| // i2c connection | |
| Adafruit_LSM9DS0 lsm = Adafruit_LSM9DS0(1000); | |
| ros::NodeHandle nh; | |
| sensor_msgs::Imu imu_msg; | |
| ros::Publisher pub_imu("/imu0", &imu_msg); | |
| char frameid[] = "/imu0"; | |
| void init_lsm9ds0() | |
| { | |
| nh.loginfo("init_lsm9ds0()::start"); | |
| // Try to initialise and warn if we couldn't detect the chip | |
| if (!lsm.begin()) | |
| { | |
| //Serial.println("Oops ... unable to initialize the LSM9DS0. Check your wiring!"); | |
| while (1) | |
| { | |
| nh.logfatal("lsm9ds0 is not connected."); | |
| digitalWrite(13, HIGH); | |
| delay(1000); | |
| digitalWrite(13, LOW); | |
| delay(1000); | |
| } | |
| } | |
| nh.loginfo("configuring sensor"); | |
| // 1.) Set the accelerometer range | |
| lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_2G); | |
| //lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_4G); | |
| //lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_6G); | |
| //lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_8G); | |
| //lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_16G); | |
| // 2.) Set the magnetometer sensitivity | |
| lsm.setupMag(lsm.LSM9DS0_MAGGAIN_2GAUSS); | |
| //lsm.setupMag(lsm.LSM9DS0_MAGGAIN_4GAUSS); | |
| //lsm.setupMag(lsm.LSM9DS0_MAGGAIN_8GAUSS); | |
| //lsm.setupMag(lsm.LSM9DS0_MAGGAIN_12GAUSS); | |
| // 3.) Setup the gyroscope | |
| lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_245DPS); | |
| //lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_500DPS); | |
| //lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_2000DPS); | |
| nh.loginfo("init_lsm9ds0()::end"); | |
| } | |
| void setup() | |
| { | |
| nh.initNode(); | |
| nh.advertise(pub_imu); | |
| nh.loginfo("setup()::start"); | |
| //delay(1000); | |
| init_lsm9ds0(); | |
| pinMode(13, OUTPUT); | |
| digitalWrite(13, HIGH); | |
| imu_msg.header.frame_id = frameid; | |
| imu_msg.orientation.x = 0.0; | |
| imu_msg.orientation.y = 0.0; | |
| imu_msg.orientation.z = 0.0; | |
| imu_msg.orientation.w = 1.0; | |
| memset(&imu_msg.orientation_covariance[0], 0.0, sizeof(float) * 9 ); // ? | |
| imu_msg.angular_velocity.x = 0.0; | |
| imu_msg.angular_velocity.y = 0.0; | |
| imu_msg.angular_velocity.z = 0.0; | |
| memset(&imu_msg.angular_velocity_covariance[0], 0.0, sizeof(float) * 9 ); // ? | |
| imu_msg.linear_acceleration.x = 0.0; | |
| imu_msg.linear_acceleration.y = 0.0; | |
| imu_msg.linear_acceleration.z = 0.0; | |
| memset(&imu_msg.linear_acceleration_covariance[0], 0.0, sizeof(float) * 9 ); // ? | |
| digitalWrite(13, LOW); | |
| } | |
| long range_time = 0; | |
| /* | |
| # This is a message to hold data from an IMU (Inertial Measurement Unit) | |
| # | |
| # Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec | |
| # | |
| # If the covariance of the measurement is known, it should be filled in (if all you know is the | |
| # variance of each measurement, e.g. from the datasheet, just put those along the diagonal) | |
| # A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the | |
| # data a covariance will have to be assumed or gotten from some other source | |
| # | |
| # If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation | |
| # estimate), please set element 0 of the associated covariance matrix to -1 | |
| # If you are interpreting this message, please check for a value of -1 in the first element of each | |
| # covariance matrix, and disregard the associated estimate. | |
| Header header | |
| geometry_msgs/Quaternion orientation | |
| float64[9] orientation_covariance # Row major about x, y, z axes | |
| geometry_msgs/Vector3 angular_velocity | |
| float64[9] angular_velocity_covariance # Row major about x, y, z axes | |
| geometry_msgs/Vector3 linear_acceleration | |
| float64[9] linear_acceleration_covariance # Row major x, y z | |
| */ | |
| void loop() | |
| { | |
| //nh.loginfo("loop()::start"); | |
| sensors_event_t accel, mag, gyro, temp; | |
| lsm.getEvent(&accel, &mag, &gyro, &temp); | |
| if ( millis() >= range_time ) | |
| { | |
| imu_msg.header.stamp = nh.now(); | |
| //nh.loginfo("publish IMU info"); | |
| // Quaternion | |
| imu_msg.orientation.x = 0.0; | |
| imu_msg.orientation.y = 0.0; | |
| imu_msg.orientation.z = 0.0; | |
| imu_msg.orientation.w = 1.0; | |
| // rad/s | |
| imu_msg.angular_velocity.x = 2.0 * M_PI * (gyro.gyro.x / 360); | |
| imu_msg.angular_velocity.y = 2.0 * M_PI * (gyro.gyro.y / 360); | |
| imu_msg.angular_velocity.z = 2.0 * M_PI * (gyro.gyro.z / 360); | |
| // m/s^2 | |
| imu_msg.linear_acceleration.x = accel.acceleration.x; | |
| imu_msg.linear_acceleration.y = accel.acceleration.y; | |
| imu_msg.linear_acceleration.z = accel.acceleration.z; | |
| pub_imu.publish(&imu_msg); | |
| range_time = millis() + 50; | |
| } | |
| //nh.loginfo("loop()::end"); | |
| nh.spinOnce(); | |
| //delay(50); | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment