diff --git a/CMakeLists.txt b/CMakeLists.txt
index f3101ccb3edf64def819ede218a7b0e14b0573ce..77026c53ba80cc5233eaadd86475582307d43b13 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,7 +9,7 @@ add_compile_options(-std=c++11)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
- sensor_msgs
+ sensor_msgs_ext
)
## System dependencies are found with CMake's conventions
diff --git a/package.xml b/package.xml
index aba2d5ff46fee45de23ed0975f371347663c9db8..aabfc13576378792593fc83b5f137b5e96e12ac3 100644
--- a/package.xml
+++ b/package.xml
@@ -10,6 +10,6 @@
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
- <depend>sensor_msgs</depend>
+ <depend>sensor_msgs_ext</depend>
</package>
diff --git a/src/ros_node.cpp b/src/ros_node.cpp
index 4d0c8fdb012774e5fac0e2c3b7efb5a6617f7b85..a216c43a5db21bda5876c698abbbffa440894d80 100644
--- a/src/ros_node.cpp
+++ b/src/ros_node.cpp
@@ -1,8 +1,9 @@
#include "ros_node.h"
-#include <sensor_msgs/Imu.h>
-#include <sensor_msgs/MagneticField.h>
-#include <sensor_msgs/Temperature.h>
+#include <sensor_msgs_ext/accelerometer.h>
+#include <sensor_msgs_ext/gyroscope.h>
+#include <sensor_msgs_ext/magnetometer.h>
+#include <sensor_msgs_ext/temperature.h>
#include <cmath>
@@ -35,9 +36,10 @@ ros_node::ros_node(driver *driver, int argc, char **argv)
private_node.param<int>("accel_fsr", param_accel_fsr, 0);
// Set up publishers.
- ros_node::m_publisher_imu = ros_node::m_node->advertise<sensor_msgs::Imu>("imu/imu", 1);
- ros_node::m_publisher_mag = ros_node::m_node->advertise<sensor_msgs::MagneticField>("imu/magneto", 1);
- ros_node::m_publisher_temp = ros_node::m_node->advertise<sensor_msgs::Temperature>("imu/temperature", 1);
+ ros_node::m_publisher_accelerometer = ros_node::m_node->advertise<sensor_msgs_ext::accelerometer>("imu/accelerometer", 1);
+ ros_node::m_publisher_gyroscope = ros_node::m_node->advertise<sensor_msgs_ext::gyroscope>("imu/gyroscope", 1);
+ ros_node::m_publisher_magnetometer = ros_node::m_node->advertise<sensor_msgs_ext::magnetometer>("imu/magnetometer", 1);
+ ros_node::m_publisher_temperature = ros_node::m_node->advertise<sensor_msgs_ext::temperature>("imu/temperature", 1);
// Initialize the driver and set parameters.
try
@@ -93,50 +95,40 @@ void ros_node::deinitialize_driver()
void ros_node::data_callback(driver::data data)
{
- // Create IMU message.
- sensor_msgs::Imu message_imu;
- message_imu.header.stamp = ros::Time::now();
- message_imu.header.frame_id = "mpu9250";
- // Set blank orientation.
- message_imu.orientation.w = std::numeric_limits<double>::quiet_NaN();
- message_imu.orientation.x = std::numeric_limits<double>::quiet_NaN();
- message_imu.orientation.y = std::numeric_limits<double>::quiet_NaN();
- message_imu.orientation.z = std::numeric_limits<double>::quiet_NaN();
- // Covariances of -1 indicate orientation not calculated.
- message_imu.orientation_covariance.fill(-1.0);
+ // Create accelerometer message.
+ sensor_msgs_ext::accelerometer message_accel;
// Set accelerations (convert from g's to m/s^2)
- message_imu.linear_acceleration.x = static_cast<double>(data.accel_x) * 9.80665;
- message_imu.linear_acceleration.y = static_cast<double>(data.accel_y) * 9.80665;
- message_imu.linear_acceleration.z = static_cast<double>(data.accel_z) * 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.z = static_cast<double>(data.accel_z) * 9.80665;
+ // Publish message.
+ ros_node::m_publisher_accelerometer.publish(message_accel);
+
+ // Create gyroscope message.
+ sensor_msgs_ext::gyroscope message_gyro;
// Set rotation rates (convert from deg/sec to rad/sec)
- message_imu.angular_velocity.x = static_cast<double>(data.gyro_x) * M_PI / 180.0;
- message_imu.angular_velocity.y = static_cast<double>(data.gyro_y) * M_PI / 180.0;
- message_imu.angular_velocity.z = static_cast<double>(data.gyro_z) * M_PI / 180.0;
- // Leave covariance matrices at zero.
- // Publish IMU message.
- ros_node::m_publisher_imu.publish(message_imu);
+ 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.z = static_cast<double>(data.gyro_z) * M_PI / 180.0;
+ // Publish message.
+ ros_node::m_publisher_gyroscope.publish(message_gyro);
// Check if there was a magneto overflow.
if(isnan(data.magneto_x) == false)
{
// Create magneto message.
- sensor_msgs::MagneticField message_mag;
- message_mag.header = message_imu.header;
+ sensor_msgs_ext::magnetometer message_mag;
// Fill magnetic field strengths (convert from uT to T)
- message_mag.magnetic_field.x = static_cast<double>(data.magneto_x) * 0.000001;
- message_mag.magnetic_field.y = static_cast<double>(data.magneto_y) * 0.000001;
- message_mag.magnetic_field.z = static_cast<double>(data.magneto_z) * 0.000001;
- // Leave covariance matrices at zero.
-
- // Publish magneto message.
- ros_node::m_publisher_mag.publish(message_mag);
+ message_mag.x = static_cast<double>(data.magneto_x) * 0.000001;
+ message_mag.y = static_cast<double>(data.magneto_y) * 0.000001;
+ message_mag.z = static_cast<double>(data.magneto_z) * 0.000001;
+ // Publish message.
+ ros_node::m_publisher_magnetometer.publish(message_mag);
}
// Create temperature message.
- sensor_msgs::Temperature message_temp;
- message_temp.header = message_imu.header;
+ sensor_msgs_ext::temperature message_temp;
message_temp.temperature = static_cast<double>(data.temp);
- message_temp.variance = 0.0;
// Publish temperature message.
- ros_node::m_publisher_temp.publish(message_temp);
+ ros_node::m_publisher_temperature.publish(message_temp);
}
diff --git a/src/ros_node.h b/src/ros_node.h
index 73e026798061e4344f2b5f9e7a15c32ac85d6463..82cb1fd5fc109c6f7b899f9dc90bc9f8e7be101e 100644
--- a/src/ros_node.h
+++ b/src/ros_node.h
@@ -7,61 +7,43 @@
#include <ros/ros.h>
-///
/// \brief Implements the driver's ROS node functionality.
-///
class ros_node
{
public:
// CONSTRUCTORS
- ///
/// \brief ros_node Initializes the ROS node.
/// \param driver The MPU9250 driver instance.
/// \param argc Number of main() args.
/// \param argv The main() args.
- ///
ros_node(driver* driver, int argc, char **argv);
~ros_node();
// METHODS
- ///
/// \brief spin Runs the node.
- ///
void spin();
private:
// VARIABLES
- ///
/// \brief m_driver The driver instance.
- ///
driver* m_driver;
- ///
/// \brief m_node The node's handle.
- ///
ros::NodeHandle* m_node;
- ///
- /// \brief m_publisher_imu The publisher for IMU messages.
- ///
- ros::Publisher m_publisher_imu;
- ///
- /// \brief m_publisher_mag The publisher for MagneticField messages.
- ///
- ros::Publisher m_publisher_mag;
- ///
- /// \brief m_publisher_temp The publisher for Temperature messages.
- ///
- ros::Publisher m_publisher_temp;
+ /// \brief Publisher for accelerometer data.
+ ros::Publisher m_publisher_accelerometer;
+ /// \brief Publisher for gyroscope data.
+ ros::Publisher m_publisher_gyroscope;
+ /// \brief Publisher for magnetometer data.
+ ros::Publisher m_publisher_magnetometer;
+ /// \brief Publisher for temperature data.
+ ros::Publisher m_publisher_temperature;
// METHODS
- ///
/// \brief deinitialize_driver Deinitializes the driver.
- ///
void deinitialize_driver();
- ///
/// \brief data_callback The callback function for when new data is available.
/// \param data The latest data read from the MPU9250/AK8963.
- ///
void data_callback(driver::data data);
};