diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp index a4500bf3b441ee640a74c58a67197eb4b168aff7..df2f34fefc71f382af1fd76326971b0a9d0207ac 100644 --- a/hello_wolf/hello_wolf_autoconf.cpp +++ b/hello_wolf/hello_wolf_autoconf.cpp @@ -118,8 +118,8 @@ int main() problem->print(4,0,1,0); // recover sensor pointers for later use (access by sensor name) - SensorBasePtr sensor_odo = problem->getSensor("odom"); - SensorBasePtr sensor_rb = problem->getSensor("rb"); + SensorBasePtr sensor_odo = problem->getSensor("sen odom"); + SensorBasePtr sensor_rb = problem->getSensor("sen rb"); // Solver. Configure a Ceres solver ceres::Solver::Options options; diff --git a/hello_wolf/hello_wolf_config.yaml b/hello_wolf/hello_wolf_config.yaml index da25853a6a6222a181560fecfa1fe828a542a0b3..62b8738103ca4ff4f14b32fd9ee70d001762648c 100644 --- a/hello_wolf/hello_wolf_config.yaml +++ b/hello_wolf/hello_wolf_config.yaml @@ -12,23 +12,25 @@ config: sensors: - type: "ODOM 2D" - name: "odom" + name: "sen odom" + extrinsic: + pose: [0,0, 0] k_disp_to_disp: 0.1 k_rot_to_rot: 0.1 - extrinsic: - pose: [0,0,0] - type: "RANGE BEARING" - name: "rb" + name: "sen rb" + extrinsic: + pose: [1,1, 0, 0] noise_range_metres_std: 0.1 noise_bearing_degrees_std: 0.5 processors: - type: "ODOM 2D" - name: "odom" - sensor_name: "odom" + name: "prc odom" + sensor_name: "sen odom" time_tolerance: 0.1 voting_active: true voting_aux_active: false @@ -41,8 +43,8 @@ config: - type: "RANGE BEARING" - name: "rb" - sensor_name: "rb" + name: "prc rb" + sensor_name: "sen rb" voting_active: false voting_aux_active: false time_tolerance: 0.1 diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp index 964b909338dfd1062012c73e1aaf0a7b33999295..4f0e8f136d08a058ed68d7426db409e34f8b6c2e 100644 --- a/hello_wolf/sensor_range_bearing.cpp +++ b/hello_wolf/sensor_range_bearing.cpp @@ -23,6 +23,12 @@ SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const 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() { // @@ -42,16 +48,6 @@ SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, // 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 */ // Register in the SensorFactory diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h index 2c33482365adf14e1b5ec468cc8cc4ca51c65154..036fe4cd1115e02ad4f6d2bd65f570e392038b57 100644 --- a/hello_wolf/sensor_range_bearing.h +++ b/hello_wolf/sensor_range_bearing.h @@ -43,14 +43,15 @@ class SensorRangeBearing : public SensorBase { public: SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const Eigen::Vector2s& _noise_std); + SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const IntrinsicsRangeBearingPtr _intr); virtual ~SensorRangeBearing(); // Factory method for high level API static SensorBasePtr create(const std::string& _unique_name, // const Eigen::VectorXs& _extrinsics, // const IntrinsicsBasePtr _intrinsics); - static SensorBasePtr createAutoConf(const std::string& _unique_name, // - const ParamsServer& _server); + WOLF_CREATE_SENSOR_AUTO(SensorRangeBearing, IntrinsicsRangeBearing, 3); + }; } /* namespace wolf */ diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h index 20b09ae4995461b34811e79cd5c53e66ffb08a66..3a1d31afc7bfc6d862b158027fe5d8269803c87d 100644 --- a/include/core/sensor/sensor_diff_drive.h +++ b/include/core/sensor/sensor_diff_drive.h @@ -63,7 +63,8 @@ class SensorDiffDrive : public SensorBase public: 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); }; diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp index 6cfe3c6121d8422d2744b3bae1f94cc737071303..cb7ac7cf7671a48bd695f22e72843a0882727a59 100644 --- a/src/sensor/sensor_diff_drive.cpp +++ b/src/sensor/sensor_diff_drive.cpp @@ -50,23 +50,23 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name, const Eig return diff_drive; } -SensorBasePtr SensorDiffDrive::createAutoConf(const std::string& _unique_name, const ParamsServer& _server) -{ - // extrinsics - Eigen::VectorXs extrinsics = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pos"); - assert(extrinsics.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D."); - - // intrinsics - IntrinsicsDiffDrivePtr params = std::make_shared<IntrinsicsDiffDrive>(_unique_name, _server); - - // build sensor - SensorDiffDrivePtr diff_drive = std::make_shared<SensorDiffDrive>(extrinsics, params); - - // last details - diff_drive ->setName(_unique_name); - - return diff_drive; -} +//SensorBasePtr SensorDiffDrive::createAutoConf(const std::string& _unique_name, const ParamsServer& _server) +//{ +// // extrinsics +// Eigen::VectorXs extrinsics = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pose"); +// assert(extrinsics.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D."); +// +// // intrinsics +// IntrinsicsDiffDrivePtr params = std::make_shared<IntrinsicsDiffDrive>(_unique_name, _server); +// +// // build sensor +// SensorDiffDrivePtr diff_drive = std::make_shared<SensorDiffDrive>(extrinsics, params); +// +// // last details +// diff_drive ->setName(_unique_name); +// +// return diff_drive; +//} } /* namespace wolf */