From 12bc297401a610a94ad8bf1ad3a4e81bfc9a9305 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Mon, 16 Mar 2020 12:34:30 +0100
Subject: [PATCH] Add check sensor type processors
---
include/core/processor/processor_odom_2d.h | 2 +-
src/processor/processor_diff_drive.cpp | 3 ++-
src/processor/processor_odom_2d.cpp | 7 ++++++-
src/processor/processor_odom_3d.cpp | 6 +++---
4 files changed, 12 insertions(+), 6 deletions(-)
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index 2dfa88f60..6d46ab61a 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -42,7 +42,7 @@ class ProcessorOdom2d : public ProcessorMotion
public:
ProcessorOdom2d(ProcessorParamsOdom2dPtr _params);
virtual ~ProcessorOdom2d();
- virtual void configure(SensorBasePtr _sensor) override { };
+ virtual void configure(SensorBasePtr _sensor) override;
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorOdom2d, ProcessorParamsOdom2d);
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 0150a5811..d80f89cce 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -36,7 +36,8 @@ void ProcessorDiffDrive::configure(SensorBasePtr _sensor)
{
assert(_sensor && "Provided sensor is nullptr");
- SensorDiffDriveConstPtr sensor_diff_drive = std::static_pointer_cast<SensorDiffDrive>(_sensor);
+ SensorDiffDriveConstPtr sensor_diff_drive = std::dynamic_pointer_cast<SensorDiffDrive>(_sensor);
+ assert(sensor_diff_drive != nullptr && "Sensor is not of type SensorDiffDrive");
radians_per_tick_ = sensor_diff_drive->getRadiansPerTick();
}
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 190f7c95a..31b7cc70b 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -15,7 +15,12 @@ ProcessorOdom2d::~ProcessorOdom2d()
{
}
-void ProcessorOdom2d::computeCurrentDelta(const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov,
+void ProcessorOdom2D::configure(SensorBasePtr _sensor)
+{
+ auto sensor_ = std::dynamic_pointer_cast<SensorOdom2D>(_sensor);
+ assert(sensor_ != nullptr && "Sensor is not of type SensorOdom2D");
+}
+void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov,
const Eigen::VectorXd& _calib, const double _dt, Eigen::VectorXd& _delta,
Eigen::MatrixXd& _delta_cov, Eigen::MatrixXd& _jacobian_calib) const
{
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 7fd2c4654..ac491d9c1 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -21,8 +21,8 @@ ProcessorOdom3d::~ProcessorOdom3d()
void ProcessorOdom3d::configure(SensorBasePtr _sensor)
{
assert (_sensor && "Trying to configure processor with a sensor but provided sensor is nullptr.");
-
- SensorOdom3dPtr sen_ptr = std::static_pointer_cast<SensorOdom3d>(_sensor);
+ auto sen_ptr = std::dynamic_pointer_cast<SensorOdom3d>(_sensor);
+ assert(sen_ptr != nullptr && "Sensor is not of type SensorOdom3d");
// we steal the parameters from the provided odom3d sensor.
k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor();
@@ -151,7 +151,7 @@ void ProcessorOdom3d::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen
void ProcessorOdom3d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
Eigen::VectorXd& _x_plus_delta) const
-{
+{
assert(_x.size() >= x_size_ && "Wrong _x vector size"); //we need a state vector which size is at least x_size_
assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
assert(_x_plus_delta.size() >= x_size_ && "Wrong _x_plus_delta vector size");
--
GitLab