Skip to content
Snippets Groups Projects
Commit bfaec511 authored by Paul D'Angio's avatar Paul D'Angio
Browse files

updated to new imu message types from sensor_msgs_ext

parent 7628a073
No related branches found
No related tags found
No related merge requests found
#include "ros_node.h" #include "ros_node.h"
#include <sensor_msgs_ext/accelerometer.h> #include <sensor_msgs_ext/acceleration.h>
#include <sensor_msgs_ext/gyroscope.h> #include <sensor_msgs_ext/angular_velocity.h>
#include <sensor_msgs_ext/magnetometer.h> #include <sensor_msgs_ext/magnetic_field.h>
#include <sensor_msgs_ext/temperature.h> #include <sensor_msgs_ext/temperature.h>
#include <cmath> #include <cmath>
...@@ -31,9 +31,9 @@ ros_node::ros_node(driver *driver, int argc, char **argv) ...@@ -31,9 +31,9 @@ ros_node::ros_node(driver *driver, int argc, char **argv)
// Set up publishers. // Set up publishers.
ros_node::m_publisher_accelerometer = ros_node::m_node->advertise<sensor_msgs_ext::accelerometer>("imu/accelerometer", 1); ros_node::m_publisher_accelerometer = ros_node::m_node->advertise<sensor_msgs_ext::acceleration>("imu/accelerometer", 1);
ros_node::m_publisher_gyroscope = ros_node::m_node->advertise<sensor_msgs_ext::gyroscope>("imu/gyroscope", 1); ros_node::m_publisher_gyroscope = ros_node::m_node->advertise<sensor_msgs_ext::angular_velocity>("imu/gyroscope", 1);
ros_node::m_publisher_magnetometer = ros_node::m_node->advertise<sensor_msgs_ext::magnetometer>("imu/magnetometer", 1); ros_node::m_publisher_magnetometer = ros_node::m_node->advertise<sensor_msgs_ext::magnetic_field>("imu/magnetometer", 1);
ros_node::m_publisher_temperature = ros_node::m_node->advertise<sensor_msgs_ext::temperature>("imu/temperature", 1); ros_node::m_publisher_temperature = ros_node::m_node->advertise<sensor_msgs_ext::temperature>("imu/temperature", 1);
// Initialize the driver and set parameters. // Initialize the driver and set parameters.
...@@ -92,7 +92,7 @@ void ros_node::deinitialize_driver() ...@@ -92,7 +92,7 @@ void ros_node::deinitialize_driver()
void ros_node::data_callback(driver::data data) void ros_node::data_callback(driver::data data)
{ {
// Create accelerometer message. // Create accelerometer message.
sensor_msgs_ext::accelerometer message_accel; sensor_msgs_ext::acceleration message_accel;
// Set accelerations (convert from g's to m/s^2) // Set accelerations (convert from g's to m/s^2)
message_accel.x = static_cast<double>(data.accel_x) * 9.80665; message_accel.x = static_cast<double>(data.accel_x) * 9.80665;
message_accel.y = static_cast<double>(data.accel_y) * 9.80665; message_accel.y = static_cast<double>(data.accel_y) * 9.80665;
...@@ -101,7 +101,7 @@ void ros_node::data_callback(driver::data data) ...@@ -101,7 +101,7 @@ void ros_node::data_callback(driver::data data)
ros_node::m_publisher_accelerometer.publish(message_accel); ros_node::m_publisher_accelerometer.publish(message_accel);
// Create gyroscope message. // Create gyroscope message.
sensor_msgs_ext::gyroscope message_gyro; sensor_msgs_ext::angular_velocity message_gyro;
// Set rotation rates (convert from deg/sec to rad/sec) // Set rotation rates (convert from deg/sec to rad/sec)
message_gyro.x = static_cast<double>(data.gyro_x) * M_PI / 180.0; message_gyro.x = static_cast<double>(data.gyro_x) * M_PI / 180.0;
message_gyro.y = static_cast<double>(data.gyro_y) * M_PI / 180.0; message_gyro.y = static_cast<double>(data.gyro_y) * M_PI / 180.0;
...@@ -113,7 +113,7 @@ void ros_node::data_callback(driver::data data) ...@@ -113,7 +113,7 @@ void ros_node::data_callback(driver::data data)
if(isnan(data.magneto_x) == false) if(isnan(data.magneto_x) == false)
{ {
// Create magneto message. // Create magneto message.
sensor_msgs_ext::magnetometer message_mag; sensor_msgs_ext::magnetic_field message_mag;
// Fill magnetic field strengths (convert from uT to T) // Fill magnetic field strengths (convert from uT to T)
message_mag.x = static_cast<double>(data.magneto_x) * 0.000001; message_mag.x = static_cast<double>(data.magneto_x) * 0.000001;
message_mag.y = static_cast<double>(data.magneto_y) * 0.000001; message_mag.y = static_cast<double>(data.magneto_y) * 0.000001;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment