diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp index 5e84c80f955d204c6cb2bf4e7989883581f38322..a63a7a248f739294a17a533dec300c11a941ee84 100644 --- a/hello_wolf/sensor_range_bearing.cpp +++ b/hello_wolf/sensor_range_bearing.cpp @@ -34,19 +34,6 @@ SensorRangeBearing::~SensorRangeBearing() // } -SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, // - const Eigen::VectorXs& _extrinsics, // - const IntrinsicsBasePtr _intrinsics) -{ - - IntrinsicsRangeBearingPtr _intrinsics_rb = std::static_pointer_cast<IntrinsicsRangeBearing>(_intrinsics); - - Eigen::Vector2s noise_std(_intrinsics_rb->noise_range_metres_std, toRad(_intrinsics_rb->noise_bearing_degrees_std)); - - SensorRangeBearingPtr sensor = std::make_shared<SensorRangeBearing>(_extrinsics, noise_std); - - return sensor; -} } /* namespace wolf */ @@ -55,8 +42,5 @@ SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, // namespace wolf { WOLF_REGISTER_SENSOR("RANGE BEARING", SensorRangeBearing) -} // namespace wolf -namespace wolf -{ WOLF_REGISTER_SENSOR_AUTO("RANGE BEARING", SensorRangeBearing) } // namespace wolf diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h index 036fe4cd1115e02ad4f6d2bd65f570e392038b57..9ff5f509ef0fdf2b6560c257257c7160711024c7 100644 --- a/hello_wolf/sensor_range_bearing.h +++ b/hello_wolf/sensor_range_bearing.h @@ -44,14 +44,9 @@ class SensorRangeBearing : public SensorBase public: SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const Eigen::Vector2s& _noise_std); SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const IntrinsicsRangeBearingPtr _intr); + WOLF_SENSOR_CREATE(SensorRangeBearing, IntrinsicsRangeBearing, 3); virtual ~SensorRangeBearing(); - // Factory method for high level API - static SensorBasePtr create(const std::string& _unique_name, // - const Eigen::VectorXs& _extrinsics, // - const IntrinsicsBasePtr _intrinsics); - WOLF_CREATE_SENSOR_AUTO(SensorRangeBearing, IntrinsicsRangeBearing, 3); - }; } /* namespace wolf */ diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 910fa12d2d28c0b2a240ad067bd0412d20b8d751..b9f196f5bac03475f39240c58c22308373a79863 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -32,21 +32,36 @@ namespace wolf { * * We recommend writing one of such constructors in your derived sensors. */ -#define WOLF_CREATE_SENSOR_AUTO(SensorClass, IntrinsicsClass, ExtrinsicsSize) \ -static SensorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server) \ +#define WOLF_SENSOR_CREATE(SensorClass, IntrinsicsClass, ExtrinsicsSize) \ +static \ +SensorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server) \ { \ Eigen::VectorXs extrinsics = _server.template getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pose"); \ \ - assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \ + assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \ \ - IntrinsicsClass##Ptr params = std::make_shared<IntrinsicsClass>(_unique_name, _server); \ + auto params = std::make_shared<IntrinsicsClass>(_unique_name, _server); \ \ - SensorClass##Ptr sensor = std::make_shared<SensorClass>(extrinsics, params); \ + auto sensor = std::make_shared<SensorClass>(extrinsics, params); \ \ - sensor ->setName(_unique_name); \ + sensor ->setName(_unique_name); \ \ return sensor; \ -} +} \ + \ +static \ +SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics, const IntrinsicsBasePtr _intrinsics) \ +{ \ + assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \ + \ + auto params = std::static_pointer_cast<IntrinsicsClass>(_intrinsics); \ + \ + auto sensor = std::make_shared<SensorClass>(_extrinsics, params); \ + \ + sensor ->setName(_unique_name); \ + \ + return sensor; \ +} \ diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h index 3a1d31afc7bfc6d862b158027fe5d8269803c87d..29866c85fc494c4a47e378ebf9726ddd0b8949a0 100644 --- a/include/core/sensor/sensor_diff_drive.h +++ b/include/core/sensor/sensor_diff_drive.h @@ -54,18 +54,12 @@ class SensorDiffDrive : public SensorBase public: SensorDiffDrive(const Eigen::VectorXs& _extrinsics, const IntrinsicsDiffDrivePtr& _intrinsics); + WOLF_SENSOR_CREATE(SensorDiffDrive, IntrinsicsDiffDrive, 3); virtual ~SensorDiffDrive(); IntrinsicsDiffDriveConstPtr getParams() const {return params_diff_drive_;} protected: IntrinsicsDiffDrivePtr params_diff_drive_; - - - public: - static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics); -// static SensorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server); - WOLF_CREATE_SENSOR_AUTO(SensorDiffDrive, IntrinsicsDiffDrive, 3); - }; } /* namespace wolf */ diff --git a/include/core/sensor/sensor_odom_2D.h b/include/core/sensor/sensor_odom_2D.h index 80936cfcdcbcfc8427c74336dc8052358b89b0f0..1c7211a6fdc434774b6f1789dd8f87993b77dcb3 100644 --- a/include/core/sensor/sensor_odom_2D.h +++ b/include/core/sensor/sensor_odom_2D.h @@ -43,6 +43,7 @@ class SensorOdom2D : public SensorBase public: SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics); SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics); + WOLF_SENSOR_CREATE(SensorOdom2D, IntrinsicsOdom2D, 3); virtual ~SensorOdom2D(); @@ -60,11 +61,6 @@ class SensorOdom2D : public SensorBase **/ Scalar getRotVarToRotNoiseFactor() const; - - public: - static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics); - static SensorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server); - }; } // namespace wolf diff --git a/include/core/sensor/sensor_odom_3D.h b/include/core/sensor/sensor_odom_3D.h index a6d7296a9aa44a711060ae3fc6aae394e2c0e2da..c41ffe72548d33b477d03074a7b4674b958c8b08 100644 --- a/include/core/sensor/sensor_odom_3D.h +++ b/include/core/sensor/sensor_odom_3D.h @@ -62,6 +62,7 @@ class SensorOdom3D : public SensorBase public: SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& params); SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr params); + WOLF_SENSOR_CREATE(SensorOdom3D, IntrinsicsOdom3D, 7); virtual ~SensorOdom3D(); @@ -71,9 +72,6 @@ class SensorOdom3D : public SensorBase Scalar getMinDispVar() const; Scalar getMinRotVar() const; - public: - static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics); - }; inline Scalar SensorOdom3D::getDispVarToDispNoiseFactor() const diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp index cb7ac7cf7671a48bd695f22e72843a0882727a59..d8c29515a1113fa65c88fb6ec83ceb29546030f8 100644 --- a/src/sensor/sensor_diff_drive.cpp +++ b/src/sensor/sensor_diff_drive.cpp @@ -30,53 +30,13 @@ SensorDiffDrive::~SensorDiffDrive() // TODO Auto-generated destructor stub } -// Define the factory method -SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name, const Eigen::VectorXs& extrinsics, - const IntrinsicsBasePtr _intrinsics) -{ - // extrinsics - assert(extrinsics.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D."); - - // intrinsics - assert(_intrinsics != nullptr && "Intrinsics provided are nullptr."); - IntrinsicsDiffDrivePtr params = std::static_pointer_cast<IntrinsicsDiffDrive>(_intrinsics); - - // build sensor - SensorDiffDrivePtr diff_drive = std::make_shared<SensorDiffDrive>(extrinsics, params); - - // last details - diff_drive->setName(_unique_name); - - return diff_drive; -} - -//SensorBasePtr SensorDiffDrive::createAutoConf(const std::string& _unique_name, const ParamsServer& _server) -//{ -// // extrinsics -// Eigen::VectorXs extrinsics = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pose"); -// assert(extrinsics.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D."); -// -// // intrinsics -// IntrinsicsDiffDrivePtr params = std::make_shared<IntrinsicsDiffDrive>(_unique_name, _server); -// -// // build sensor -// SensorDiffDrivePtr diff_drive = std::make_shared<SensorDiffDrive>(extrinsics, params); -// -// // last details -// diff_drive ->setName(_unique_name); -// -// return diff_drive; -//} } /* namespace wolf */ // Register in the SensorFactory #include "core/sensor/sensor_factory.h" namespace wolf { -WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive) -} // namespace wolf -#include "core/sensor/autoconf_sensor_factory.h" -namespace wolf { -WOLF_REGISTER_SENSOR_AUTO("DIFF DRIVE", SensorDiffDrive) +WOLF_REGISTER_SENSOR("DIFF DRIVE", SensorDiffDrive); +WOLF_REGISTER_SENSOR_AUTO("DIFF DRIVE", SensorDiffDrive); } // namespace wolf diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp index c96059e58b2c40358f75e0b520f9166944bd08ee..44d69e34f81044c6706d952c2f1eeff1b9cdc533 100644 --- a/src/sensor/sensor_odom_2D.cpp +++ b/src/sensor/sensor_odom_2D.cpp @@ -34,53 +34,11 @@ Scalar SensorOdom2D::getRotVarToRotNoiseFactor() const return k_rot_to_rot_; } -// Define the factory method -SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po, - const IntrinsicsBasePtr _intrinsics) -{ - // decode extrinsics vector - assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D."); - - SensorOdom2DPtr odo; - if (_intrinsics) - { - std::shared_ptr<IntrinsicsOdom2D> params = std::static_pointer_cast<IntrinsicsOdom2D>(_intrinsics); - odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params); - } - else - { - IntrinsicsOdom2D params; - params.k_disp_to_disp = 1; - params.k_rot_to_rot = 1; - odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params); - } - odo->setName(_unique_name); - return odo; -} - -SensorBasePtr SensorOdom2D::createAutoConf(const std::string& _unique_name, const ParamsServer& _server) -{ - // Eigen::VectorXs _extrinsics_po = Eigen::Vector3s(0,0,0); - Eigen::VectorXs _extrinsics_po = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pose"); - - assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D."); - - IntrinsicsOdom2DPtr params = std::make_shared<IntrinsicsOdom2D>(_unique_name, _server); - - SensorOdom2DPtr odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params); - odo->setName(_unique_name); - - return odo; -} - } // Register in the SensorFactory #include "core/sensor/sensor_factory.h" namespace wolf { -WOLF_REGISTER_SENSOR("ODOM 2D", SensorOdom2D) -} // namespace wolf -#include "core/sensor/autoconf_sensor_factory.h" -namespace wolf { -WOLF_REGISTER_SENSOR_AUTO("ODOM 2D", SensorOdom2D) +WOLF_REGISTER_SENSOR("ODOM 2D", SensorOdom2D); +WOLF_REGISTER_SENSOR_AUTO("ODOM 2D", SensorOdom2D); } // namespace wolf diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp index cf6431489efb45c0e0bf4599899bc9c850ebf902..04de2bbd642491004a640971dba01c99032c6607 100644 --- a/src/sensor/sensor_odom_3D.cpp +++ b/src/sensor/sensor_odom_3D.cpp @@ -37,27 +37,11 @@ SensorOdom3D::~SensorOdom3D() // } -// Define the factory method -SensorBasePtr SensorOdom3D::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, - const IntrinsicsBasePtr _intrinsics) -{ - // decode extrinsics vector - assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D."); - - // cast intrinsics into derived type - IntrinsicsOdom3DPtr params = std::static_pointer_cast<IntrinsicsOdom3D>(_intrinsics); - - // Call constructor and finish - SensorBasePtr odo = std::make_shared<SensorOdom3D>(_extrinsics_pq, params); - odo->setName(_unique_name); - - return odo; -} - } // namespace wolf // Register in the SensorFactory #include "core/sensor/sensor_factory.h" namespace wolf { -WOLF_REGISTER_SENSOR("ODOM 3D", SensorOdom3D) +WOLF_REGISTER_SENSOR("ODOM 3D", SensorOdom3D); +WOLF_REGISTER_SENSOR_AUTO("ODOM 3D", SensorOdom3D); } diff --git a/src/yaml/sensor_odom_2D_yaml.cpp b/src/yaml/sensor_odom_2D_yaml.cpp new file mode 100644 index 0000000000000000000000000000000000000000..99220af9bcb49ff574228c059cb09b8b6a8ee1a3 --- /dev/null +++ b/src/yaml/sensor_odom_2D_yaml.cpp @@ -0,0 +1,48 @@ +/** + * \file sensor_odom_2D_yaml.cpp + * + * Created on: Aug 3, 2019 + * \author: jsola + */ + + + +// wolf yaml +#include "core/yaml/yaml_conversion.h" + +// wolf +#include "core/sensor/sensor_odom_2D.h" + +// yaml-cpp library +#include <yaml-cpp/yaml.h> + +namespace wolf +{ + +namespace +{ +static IntrinsicsBasePtr createIntrinsicsOdom2D(const std::string & _filename_dot_yaml) +{ + YAML::Node config = YAML::LoadFile(_filename_dot_yaml); + + if (config["type"].as<std::string>() == "ODOM 2D") + { + auto params = std::make_shared<IntrinsicsOdom2D>(); + + params->k_disp_to_disp = config["k_disp_to_disp"] .as<Scalar>(); + params->k_rot_to_rot = config["k_rot_to_rot"] .as<Scalar>(); + + return params; + } + + std::cout << "Bad configuration file. No sensor type found." << std::endl; + return nullptr; +} + +// Register in the SensorFactory +const bool WOLF_UNUSED registered_odom_2D_intr = IntrinsicsFactory::get().registerCreator("ODOM 2D", createIntrinsicsOdom2D); + +} // namespace [unnamed] + +} // namespace wolf + diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp index 31c8dd77ae9ac1df078b4218c3ea2d389757aa42..4970ddc97e0e8f63079c9b55b82e2d75ec243c25 100644 --- a/test/gtest_odom_2D.cpp +++ b/test/gtest_odom_2D.cpp @@ -179,6 +179,8 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D) TEST(Odom2D, VoteForKfAndSolve) { + std::string wolf_root = _WOLF_ROOT_DIR; + std::cout << std::setprecision(4); // time TimeStamp t0(0.0), t = t0; @@ -193,7 +195,7 @@ TEST(Odom2D, VoteForKfAndSolve) // Create Wolf tree nodes ProblemPtr problem = Problem::create("PO", 2); - SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0)); + SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"); ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>()); params->voting_active = true; params->dist_traveled = 100; @@ -302,6 +304,8 @@ TEST(Odom2D, KF_callback) using std::cout; using std::endl; + std::string wolf_root = _WOLF_ROOT_DIR; + std::cout << std::setprecision(4); // time TimeStamp t0(0.0), t = t0; @@ -322,7 +326,7 @@ TEST(Odom2D, KF_callback) // Create Wolf tree nodes ProblemPtr problem = Problem::create("PO", 2); - SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0)); + SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"); ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>()); params->dist_traveled = 100; params->angle_turned = 6.28; diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 5c64c90b5dd35b0efa6bc85cc77d39b383b4940f..53ebf672d6104ba4bfba1484a4a87f5424c143f0 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -236,16 +236,16 @@ TEST(Problem, StateBlocks) SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml"); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2); - // 2 state blocks, fixed - SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, ""); - ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2)); +// // 2 state blocks, fixed +// SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, ""); +// ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 2)); auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer"); - auto pm = P->installProcessor("ODOM 3D", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml"); + auto pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml"); // 2 state blocks, estimated auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0); - ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2 + 2)); + ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); // Notifications Notification notif; @@ -256,14 +256,14 @@ TEST(Problem, StateBlocks) // P->print(4,1,1,1); // change some SB properties - St->unfixExtrinsics(); - ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2 + 2)); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE + Sm->unfixExtrinsics(); + ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE // P->print(4,1,1,1); // consume notifications DummySolverManagerPtr SM = std::make_shared<DummySolverManager>(P); - ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2 + 2)); + ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 2)); SM->update(); // calls P->consumeStateBlockNotificationMap(); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (0)); // consume empties the notification map @@ -287,15 +287,15 @@ TEST(Problem, Covariances) Eigen::Vector7s xs3d; Eigen::Vector3s xs2d; - SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml"); - SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, ""); + SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml"); + SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, wolf_root + "/test/yaml/sensor_odom_2D.yaml"); ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>(); params->time_tolerance = 0.1; params->max_new_features = 5; params->min_features_for_keyframe = 10; - auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", "odometer"); + auto pt = P->installProcessor("TRACKER FEATURE DUMMY", "dummy", Sm, params); auto pm = P->installProcessor("ODOM 3D", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3D.yaml"); // 4 state blocks, estimated diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 407ccf597b10b308d5cdb4933bcc26d56ea97126..36ad812454b7eb0f21c2997762e3a3ccc3096838 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -40,6 +40,8 @@ TEST(ProcessorBase, KeyFrameCallback) using std::static_pointer_cast; using Eigen::Vector2s; + std::string wolf_root = _WOLF_ROOT_DIR; + Scalar dt = 0.01; // Wolf problem @@ -54,7 +56,7 @@ TEST(ProcessorBase, KeyFrameCallback) auto proc_trk = problem->installProcessor("TRACKER FEATURE DUMMY", "dummy", sens_trk); // Install odometer (sensor and processor) - SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), ""); + SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"); ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>(); proc_odo_params->time_tolerance = dt/2; ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 2D", "odom processor", sens_odo, proc_odo_params); diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index e27e5cfbfe531427802ea12c40a157875596afbc..2c02ccf5764fc2555d0b922efdcdb0e14c8d29a1 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -35,6 +35,8 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{ virtual void SetUp() { + std::string wolf_root = _WOLF_ROOT_DIR; + dt = 1.0; problem = Problem::create("PO", 2); ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>()); @@ -44,7 +46,7 @@ class ProcessorMotion_test : public ProcessorOdom2D, public testing::Test{ params->max_time_span = 2.5*dt; // force KF at every third process() params->cov_det = 100; params->unmeasured_perturbation_std = 0.001; - sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0))); + sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml")); processor = static_pointer_cast<ProcessorOdom2D>(problem->installProcessor("ODOM 2D", "odom", sensor, params)); capture = std::make_shared<CaptureMotion>("ODOM 2D", 0.0, sensor, data, data_cov, 3, 3, nullptr); diff --git a/test/yaml/sensor_odom_2D.yaml b/test/yaml/sensor_odom_2D.yaml new file mode 100644 index 0000000000000000000000000000000000000000..40677f3e1cb84d7eda527e942bd54880c0ac7e37 --- /dev/null +++ b/test/yaml/sensor_odom_2D.yaml @@ -0,0 +1,4 @@ +type: "ODOM 2D" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +k_disp_to_disp: 0.02 # m^2 / m +k_rot_to_rot: 0.01 # rad^2 / rad