From 5fa0b9fd9147cb4ed66cf698b31f951a2ab6cfa8 Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Mon, 17 Jun 2019 11:53:17 +0200
Subject: [PATCH] All parameters objects should be able to fill themselves from
params server.
---
hello_wolf/processor_range_bearing.h | 13 +++++++++++++
hello_wolf/sensor_range_bearing.h | 5 +++++
include/core/common/wolf.h | 4 ++++
include/core/processor/processor_base.h | 7 +++++++
.../core/processor/processor_capture_holder.h | 10 ++++++++++
include/core/processor/processor_diff_drive.h | 4 ++++
include/core/processor/processor_motion.h | 9 +++++++++
include/core/processor/processor_odom_2D.h | 7 +++++--
include/core/processor/processor_odom_3D.h | 4 ++++
include/core/processor/processor_tracker.h | 9 +++++++++
include/core/sensor/sensor_base.h | 6 +++++-
include/core/sensor/sensor_diff_drive.h | 18 ++++++++++++++++++
include/core/sensor/sensor_odom_2D.h | 5 +++++
include/core/sensor/sensor_odom_3D.h | 8 ++++++++
14 files changed, 106 insertions(+), 3 deletions(-)
diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h
index 00592e5d6..a6b2e7020 100644
--- a/hello_wolf/processor_range_bearing.h
+++ b/hello_wolf/processor_range_bearing.h
@@ -22,6 +22,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsRangeBearing);
struct ProcessorParamsRangeBearing : public ProcessorParamsBase
{
// We do not need special parameters, but in case you need they should be defined here.
+ ProcessorParamsRangeBearing()
+ {
+ //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+ }
+ ProcessorParamsRangeBearing(std::string _unique_name, const paramsServer& _server):
+ ProcessorParamsBase(_unique_name, _server)
+ {
+ //
+ }
+ std::string print()
+ {
+ return "\n" + ProcessorParamsBase::print();
+ }
};
using namespace Eigen;
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index 5b80ff6b8..587259b40 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -29,6 +29,11 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase
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");
}
+ std::string print()
+ {
+ return "\n" + IntrinsicsBase::print() + "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n"
+ + "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n";
+ }
};
WOLF_PTR_TYPEDEFS(SensorRangeBearing)
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index d3e9e1ef4..d79f9e143 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -375,6 +375,10 @@ struct ParamsBase
}
virtual ~ParamsBase() = default;
+ std::string print()
+ {
+ return "";
+ }
};
} // namespace wolf
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 3955b3cb3..372c25282 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -122,6 +122,7 @@ struct ProcessorParamsBase : public ParamsBase
ParamsBase(_unique_name, _server)
{
voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false");
+ voting_aux_active = _server.getParam<bool>(_unique_name + "/voting_aux_active", "false");
time_tolerance = _server.getParam<Scalar>(_unique_name + "/time_tolerance", "0");
}
@@ -134,6 +135,12 @@ struct ProcessorParamsBase : public ParamsBase
/// a particular Capture of this processor to allow assigning
/// this Capture to the Keyframe.
Scalar time_tolerance = Scalar(0);
+ std::string print()
+ {
+ return ParamsBase::print() + "\n" + "voting_active: " + std::to_string(voting_active) + "\n"
+ + "voting_aux_active: " + std::to_string(voting_aux_active) + "\n"
+ + "time_tolerance: " + std::to_string(time_tolerance) + "\n";
+ }
};
//class ProcessorBase
diff --git a/include/core/processor/processor_capture_holder.h b/include/core/processor/processor_capture_holder.h
index 1746fd548..46fc538d9 100644
--- a/include/core/processor/processor_capture_holder.h
+++ b/include/core/processor/processor_capture_holder.h
@@ -24,6 +24,16 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsCaptureHolder);
struct ProcessorParamsCaptureHolder : public ProcessorParamsBase
{
Scalar buffer_size = 30;
+ ProcessorParamsCaptureHolder() = default;
+ ProcessorParamsCaptureHolder(std::string _unique_name, const wolf::paramsServer & _server):
+ ProcessorParamsBase(_unique_name, _server)
+ {
+ buffer_size = _server.getParam<Scalar>(_unique_name + "/buffer_size");
+ }
+ std::string print()
+ {
+ return "\n" + ProcessorParamsBase::print() + "buffer_size: " + std::to_string(buffer_size) + "\n";
+ }
};
/**
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 2b5f1a494..60fbf8d29 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -25,6 +25,10 @@ struct ProcessorParamsDiffDrive : public ProcessorParamsMotion
{
unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.0001");
}
+ std::string print()
+ {
+ return "\n" + ProcessorParamsMotion::print() + "unmeasured_perturbation_std: " + std::to_string(unmeasured_perturbation_std) + "\n";
+ }
};
/**
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 5b971769b..d6cacdccd 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -39,6 +39,15 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
angle_turned = _server.getParam<Scalar>(_unique_name + "/angle_turned", "0.5");
unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "1e-4");
}
+ std::string print()
+ {
+ return "\n" + ProcessorParamsBase::print() + "max_time_span: " + std::to_string(max_time_span) + "\n"
+ + "max_buff_length: " + std::to_string(max_buff_length) + "\n"
+ + "dist_traveled: " + std::to_string(dist_traveled) + "\n"
+ + "angle_turned: " +std::to_string(angle_turned) + "\n"
+ + "unmeasured_perturbation_std: " + std::to_string(unmeasured_perturbation_std) + "\n";
+ }
+
};
/** \brief class for Motion processors
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index 90d47452d..fdcf82085 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -26,9 +26,12 @@ struct ProcessorParamsOdom2D : public ProcessorParamsMotion
ProcessorParamsOdom2D(std::string _unique_name, const wolf::paramsServer & _server):
ProcessorParamsMotion(_unique_name, _server)
{
- cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0");
- unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.001");
+ cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0");
}
+ std::string print()
+ {
+ return "\n" + ProcessorParamsMotion::print() + "cov_det: " + std::to_string(cov_det) + "\n";
+ }
};
class ProcessorOdom2D : public ProcessorMotion
{
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h
index 38683c57f..b5fc2dedf 100644
--- a/include/core/processor/processor_odom_3D.h
+++ b/include/core/processor/processor_odom_3D.h
@@ -27,6 +27,10 @@ struct ProcessorParamsOdom3D : public ProcessorParamsMotion
{
//
}
+ std::string print()
+ {
+ return "\n" + ProcessorParamsMotion::print();
+ }
};
WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 56be18b64..9f85d65f6 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -27,6 +27,12 @@ struct ProcessorParamsTracker : public ProcessorParamsBase
min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe", "1");
max_new_features = _server.getParam<int>(_unique_name + "/max_new_features", "-1");
}
+ std::string print()
+ {
+ return ProcessorParamsBase::print() + "\n" + "min_features_for_keyframe: " + std::to_string(min_features_for_keyframe) + "\n"
+ + "max_new_features: " + std::to_string(max_new_features) + "\n";
+ }
+
};
WOLF_PTR_TYPEDEFS(ProcessorTracker);
@@ -213,6 +219,9 @@ class ProcessorTracker : public ProcessorBase
{
return number_of_tracks_;
}
+ std::string print(){
+ return this->params_tracker_->print();
+ }
protected:
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 2e05d2fe5..9002ae8fe 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -23,8 +23,12 @@ namespace wolf {
*/
struct IntrinsicsBase: public ParamsBase
{
- virtual ~IntrinsicsBase() = default;
+ virtual ~IntrinsicsBase() = default;
using ParamsBase::ParamsBase;
+ std::string print()
+ {
+ return "";
+ }
};
class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase>
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index c1cbd7c38..114be0e4d 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -61,6 +61,24 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase
right_gain_ = _server.getParam<Scalar>(_unique_name + "/right_gain", "0.01");
}
virtual ~IntrinsicsDiffDrive() = default;
+ std::string print()
+ {
+ std::string model_string;
+
+ if(model_ == DiffDriveModel::Two_Factor_Model) model_string = "Two Factor Model";
+ else if(model_ == DiffDriveModel::Three_Factor_Model) model_string = "Three Factor Model";
+ else if(model_ == DiffDriveModel::Five_Factor_Model) model_string = "Five Factor Model";
+
+ return "\n" + IntrinsicsBase::print() + "left_radius: " + std::to_string(left_radius_) + "\n"
+ + "right_radius: " + std::to_string(right_radius_) + "\n"
+ + "separation_: " + std::to_string(separation_) + "\n"
+ + "model_string: " + model_string + "\n"
+ + "factors_: " + converter<std::string>::convert(factors_) + "\n"
+ + "left_resolution_: " + std::to_string(left_resolution_) + "\n"
+ + "right_resolution_: " + std::to_string(right_resolution_) + "\n"
+ + "left_gain_: " + std::to_string(left_gain_) + "\n"
+ + "right_gain_: " + std::to_string(right_gain_) + "\n";
+ }
};
typedef std::shared_ptr<IntrinsicsDiffDrive> IntrinsicsDiffDrivePtr;
diff --git a/include/core/sensor/sensor_odom_2D.h b/include/core/sensor/sensor_odom_2D.h
index 02218e869..94bbd03d3 100644
--- a/include/core/sensor/sensor_odom_2D.h
+++ b/include/core/sensor/sensor_odom_2D.h
@@ -25,6 +25,11 @@ struct IntrinsicsOdom2D : public IntrinsicsBase
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");
}
+ std::string print()
+ {
+ return "\n" + IntrinsicsBase::print() + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
+ + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n";
+ }
};
WOLF_PTR_TYPEDEFS(SensorOdom2D);
diff --git a/include/core/sensor/sensor_odom_3D.h b/include/core/sensor/sensor_odom_3D.h
index dffaf433e..827e4e2cf 100644
--- a/include/core/sensor/sensor_odom_3D.h
+++ b/include/core/sensor/sensor_odom_3D.h
@@ -36,6 +36,14 @@ struct IntrinsicsOdom3D : public IntrinsicsBase
min_disp_var = _server.getParam<Scalar>(_unique_name + "/min_disp_var");
min_rot_var = _server.getParam<Scalar>(_unique_name + "/min_rot_var");
}
+ std::string print()
+ {
+ return "\n" + IntrinsicsBase::print() + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
+ + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n"
+ + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"
+ + "min_disp_var: " + std::to_string(min_disp_var) + "\n"
+ + "min_rot_var: " + std::to_string(min_rot_var) + "\n";
+ }
virtual ~IntrinsicsOdom3D() = default;
};
--
GitLab