diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h index 00592e5d6053dc0e75292b077529c19cbdc1a00d..a6b2e702003978c80a8c301513eccca05f28b1c5 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 5b80ff6b8f03ab7b9151252f29b99341212975b6..587259b40fd486015460a9d1e29c7573b589dcb9 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 d3e9e1ef41788c1c94dd3759fe06524ca4e1930a..d79f9e143eef67e982233b92df7a3cf289f9c100 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 3955b3cb35b1e5846f6d9260ef361b043bd3bfd8..372c25282212f5990c2cb7ebb8c2c4e4fec0ddfc 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 1746fd548a5a7dfb0e5f9cdaa860527b720992ae..46fc538d9a05f53181f7d3d4d5e46a7e0e6a4dc4 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 2b5f1a4942f662d78b3fd97acb9714f8a00ff9ee..60fbf8d295e0dd519e4d5a69cf4dc52e18349811 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 5b971769b72a357d2b775fa4503670ada36711c9..d6cacdccdd701c40fb38b0db1349ab39f4580dd0 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 90d47452d1334d731b39c6154edfef8400573b0a..fdcf820856bb061e445273a72334b20698b9564b 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 38683c57f04c5b6908bac19e26b5cc9ee3353e49..b5fc2dedf099e74bfbd64e7b28ca313cdd1bd412 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 56be18b64f9e71b2dabe5b4fde223174711326ce..9f85d65f658b1790f886ae01300c7aeb550d5c5d 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 2e05d2fe57dfb3494cd3b33c2d697510ff493849..9002ae8fe51a10c4fbb0bdbc39f4600d4e063fe1 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 c1cbd7c382c58244e0e6a12f31b27d472b3507de..114be0e4d3342ed4936be3d0d9a6938e0735f898 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 02218e8698790d207e61594befd403bca916862a..94bbd03d3778221d1a7a2a0d51935f993e9ddbb7 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 dffaf433ebd1ff29a258aec53e5478874b51f7c7..827e4e2cf59d4d660a7d72ea5811f6a66d27f775 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; };