using SensorOdom2d::computeCovFromMotion() to compute cov

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:
 

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;