diff --git a/include/laser/sensor/sensor_laser_2D.h b/include/laser/sensor/sensor_laser_2D.h index 89d3ce7687247615237b4bfcd0f1055d047c3ce7..64caa44e3fd96f374c90b08faa7f1b0336a3c218 100644 --- a/include/laser/sensor/sensor_laser_2D.h +++ b/include/laser/sensor/sensor_laser_2D.h @@ -31,11 +31,11 @@ class SensorLaser2D : public SensorBase public: /** \brief Constructor with extrinsics - * + * * \param _p_ptr StateBlock pointer to the sensor position * \param _o_ptr StateBlock pointer to the sensor orientation - * - **/ + * + **/ SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr); /** \brief Constructor with extrinsics and scan parameters @@ -58,25 +58,26 @@ class SensorLaser2D : public SensorBase SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params); virtual ~SensorLaser2D(); - + void setDefaultScanParams(); - + /** \brief Set scanner intrinsic parameters - * + * * \param _params struct with scanner intrinsic parameters. See laser_scan_utils library API for reference. - * - **/ + * + **/ void setScanParams(const laserscanutils::LaserScanParams & _params); /** \brief Get scanner intrinsic parameters - * + * * Get scanner intrinsic parameters - * - **/ + * + **/ const laserscanutils::LaserScanParams & getScanParams() const; public: static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_po, const IntrinsicsBasePtr _intrinsics); + static SensorBasePtr createAutoConf(const std::string& _unique_name, const paramsServer& _server); static IntrinsicsBasePtr createParams(const std::string& _filename_dot_yaml); }; diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp index 5a7edd7725a5a604c49ded7ecb8ed8a4fd04b471..f1ddf33abe02f9d732a62e6182f7dcec8d1b1dbc 100644 --- a/src/sensor/sensor_laser_2D.cpp +++ b/src/sensor/sensor_laser_2D.cpp @@ -79,6 +79,23 @@ SensorBasePtr SensorLaser2D::create(const std::string& _unique_name, const Eigen return sen; } +SensorBasePtr SensorLaser2D::createAutoConf(const std::string& _unique_name, const paramsServer& _server) +{ + // decode extrinsics vector + Eigen::VectorXs _extrinsics_po = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pos", "[0,0,0]"); + assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D."); + StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_po.head(2), true); + StateBlockPtr ori_ptr = std::make_shared<StateBlock>(_extrinsics_po.tail(1), true); + + SensorLaser2DPtr sen; + IntrinsicsLaser2D params; + + sen = std::make_shared<SensorLaser2D>(pos_ptr, ori_ptr, params.scan_params); + + sen->setName(_unique_name); + return sen; +} + } // namespace wolf // Register in the SensorFactory and the ParameterFactory @@ -88,3 +105,7 @@ namespace wolf { WOLF_REGISTER_SENSOR("LASER 2D", SensorLaser2D) //const bool registered_laser_params = IntrinsicsFactory::get().registerCreator("LASER 2D", createIntrinsicsLaser2D); } // namespace wolf +#include "core/sensor/autoconf_sensor_factory.h" +namespace wolf { +WOLF_REGISTER_SENSOR_AUTO("LASER 2D", SensorLaser2D) +} // namespace wolf