diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index e74d8cc6e8ee6acdb779f646b803ce2f7ce28a97..140cc7483d65d4c16e8ba6fa67120b5a3801f0c6 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -9,6 +9,7 @@
#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
#include "base/sensor/sensor_base.h"
+#include "base/params_server.hpp"
namespace wolf
{
@@ -18,6 +19,12 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase
{
Scalar noise_range_metres_std = 0.05;
Scalar noise_bearing_degrees_std = 0.5;
+ IntrinsicsRangeBearing(std::string _unique_name, const paramsServer& _server):
+ IntrinsicsBase(_unique_name, _server)
+ {
+ noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise_range_metres_std", "0.05");
+ noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise_bearing_degrees_std", "0.5");
+ }
};
WOLF_PTR_TYPEDEFS(SensorRangeBearing)
diff --git a/include/base/sensor/sensor_GPS.h b/include/base/sensor/sensor_GPS.h
index 17b6f20331e6e990b876cb52b1b83277d404f404..046c5fd94f34ea80d357bc772a9d9e901cfa7d91 100644
--- a/include/base/sensor/sensor_GPS.h
+++ b/include/base/sensor/sensor_GPS.h
@@ -20,6 +20,7 @@ namespace wolf {
struct IntrinsicsGPS : public IntrinsicsBase
{
// add GPS parameters here
+ using IntrinsicsBase::IntrinsicsBase;
virtual ~IntrinsicsGPS() = default;
};
diff --git a/include/base/sensor/sensor_GPS_fix.h b/include/base/sensor/sensor_GPS_fix.h
index 17db7c12c11574c9be64e7adac312d8a671587af..e1f443f8a22d218813bc0e9d48e8df8aeec1b320 100644
--- a/include/base/sensor/sensor_GPS_fix.h
+++ b/include/base/sensor/sensor_GPS_fix.h
@@ -4,6 +4,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
+#include "base/params_server.hpp"
// std includes
@@ -14,6 +15,11 @@ struct IntrinsicsGPSFix : public IntrinsicsBase
{
Eigen::Vector3s noise_std;
// Empty -- it acts only as a typedef for IntrinsicsBase, but allows future extension with parameters
+ IntrinsicsGPSFix(std::string _unique_name, const paramsServer& _server):
+ IntrinsicsBase(_unique_name, _server)
+ {
+ noise_std = _server.getParam<Eigen::Vector3s>(_unique_name + "/noise_std", "");
+ }
virtual ~IntrinsicsGPSFix() = default;
};
diff --git a/include/base/sensor/sensor_IMU.h b/include/base/sensor/sensor_IMU.h
index 2c8efa2c9312f62bca4d9d1c2578a458914403bb..6e67632aa21d3800136c78e5df427e0734410aad 100644
--- a/include/base/sensor/sensor_IMU.h
+++ b/include/base/sensor/sensor_IMU.h
@@ -3,6 +3,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
+#include "base/params_server.hpp"
#include <iostream>
namespace wolf {
@@ -27,6 +28,16 @@ struct IntrinsicsIMU : public IntrinsicsBase
Scalar wb_rate_stdev = 0.00001;
virtual ~IntrinsicsIMU() = default;
+ IntrinsicsIMU(std::string _unique_name, const paramsServer& _server):
+ IntrinsicsBase(_unique_name, _server)
+ {
+ w_noise = _server.getParam<Scalar>(_unique_name + "/w_noise", "0.001");
+ a_noise = _server.getParam<Scalar>(_unique_name + "/a_noise", "0.004");
+ ab_initial_stdev = _server.getParam<Scalar>(_unique_name + "/ab_initial_stdev", "0.01");
+ wb_initial_stdev = _server.getParam<Scalar>(_unique_name + "/wb_initial_stdev", "0.01");
+ ab_rate_stdev = _server.getParam<Scalar>(_unique_name + "/ab_rate_stdev", "0.00001");
+ wb_rate_stdev = _server.getParam<Scalar>(_unique_name + "/wb_rate_stdev", "0.00001");
+ }
};
WOLF_PTR_TYPEDEFS(SensorIMU);
diff --git a/include/base/sensor/sensor_camera.h b/include/base/sensor/sensor_camera.h
index c7db047c3c28808c9dd40374bdf93adcdd894a2f..a4961ae7f45ef7aa5fda18e8c79e0bd56cfd8f1f 100644
--- a/include/base/sensor/sensor_camera.h
+++ b/include/base/sensor/sensor_camera.h
@@ -3,6 +3,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
+#include "base/params_server.hpp"
namespace wolf
{
@@ -18,6 +19,15 @@ struct IntrinsicsCamera : public IntrinsicsBase
Eigen::Vector4s pinhole_model_rectified;///< k = [u_0, v_0, alpha_u, alpha_v] vector of pinhole intrinsic parameters
Eigen::VectorXs distortion; ///< d = [d_1, d_2, d_3, ...] radial distortion coefficients
+ IntrinsicsCamera(std::string _unique_name, const paramsServer& _server):
+ IntrinsicsBase(_unique_name, _server)
+ {
+ width = _server.getParam<unsigned int>(_unique_name + "/width", "");
+ height = _server.getParam<unsigned int>(_unique_name + "/height", "");
+ pinhole_model_raw = _server.getParam<Eigen::Vector4s>(_unique_name + "/pinhole_model_raw", "");
+ pinhole_model_rectified = _server.getParam<Eigen::Vector4s>(_unique_name + "/pinhole_model_rectified", "");
+ distortion = _server.getParam<Eigen::VectorXs>(_unique_name + "/distortion", "");
+ }
virtual ~IntrinsicsCamera() = default;
};
diff --git a/include/base/sensor/sensor_diff_drive.h b/include/base/sensor/sensor_diff_drive.h
index 0f5cf42592ddf2cf1929ff6e31a9aa294a1963e4..1ca24f1b9020cf1753bb1110319453febef3be2f 100644
--- a/include/base/sensor/sensor_diff_drive.h
+++ b/include/base/sensor/sensor_diff_drive.h
@@ -11,6 +11,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
#include "base/diff_drive_tools.h"
+#include "base/params_server.hpp"
namespace wolf {
@@ -32,7 +33,28 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase
Scalar left_gain_ = 0.01;
Scalar right_gain_ = 0.01;
+ IntrinsicsDiffDrive(std::string _unique_name, const paramsServer& _server):
+ IntrinsicsBase(_unique_name, _server)
+ {
+ left_radius_ = _server.getParam<Scalar>(_unique_name + "/left_radius_", "");
+ right_radius_ = _server.getParam<Scalar>(_unique_name + "/right_radius_", "");
+ separation_ = _server.getParam<Scalar>(_unique_name + "/separation_", "");
+
+ auto model_str = _server.getParam<std::string>(_unique_name + "/model", "");
+ if(model_str.compare("Two_Factor_Model")) model_ = DiffDriveModel::Two_Factor_Model;
+ else if(model_str.compare("Three_Factor_Model")) model_ = DiffDriveModel::Three_Factor_Model;
+ else if(model_str.compare("Five_Factor_Model")) model_ = DiffDriveModel::Five_Factor_Model;
+ else throw std::runtime_error("Failed to fetch a valid value for the enumerate DiffDriveModel. Value provided: " + model_str);
+
+ factors_ = _server.getParam<Eigen::VectorXs>(_unique_name + "/factors", "[1,1,1]");
+
+ left_resolution_ = _server.getParam<Scalar>(_unique_name + "/left_resolution_", "");
+ right_resolution_ = _server.getParam<Scalar>(_unique_name + "/right_resolution_", "");
+
+ left_gain_ = _server.getParam<Scalar>(_unique_name + "/left_gain", "0.01");
+ right_gain_ = _server.getParam<Scalar>(_unique_name + "/right_gain", "0.01");
+ }
virtual ~IntrinsicsDiffDrive() = default;
};
diff --git a/include/base/sensor/sensor_odom_2D.h b/include/base/sensor/sensor_odom_2D.h
index 798d1bbdd314e8e70725dd1f135c2c117f6e9850..c489bde2ededa14c1fb79065f28c9c2260945abd 100644
--- a/include/base/sensor/sensor_odom_2D.h
+++ b/include/base/sensor/sensor_odom_2D.h
@@ -15,6 +15,12 @@ struct IntrinsicsOdom2D : public IntrinsicsBase
Scalar k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
Scalar k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
+ IntrinsicsOdom2D(std::string _unique_name, const paramsServer& _server):
+ IntrinsicsBase(_unique_name, _server)
+ {
+ k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp", "");
+ k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot", "");
+ }
};
WOLF_PTR_TYPEDEFS(SensorOdom2D);
diff --git a/include/base/sensor/sensor_odom_3D.h b/include/base/sensor/sensor_odom_3D.h
index be3694b2da8b69ad7201d57251f0246deb6dba92..a88b2aed2511cab3ee4a9dee5ebbf80f8482fffb 100644
--- a/include/base/sensor/sensor_odom_3D.h
+++ b/include/base/sensor/sensor_odom_3D.h
@@ -10,6 +10,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
+#include "base/params_server.hpp"
namespace wolf {
@@ -22,7 +23,15 @@ struct IntrinsicsOdom3D : public IntrinsicsBase
Scalar k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
Scalar min_disp_var;
Scalar min_rot_var;
-
+ IntrinsicsOdom3D(std::string _unique_name, const paramsServer& _server):
+ IntrinsicsBase(_unique_name, _server)
+ {
+ k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp", "");
+ k_disp_to_rot = _server.getParam<Scalar>(_unique_name + "/k_disp_to_rot", "");
+ k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot", "");
+ min_disp_var = _server.getParam<Scalar>(_unique_name + "/min_disp_var", "");
+ min_rot_var = _server.getParam<Scalar>(_unique_name + "/min_rot_var", "");
+ }
virtual ~IntrinsicsOdom3D() = default;
};