From 41e2163cd26d086d6fff8f7d4aa749391cdd39ae Mon Sep 17 00:00:00 2001
From: Paul D'Angio <pcdangio@gmail.com>
Date: Tue, 17 Nov 2020 23:01:48 -0500
Subject: [PATCH] added covariance publishing
---
driver_mpu9250/src/ros_node.cpp | 46 +++++++++++++++++++++++++++++++--
driver_mpu9250/src/ros_node.h | 12 ++++++++-
2 files changed, 55 insertions(+), 3 deletions(-)
diff --git a/driver_mpu9250/src/ros_node.cpp b/driver_mpu9250/src/ros_node.cpp
index 8a44e42..706a649 100644
--- a/driver_mpu9250/src/ros_node.cpp
+++ b/driver_mpu9250/src/ros_node.cpp
@@ -4,6 +4,7 @@
#include <sensor_msgs_ext/angular_velocity.h>
#include <sensor_msgs_ext/magnetic_field.h>
#include <sensor_msgs_ext/temperature.h>
+#include <sensor_msgs_ext/covariance.h>
#include <cmath>
@@ -37,12 +38,12 @@ ros_node::ros_node(std::shared_ptr<driver> driver, int argc, char **argv)
ros_node::m_calibration_accelerometer.load(private_node, "calibration_accelerometer");
ros_node::m_calibration_magnetometer.load(private_node, "calibration_magnetometer");
- // Set up publishers.
+ // Set up data publishers.
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::angular_velocity>("imu/gyroscope", 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);
-
+
// Initialize the driver and set parameters.
try
{
@@ -64,6 +65,9 @@ ros_node::ros_node(std::shared_ptr<driver> driver, int argc, char **argv)
exit(1);
}
+ // Publish covariance matrices.
+ ros_node::publish_covariance();
+
// Set up services.
ros_node::m_service_calibrate_gyroscope = ros_node::m_node->advertiseService("imu/calibrate_gyroscope", &ros_node::service_calibrate_gyroscope, this);
@@ -143,6 +147,44 @@ bool ros_node::calibrate_gyroscope(uint32_t averaging_period)
}
// METHODS
+void ros_node::publish_covariance()
+{
+ // Get a private nodehandle for reading parameters.
+ ros::NodeHandle private_node("~");
+
+ // Create a publisher and publish a latched covariance message for each sensor.
+ // NOTE: Variances are read from parameters. Default values are from testing.
+
+ // Accelerometer
+ ros_node::m_publisher_covariance_accelerometer = ros_node::m_node->advertise<sensor_msgs_ext::covariance>("imu/accelerometer/covariance", 1, true);
+ sensor_msgs_ext::covariance covariance_accelerometer;
+ covariance_accelerometer.dimensions = 3;
+ covariance_accelerometer.covariance.resize(9, 0.0);
+ covariance_accelerometer.covariance[0] = private_node.param<double>("variance/accelerometer/x", 5e-4);
+ covariance_accelerometer.covariance[4] = private_node.param<double>("variance/accelerometer/y", 5e-4);
+ covariance_accelerometer.covariance[8] = private_node.param<double>("variance/accelerometer/z", 5e-4);
+ ros_node::m_publisher_covariance_accelerometer.publish(covariance_accelerometer);
+
+ // Gyroscope
+ ros_node::m_publisher_covariance_gyroscope = ros_node::m_node->advertise<sensor_msgs_ext::covariance>("imu/gyroscope/covariance", 1, true);
+ sensor_msgs_ext::covariance covariance_gyroscope;
+ covariance_gyroscope.dimensions = 3;
+ covariance_gyroscope.covariance.resize(9, 0.0);
+ covariance_gyroscope.covariance[0] = private_node.param<double>("variance/gyroscope/x", 3e-6);
+ covariance_gyroscope.covariance[4] = private_node.param<double>("variance/gyroscope/y", 3e-6);
+ covariance_gyroscope.covariance[8] = private_node.param<double>("variance/gyroscope/z", 3e-6);
+ ros_node::m_publisher_covariance_gyroscope.publish(covariance_gyroscope);
+
+ // Magnetometer
+ ros_node::m_publisher_covariance_magnetometer = ros_node::m_node->advertise<sensor_msgs_ext::covariance>("imu/magnetometer/covariance", 1, true);
+ sensor_msgs_ext::covariance covariance_magnetometer;
+ covariance_magnetometer.dimensions = 3;
+ covariance_magnetometer.covariance.resize(9, 0.0);
+ covariance_magnetometer.covariance[0] = private_node.param<double>("variance/magnetometer/x", 4.8e-13);
+ covariance_magnetometer.covariance[4] = private_node.param<double>("variance/magnetometer/y", 4.8e-13);
+ covariance_magnetometer.covariance[8] = private_node.param<double>("variance/magnetometer/z", 4.8e-13);
+ ros_node::m_publisher_covariance_magnetometer.publish(covariance_magnetometer);
+}
void ros_node::deinitialize_driver()
{
try
diff --git a/driver_mpu9250/src/ros_node.h b/driver_mpu9250/src/ros_node.h
index 1aafd49..6a8952e 100644
--- a/driver_mpu9250/src/ros_node.h
+++ b/driver_mpu9250/src/ros_node.h
@@ -54,7 +54,7 @@ private:
/// \brief m_node The node's handle.
std::shared_ptr<ros::NodeHandle> m_node;
- // PUBLISHERS
+ // PUBLISHERS - DATA
/// \brief Publisher for accelerometer data.
ros::Publisher m_publisher_accelerometer;
/// \brief Publisher for gyroscope data.
@@ -64,6 +64,14 @@ private:
/// \brief Publisher for temperature data.
ros::Publisher m_publisher_temperature;
+ // PUBLISHERS - COVARIANCE
+ /// \brief Publisher for accelerometer covariance.
+ ros::Publisher m_publisher_covariance_accelerometer;
+ /// \brief Publisher for gyroscope covariance.
+ ros::Publisher m_publisher_covariance_gyroscope;
+ /// \brief Publisher for magnetometer covariance.
+ ros::Publisher m_publisher_covariance_magnetometer;
+
// SERVICES
/// \brief Service server for calibrating the gyroscope.
ros::ServiceServer m_service_calibrate_gyroscope;
@@ -74,6 +82,8 @@ private:
bool service_calibrate_gyroscope(driver_mpu9250_msgs::calibrate_gyroscopeRequest& request, driver_mpu9250_msgs::calibrate_gyroscopeResponse& response);
// METHODS
+ /// \brief Publishes covariance matrices.
+ void publish_covariance();
/// \brief deinitialize_driver Deinitializes the driver.
void deinitialize_driver();
--
GitLab