From a00c7ca97c3a452069c646981c4867860dd27eba Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Tue, 19 Mar 2019 09:25:12 +0100
Subject: [PATCH] Added auto_conf for sensors. Missing Sensor_Laser_2D
---
hello_wolf/sensor_range_bearing.h | 7 +++++++
include/base/sensor/sensor_GPS.h | 1 +
include/base/sensor/sensor_GPS_fix.h | 6 ++++++
include/base/sensor/sensor_IMU.h | 11 +++++++++++
include/base/sensor/sensor_camera.h | 10 ++++++++++
include/base/sensor/sensor_diff_drive.h | 22 ++++++++++++++++++++++
include/base/sensor/sensor_odom_2D.h | 6 ++++++
include/base/sensor/sensor_odom_3D.h | 11 ++++++++++-
8 files changed, 73 insertions(+), 1 deletion(-)
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index e74d8cc6e..140cc7483 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 17b6f2033..046c5fd94 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 17db7c12c..e1f443f8a 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 2c8efa2c9..6e67632aa 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 c7db047c3..a4961ae7f 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 0f5cf4259..1ca24f1b9 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 798d1bbdd..c489bde2e 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 be3694b2d..a88b2aed2 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;
};
--
GitLab