Created
January 27, 2017 02:29
-
-
Save mitsuhito/46f7b3bfdff6ee82682745a653f131dc to your computer and use it in GitHub Desktop.
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); | |
| #define LSM9DS0_SCLK 13 | |
| #define LSM9DS0_MISO 12 | |
| #define LSM9DS0_MOSI 11 | |
| #define LSM9DS0_XM_CS 10 | |
| #define LSM9DS0_GYRO_CS 9 | |
| // spi connection | |
| Adafruit_LSM9DS0 lsm = Adafruit_LSM9DS0(LSM9DS0_SCLK, LSM9DS0_MISO, LSM9DS0_MOSI, LSM9DS0_XM_CS, LSM9DS0_GYRO_CS, 1000); | |
| // ros node handler | |
| ros::NodeHandle nh; | |
| // imu message | |
| sensor_msgs::Imu imu_msg; | |
| ros::Publisher pub_imu("/imu0", &imu_msg); | |
| char frameid[] = "/imu0"; | |
| // initialize lsm9ds0 | |
| void init_lsm9ds0() | |
| { | |
| nh.loginfo("init_lsm9ds0()::start"); | |
| if (!lsm.begin()) | |
| { | |
| 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"); | |
| init_lsm9ds0(); | |
| 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 ); // ? | |
| } | |
| void loop() | |
| { | |
| //nh.loginfo("loop()::start"); | |
| sensors_event_t accel, mag, gyro, temp; | |
| lsm.getEvent(&accel, &mag, &gyro, &temp); | |
| 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.0); | |
| imu_msg.angular_velocity.y = 2.0 * M_PI * (gyro.gyro.y / 360.0); | |
| imu_msg.angular_velocity.z = 2.0 * M_PI * (gyro.gyro.z / 360.0); | |
| // 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); | |
| //nh.loginfo("loop()::end"); | |
| nh.spinOnce(); | |
| //delay(50); | |
| } |
Author
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
rostopic hz /imu0 で見ると18hz程度しかでていないため別実装を検討する。