WIP

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);
 
 };

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