Parsers for SensorLaser2D

include/laser/sensor/sensor_laser_2D.h

@@ -20,6 +20,33 @@ struct IntrinsicsLaser2D : public IntrinsicsBase
         virtual ~IntrinsicsLaser2D() = default;
 
         laserscanutils::LaserScanParams scan_params;
+
+        IntrinsicsLaser2D() = default;
+        IntrinsicsLaser2D(std::string _unique_name, const wolf::ParamsServer & _server) :
+            IntrinsicsBase(_unique_name, _server)
+        {
+            scan_params.angle_min_      = _server.getParam<double>(_unique_name + "/angle_min");
+            scan_params.angle_max_      = _server.getParam<double>(_unique_name + "/angle_max");
+            scan_params.angle_step_     = _server.getParam<double>(_unique_name + "/angle_step");
+            scan_params.scan_time_      = _server.getParam<double>(_unique_name + "/scan_time");
+            scan_params.range_min_      = _server.getParam<double>(_unique_name + "/range_min");
+            scan_params.range_max_      = _server.getParam<double>(_unique_name + "/range_max");
+            scan_params.range_std_dev_  = _server.getParam<double>(_unique_name + "/range_std_dev");
+            scan_params.angle_std_dev_  = _server.getParam<double>(_unique_name + "/angle_std_dev");
+        }
+        std::string print()
+        {
+            return "\n" + IntrinsicsBase::print()                                   + "\n"
+            + "angle_min: "         + std::to_string(scan_params.angle_min_)        + "\n"
+            + "angle_max: "         + std::to_string(scan_params.angle_max_)        + "\n"
+            + "angle_step: "        + std::to_string(scan_params.angle_step_)       + "\n"
+            + "scan_time: "         + std::to_string(scan_params.scan_time_)        + "\n"
+            + "range_min: "         + std::to_string(scan_params.range_min_)        + "\n"
+            + "range_max: "         + std::to_string(scan_params.range_max_)        + "\n"
+            + "range_std_dev: "     + std::to_string(scan_params.range_std_dev_)    + "\n"
+            + "angle_std_dev: "     + std::to_string(scan_params.angle_std_dev_)    + "\n";
+        }
+
 };
 
 WOLF_PTR_TYPEDEFS(SensorLaser2D);

src/yaml/sensor_laser_2D_yaml.cpp

@@ -24,8 +24,31 @@ IntrinsicsBasePtr createIntrinsicsLaser2D(const std::string& _filename_dot_yaml)
 {
     // If required: Parse YAML
 
-    IntrinsicsLaser2DPtr params; // dummy
+    WOLF_INFO("IntrinsicsLaser2D: Parsing file: ", _filename_dot_yaml);
+
+    YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+    if (config["type"].as<std::string>() == "LASER 2D")
+    {
+
+
+    IntrinsicsLaser2DPtr params = std::make_shared<IntrinsicsLaser2D>();
+
+    params->scan_params.angle_min_      = config["/angle_min"]      .as<double>();
+    params->scan_params.angle_max_      = config["/angle_max"]      .as<double>();
+    params->scan_params.angle_step_     = config["/angle_step"]     .as<double>();
+    params->scan_params.scan_time_      = config["/scan_time"]      .as<double>();
+    params->scan_params.range_min_      = config["/range_min"]      .as<double>();
+    params->scan_params.range_max_      = config["/range_max"]      .as<double>();
+    params->scan_params.range_std_dev_  = config["/range_std_dev"]  .as<double>();
+    params->scan_params.angle_std_dev_  = config["/angle_std_dev"]  .as<double>();
+
     return params;
+    }
+
+    std::cout << "Bad configuration file. No processor type found." << std::endl;
+    return nullptr;
+
 }
 
 // register into factory

test/yaml/sensor_laser_2D.yaml

+type          : "LASER 2D"
+
+# from sensor base
+  # no params so far
+  
+# from sensor laser 2D
+angle_min     : 1
+angle_max     : 2
+angle_step    : 3
+scan_time     : 4
+range_min     : 5
+range_max     : 6
+range_std_dev : 7
+angle_std_dev : 8