diff --git a/include/laser/sensor/sensor_laser_2D.h b/include/laser/sensor/sensor_laser_2D.h index 200f00badcd386b2ea2d6945f6d244e71b993b1a..f352cfc46a4d693398a228242873a0c2a67eea26 100644 --- a/include/laser/sensor/sensor_laser_2D.h +++ b/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); diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp index d741f365f3c12239074fb5350e4b269d0a23cfc2..7edaac553c2bc2190deb71f978cfb48bff58d55c 100644 --- a/src/yaml/sensor_laser_2D_yaml.cpp +++ b/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 diff --git a/test/yaml/sensor_laser_2D.yaml b/test/yaml/sensor_laser_2D.yaml new file mode 100644 index 0000000000000000000000000000000000000000..709c3fe33b82769fc5ba0941351106589ec4546a --- /dev/null +++ b/test/yaml/sensor_laser_2D.yaml @@ -0,0 +1,14 @@ +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