diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 605400d49e6a44b4acc85d264e1404c574c581c1..cd4f7acdc3715d318b5048532ccf090bff9aa567 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -37,7 +37,8 @@ class ParamsServer; #include "core/common/time_stamp.h" #include "core/common/params_base.h" #include "core/state_block/has_state_blocks.h" -#include "core/yaml/parser_yaml.h" +//#include "core/yaml/parser_yaml.h" +#include "yaml-schema-cpp/yaml_schema.h" //std includes @@ -67,7 +68,10 @@ static SensorBasePtr create(SizeEigen _dim, const std::vector<std::string>& folders_schema) \ { \ std::stringstream log; \ - if (not checkNode(_input_node, SensorClass, folders_schema, log)) \ + if (not yaml-schema-cpp::checkNode(_input_node, \ + SensorClass, \ + folders_schema, \ + log)) \ { \ WOLF_ERROR(log.str()); \ return nullptr; \ @@ -124,12 +128,13 @@ struct ParamsSensorBase: public ParamsBase ParamsSensorBase() = default; ParamsSensorBase(const YAML::Node& _input_node) { + name = _input_node["name"].as<std::string>(); } ~ParamsSensorBase() override = default; std::string print() const override { - return ""; + return "name: " + name; } }; @@ -167,14 +172,12 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh * * Constructor with parameter vector * \param _tp Type of the sensor (types defined at wolf.h) - * \param _unique_name Name of the sensor * \param _dim Problem dimension * \param _params params struct * \param _priors priors of the sensor state blocks * **/ SensorBase(const std::string& _type, - const std::string& _unique_name, const SizeEigen& _dim, ParamsSensorBasePtr _params, const Priors& _priors); diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h index c122164275a29675a1f617363dc6b74ccc036b90..9cfd61024cfa411019a8e808230f5b2206cd187b 100644 --- a/include/core/sensor/sensor_diff_drive.h +++ b/include/core/sensor/sensor_diff_drive.h @@ -32,15 +32,15 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDiffDrive); struct ParamsSensorDiffDrive : public ParamsSensorBase { - double ticks_per_wheel_revolution; - double ticks_std_factor; + double ticks_per_wheel_revolution; + double ticks_std_factor; ParamsSensorDiffDrive() = default; - ParamsSensorDiffDrive(std::string _unique_name, const wolf::ParamsServer& _server) - : ParamsSensorBase(_unique_name, _server) + ParamsSensorDiffDrive(const YAML::Node& _input_node) + : ParamsSensorBase(_input_node) { - ticks_per_wheel_revolution = _server.getParam<double>(prefix + _unique_name + "/ticks_per_wheel_revolution"); - ticks_std_factor = _server.getParam<double>(prefix + _unique_name + "/ticks_std_factor"); + ticks_per_wheel_revolution = _input_node["ticks_per_wheel_revolution"].as<double>(); + ticks_std_factor = _input_node["ticks_std_factor"].as<<double>(); } ~ParamsSensorDiffDrive() = default; std::string print() const override @@ -63,11 +63,6 @@ class SensorDiffDrive : public SensorBase ParamsSensorDiffDrivePtr _params, const Priors& _priors); - SensorDiffDrive(const std::string& _unique_name, - const SizeEigen& _dim, - ParamsSensorDiffDrivePtr _params, - const ParamsServer& _server); - WOLF_SENSOR_CREATE(SensorDiffDrive, ParamsSensorDiffDrive); ~SensorDiffDrive() override; diff --git a/include/core/sensor/sensor_motion_model.h b/include/core/sensor/sensor_motion_model.h index f56ff1d72e1a322a1280039c74194e188cad3fbb..90612cc8bea6af53db9d95eb542e213795348057 100644 --- a/include/core/sensor/sensor_motion_model.h +++ b/include/core/sensor/sensor_motion_model.h @@ -37,11 +37,6 @@ class SensorMotionModel : public SensorBase ParamsSensorBasePtr _params, const Priors& _priors); - SensorMotionModel(const std::string& _unique_name, - const SizeEigen& _dim, - ParamsSensorBasePtr _params, - const ParamsServer& _server); - WOLF_SENSOR_CREATE(SensorMotionModel, ParamsSensorBase); ~SensorMotionModel() override; diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h index 41590e1cf76f6aca49d1f2a9d07b13b63d9d1dec..bcba531776bf9a4e6c53afa2296d21f66144de24 100644 --- a/include/core/sensor/sensor_odom.h +++ b/include/core/sensor/sensor_odom.h @@ -39,14 +39,14 @@ struct ParamsSensorOdom : public ParamsSensorBase double min_rot_var; ParamsSensorOdom() = default; - ParamsSensorOdom(std::string _unique_name, const ParamsServer& _server): - ParamsSensorBase(_unique_name, _server) + ParamsSensorOdom(const YAML::Node& _input_node): + ParamsSensorBase(_input_node) { - k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp"); - k_disp_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_rot"); - k_rot_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot"); - min_disp_var = _server.getParam<double>(prefix + _unique_name + "/min_disp_var"); - min_rot_var = _server.getParam<double>(prefix + _unique_name + "/min_rot_var"); + k_disp_to_disp = _input_node["k_disp_to_disp"].as<double>(); + k_disp_to_rot = _input_node["k_disp_to_rot"].as<double>(); + k_rot_to_rot = _input_node["k_rot_to_rot"].as<double>(); + min_disp_var = _input_node["min_disp_var"].as<double>(); + min_rot_var = _input_node["min_rot_var"].as<double>(); } ~ParamsSensorOdom() override = default; std::string print() const override diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h index 344de02c9d75e6d9c4a0633eda20a683b6f92a33..022c44cf4bb702deeb90cc02660830f78cda7bb4 100644 --- a/include/core/sensor/sensor_pose.h +++ b/include/core/sensor/sensor_pose.h @@ -42,11 +42,11 @@ struct ParamsSensorPose : public ParamsSensorBase double std_p = 1; // m double std_o = 1; // rad ParamsSensorPose() = default; - ParamsSensorPose(std::string _unique_name, const ParamsServer& _server): - ParamsSensorBase(_unique_name, _server) + ParamsSensorPose(const YAML::Node& _input_node): + ParamsSensorBase(_input_node) { - std_p = _server.getParam<double>(prefix + _unique_name + "/std_p"); - std_o = _server.getParam<double>(prefix + _unique_name + "/std_o"); + std_p = _input_node["std_p"].as<double>(); + std_o = _input_node["std_o"].as<double>(); } ~ParamsSensorPose() override = default; std::string print() const override @@ -71,11 +71,6 @@ class SensorPose : public SensorBase ParamsSensorPosePtr _params, const Priors& _priors); - SensorPose(const std::string& _unique_name, - const SizeEigen& _dim, - ParamsSensorPosePtr _params, - const ParamsServer& _server); - WOLF_SENSOR_CREATE(SensorPose, ParamsSensorPose); ~SensorPose() override; diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 70e80e1ce766a5fee328f1a9ffeab3dd5ddc9d91..ba54241a29820880703c00f2a799caa7178d0f66 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -126,7 +126,6 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server) catch (MissingValueException& e) { WOLF_TRACE("No raw libraries to load..."); - raw_libs = std::vector<std::string>(); } for (auto lib : raw_libs) { WOLF_TRACE("Loading raw lib " + lib); diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 8f61345d8622d3417eef1bc2eac870477aa6290b..394cf9b6e7d667096b80b1b55dcf840e37b6372a 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -34,11 +34,10 @@ namespace wolf { unsigned int SensorBase::sensor_id_count_ = 0; SensorBase::SensorBase(const std::string& _type, - const std::string& _unique_name, const SizeEigen& _dim, ParamsSensorBasePtr _params, const Priors& _priors) : - NodeBase("SENSOR", _type, _unique_name), + NodeBase("SENSOR", _type, _params["name"].as<std::string>()), HasStateBlocks("PO"), hardware_ptr_(), sensor_id_(++sensor_id_count_), // simple ID factory @@ -48,10 +47,6 @@ SensorBase::SensorBase(const std::string& _type, last_capture_(nullptr) { assert(_dim == 2 or _dim == 3); - - // check params valid - if (not params_) - throw std::runtime_error("SensorBase: in constructor, ParamsSensorBase pointer is null"); // load priors loadPriors(_priors, _dim); diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp index 4ed4d49589bc92b8a1e5976b8217df0796adb621..89151dd40e548f0e0eb8a232f74a9358cb976d5e 100644 --- a/src/sensor/sensor_diff_drive.cpp +++ b/src/sensor/sensor_diff_drive.cpp @@ -32,20 +32,17 @@ namespace wolf { -SpecStates specs_states_diff_drive({{'I',SpecState("StateBlock", - 3, - "0: left wheel radius (m), 1: right wheel radius (m), 2: wheel separation (m)")}}); +// SpecStates specs_states_diff_drive({{'I',SpecState("StateBlock", +// 3, +// "0: left wheel radius (m), 1: right wheel radius (m), 2: wheel separation (m)")}}); -SensorDiffDrive::SensorDiffDrive(const std::string& _unique_name, - const SizeEigen& _dim, +SensorDiffDrive::SensorDiffDrive(const SizeEigen& _dim, ParamsSensorDiffDrivePtr _params, const Priors& _priors) : SensorBase("SensorDiffDrive", - _unique_name, _dim, _params, - _priors, - specs_states_diff_drive), + _priors), params_diff_drive_(_params) { assert(_dim == 2 and "SensorDiffDrive only defined for 2D problems"); @@ -54,24 +51,6 @@ SensorDiffDrive::SensorDiffDrive(const std::string& _unique_name, throw std::runtime_error("SensorDiffDrive in constructor, provided state for 'I' of wrong size. Is should be 3"); } -SensorDiffDrive::SensorDiffDrive(const std::string& _unique_name, - const SizeEigen& _dim, - ParamsSensorDiffDrivePtr _params, - const ParamsServer& _server) : - SensorBase("SensorDiffDrive", - _unique_name, - _dim, - _params, - _server, - specs_states_diff_drive), - params_diff_drive_(_params) -{ - assert(_dim == 2 and "SensorDiffDrive only defined for 2D problems"); - - if (this->getStateBlockDynamic('I')->getState().size() != 3) - throw std::runtime_error("SensorDiffDrive in constructor, provided state for 'I' of wrong size. It should be 3"); -} - SensorDiffDrive::~SensorDiffDrive() { } diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp index 167d33528306b2e8791127df59d640e2786eccf2..a3711abd5e857f7bd262caf6f6bbd34b2aba3a90 100644 --- a/src/sensor/sensor_motion_model.cpp +++ b/src/sensor/sensor_motion_model.cpp @@ -23,30 +23,16 @@ namespace wolf { -SensorMotionModel::SensorMotionModel(const std::string& _unique_name, - const SizeEigen& _dim, +SensorMotionModel::SensorMotionModel(const SizeEigen& _dim, ParamsSensorBasePtr _params, const Priors& _priors) : SensorBase("SensorMotionModel", - _unique_name, _dim, _params, _priors) { } -SensorMotionModel::SensorMotionModel(const std::string& _unique_name, - const SizeEigen& _dim, - ParamsSensorBasePtr _params, - const ParamsServer& _server) : - SensorBase("SensorMotionModel", - _unique_name, - _dim, - _params, - _server) -{ -} - SensorMotionModel::~SensorMotionModel() { } diff --git a/src/sensor/sensor_odom.cpp b/src/sensor/sensor_odom.cpp index 4d4a1ed6a44a9fa6af1c8bfcd08b5f69e08c452e..e216ba19d9ff4509d0b5b7b4a751e18242317741 100644 --- a/src/sensor/sensor_odom.cpp +++ b/src/sensor/sensor_odom.cpp @@ -23,12 +23,10 @@ namespace wolf { -SensorOdom::SensorOdom(const std::string& _unique_name, - const SizeEigen& _dim, +SensorOdom::SensorOdom(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors) : SensorBase("SensorOdom", - _unique_name, _dim, _params, _priors), @@ -38,21 +36,6 @@ SensorOdom::SensorOdom(const std::string& _unique_name, assert(dim_ == 2 or dim_ == 3); } -SensorOdom::SensorOdom(const std::string& _unique_name, - const SizeEigen& _dim, - ParamsSensorOdomPtr _params, - const ParamsServer& _server) : - SensorBase("SensorOdom", - _unique_name, - _dim, - _params, - _server), - dim_(_dim), - params_odom_(_params) -{ - assert(dim_ == 2 or dim_ == 3); -} - Eigen::MatrixXd SensorOdom::computeNoiseCov(const Eigen::VectorXd & _data) const { double d; // displacement diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp index 4eb43f29104b6b8b01194d1064190dc23d4d1daf..7af50dc3ebac9fc7d47e23814edc4b2f13afa000 100644 --- a/src/sensor/sensor_pose.cpp +++ b/src/sensor/sensor_pose.cpp @@ -30,12 +30,10 @@ namespace wolf { -SensorPose::SensorPose(const std::string& _unique_name, - const SizeEigen& _dim, +SensorPose::SensorPose(const SizeEigen& _dim, ParamsSensorPosePtr _params, const Priors& _priors) : SensorBase("SensorPose", - _unique_name, _dim, _params, _priors), @@ -44,20 +42,6 @@ SensorPose::SensorPose(const std::string& _unique_name, { } -SensorPose::SensorPose(const std::string& _unique_name, - const SizeEigen& _dim, - ParamsSensorPosePtr _params, - const ParamsServer& _server) : - SensorBase("SensorPose", - _unique_name, - _dim, - _params, - _server), - dim_(_dim), - params_pose_(_params) -{ -} - SensorPose::~SensorPose() {