From 55c7e7b1769b828449349a48c5b14b6ebafa5991 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 3 Feb 2021 18:09:38 +0100
Subject: [PATCH] using SensorOdom2d::computeCovFromMotion() to compute cov
---
include/subscriber_odom2d.h | 2 +-
src/subscriber_odom2d.cpp | 10 +++++-----
2 files changed, 6 insertions(+), 6 deletions(-)
diff --git a/include/subscriber_odom2d.h b/include/subscriber_odom2d.h
index 50a3a7d..d59ae53 100644
--- a/include/subscriber_odom2d.h
+++ b/include/subscriber_odom2d.h
@@ -18,7 +18,7 @@ class SubscriberOdom2d : public Subscriber
{
protected:
ros::Time last_odom_stamp_;
- double odometry_translational_cov_factor_, odometry_rotational_cov_factor_;
+ SensorOdom2dPtr sensor_odom_;
public:
diff --git a/src/subscriber_odom2d.cpp b/src/subscriber_odom2d.cpp
index e9645b4..0643bc0 100644
--- a/src/subscriber_odom2d.cpp
+++ b/src/subscriber_odom2d.cpp
@@ -28,9 +28,9 @@ SubscriberOdom2d::SubscriberOdom2d(const std::string& _unique_name,
const SensorBasePtr _sensor_ptr)
: Subscriber(_unique_name, _server, _sensor_ptr)
, last_odom_stamp_(ros::Time(0))
- , odometry_translational_cov_factor_(std::static_pointer_cast<SensorOdom2d>(_sensor_ptr)->getDispVarToDispNoiseFactor())
- , odometry_rotational_cov_factor_(std::static_pointer_cast<SensorOdom2d>(_sensor_ptr)->getRotVarToRotNoiseFactor())
+ , sensor_odom_(std::static_pointer_cast<SensorOdom2d>(_sensor_ptr))
{
+ assert(std::dynamic_pointer_cast<SensorOdom2d>(_sensor_ptr) != nullptr && "SubscriberOdom2d: sensor provided is not of type SensorOdom2d!");
}
void SubscriberOdom2d::initialize(ros::NodeHandle& nh, const std::string& topic)
@@ -45,12 +45,12 @@ void SubscriberOdom2d::callback(const nav_msgs::Odometry::ConstPtr& msg)
if (last_odom_stamp_ != ros::Time(0))
{
double dt = (msg->header.stamp - last_odom_stamp_).toSec();
+ Eigen::Vector2d data(msg->twist.twist.linear.x * dt, msg->twist.twist.angular.z * dt);
CaptureOdom2dPtr new_capture = std::make_shared<CaptureOdom2d>(
TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
sensor_ptr_,
- Eigen::Vector2d(msg->twist.twist.linear.x * dt, msg->twist.twist.angular.z * dt),
- Eigen::DiagonalMatrix<double, 2>(msg->twist.twist.linear.x * dt * (double)odometry_translational_cov_factor_,
- msg->twist.twist.angular.z * dt * (double)odometry_rotational_cov_factor_));
+ data,
+ sensor_odom_->computeCovFromMotion(data));
sensor_ptr_->process(new_capture);
}
last_odom_stamp_ = msg->header.stamp;
--
GitLab