From 852044255a9f4d4323f7a5a19f16e79315deedb7 Mon Sep 17 00:00:00 2001 From: jcasals <jcasals@iri.upc.edu> Date: Fri, 24 Apr 2020 16:05:22 +0200 Subject: [PATCH] Revert "Add check sensor type processors" This reverts commit 12bc297401a610a94ad8bf1ad3a4e81bfc9a9305. --- include/core/processor/processor_odom_2d.h | 2 +- src/processor/processor_diff_drive.cpp | 3 +-- src/processor/processor_odom_2d.cpp | 5 ----- src/processor/processor_odom_3d.cpp | 6 +++--- 4 files changed, 5 insertions(+), 11 deletions(-) diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h index a626833c9..567c73920 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(ParamsProcessorOdom2dPtr _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, ParamsProcessorOdom2d); diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp index 1bd0ca0ef..6993332c3 100644 --- a/src/processor/processor_diff_drive.cpp +++ b/src/processor/processor_diff_drive.cpp @@ -36,8 +36,7 @@ void ProcessorDiffDrive::configure(SensorBasePtr _sensor) { assert(_sensor && "Provided sensor is nullptr"); - SensorDiffDriveConstPtr sensor_diff_drive = std::dynamic_pointer_cast<SensorDiffDrive>(_sensor); - assert(sensor_diff_drive != nullptr && "Sensor is not of type SensorDiffDrive"); + SensorDiffDriveConstPtr sensor_diff_drive = std::static_pointer_cast<SensorDiffDrive>(_sensor); radians_per_tick_ = sensor_diff_drive->getRadiansPerTick(); } diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index 4a126d901..04265e964 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -16,11 +16,6 @@ ProcessorOdom2d::~ProcessorOdom2d() { } -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 71c77e1e1..4bce5f885 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."); - auto sen_ptr = std::dynamic_pointer_cast<SensorOdom3d>(_sensor); - assert(sen_ptr != nullptr && "Sensor is not of type SensorOdom3d"); + + SensorOdom3dPtr sen_ptr = std::static_pointer_cast<SensorOdom3d>(_sensor); // 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