From 87a1b8d84ce9fef5240dd2b4b9f9b22bb0072823 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Josep=20Mart=C3=AD?= <jmarti@iri.upc.edu>
Date: Mon, 20 May 2019 11:46:01 +0200
Subject: [PATCH] WIP
---
include/laser/sensor/sensor_laser_2D.h | 23 ++++++++++++-----------
src/sensor/sensor_laser_2D.cpp | 21 +++++++++++++++++++++
2 files changed, 33 insertions(+), 11 deletions(-)
diff --git a/include/laser/sensor/sensor_laser_2D.h b/include/laser/sensor/sensor_laser_2D.h
index 89d3ce768..64caa44e3 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 5a7edd772..f1ddf33ab 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
--
GitLab