Skip to content
Snippets Groups Projects
Commit 72bf7682 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

[skip ci] wip yaml server

parent fffffcb7
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Pipeline #11960 skipped
Hide whitespace changes
Inline Side-by-side
Showing
with 34 additions and 120 deletions
include/core/sensor/sensor_base.h
+ 8
− 5
View file @ 72bf7682
......@@ -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);
......
include/core/sensor/sensor_diff_drive.h
+ 6
− 11
View file @ 72bf7682
......@@ -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;
......
include/core/sensor/sensor_motion_model.h
+ 0
− 5
View file @ 72bf7682
......@@ -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;
......
include/core/sensor/sensor_odom.h
+ 7
− 7
View file @ 72bf7682
......@@ -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
......
include/core/sensor/sensor_pose.h
+ 4
− 9
View file @ 72bf7682
......@@ -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;
......
src/problem/problem.cpp
+ 0
− 1
View file @ 72bf7682
......@@ -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);
......
src/sensor/sensor_base.cpp
+ 1
− 6
View file @ 72bf7682
......@@ -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);
......
src/sensor/sensor_diff_drive.cpp
+ 5
− 26
View file @ 72bf7682
......@@ -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()
{
}
......
src/sensor/sensor_motion_model.cpp
+ 1
− 15
View file @ 72bf7682
......@@ -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()
{
}
......
src/sensor/sensor_odom.cpp
+ 1
− 18
View file @ 72bf7682
......@@ -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
......
src/sensor/sensor_pose.cpp
+ 1
− 17
View file @ 72bf7682
......@@ -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()
{
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment