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

Use new macro for createAutoConf()

parent ceb1af00
No related branches found
No related tags found
2 merge requests!313WIP: Resolve "Processor constructors and creators requiring a sensor pointer?",!305WIP: Resolve "Do not use default YAML params"
...@@ -118,8 +118,8 @@ int main() ...@@ -118,8 +118,8 @@ int main()
problem->print(4,0,1,0); problem->print(4,0,1,0);
// recover sensor pointers for later use (access by sensor name) // recover sensor pointers for later use (access by sensor name)
SensorBasePtr sensor_odo = problem->getSensor("odom"); SensorBasePtr sensor_odo = problem->getSensor("sen odom");
SensorBasePtr sensor_rb = problem->getSensor("rb"); SensorBasePtr sensor_rb = problem->getSensor("sen rb");
// Solver. Configure a Ceres solver // Solver. Configure a Ceres solver
ceres::Solver::Options options; ceres::Solver::Options options;
......
...@@ -12,23 +12,25 @@ config: ...@@ -12,23 +12,25 @@ config:
sensors: sensors:
- -
type: "ODOM 2D" type: "ODOM 2D"
name: "odom" name: "sen odom"
extrinsic:
pose: [0,0, 0]
k_disp_to_disp: 0.1 k_disp_to_disp: 0.1
k_rot_to_rot: 0.1 k_rot_to_rot: 0.1
extrinsic:
pose: [0,0,0]
- -
type: "RANGE BEARING" type: "RANGE BEARING"
name: "rb" name: "sen rb"
extrinsic:
pose: [1,1, 0, 0]
noise_range_metres_std: 0.1 noise_range_metres_std: 0.1
noise_bearing_degrees_std: 0.5 noise_bearing_degrees_std: 0.5
processors: processors:
- -
type: "ODOM 2D" type: "ODOM 2D"
name: "odom" name: "prc odom"
sensor_name: "odom" sensor_name: "sen odom"
time_tolerance: 0.1 time_tolerance: 0.1
voting_active: true voting_active: true
voting_aux_active: false voting_aux_active: false
...@@ -41,8 +43,8 @@ config: ...@@ -41,8 +43,8 @@ config:
- -
type: "RANGE BEARING" type: "RANGE BEARING"
name: "rb" name: "prc rb"
sensor_name: "rb" sensor_name: "sen rb"
voting_active: false voting_active: false
voting_aux_active: false voting_aux_active: false
time_tolerance: 0.1 time_tolerance: 0.1
......
...@@ -23,6 +23,12 @@ SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const ...@@ -23,6 +23,12 @@ SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const
assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2D"); assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2D");
} }
SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const IntrinsicsRangeBearingPtr _intr) :
SensorRangeBearing(_extrinsics, Eigen::Vector2s(_intr->noise_range_metres_std, toRad(_intr->noise_bearing_degrees_std)))
{
//
}
SensorRangeBearing::~SensorRangeBearing() SensorRangeBearing::~SensorRangeBearing()
{ {
// //
...@@ -42,16 +48,6 @@ SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, // ...@@ -42,16 +48,6 @@ SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, //
return sensor; return sensor;
} }
SensorBasePtr SensorRangeBearing::createAutoConf(const std::string& _unique_name, //
const ParamsServer& _server)
{
IntrinsicsRangeBearing intr(_unique_name, _server);
Eigen::Vector2s noise_std(intr.noise_range_metres_std, intr.noise_bearing_degrees_std);
SensorRangeBearingPtr sensor = std::make_shared<SensorRangeBearing>(Eigen::Vector3s(1,1,0), noise_std);
sensor->setName(_unique_name);
return sensor;
}
} /* namespace wolf */ } /* namespace wolf */
// Register in the SensorFactory // Register in the SensorFactory
......
...@@ -43,14 +43,15 @@ class SensorRangeBearing : public SensorBase ...@@ -43,14 +43,15 @@ class SensorRangeBearing : public SensorBase
{ {
public: public:
SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const Eigen::Vector2s& _noise_std); SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const Eigen::Vector2s& _noise_std);
SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const IntrinsicsRangeBearingPtr _intr);
virtual ~SensorRangeBearing(); virtual ~SensorRangeBearing();
// Factory method for high level API // Factory method for high level API
static SensorBasePtr create(const std::string& _unique_name, // static SensorBasePtr create(const std::string& _unique_name, //
const Eigen::VectorXs& _extrinsics, // const Eigen::VectorXs& _extrinsics, //
const IntrinsicsBasePtr _intrinsics); const IntrinsicsBasePtr _intrinsics);
static SensorBasePtr createAutoConf(const std::string& _unique_name, // WOLF_CREATE_SENSOR_AUTO(SensorRangeBearing, IntrinsicsRangeBearing, 3);
const ParamsServer& _server);
}; };
} /* namespace wolf */ } /* namespace wolf */
......
...@@ -63,7 +63,8 @@ class SensorDiffDrive : public SensorBase ...@@ -63,7 +63,8 @@ class SensorDiffDrive : public SensorBase
public: public:
static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics); static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
static SensorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server); // static SensorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server);
WOLF_CREATE_SENSOR_AUTO(SensorDiffDrive, IntrinsicsDiffDrive, 3);
}; };
......
...@@ -50,23 +50,23 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name, const Eig ...@@ -50,23 +50,23 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name, const Eig
return diff_drive; return diff_drive;
} }
SensorBasePtr SensorDiffDrive::createAutoConf(const std::string& _unique_name, const ParamsServer& _server) //SensorBasePtr SensorDiffDrive::createAutoConf(const std::string& _unique_name, const ParamsServer& _server)
{ //{
// extrinsics // // extrinsics
Eigen::VectorXs extrinsics = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pos"); // Eigen::VectorXs extrinsics = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pose");
assert(extrinsics.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D."); // assert(extrinsics.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D.");
//
// intrinsics // // intrinsics
IntrinsicsDiffDrivePtr params = std::make_shared<IntrinsicsDiffDrive>(_unique_name, _server); // IntrinsicsDiffDrivePtr params = std::make_shared<IntrinsicsDiffDrive>(_unique_name, _server);
//
// build sensor // // build sensor
SensorDiffDrivePtr diff_drive = std::make_shared<SensorDiffDrive>(extrinsics, params); // SensorDiffDrivePtr diff_drive = std::make_shared<SensorDiffDrive>(extrinsics, params);
//
// last details // // last details
diff_drive ->setName(_unique_name); // diff_drive ->setName(_unique_name);
//
return diff_drive; // return diff_drive;
} //}
} /* namespace wolf */ } /* namespace wolf */
......
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