Skip to content
Snippets Groups Projects
Commit 99fa8038 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Remove sensorPtr from processor constructors' API

parent 955ef21e
No related branches found
No related tags found
1 merge request!313WIP: Resolve "Processor constructors and creators requiring a sensor pointer?"
Pipeline #4162 passed
...@@ -14,10 +14,10 @@ ...@@ -14,10 +14,10 @@
namespace wolf namespace wolf
{ {
ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params) : ProcessorRangeBearing::ProcessorRangeBearing(ProcessorParamsBasePtr _params) :
ProcessorBase("RANGE BEARING", _params) ProcessorBase("RANGE BEARING", _params)
{ {
H_r_s = transform(_sensor_ptr->getP()->getState(), _sensor_ptr->getO()->getState()); //
} }
void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
...@@ -103,11 +103,10 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name, ...@@ -103,11 +103,10 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params, const ProcessorParamsBasePtr _params,
const SensorBasePtr _sen_ptr) const SensorBasePtr _sen_ptr)
{ {
SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sen_ptr); auto params = std::static_pointer_cast<ProcessorParamsRangeBearing>(_params);
ProcessorParamsRangeBearingPtr params = std::static_pointer_cast<ProcessorParamsRangeBearing>(_params);
// construct processor // construct processor
ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params); auto prc = std::make_shared<ProcessorRangeBearing>(params);
// setup processor // setup processor
prc->setName(_unique_name); prc->setName(_unique_name);
...@@ -119,11 +118,10 @@ ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _uniqu ...@@ -119,11 +118,10 @@ ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _uniqu
const ParamsServer& _server, const ParamsServer& _server,
const SensorBasePtr _sensor_ptr) const SensorBasePtr _sensor_ptr)
{ {
SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sensor_ptr); auto params = std::make_shared<ProcessorParamsRangeBearing>(_unique_name, _server);
ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>(_unique_name, _server);
// construct processor // construct processor
ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params); auto prc = std::make_shared<ProcessorRangeBearing>(params);
// setup processor // setup processor
prc->setName(_unique_name); prc->setName(_unique_name);
......
...@@ -45,9 +45,9 @@ class ProcessorRangeBearing : public ProcessorBase ...@@ -45,9 +45,9 @@ class ProcessorRangeBearing : public ProcessorBase
public: public:
typedef Eigen::Transform<Scalar, 2, Eigen::Isometry> Trf; typedef Eigen::Transform<Scalar, 2, Eigen::Isometry> Trf;
ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params); ProcessorRangeBearing(ProcessorParamsBasePtr _params);
virtual ~ProcessorRangeBearing() {/* empty */} virtual ~ProcessorRangeBearing() {/* empty */}
virtual void configure(SensorBasePtr _sensor) override { } virtual void configure(SensorBasePtr _sensor) override;
// Factory method for high level API // Factory method for high level API
static ProcessorBasePtr create(const std::string& _unique_name, static ProcessorBasePtr create(const std::string& _unique_name,
...@@ -84,6 +84,11 @@ class ProcessorRangeBearing : public ProcessorBase ...@@ -84,6 +84,11 @@ class ProcessorRangeBearing : public ProcessorBase
Eigen::Vector2s rect (Scalar range, Scalar bearing) const; Eigen::Vector2s rect (Scalar range, Scalar bearing) const;
}; };
inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
{
H_r_s = transform(_sensor->getP()->getState(), _sensor->getO()->getState());
}
} /* namespace wolf */ } /* namespace wolf */
#endif /* HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ */ #endif /* HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ */
...@@ -60,7 +60,7 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3D); ...@@ -60,7 +60,7 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3D);
class ProcessorOdom3D : public ProcessorMotion class ProcessorOdom3D : public ProcessorMotion
{ {
public: public:
ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DPtr _sensor_ptr = nullptr); ProcessorOdom3D(ProcessorParamsOdom3DPtr _params);
virtual ~ProcessorOdom3D(); virtual ~ProcessorOdom3D();
virtual void configure(SensorBasePtr _sensor) override; virtual void configure(SensorBasePtr _sensor) override;
......
...@@ -2,18 +2,19 @@ ...@@ -2,18 +2,19 @@
namespace wolf namespace wolf
{ {
ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DPtr _sensor_ptr) : ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params) :
ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, _params), ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, _params),
params_odom_3D_(_params), params_odom_3D_ (_params),
p1_(nullptr), p2_(nullptr), p_out_(nullptr), k_disp_to_disp_ (0),
q1_(nullptr), q2_(nullptr), q_out_(nullptr) k_disp_to_rot_ (0),
{ k_rot_to_rot_ (0),
configure(_sensor_ptr); min_disp_var_ (0.1), // around 10cm error
delta_ = deltaZero(); min_rot_var_ (0.1), // around 6 degrees error
delta_integrated_ = deltaZero(); p1_(nullptr), p2_(nullptr), p_out_(nullptr),
jacobian_delta_preint_.setZero(delta_cov_size_, delta_cov_size_); q1_(nullptr), q2_(nullptr), q_out_(nullptr)
jacobian_delta_.setZero(delta_cov_size_, delta_cov_size_); {
} //
}
ProcessorOdom3D::~ProcessorOdom3D() ProcessorOdom3D::~ProcessorOdom3D()
{ {
...@@ -21,25 +22,16 @@ ProcessorOdom3D::~ProcessorOdom3D() ...@@ -21,25 +22,16 @@ ProcessorOdom3D::~ProcessorOdom3D()
void ProcessorOdom3D::configure(SensorBasePtr _sensor) void ProcessorOdom3D::configure(SensorBasePtr _sensor)
{ {
if (_sensor) assert (_sensor && "Trying to configure processor with a sensor but provided sensor is nullptr.");
{
SensorOdom3DPtr sen_ptr = std::static_pointer_cast<SensorOdom3D>(_sensor); SensorOdom3DPtr sen_ptr = std::static_pointer_cast<SensorOdom3D>(_sensor);
// we steal the parameters from the provided odom3D sensor.
k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor(); // we steal the parameters from the provided odom3D sensor.
k_disp_to_rot_ = sen_ptr->getDispVarToRotNoiseFactor(); k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor();
k_rot_to_rot_ = sen_ptr->getRotVarToRotNoiseFactor(); k_disp_to_rot_ = sen_ptr->getDispVarToRotNoiseFactor();
min_disp_var_ = sen_ptr->getMinDispVar(); k_rot_to_rot_ = sen_ptr->getRotVarToRotNoiseFactor();
min_rot_var_ = sen_ptr->getMinRotVar(); min_disp_var_ = sen_ptr->getMinDispVar();
} min_rot_var_ = sen_ptr->getMinRotVar();
else
{
// we put default params.
k_disp_to_disp_ = 0;
k_disp_to_rot_ = 0;
k_rot_to_rot_ = 0;
min_disp_var_ = 0.1; // around 30cm error
min_rot_var_ = 0.1; // around 9 degrees error
}
} }
void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data, void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data,
...@@ -344,12 +336,10 @@ Motion ProcessorOdom3D::interpolate(const Motion& _ref1, ...@@ -344,12 +336,10 @@ Motion ProcessorOdom3D::interpolate(const Motion& _ref1,
ProcessorBasePtr ProcessorOdom3D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr) ProcessorBasePtr ProcessorOdom3D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
{ {
// cast inputs to the correct type // cast inputs to the correct type
std::shared_ptr<ProcessorParamsOdom3D> prc_odo_params = std::static_pointer_cast<ProcessorParamsOdom3D>(_params); auto params = std::static_pointer_cast<ProcessorParamsOdom3D>(_params);
SensorOdom3DPtr sen_odo =std::static_pointer_cast<SensorOdom3D>(_sen_ptr);
// construct processor // construct processor
ProcessorOdom3DPtr prc_odo = std::make_shared<ProcessorOdom3D>(prc_odo_params, sen_odo); auto prc_odo = std::make_shared<ProcessorOdom3D>(params);
// setup processor // setup processor
prc_odo->setName(_unique_name); prc_odo->setName(_unique_name);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment