From 85ae44dff92f57b279b11d546b5d56a960a3ebee Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 19 Mar 2020 17:28:10 +0100
Subject: [PATCH] assertion on sensor
---
src/wolf_subscriber_imu.cpp | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/src/wolf_subscriber_imu.cpp b/src/wolf_subscriber_imu.cpp
index 1335726..69c3ab1 100644
--- a/src/wolf_subscriber_imu.cpp
+++ b/src/wolf_subscriber_imu.cpp
@@ -6,6 +6,7 @@ using namespace wolf;
WolfSubscriberImu::WolfSubscriberImu(const SensorBasePtr& sensor_ptr) :
WolfSubscriber(sensor_ptr)
{
+ assert(std::dynamic_pointer_cast<SensorIMU>(sensor_ptr) != nullptr);
}
void WolfSubscriberImu::initSubscriber(ros::NodeHandle& nh, const std::string& topic)
@@ -27,6 +28,13 @@ void WolfSubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
cov.topLeftCorner<3,3>() = Eigen::Map<const Eigen::Matrix3d>(msg->linear_acceleration_covariance.data());
cov.bottomRightCorner<3,3>()= Eigen::Map<const Eigen::Matrix3d>(msg->angular_velocity_covariance.data());
+ // if covariance not filled
+ if (cov.topLeftCorner<3,3>().isApprox(Eigen::Matrix3d::Zero(),1e-9))
+ cov.topLeftCorner<3,3>() = std::pow(std::static_pointer_cast<SensorIMU>(sensor_ptr_)->getAccelNoise(),2.0)*Eigen::Matrix3d::Identity();
+
+ if (cov.bottomRightCorner<3,3>().isApprox(Eigen::Matrix3d::Zero(),1e-9))
+ cov.bottomRightCorner<3,3>() = std::pow(std::static_pointer_cast<SensorIMU>(sensor_ptr_)->getGyroNoise(),2.0)*Eigen::Matrix3d::Identity();
+
CaptureIMUPtr new_capture = std::make_shared<CaptureIMU>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
sensor_ptr_,
data,
--
GitLab