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()
{